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()