'WalkingRobotControlProgram

'M.EricCarr/Paleotechnologist.Net
'eric@paleotechnologist.net
'WrittenforMintorisBASIC4.0for
'Androiddevices;targetistheSIGMA
'walkingrobotwithaRS41Bluetooth
'modulecontrollerfromSparkfun.com.
'TestedonanEpic4GrunningFroyo.

dima$(0)
dimc(4)
'Accelerometerthresholdvalues(inmeters/sec^2)
'
stopThreshold=3'Thresholdforstopvsslow
slowThreshold=6'Thresholdforslowvsfast
status=1'Usedtostorethecurrentstate;
'onlysendcommandsifitchanges.
'1isinvalidandwillupdateon
'thefirstpass.

'MakealistofthepairedBluetoothdevices
'
a$()=btgetpaired$()

'Showthelistandchooseone;putitind$
'
lista$(),d$

'StartaBluetoothconnectionattempt
'
print"Connectingto:";d$
name$=ItemExtract$(d$,0)
address$=ItemExtract$(d$,1)

BTConnect1,address$

'Polltheconnectionstate,
'waitingforaconnection.
'
fori=1to100
printBTGetState(1);"/";
printBTGetError$(1)
ifBTGetstate(1)=4then
print"Connected!"
exitfor
endif
wait500
nexti

'Bluetoothconnected.
'PuttheremotemodeminCommandMode.
'
print"Initializing";
forx=1to5
BTWrite1,"$$$"
print".";
wait500
nextx
print

'SetallBTmoduleGPIOstoOutputmode
'
BTWrite1,"S@,FFFF"
BTWrite1,chr$(13)
BTWrite1,chr$(10)

'Turnonthephoneaccelerometer
'
sensorson

'Mainprogramloop
'===================================
Top:
'Robotcommandreference:
'("Translated"meansD3/D2/D1/D0>D7/D6/D4/D3
'becausenotallGPIOareavailableonBTmodule)
'ThecommandconsistsofliteralS&,twohex
'nibblesforaffectedports(all),andtwohex
'nibblesforportstatus(highorlow).
'ThesecommandsarespecifictotheRS41
'BluetoothmodulefromSparkfun,soit'snormal
'thatthey'renotveryintuitive.Theoriginal
'codes(00through0C)aretheimplemented
'commandcodesfortheSIGMAservoMCU.
'=================================================
'CodeDescriptionTrans.Command(string)
'
0x00=Donothing0x00S&,FF00
'
0x01=Forward0x08S&,FF08
'
0x02=Backward0x10S&,FF10
'
0x03=Turnleft0x18S&,FF18
'
0x04=Turnright0x40S&,FF40
'
0x05=Spinleft0x48S&,FF48
'
0x06=Spinright0x50S&,FF50
'
0x07=Standlow0x58S&,FF58
'
0x08=Standmid0x80S&,FF80
'
0x09=Standhigh0x88S&,FF88
'
0x0A=Crouch0x90S&,FF90
'
0x0B=Liedown0x98S&,FF98
'
0x0C="Sit"0xC0S&,FFC0
'0x0D=(unused)0xC8S&,FFC8
'0x0E=(unused)0xD0S&,FFD0
'0x0F=(unused)0xD8S&,FFD8

'GetXYZaccelerations;storetheminc()
c()=getaccelerometer()
'c(0)isxunusedsofar
'c(1)isypositivemeansturnright
'c(2)iszpositivemeansgoforward

'Handletheindividualcases
'===========================
'IfYandZarenearzero,send"StandLow"(S&,FF58)
'(Thismightbebetteroffas"DoNothing"(S&,FF00)
'
ifabs(c(1))<stopThresholdandabs(c(2))<stopThresholdthen
ifstatus<>0then
BTWrite1,"S&,FF58"+chr$(13)+chr$(10)
print"StandLow"
status=0
gotoSkipout
endif
endif
'IfYisnearzeroandZisoverthresholdpositive,
'send"Forward"(S&,FF08)
'
ifabs(c(1))<stopThresholdandc(2)>=stopThresholdthen
ifstatus<>1then
BTWrite1,"S&,FF08"+chr$(13)+chr$(10)
print"Forward"
status=1
gotoSkipout
endif
endif
'IfYisnearzeroandZisoverthresholdnegative,
'send"Backward"(S&,FF10)
'
ifabs(c(1))<slowThresholdandc(2)<=(0stopThreshold)then
ifstatus<>2then
BTWrite1,"S&,FF10"+chr$(13)+chr$(10)
print"Backward"
status=2
gotoSkipout
endif
endif

'IfZisnearzeroandYisoverthresholdpositive,
'send"Spinright"(S&,FF50)
'
ifc(1)>=stopThresholdandabs(c(2))<stopThresholdthen
ifstatus<>6then
BTWrite1,"S&,FF50"+chr$(13)+chr$(10)
print"Spinright"
status=6
gotoSkipout
endif
endif
'IfZisnearzeroandYisoverthresholdnegative,
'send"Spinleft"(S&,FF48)
'
ifc(1)<=(0stopThreshold)andabs(c(2))<stopThresholdthen
ifstatus<>5then
BTWrite1,"S&,FF48"+chr$(13)+chr$(10)
print"Spinleft"
status=5
gotoSkipout
endif
endif
'IfYisoverhighthresholdpositive,
'send"Spinright"(S&,FF50)
'
ifc(1)>=slowThresholdthen
ifstatus<>6then
BTWrite1,"S&,FF50"+chr$(13)+chr$(10)
print"Spinright"
status=6
gotoSkipout
endif
endif
'IfYisoverhighthresholdnegative,
'send"Spinleft"(S&,FF48)
'
ifc(1)<=(0slowThreshold)then
ifstatus<>5then

BTWrite1,"S&,FF48"+chr$(13)+chr$(10)
print"Spinleft"
status=5
gotoSkipout
endif
endif

'IfYandZareoverthresholdpositive,
'send"Turnright"(S&,FF40)
'
ifc(1)>=stopThresholdandc(2)>=stopThresholdthen
ifstatus<>4then
BTWrite1,"S&,FF40"+chr$(13)+chr$(10)
print"Turnright"
status=4
gotoSkipout
endif
endif
'IfYisoverthresholdnegativeandZisoverthreshold
positive,
'send"Turnleft"(S&,FF18)
'
ifc(1)<=(0stopThreshold)andc(2)>=stopThresholdthen
ifstatus<>3then
BTWrite1,"S&,FF18"+chr$(13)+chr$(10)
print"Turnleft"
status=3
gotoSkipout
endif
endif

'(Ifwegethere,donothing.)

Skipout:
wait100'Onlyupdateat10Hzsoasnottoconfusethe
servos.
'(eventhisisprobablytoofast.)

'Gobackanddoitallagain.
'
gotoTop

'ClosetheBTconnectionandturnthesensorsoff
'(eventhoughwe'llnevergethere...)
'
BTClose1
sensorsoff