8/9/2019 Bt Robocon Troll

1/7

'Walking Robot Control Program'M. Eric Carr / Paleotechnologist.Net'eric@paleotechnologist.net

'Written for Mintoris BASIC 4.0 for'Android devices; target is the SIGMA'walking robot with a RS-41 Bluetooth'module controller from Sparkfun.com.

'Tested on an Epic 4G running Froyo.

dim a$(0)dim c(4)

'Accelerometer threshold values (in meters/sec^2)'------------------------------------------------stopThreshold=3 'Threshold for stop-vs-slowslowThreshold=6 'Threshold for slow-vs-fast

status=-1 'Used to store the current state; ' only send commands if it changes. '-1 is invalid and will update on ' the first pass.

'Make a list of the paired Bluetooth devices'-------------------------------------------a$()=btgetpaired$()

'Show the list and choose one; put it in d$'------------------------------------------list a$(), d$

'Start a Bluetooth connection attempt'------------------------------------print "Connecting to: ";d$name$ = ItemExtract$(d$, 0)address$ = ItemExtract$(d$, 1)

8/9/2019 Bt Robocon Troll

2/7

BTConnect 1, address$

'Poll the connection state,'waiting for a connection.'--------------------------for i = 1 to 100 print BTGetState(1);" / "; print BTGetError$(1) if BTGetstate(1)=4 then

print "Connected!" exit for endif wait 500 next i

'Bluetooth connected.'Put the remote modem in Command Mode.'--------------------------------------print "Initializing";for x=1 to 5 BTWrite 1,"$$$" print ".";

wait 500 next xprint

'Set all BT module GPIOs to Output mode'--------------------------------------BTWrite 1, "S@,FFFF"BTWrite 1, chr$(13)BTWrite 1, chr$(10)

'Turn on the phone accelerometer'-------------------------------sensors on

8/9/2019 Bt Robocon Troll

3/7

'Main program loop'===================================Top:

'Robot command reference:'("Translated" means D3/D2/D1/D0 --> D7/D6/D4/D3' because not all GPIO are available on BT module)'The command consists of literal S& , two hex' nibbles for affected ports (all), and two hex' nibbles for port status (high or low).'These commands are specific to the RS-41' Bluetooth module from Sparkfun, so it's normal' that they're not very intuitive. The original' codes (00 through 0C) are the implemented' command codes for the SIGMA servo MCU.'=================================================' Code Description Trans. Command (string)' 0x00 = Do nothing 0x00 S&,FF00' 0x01 = Forward 0x08 S&,FF08' 0x02 = Backward 0x10 S&,FF10' 0x03 = Turn left 0x18 S&,FF18' 0x04 = Turn right 0x40 S&,FF40' 0x05 = Spin left 0x48 S&,FF48' 0x06 = Spin right 0x50 S&,FF50

' 0x07 = Stand low 0x58 S&,FF58' 0x08 = Stand mid 0x80 S&,FF80' 0x09 = Stand high 0x88 S&,FF88' 0x0A = Crouch 0x90 S&,FF90' 0x0B = Lie down 0x98 S&,FF98' 0x0C = "Sit" 0xC0 S&,FFC0' 0x0D = (unused) 0xC8 S&,FFC8' 0x0E = (unused) 0xD0 S&,FFD0' 0x0F = (unused) 0xD8 S&,FFD8

'Get XYZ accelerations; store them in c()c()=getaccelerometer()

'c(0) is x -- unused so far'c(1) is y -- positive means turn right'c(2) is z -- positive means go forward

8/9/2019 Bt Robocon Troll

4/7

'Handle the individual cases'===========================

'If Y and Z are near zero, send "Stand Low" (S&,FF58)'(This might be better off as "Do Nothing" (S&,FF00)'----------------------------------------------------if abs(c(1))

8/9/2019 Bt Robocon Troll

5/7

'If Z is near zero and Y is over threshold positive,' send "Spin right" (S&,FF50)'----------------------------------------------------if c(1)>=stopThreshold and abs(c(2))=slowThreshold then if status6 then BTWrite 1,"S&,FF50"+chr$(13)+chr$(10) print "Spin right" status=6 goto Skipout endif

endif

'If Y is over high threshold negative,'send "Spin left" (S&,FF48)'----------------------------------------------------if c(1)

8/9/2019 Bt Robocon Troll

6/7

BTWrite 1,"S&,FF48"+chr$(13)+chr$(10) print "Spin left" status=5 goto Skipout endif endif

'If Y and Z are over threshold positive,' send "Turn right" (S&,FF40)'----------------------------------------------------if c(1)>=stopThreshold and c(2)>=stopThreshold then if status4 then BTWrite 1,"S&,FF40"+chr$(13)+chr$(10) print "Turn right" status=4 goto Skipout endif endif

'If Y is over threshold negative and Z is over thresholdpositive,' send "Turn left" (S&,FF18)'----------------------------------------------------

if c(1)=stopThreshold then if status3 then BTWrite 1,"S&,FF18"+chr$(13)+chr$(10) print "Turn left" status=3 goto Skipout endif endif

'(If we get here, do nothing.)

Skipout:wait 100 'Only update at 10Hz so as not to confuse theservos. '(even this is probably too fast.)

8/9/2019 Bt Robocon Troll

7/7

'Go back and do it all again.'----------------------------goto Top

'Close the BT connection and turn the sensors off'(even though we'll never get here...)'------------------------------------------------BTClose 1sensors off