Etiqueta: motor

IR remote control – speed and position

iConor’s second LIRC configuration file,Combo_PWM.conf, allows us to use LIRC command ‘irsend’ to control the speed of a LEGO Power Functions Motor or the position of a LEGO Power Functions Servo Motor much like the 8879 IR Speed Remote Control.

This shell script sends a sequence of 7 increasingly values then returns to zero and repeat it in the opposite direction:

#!/bin/bash
#0
irsend SEND_ONCE LEGO_Combo_PWM 400B ; sleep  1.2
#1/7
irsend SEND_ONCE LEGO_Combo_PWM 401A ; sleep  1.2
#2/7
irsend SEND_ONCE LEGO_Combo_PWM 4029 ; sleep  1.2
#3/7
irsend SEND_ONCE LEGO_Combo_PWM 4038 ; sleep  1.2
#4/7
irsend SEND_ONCE LEGO_Combo_PWM 404F ; sleep  1.2
#5/7
irsend SEND_ONCE LEGO_Combo_PWM 405E ; sleep  1.2
#6/7
irsend SEND_ONCE LEGO_Combo_PWM 406D ; sleep  1.2
#7/7
irsend SEND_ONCE LEGO_Combo_PWM 407C ; sleep  1.2
#0
irsend SEND_ONCE LEGO_Combo_PWM 4083 ; sleep  1.2
#-1/7
irsend SEND_ONCE LEGO_Combo_PWM 40F4 ; sleep  1.2
#-2/7
irsend SEND_ONCE LEGO_Combo_PWM 40E5 ; sleep  1.2
#-3/7
irsend SEND_ONCE LEGO_Combo_PWM 40D6 ; sleep  1.2
#-4/7
irsend SEND_ONCE LEGO_Combo_PWM 40C7 ; sleep  1.2
#-5/7
irsend SEND_ONCE LEGO_Combo_PWM 40B0 ; sleep  1.2
#-6/7
irsend SEND_ONCE LEGO_Combo_PWM 40A1 ; sleep  1.2
#-7/7
irsend SEND_ONCE LEGO_Combo_PWM 4092 ; sleep  1.2

There is a total of 256 key codes in the configuration file (16 different values for the red plug x 16 different values for the blue plug). I used just the first subset of 16 so the first byte (for the blue plug) is always ’40’ but it doesn’t matter because I connected both Servo and M-motor to the red plug .

The sequence of key codes is not exactly the same as in the configuration file – I reordered the last 7 key codes.

The ‘sleep 1.2’  makes the script wait 1.2 seconds between each command. It is more or less the time the IR keeps the command active (bigger values would make the Servo return to center and the M-motor stop).

SBrick – controlo remoto com um gamepad

Apresento o meu script em python para controlar o SBrick com um gamepad a partir do Linux. Recorro à biblioteca PyGame para ler o gamepad (assumindo que o gamepad é suportado nativamente pelo Linux, ver também o meu artigo sobre como utiizar um gamepad com ev3dev) e ao comando gatttool do BlueZ 5.0 para comunicar via Bluetooth BLE com o SBrick (assumindo também a presença de um dongle Bluetooth 4.0).

 

Este script funciona bem com Ubuntu mas deverá também funcionar em qualquer variante de Debian incluindo Raspbian (no Raspberry Pi) e ev3dev (no LEGO Mindstorms EV3, onde utilizei uma versão inicial deste script).

#!/usr/bin/env python

# sudo apt-get install python-pygame

import sys, traceback, os
os.environ['SDL_VIDEODRIVER'] = 'dummy'
from math import log10
from subprocess import call
from time import sleep
from pygame import joystick, event, display

### buttons ###
B_TRIANG = 0
B_CIRC = 1
B_CROSS = 2
B_SQUARE = 3
B_LTRIG2 = 4
B_RTRIG2 = 5
B_LTRIG = 6
B_RTRIG = 7
B_SELECT = 8
B_LJOY = 10
B_RJOY = 11
B_START = 9


def main():
  try:
    display.init();
    joystick.init();
    js=joystick.Joystick(0);
    js.init();

    DRIVE_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0102"
    DRIVE_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0103"
    COAST_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01020000"
    COAST_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01030000"
    BREAK_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0002"
    BREAK_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0003"

    ### starts in Joystick mode ###
    control_by_JOYSTICK=True;

    num_axes=js.get_numaxes();
    num_buttons=js.get_numbuttons();
    num_hats=js.get_numhats();

    ### assuming 4 axes, 13 buttons and 1 hat

    flag=False;

    while True:
      x=y=motor_r=motor_l=0.0;
      event.pump();

      button_mode=js.get_button(B_SELECT);
      button_shot=js.get_button(B_SQUARE);

      if button_mode ==1:

        if control_by_JOYSTICK==True:
          control_by_JOYSTICK=False;
          print 'Control Mode=HAT';
        else:
          control_by_JOYSTICK=True;
          print 'Control Mode=JOYSTICK';


      ### joysticks axis [-1, +1]
      ### x=axis2 , y=-axis3
      ### ignore less than 0.2 (dead zone)
      ### apply log10(100x) (to reforce lower values)
      ### result is less or equal than 2 = log10(100)

      if control_by_JOYSTICK==True:

            # Control by Right Joystick, Axis 2 e 3

            axis2=js.get_axis(2);
            axis3=js.get_axis(3);

        if axis2>0:
          if axis2<0.2:
            x=0;
          else:
            x=log10(axis2*100);
        elif axis2<0:
          if axis2>-0.2:
            x=0;
          else:
            x=-log10(-axis2*100);
            else:
              x=0;

        if axis3>0:
          if axis3<0.2:
            y=0;
          else:
            y=-log10(axis3*100);
        elif axis3<0:
          if axis3>-0.2:
            y=0;
          else:
            y=log10(-axis3*100);
        else:
          y=0;

        if y<>0:
          if x<0:
            motor_r=100*y;
            # turn left => slow motor_l
               motor_l=y*(100+25*x);
          else:
            motor_el=100*y;
            # turn right => slow motor_r
            motor_r=y*(100-25*x);
        elif x<>0:
           # y=0, just turn
           motor_l=100*x;
           motor_r=-motor_l;

      else:

         # Control by HAT keys

         hat=js.get_hat(0);

         if hat==(0,1):
#            print 'FRONT';
            motor_r=100;
            motor_l=100;
          elif hat==(1,0):
#            print 'RIGHT';
            motor_l=100;
            motor_r=-100;
         elif hat==(0,-1):
#            print 'BACK';
            motor_r=-100;
            motor_l=-100;
         elif hat==(-1,0):
#            print 'LEFT';
            motor_l=-100;
            motor_r=100;
         elif hat==(1,1):
#            print 'FRONT+RIGHT';
            motor_l=100;
            motor_r=50;
         elif hat==(-1,1):
#            print 'FRONT+LEFT';
            motor_l=50;
            motor_r=100;
         elif hat==(-1,-1):
#            print 'BACK+LEFT';
            motor_l=-100;
            motor_r=-50;
         elif hat==(1,-1):
#            print 'BACK+RIGHT';
            motor_l=-50;
            motor_r=-100;


       # get direction and duty cycle

      if (motor_l<0):
     dir_l="00"
     duty_l=str(hex(int(-motor_l)))
      else:
         dir_l="01"
         duty_l=str(hex(int(motor_l)))

      if (motor_r<0):
     dir_r="01"
     duty_r=str(hex(int(-motor_r)))
      else:
         dir_r="00"
         duty_r=str(hex(int(motor_r)))

      # command+direction+dutycyle

      command_A=DRIVE_A+dir_r+duty_r[2:]
      command_B=DRIVE_B+dir_l+duty_l[2:]
      call(command_A, shell=True);
      call(command_B, shell=True);
      sleep(0.1)
#      call(BREAK_A,shell=True);
#      call(BREAK_B,shell=True);
      call(COAST_A,shell=True);
      call(COAST_B,shell=True);

    # end while

  except (KeyboardInterrupt, SystemExit):
      print "Exiting...";
  except Exception:
      traceback.print_exc(file=sys.stdout);

  js.quit();
  joystick.quit();
  display.quit();
  sys.exit(0);

if __name__ == "__main__":
    main()

SBrick – remote control with a wireless gamepad

Here is my python script for controlling SBrick with a gamepad from Linux. It uses pygame for reading the gamepad (as long as it’s supported by the kernel, see also my post about using a gamepad with ev3dev) and gatttool from BlueZ 5.x to talk to the SBrick (you need a BT 4.0 USB dongle)

It should work in Ubuntu and other Debian variants including Raspbian (Raspberry Pi) or ev3dev (LEGO Mindstorms EV3)

#!/usr/bin/env python

# sudo apt-get install python-pygame

import sys, traceback, os
os.environ['SDL_VIDEODRIVER'] = 'dummy'
from math import log10
from subprocess import call
from time import sleep
from pygame import joystick, event, display

### buttons ###
B_TRIANG = 0
B_CIRC = 1
B_CROSS = 2
B_SQUARE = 3
B_LTRIG2 = 4
B_RTRIG2 = 5
B_LTRIG = 6
B_RTRIG = 7
B_SELECT = 8
B_LJOY = 10
B_RJOY = 11
B_START = 9


def main():
  try:
    display.init();
    joystick.init();
    js=joystick.Joystick(0);
    js.init();

    DRIVE_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0102"
    DRIVE_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0103"
    COAST_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01020000"
    COAST_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01030000"
    BREAK_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0002"
    BREAK_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0003"

    ### starts in Joystick mode ###
    control_by_JOYSTICK=True;

    num_axes=js.get_numaxes();
    num_buttons=js.get_numbuttons();
    num_hats=js.get_numhats();

    ### assuming 4 axes, 13 buttons and 1 hat

    flag=False;

    while True:
      x=y=motor_r=motor_l=0.0;
      event.pump();

      button_mode=js.get_button(B_SELECT);
      button_shot=js.get_button(B_SQUARE);

      if button_mode ==1:

        if control_by_JOYSTICK==True:
          control_by_JOYSTICK=False;
          print 'Control Mode=HAT';
        else:
          control_by_JOYSTICK=True;
          print 'Control Mode=JOYSTICK';


      ### joysticks axis [-1, +1]
      ### x=axis2 , y=-axis3
      ### ignore less than 0.2 (dead zone)
      ### apply log10(100x) (to reforce lower values)
      ### result is less or equal than 2 = log10(100)

      if control_by_JOYSTICK==True:

            # Control by Right Joystick, Axis 2 e 3

            axis2=js.get_axis(2);
            axis3=js.get_axis(3);

        if axis2>0:
          if axis2<0.2:
            x=0;
          else:
            x=log10(axis2*100);
        elif axis2<0:
          if axis2>-0.2:
            x=0;
          else:
            x=-log10(-axis2*100);
            else:
              x=0;

        if axis3>0:
          if axis3<0.2:
            y=0;
          else:
            y=-log10(axis3*100);
        elif axis3<0:
          if axis3>-0.2:
            y=0;
          else:
            y=log10(-axis3*100);
        else:
          y=0;

        if y<>0:
          if x<0:
            motor_r=100*y;
            # turn left => slow motor_l
               motor_l=y*(100+25*x);
          else:
            motor_el=100*y;
            # turn right => slow motor_r
            motor_r=y*(100-25*x);
        elif x<>0:
           # y=0, just turn
           motor_l=100*x;
           motor_r=-motor_l;

      else:

         # Control by HAT keys

         hat=js.get_hat(0);

         if hat==(0,1):
#            print 'FRONT';
            motor_r=100;
            motor_l=100;
          elif hat==(1,0):
#            print 'RIGHT';
            motor_l=100;
            motor_r=-100;
         elif hat==(0,-1):
#            print 'BACK';
            motor_r=-100;
            motor_l=-100;
         elif hat==(-1,0):
#            print 'LEFT';
            motor_l=-100;
            motor_r=100;
         elif hat==(1,1):
#            print 'FRONT+RIGHT';
            motor_l=100;
            motor_r=50;
         elif hat==(-1,1):
#            print 'FRONT+LEFT';
            motor_l=50;
            motor_r=100;
         elif hat==(-1,-1):
#            print 'BACK+LEFT';
            motor_l=-100;
            motor_r=-50;
         elif hat==(1,-1):
#            print 'BACK+RIGHT';
            motor_l=-50;
            motor_r=-100;


       # get direction and duty cycle

      if (motor_l<0):
     dir_l="00"
     duty_l=str(hex(int(-motor_l)))
      else:
         dir_l="01"
         duty_l=str(hex(int(motor_l)))

      if (motor_r<0):
     dir_r="01"
     duty_r=str(hex(int(-motor_r)))
      else:
         dir_r="00"
         duty_r=str(hex(int(motor_r)))

      # command+direction+dutycyle

      command_A=DRIVE_A+dir_r+duty_r[2:]
      command_B=DRIVE_B+dir_l+duty_l[2:]
      call(command_A, shell=True);
      call(command_B, shell=True);
      sleep(0.1)
#      call(BREAK_A,shell=True);
#      call(BREAK_B,shell=True);
      call(COAST_A,shell=True);
      call(COAST_B,shell=True);

    # end while

  except (KeyboardInterrupt, SystemExit):
      print "Exiting...";
  except Exception:
      traceback.print_exc(file=sys.stdout);

  js.quit();
  joystick.quit();
  display.quit();
  sys.exit(0);

if __name__ == "__main__":
    main()

 

 

Relés USB

Adquiri um módulo USB-RLY16 que permite comandar 8 relés através de uma porta USB. A ideia original é controlar as minifiguras LEGO em palco pelo que com 8 relés poderei controlar 4 micromotores LEGO de forma independente e em ambos os sentidos (os Xutos&Pontapés são 5 pelo que 2 deles terão de partilhar o mesmo motor ou terei de arranjar outra forma para controlar um quinto motor).

O módulo é baseado num circuito FT232RL que emula uma porta série através da ligação USB. Em Linux o suporte a este tipo de devices já vem incluído no kernel sendo associado um device do tipo ‘/dev/ttyUSBn’. No meu PC com Ubuntu 14.04 funcionou à primeira, tal como no EV3 com ev3dev e estou 100% convencido que com Raspberry Pi e Raspbian acontecerá o mesmo.

São necessárias duas alimentações: uma para o accionamento dos relés e outra (ou outras) para os circuitos controlados pelos relés. Teoricamente podem ser as mesmas mas optei por separá-las:

  • uma pilha PP3 com ficha de 2.1mm para o accionamento dos relés
  • uma pilha PP3 em caixa LEGO para os motores

Nota: a pseudo-documentação afirma serem necessários pelo menos 12V/500 mA para o accionamento dos relés mas dei-me bem com os 9V de uma pilha alcalina PP3.

Ligando ao EV3:

$ dmesg
(...)
usb 1-1.4: new full-speed USB device number 6 using ohci
usbcore: registered new interface driver usbserial
usbcore: registered new interface driver ftdi_sio
usbserial: USB Serial support registered for FTDI USB Serial Device
ftdi_sio 1-1.4:1.0: FTDI USB Serial Device converter detected
usb 1-1.4: Detected FT232RL
usb 1-1.4: Number of endpoints 2
usb 1-1.4: Endpoint 1 MaxPacketSize 64
usb 1-1.4: Endpoint 2 MaxPacketSize 64
usb 1-1.4: Setting MaxPacketSize 64
usb 1-1.4: FTDI USB Serial Device converter now attached to ttyUSB0

Para comunicar com a placa é necessário configurar a porta série virtual (/dev/ttyUSB0) para comunicar a 19200 baud, 8 databits, 2 stop bits, sem paridade (19200 8N2) e sem hardware nem software handshaking. No Ubuntu usei o ‘puTTy’ mas como requer ambiente gráfico no ev3dev optei pelo ‘minicom’:

$ apt-get install minicom

O minicom tem um modo de setup:

$ minicom -s

depois de configurado (19200 8N2, no flow control) se carregarmos na tecla ‘d’ accionamos todos os relés e na tecla ‘n’ desactivamos todos (confirmem numa tabela ASCII que correspondem aos códigos decimais 100 e 110). Confirmado o funcionamento podemos sair (‘Control+A’ seguido de ‘Q’), a porta virtual permanece configurada até ao próximo reboot.

Podemos agora enviar comandos directamente da shell:

$ echo d > /dev/ttyUSB0
$ echo n > /dev/ttyUSB0

Em python podemos comunicar directamente com a porta virtual através da biblioteca pyserial (http://pyserial.sourceforge.net/):

$ apt-get install python-serial
import serial
from time import sleep

port = serial.Serial("/dev/ttyUSB0", baudrate=19200, bytesize=8, parity='N', stopbits=2, timeout=None, xonxoff=0, rtscts=0)
port.write("d");
sleep(1);
port.write("n");
sleep(1);
port.close()

O video acima corresponde ao script python abaixo:

import serial
from time import sleep


# da esquerda para a direita:
# motores A, B, C e D
#
# motor A e/f o/p
# motor B g/h q/r
# motor C i/j s/t
# motor D k/l u/v
#

def relays_all_off():
  port.write("n");

def relays_all_on():
  port.write("d");

### Motor A ###
def motor_A_Right():
  port.write("ep");

def motor_A_Left():
  port.write("of");

def motor_A_OFF():
  port.write("op");

### Motor B ###
def motor_B_Right():
  port.write("gr");

def motor_B_Left():
  port.write("qh");

def motor_B_OFF():
  port.write("qr");

### Motor C ###
def motor_C_Right():
  port.write("it");

def motor_C_Left():
  port.write("sj");

def motor_C_OFF():
  port.write("st");

### Motor D ###
def motor_D_Right():
  port.write("kv");

def motor_D_Left():
  port.write("ul");

def motor_D_OFF():
  port.write("uv");


port = serial.Serial("/dev/ttyUSB0", baudrate=19200, bytesize=8, parity='N', stopbits=2, timeout=None, xonxoff=0, rtscts=0)

motor_A_Left();
motor_B_Left();
motor_C_Left();
motor_D_Left();
sleep(1);
motor_A_OFF();
motor_B_OFF();
motor_C_OFF();
motor_D_OFF();
sleep(1);
motor_A_Right();
motor_B_Right();
motor_C_Right();
motor_D_Right();
sleep(1);
motor_A_OFF();
motor_B_OFF();
motor_C_OFF();
motor_D_OFF();
sleep(2);

motor_A_Left();
sleep(1);
motor_A_OFF();
sleep(1);
motor_B_Left();
sleep(1);
motor_B_OFF();
sleep(1);
motor_C_Left();
sleep(1);
motor_C_OFF();
sleep(1);
motor_D_Left();
sleep(1);
motor_D_OFF();
sleep(1);

port.close();

 

ev3dev – controlando motores

A utilização básica do LEGO Mindstorms EV3 com ev3dev fica completa com a utilização de motores.

Ligando um motor (45503 EV3 Medium Servo Motor) na ficha A, o sistema operativo acusa o evento:

root@ev3dev:~# dmesg
(...)
tacho-motor tacho-motor0: Tacho motor registered.
ev3-tacho-motor outA:motor: A Tacho Motor connected to port outA gpio 91 irq 192

Uma vez mais é necessário consultar a wiki do projecto ev3dev para entender como aceder aos motores. Estes também são mapeados pelo sistema operativo, desta vez em “/sys/bus/legoev3/devices/” e em ” /sys/class/tacho-motor/”:

root@ev3dev:~# ls /sys/bus/legoev3/devices/
in2  in3  in4  outA  outA:motor  outB  outC  outD
root@ev3dev:~# ls /sys/class/tacho-motor/
tacho-motor0
root@ev3dev:~# ls /sys/class/tacho-motor/tacho-motor0
device           port_name      pulses_per_second     reset        speed_regulation_K  subsystem
duty_cycle     position       pulses_per_second_sp  run            speed_regulation_P  time_sp
duty_cycle_sp  position_mode  ramp_down_sp        run_mode        state            type
estop           position_sp    ramp_up_sp        speed_regulation_D    stop_mode        uevent
polarity_mode  power          regulation_mode        speed_regulation_I    stop_modes

Muito informação para processar mas para começar podemos confirmar apenas o tipo de motor:

root@ev3dev:~# cat /sys/class/tacho-motor/tacho-motor0/type
minitacho

Os motores Mindstorms são inicializados no modo “run forever”, podemos também confirmar isso:

root@ev3dev:~# cat /sys/class/tacho-motor/tacho-motor0/run_mode
forever

Neste modo podemos controlar o motor controlando 2 parâmetros:

  • /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
  • /sys/class/tacho-motor/tacho-motor0/run

o shell script abaixo faz o motor rodar 5 segundos num sentido e 5 segundos no sentido inverso, com um duty factor de 50%:

 #!/bin/bash
 echo   0 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
 echo   1 > /sys/class/tacho-motor/tacho-motor0/run
 echo   50 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
 sleep 5s
 echo   0 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
 sleep 1s
 echo   -50 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
 sleep 5s
 echo   0 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp
 echo   0 > /sys/class/tacho-motor/tacho-motor0/run