Due to budget reasons, I cannot get an ATOM for quite a while. And since I’m free during the summer I figured that I would try to make the BRAT work with the BS2, SSC-32, and BB2.
Of course I based my code entirely off of the ATOM autonomous code:
lynxmotion.com/images/files/brata1.bas
Theres a few sections I have not yet changed, but the idea is basically there.
Also, I’m going to have problems with the variables. I believe BS2 can only support 13 16bit variables. For now the variable size have been left to bytes and nibs. I tried to remove as much variable as possible.
Also, the BS2 does not support the HSERVO command. So I did some work:
SEROUT 15,6,"#0P", DEC -lefthippos*160,"S", DEC lhspeed, "#1P", DEC righthippos*160,"S", DEC rhspeed, "#2P", DEC -leftkneepos*160,"S", DEC lkspeed, "#16P", DEC rightkneepos, DEC rkspeed,"#17P", DEC -leftanklepos*160, DEC raspeed, "#18P", DEC rightanklepos*160, DEC lkspeed,"T", 1000, 13]Here i’m assuming the stepsperdegree is 160, the number doesn’t matter here.
The var lefthippos, righthippos, etc , are used as positions in HSERVO, but I’m not sure if it can serve as the pulsewidth in SSC-32. However I’m pretty sure that the speed in both HSERVO and SSC-32 are the same.
Here is the code:
[code]’ {$STAMP BS2}
’ {$PBASIC 2.5}
'variable size needs to be fixed, but kept like this for now
lefthippos VAR Byte
leftkneepos VAR Byte
leftanklepos VAR Byte
righthippos VAR Byte
rightkneepos VAR Byte
rightanklepos VAR Byte
last_lefthippos VAR Byte
last_leftkneepos VAR Byte
last_leftanklepos VAR Byte
last_righthippos VAR Byte
last_rightkneepos VAR Byte
last_rightanklepos VAR Byte
lhspeed VAR Byte
lkspeed VAR Byte
laspeed VAR Byte
rhspeed VAR Byte
rkspeed VAR Byte
raspeed VAR Byte
speed VAR Byte
longest VAR Byte
one VAR Nib
two VAR Nib
three VAR Nib
four VAR Nib
five VAR Nib
six VAR Nib
xx VAR Byte
command VAR Byte
'change to alter the sequence
main:
command = 1
FOR xx = 1 TO 5
GOSUB move
NEXT
command = 11
FOR xx = 1 TO 2
GOSUB move
NEXT
command = 3
FOR xx = 1 TO 3
GOSUB move
NEXT
command = 5
GOSUB move
command = 11
FOR xx = 1 TO 2
GOSUB move
NEXT
command = 1
FOR xx = 1 TO 4
GOSUB move
NEXT
command = 9
GOSUB move
end
'of course this section does not work in the BS2, but the basic idea is there
'_____________________________________________________________________________________________________________
move:
IF(command = 1) THEN ’ Walk Forward
lefthippos = 0
leftkneepos = 0
leftanklepos = 0
righthippos = 0
rightkneepos = 0
rightanklepos = 0
speed=500
GOSUB movement
'GOSUB movement 7.0,-20.0,-20.0, -7.0, 20.0, 20.0, 500.0]
’ GOSUB movement -7.0,-20.0,-20.0, 7.0, 20.0, 20.0, 500.0]
’ GOSUB movement -7.0, 20.0, 20.0, 7.0,-20.0,-20.0, 500.0]
’ GOSUB movement 7.0, 20.0, 20.0, -7.0,-20.0,-20.0, 500.0]
ELSEIF(command = 2) 'Walk Backwards
GOSUB movement -7.0,-20.0,-20.0, 7.0, 20.0, 20.0, 500.0]
GOSUB movement 7.0,-20.0,-20.0, -7.0, 20.0, 20.0, 500.0]
GOSUB movement 7.0, 20.0, 20.0, -7.0,-20.0,-20.0, 500.0]
GOSUB movement -7.0, 20.0, 20.0, 7.0,-20.0,-20.0, 500.0]
ELSEIF(command = 3) ’ Long Stride Forward
GOSUB movement 12.0,-45.0,-45.0,-12.0, 45.0, 45.0, 750.0]
GOSUB movement -12.0,-45.0,-45.0, 12.0, 45.0, 45.0, 750.0]
GOSUB movement -12.0, 45.0, 45.0, 12.0,-45.0,-45.0, 750.0]
GOSUB movement 12.0, 45.0, 45.0,-12.0,-45.0,-45.0, 750.0]
ELSEIF(command = 4) ’ Long Stride Backward
GOSUB movement -12.0,-45.0,-45.0, 12.0, 45.0, 45.0, 750.0]
GOSUB movement 12.0,-45.0,-45.0,-12.0, 45.0, 45.0, 750.0]
GOSUB movement 12.0, 45.0, 45.0,-12.0,-45.0,-45.0, 750.0]
GOSUB movement -12.0, 45.0, 45.0, 12.0,-45.0,-45.0, 750.0]
ELSEIF(command = 5) ’ Kick
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
GOSUB movement 42.0, 0.0, 0.0,-14.0, 0.0, 0.0, 500.0]
GOSUB movement 0.0,-32.0, 41.0,-23.0, 0.0, 0.0, 500.0]
GOSUB movement 0.0, 24.0,-20.0,-23.0, 0.0, 0.0, 250.0]
GOSUB movement 0.0, 0.0, 0.0,-18.0, 0.0, 0.0, 500.0]
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
sound 9,[50\3960]
PAUSE 50
sound 9,[50\4400]
PAUSE 50
sound 9,[50\3960]
ELSEIF(command = 7) ’ Get up from front
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
GOSUB movement 0.0, 90.0, 45.0, 0.0, 90.0, 45.0, 500.0]
GOSUB movement 40.0, 90.0,-37.0, 0.0, 90.0, 45.0, 500.0]
GOSUB movement 0.0, 90.0,-65.0, 0.0, 90.0,-65.0, 500.0]
PAUSE 500 ’ THEN Bow
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 750.0]
sound 9,[50\4400]
PAUSE 50
sound 9,[50\4400]
PAUSE 50
sound 9,[50\3960]
ELSEIF(command = 8) ’ Get up from back
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,1000.0]
GOSUB movement 0.0,-90.0, 5.0, 0.0,-90.0, 5.0,1000.0]
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,1000.0]
PAUSE 1000
;GOSUB movement 0.0, 22.0,-80.0, 0.0, 22.0,-80.0,1000.0]
;GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,1000.0]
sound 9,[50\3960]
PAUSE 50
sound 9,[50\3960]
PAUSE 50
sound 9,[50\4400]
ELSEIF(command = 0) ' HOME Position
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
sound 9,[50\4400]
PAUSE 50
sound 9,[50\4400]
PAUSE 50
sound 9,[50\4400]
ELSEIF(command = 6) ' Headbutt
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
GOSUB movement 0.0,-50.0,-90.0, 0.0,-50.0,-90.0, 500.0]
GOSUB movement 0.0, 32.0,-58.0, 0.0, 32.0,-58.0, 400.0]
PAUSE 200
;GOSUB movement 0.0,-11.0, 7.0, 0.0,-11.0, 7.0, 500.0]
GOSUB movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0]
sound 9,[50\4400]
PAUSE 50
sound 9,[50\3960]
PAUSE 50
sound 9,[50\3960]
ELSEIF(command = 11) ' Turn
GOSUB movement 0.0,-35.0,-40.0, 0.0, 35.0, 37.0,500.0]
GOSUB movement 0.0, 35.0, 37.0, 0.0,-35.0,-40.0,500.0]
GOSUB movement -14.0, 35.0, 37.0, 20.0,-35.0,-40.0,500.0]
GOSUB movement -14.0, 35.0, 37.0, 20.0, 35.0, 37.0,500.0]
GOSUB movement 20.0, 35.0, 37.0,-14.0, 35.0, 37.0,500.0]
GOSUB movement 20.0,-35.0,-40.0,-14.0, 35.0, 37.0,500.0]
;GOSUB movement 0.0,-35.0,-70.0, 0.0, 35.0, 7.0,500.0]
;GOSUB movement 0.0, 35.0, 7.0, 0.0,-35.0,-70.0,500.0]
;GOSUB movement -14.0, 35.0, 7.0, 20.0,-35.0,-70.0,500.0]
;GOSUB movement -14.0, 35.0, 7.0, 20.0, 35.0, 7.0,500.0]
;GOSUB movement 20.0, 35.0, 7.0,-14.0, 35.0, 7.0,500.0]
;GOSUB movement 20.0,-35.0,-70.0,-14.0, 35.0, 7.0,500.0]
ELSEIF(command = 9) ' Rest Position
GOSUB movement 0.0, 45.0, 45.0, 0.0, 45.0, 45.0, 500.0]
ENDIF
return
'________________________________________________________________________________________________________________
movement:
IF(speed<>0)THEN
one=lefthippos-last_lefthippos
two=leftkneepos-last_leftkneepos
three= leftanklepos-last_leftanklepos
four= righthippos-last_righthippos
five=rightkneepos-last_rightkneepos
six= rightanklepos-last_rightanklepos
GOSUB getlongest
speed = ((longest*160)/(speed/20))
IF(lefthippos>last_lefthippos) THEN
'((newpos-oldpos)/longest)*maxval
lhspeed=((lefthippos-last_lefthippos)/longest)*speed
ELSE
lhspeed=((-lefthippos+last_lefthippos)/longest)*speed
ENDIF
IF(leftkneepos>last_leftkneepos)THEN
lkspeed=((leftkneepos-last_leftkneepos)/longest)*speed
ELSE
lkspeed=((-leftkneepos+last_leftkneepos)/longest)*speed
ENDIF
IF(leftanklepos>last_leftanklepos) THEN
laspeed=((leftanklepos-last_leftanklepos)/longest)*speed
ELSE
laspeed=((-leftanklepos+last_leftanklepos)/longest)*speed
ENDIF
IF(righthippos>last_righthippos) THEN
rhspeed=((righthippos-last_righthippos)/longest)*speed
ELSE
rhspeed=((-righthippos+last_righthippos)/longest)*speed
eNDIF
IF(rightkneepos>last_rightkneepos) THEN
rkspeed=((rightkneepos-last_rightkneepos)/longest)*speed
ELSE
rkspeed=((-rightkneepos+last_rightkneepos)/longest)*speed
ENDIF
IF(rightanklepos>last_rightanklepos)THEN
raspeed=((rightanklepos-last_rightanklepos)/longest)*speed
ELSE
raspeed=((-rightanklepos+last_rightanklepos)/longest)*speed
ENDIF
' GOSUB getspeed[lefthippos,last_lefthippos,longestmove,speed],lhspeed
’ GOSUB getspeed[leftkneepos,last_leftkneepos,longestmove,speed],lkspeed
’ GOSUB getspeed[leftanklepos,last_leftanklepos,longestmove,speed],laspeed
’ GOSUB getspeed[righthippos,last_righthippos,longestmove,speed],rhspeed
’ GOSUB getspeed[rightkneepos,last_rightkneepos,longestmove,speed],rkspeed
’ GOSUB getspeed[rightanklepos,last_rightanklepos,longestmove,speed],raspeed
ELSE
lhspeed=0
lkspeed=0
laspeed=0
rhspeed=0
rkspeed=0
raspeed=0
ENDIF
’ hservo [lefthip\TOINT (-lefthipposstepsperdegree) + lefthip_start\TOINT lhspeed, |
’ righthip\TOINT (righthipposstepsperdegree) + righthip_start\TOINT rhspeed, |
’ leftknee\TOINT (-leftkneeposstepsperdegree) + leftknee_start\TOINT lkspeed, |
’ rightknee\TOINT (rightkneeposstepsperdegree) + rightknee_start\TOINT rkspeed, |
’ leftankle\TOINT (-leftankleposstepsperdegree) + leftankle_start\TOINT laspeed, |
’ rightankle\TOINT (rightankleposstepsperdegree) + rightankle_start\TOINT raspeed]
SEROUT 15,6,"#0 P1500 #1 P1500 #2 P1500 #16 P1500 #17 P1500 #18 P1500",13]
SEROUT 15,6,"#0P", DEC -lefthippos160,“S”, DEC lhspeed, “#1P”, DEC righthippos160,“S”, DEC rhspeed, “#2P”, DEC -leftkneepos160,“S”, DEC lkspeed, “#16P”, DEC rightkneepos, DEC rkspeed,"#17P", DEC -leftanklepos160, DEC raspeed, “#18P”, DEC rightanklepos*160, DEC lkspeed,“T”, 1000, 13]
getlongest:
IF(one<0)THEN
one=-1one
ENDIF
IF(two<0)THEN
two=-1two
ENDIF
IF(three<0)THEN
three=-1three
ENDIF
IF(four<0)THEN
four=-1four
ENDIF
IF(five<0)THEN
five=-1five
ENDIF
IF(six<0)THEN
six=-1six
ENDIF
IF(one<two)THEN
one=two
ENDIF
IF(one<three)THEN
one=three
ENDIF
IF(one<four)THEN
one=four
ENDIF
IF(one<five)THEN
one=five
ENDIF
IF(one<six)THEN
one=six
ENDIF
longest=one
RETURN
'also not yet converted to BS2 usage
idle VAR Byte
finished VAR Byte
junk VAR Word
sensorloop:
finished = true
gethservo lefthip,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
gethservo righthip,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
gethservo leftknee,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
gethservo rightknee,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
gethservo leftankle,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
gethservo rightankle,junk,idle
IF(NOT idle)THEN
finished=false
ENDIF
'add sensor handling code here
IF(NOT finished)THEN sensorloop
last_lefthippos = lefthippos
last_leftkneepos = leftkneepos
last_leftanklepos = leftanklepos
last_righthippos = righthippos
last_rightkneepos = rightkneepos
last_rightanklepos = rightanklepos
RETURN[/code]
can I get any pointers here? Or am I even on the right track? I was simply converting everything so that the BS2 could potentially understand it, but obviously its not that simple.
Thanks!