Categoria: LEGO

Para terminar a apresentação do AD4M4ST0R faltava apenas o código.

O programa principal é um script escrito em python ‘joy.py’ que lida com o gamepad e os motores. O controlo da webcam e do buzzer é delegado em dois bash scripts (‘saycheese.sh’ e ‘beep.sh’).

O programa principal, “joy.py’:

import sys, traceback, os
os.environ['SDL_VIDEODRIVER'] = 'dummy'

# from math import sqrt
from math import log10
from subprocess import call
from time import sleep
from pygame import joystick, event, display

### botoes ###
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();

    ### arranca em modo Joystick ###
    control_by_JOY=True;

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

    ### sabemos a priori que tem 4 axes, 13 buttons e 1 hat

    ### inicializa motores com Duty Cycle=0
        call("echo 0 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp", shell=True);
        call("echo 0 > /sys/class/tacho-motor/tacho-motor1/duty_cycle_sp", shell=True);
        call("echo 1 > /sys/class/tacho-motor/tacho-motor0/run", shell=True);
        call("echo 1 > /sys/class/tacho-motor/tacho-motor1/run", shell=True);

    flag=False;

    while True:
      x=y=motor_direito=motor_esquerdo=0.0;
      event.pump();

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

      if button_mode ==1:

        call("./beep.sh" , shell=True);

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

      if button_shot==1:
        print 'Cheese!!!';
        call("/root/saycheese.sh", shell=True);


      ### eixo dos joysticks entre -1 e +1
        ### x=axis2 , y=-axis3
      ### ignorar valores abaixo de 0.1 (dead zone)
      ### aplicar log10(100x) aos restantes (para dar enfase aos mais baixos)
      ### o resultado e' 0 ou 1 a 2 por isso
      ###    multiplicar por 25 da' valores Duty Cycle nulos ou entre 25 e 50

      if control_by_JOY==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.1:
            x=0;
          else:
            x=log10(axis2*100);
        elif axis2<0:
          if axis2>-0.1:
            x=0;
          else:
            x=-log10(-axis2*100);

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

        if y<>0:
          if x<0:
            motor_direito=50*y/2;
            # rodar para a esquerda => reduzir motor esquerdo
               motor_esquerdo=motor_direito*(1+x/3);
          else:
            motor_esquerdo=50*y/2;
            # rodar para a direita => reduzir motor direito
            motor_direito=motor_esquerdo*(1-x/3);
        elif x<>0:
           # y=0, apenas roda
           motor_esquerdo=25*x;
           motor_direito=-motor_esquerdo;

        else:

         # Control by HAT keys

         hat=js.get_hat(0);

         if hat==(0,1):
            ### anda em frente ###
            motor_direito=50;
            motor_esquerdo=50;
          elif hat==(1,0):
            ### roda para a direita ###
            motor_esquerdo=50;
            motor_direito=-50;
         elif hat==(0,-1):
            ### recua ###
            motor_direito=-50;
            motor_esquerdo=-50;
         elif hat==(-1,0):
            ### roda para a esquerda ###
            motor_esquerdo=-50;
            motor_direito=50;
         elif hat==(1,1):
            ### frente e roda para a direita ###
            motor_esquerdo=50;
            motor_direito=25;
         elif hat==(-1,1):
            ### frente e roda para a esquerda ###
            motor_esquerdo=25;
            motor_direito=50;
         elif hat==(-1,-1):
            ### recua e roda para a esquerda ###
            motor_esquerdo=-50;
            motor_direito=-25;
         elif hat==(1,-1):
            ### recua e roda para a direita ###
            motor_esquerdo=-25;
            motor_direito=-50;

       comando_motordir = "echo " + repr(motor_direito) + " > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp";
       comando_motoresq = "echo " + repr(motor_esquerdo) + " > /sys/class/tacho-motor/tacho-motor1/duty_cycle_sp";

       call(comando_motordir, shell=True);
       call(comando_motoresq, shell=True);

    # end while

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

    call("echo 0 > /sys/class/tacho-motor/tacho-motor0/duty_cycle_sp", shell=True);
    call("echo 0 > /sys/class/tacho-motor/tacho-motor1/duty_cycle_sp", shell=True);
    call("echo 0 > /sys/class/tacho-motor/tacho-motor0/run", shell=True);
    call("echo 0 > /sys/class/tacho-motor/tacho-motor1/run", shell=True);

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

if __name__ == "__main__":
    main()

O controlo da webcam, ‘saycheese.sh’:

#!/bin/sh
filename=$(date +"%d-%m-%y_%Hh%Mm%Ss")
fswebcam -d /dev/video0 -p MJPEG -r 640x480 -q -D 1 -S 3 -s brightness=10 -s contrast=20  $filename.jpg

O controlo do buzzer, ‘beep.sh’:

#!/bin/bash
echo -en "07" > /dev/tty1

A quem o considerar útil, esteja à vontade para utilizar / feel free to use as you wish.