Controlling the SBrick with a wiimote

I found out that is really easy to use a wiimote with Linux for so as a sequel to my previous ‘Sbrick – remote control with a wireless gamepad‘ I now present you my ‘Controlling the SBrick with a wiimote’ (actually a cheap clone, a N-Play Remote Plus, unfortunately I haven’t found a version with Motion Plus).

The wiimote uses Bluetooth, but doesn’t strictly follow the rules. If we have bluetooth on our PC and want to check if it will work, just press both ‘1’ and ‘2’buttons for it to advertise for a while and then:

$ hcitool -i hci0 scan
Scanning ...
    04:02:16:01:1C:E7    Nintendo RVL-CNT-01

It’s not possible to pair with the wiimote but there are tools for that, like cwiid. As I’m going to use python I installed their python library:

$ sudo apt-get install python-cwiid

There are lots of examples so I’ll show only my final result:

 

And the script I used for the video above:

# apt-get install python-cwiid
import cwiid
from time import sleep
from subprocess import call
from math import log10

# macros for the SBrick
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"


# connecting to the wiimote. This allows several attempts
# as first few often fail.

print 'Press 1+2 on your Wiimote now...'
wm = None
i=1
while not wm:
    try:
        wm=cwiid.Wiimote()
    except RuntimeError:
        if (i>5):
            print("cannot create connection")
            quit()
        print "Error opening wiimote connection"
        print "attempt " + str(i)
        i +=1

#set wiimote to report button presses and accelerometer state
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC

#turn on all led and ruble to show connected
wm.led=15
wm.rumble=True
sleep(0.5)
wm.rumble=False
wm.led=0
sleep(1.5)

# roll = accelerometer[0], standby ~125
# pitch = accelerometer[1], standby ~125

while True:
    buttons = wm.state['buttons']

    #only pay attention when button '1' pressed
    if (buttons & cwiid.BTN_1):

        roll=(wm.state['acc'][0]-125)
        pitch=(wm.state['acc'][1]-125)
        
        if (roll<0):
            if (roll<-4):
                if (roll<-25):
                    roll=-100
                else:
                    roll=-50*log10(-4*roll)
            else:
                roll=0

        if (roll>0):
            if (roll>4):
                if (roll>25):
                    roll=100
                else:
                    roll=50*log10(4*roll)
            else:
                roll=0

        if (pitch>0):
            if (pitch>4):
                if (pitch>25):
                    pitch=100
                else:
                    pitch=50*log10(4*pitch)

            else:
                pitch=0

        if (pitch<0):
            if (pitch<-4):
                if(pitch<-25):
                    pitch=-100
                else:
                    pitch=-50*log10(-4*pitch)
            else:
                pitch=0

        if ((pitch<>0)or(roll<>0)):

            roll=2.5*roll
            pitch=2.5*pitch

            if(pitch<>0):
                if(roll>0):
                    # turn right
                    motor_L=pitch
                    motor_R=-pitch-roll/2

                else:
                    # turn left
                    motor_R=-pitch
                    motor_L=pitch+roll/2

            elif(roll<>0):
                #just rotate
                motor_R=motor_L=roll;

            else:
                # does nothing
                motor_R=motor_L=0

            if((motor_R<>0)or(motor_L<>0)):

                if(motor_R<0):
                    duty=str(hex(int(-motor_R)))
                    command_A=DRIVE_A+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_R)))
                    command_A=DRIVE_A+"01"+duty[2:]

                if(motor_L<0):
                    duty=str(hex(int(-motor_L)))
                    command_B=DRIVE_B+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_L)))
                    command_B=DRIVE_B+"01"+duty[2:]

                #send motors commands to SBrick
                call(command_A, shell=True);
                call(command_B, shell=True);
                sleep(0.1)

                #send COAST commands to SBrick
                call(COAST_A, shell=True);
                call(COAST_B, shell=True);

    else:
        # inactive
        sleep(0.01)