'{$STAMP BS2sx} ' {$PBASIC 2.5} '>Frea8_1.bsx 'History '------- 'Frea8_1.bsx no change from Frea7_1.bs2 '============================================================== 'BRANCH cmnd,[,_FD,_BK,_TURN,_RT,_LT,_STRETCH] '============================================================== '****Common - copy this section into other progs*************** 'Common RAM '---------- 'CRAM_0 spare CRAM_cmnd CON 1 'PROG0 P0_done CON 1 'PROG1 FD CON 1 BK CON 2 TURN CON 3 RT CON 4 LT CON 5 STRETCH CON 6 'PROG2 setlegs CON 1 getup CON 2 CRAM_continuepoint CON 2 'pgm position to GOTO (not used) CRAM_S_speed CON 3 CRAM_flags CON 4 CRAM_LAat CON 5 CRAM_LHat CON 6 CRAM_RAat CON 7 CRAM_RHat CON 8 CRAM_happy CON 9 'P0 only CRAM_avoidctrl CON 10 'P0 only CRAM_lovelight CON 11 'P0 only CRAM63_thisprog CON 63 'Active prog run no '-------------------------------------------------------------- 'PINS '---- redled_ CON 0 'red led greenled_ CON 1 'green eye led spkr CON 2 'speaker ' 3 eyepin CON 4 'LDR ' 5 ' 6 potpin CON 7 'pot 10k + 0u1 => 1 - 1609 s_LH CON 8 'left leg in+ out- Ltoe VAR IN9 'input from left foot 1=>activated s_LA CON 10 'servo left toe in+ out- Sx pin bad - won't input tipF_ VAR IN11 'input from fallen forwards switch tipB_ VAR IN12 'input from fallen backwards switch s_RA CON 13 'right toe in- out+ Sx pin bad - won't input Rtoe VAR IN14 'input from right foot 1=>activated s_RH CON 15 'right leg in- out+ f_flags VAR Byte f_Rfwd VAR f_flags.BIT0 '1 =>rightfootfwd f_RT VAR f_flags.BIT1 '1 =>RT f_stood VAR f_flags.BIT2 '1=>stood cmnd VAR Byte PROG VAR Byte '-------------------------------------------------------------- 'SERVOS '------ LHat VAR Byte'left leg up / down -ve in, +ve out LAat VAR Byte'left foot rotate -ve in, +ve out RAat VAR Byte'right foot rotate -ve in, +ve out RHat VAR Byte'right leg up / down -ve in, +ve out sp VAR Byte'servo pulse, counter - for/next servopulses VAR Byte'end of FOR NEXT framepause CON 20 'pad servo frame pscaleSx CON 10 'servo pulse, 255*10*0.8= 2040us, so servo pulse is from ~0.5ms-2.5ms pscaleBS2 CON 4 'servo pulse, 255*4*2= 2040us pscaleBS2e CON 4 'servo pulse, 255*4*2= 2040us pscaleP CON 11 'servo pulse, 255*10.66*0.75= 2040us - not a good number!! alters all values '250*10*0.8=2000, 250*11*0.75=2062.5, 250*10*0.75=1875, 242.42*11*0.75=2000, ie 11=>+3% 'halfstride 20 =>21, stepheightU 20 =>21, stepheightD 25 =>26, turn 45 =>46 'probably not worth changing them pscale CON pscaleSx '1 logical count is 8us basepulseSx CON 62 'servo pulse,62*10*0.8=496us basepulseBS2 CON 62 'servo pulse,62*4*2=496us basepulseBS2e CON 62 'servo pulse,62*4*2=496us basepulseP CON 60 'servo pulse,60.12*11*0.75=496us, 60*11*0.75=495us, little difference basepulse CON basepulseSx stepheightU CON 20 'empirical, see at servos below, max before leg hits body stepheightD CON 35 'empirical, see at servos below, give a bit extra height halfstride CON 13 'SD200 'empirical stride CON halfstride +halfstride turnstride CON halfstride +15 'SD200 'empirical S_speed VAR Byte s_stepspeed VAR S_speed.LOWNIB 'servo, increment per frame, 6 goes over range, count goes wrong s_turnspeed VAR S_speed.HIGHNIB 'servo, increment per frame, 9 very snappy but Hg bounces in switches s_turnspeedf CON 5 'fast - servo, increment per frame, only used to rotate up foot in turn 'New servos Oct06 'PulseValue = memory +62 *10 620=>496uS 'uS = PulseValue *0.8 =>500 - 2500 'uS = memory +62 *10 *0.8 'memory = uS /0.8 /10 -62 or uS -500 /10 /0.8 so 127 =>1500 ' 'Servo uS 'RAin 2440 'not linear after 'RAmid 1440 'mid toes equal about CL 'RAout 560 'not linear after 'RHin 510 'leg hits body at 500 'RHmid 800 'feet flat =500+300 300=>38 'RHout 2450 'servo max =496+1954 1954=>244 'LAin 500 'not linear after 'LAmid 1480 'mid toes equal about CL 'LAout 2210 'not linear after 'LHin 2520 'servo max - no change if >, leg a little clear of body 'oldLHmid 2300 'feet flat =500+1810 1810=>226 'LHmid 2260 'feet flat =500+1760 1760/10=176 176/0.8=220 'LHout 560 'servo min =496+64 64=>8 556-8=248 'Actual servo byte values 'sRAin CON 3 sRAmid CON 118 '1440=>118 'sRAout CON 225 'sRHin CON 2 ? sRHmid CON 38 ' 38=>800 'sRHout CON 240 ? 'sLAout CON 24 sLAmid CON 123 '1480=>123 'sLAin CON 226 'sLHout CON ? sLHmid CON 220 ' 220=>2260 'sLHin CON ? 'Logical servo byte values - logical to physical conversion is done at pulsout time 'RAin con sRAin RAinstand CON 20 'Prog1 RAtbr CON RAmid -turnstride RAwbr CON RAmid -halfstride RAmid CON sRAmid RAwfr CON RAmid +halfstride RAoutstand CON 200 'Prog1 'RAout con 255 -sRAout 'RHin CON sRHin RHwu CON RHmid -stepheightU '38-20=18 RHmid CON sRHmid '38 RHwd CON RHmid +stepheightD '38+25 RHlieout CON 220 'RHout CON sRHout 'LAin con sLAin '254 LAinstand CON 20 'Prog1 LAtbl CON sLAmid -turnstride LAwbl CON LAmid -halfstride LAmid CON 255 -sLAmid LAwfl CON LAmid +halfstride LAoutstand CON 200 'Prog1 'LAout con sLAout '5 'LHin CON 255 -sLHin LHwu CON LHmid -stepheightU' '29-20=9 LHmid CON 255 -sLHmid '255-226=29 LHwd CON LHmid +stepheightD' '29+25 LHlieout CON 220 'LHout CON 255 -sLHout '****END common *********************************************** '============================================================== start: DEBUG CR,"P1" GET CRAM_cmnd,cmnd GET CRAM_flags,f_flags GET CRAM_LAat,LAat GET CRAM_LHat,LHat GET CRAM_RAat,RAat GET CRAM_RHat,RHat GET CRAM_S_speed,S_speed BRANCH cmnd,[_QUIT,_FD,_BK,_TURN,_RT,_LT,_STRETCH] PUT CRAM_cmnd,0 'invalid option, don't know what to do GOTO _QUIT '-------------------------------------------------------------- 'RUNleaves _QUIT: _DONE: PUT CRAM_cmnd,P0_Done PUT CRAM_flags,f_flags PUT CRAM_LAat,LAat PUT CRAM_LHat,LHat PUT CRAM_RAat,RAat PUT CRAM_RHat,RHat RUN 0 '-------------------------------------------------------------- _FD: DEBUG" F" IF f_Rfwd=0 THEN _rightFD 'else drop through to leftfwd _leftFD: 'entered standing on both 'DEBUG" Lf" GOSUB standonright 'left leg lift - weight on right leg GOSUB toesleft GOSUB standon2fromLU 'left leg lower f_Rfwd =0 'set flag 'DEBUG" LA=",dec LAat," LH=",dec LHat," RH=",dec RHat," RA=",dec RAat,cr GOTO _DONE _rightFD: 'entered standing on both 'DEBUG" Rf" GOSUB standonleft 'right leg lift - weight on left leg GOSUB toesright GOSUB standon2fromRU 'right leg lower 'DEBUG" LA=",LAat," LH=",LHat," RH=",dec RHat," RA=",dec RAat,cr f_Rfwd =1 'set flag on right forward to 1 GOTO _DONE '-------------- _BK: DEBUG" B" IF f_Rfwd=1 THEN _rightBK 'else drop through to _leftBK _leftBK: 'entered standing on both 'DEBUG" Lb" GOSUB standonright 'left leg lift - weight on right leg GOSUB toesright GOSUB standon2fromLU 'left leg lower f_Rfwd =1 GOTO _DONE _rightBK: 'entered standing on both 'DEBUG" Rb" GOSUB standonleft 'right leg lift - weight on left leg GOSUB toesleft GOSUB standon2fromRU 'right leg lower f_Rfwd =0 GOTO _DONE '-------------- _TURN: IF f_RT=1 THEN _RT 'else drop through to LT _LT: 'entered standing on both DEBUG" LT" GOSUB standonleft servopulses =ABS(LAat -LAtbl) MIN ABS(RAat -RAwbr) /S_turnspeed FOR sp =0 TO servopulses LAat =LAat MIN s_turnspeed -s_turnspeed MIN LAtbl 'left foot in RAat =RAat MIN s_turnspeed -s_turnspeed MIN RAwbr 'right foot in GOSUB tellservos PAUSE framepause NEXT GOSUB standon2fromRU f_Rfwd =1 '[13Dec06] IF Ltoe=0 OR Rtoe=0 THEN _QUIT 'a toe is off GOSUB standonright servopulses =ABS(LAwfl -LAat)/S_turnspeedf FOR sp =0 TO servopulses LAat =LAat +s_turnspeedf MAX LAwfl 'left foot out GOSUB tellservos PAUSE framepause NEXT GOSUB standon2fromLU f_Rfwd =0 GOTO _DONE _RT: 'entered standing on both DEBUG" RT" DEBUG" RT" GOSUB standonright servopulses =ABS(RAat -RAtbr) MIN ABS(LAat -LAwbl) /S_turnspeed ' DEBUG ?servopulses ' DEBUG ?LAat,?LAwbl,?RAat,?RAtbr FOR sp =0 TO servopulses LAat =LAat MIN s_turnspeed -s_turnspeed MIN LAwbl 'left foot in RAat =RAat MIN s_turnspeed -s_turnspeed MIN RAtbr 'right foot in -t turns more in than w GOSUB tellservos PAUSE framepause NEXT GOSUB standon2fromLU f_Rfwd =0 '[13Dec06] IF Ltoe=0 OR Rtoe=0 THEN _QUIT 'a toe is off GOSUB standonleft servopulses =ABS(RAwfr -RAat)/S_turnspeedf FOR sp =0 TO servopulses RAat =RAat +s_turnspeedf MAX RAwfr 'right foot out GOSUB tellservos PAUSE framepause NEXT GOSUB standon2fromRU f_Rfwd =1 GOTO _DONE '-------------- _STRETCH: GOSUB spreadboth GOSUB stand2fromspread GOTO _DONE '-------------------------------------------------------------- spreadboth: servopulses =ABS(LHwd -LHat) MIN ABS(RHwd -RHat) /S_stepspeed FOR sp =0 TO servopulses LHat =LHat +S_stepspeed MAX LHwd 'left leg out RHat =RHat +S_stepspeed MAX RHwd 'right leg out GOSUB tellservos PAUSE framepause NEXT RETURN '-------------- stand2fromspread: servopulses =(LHat -LHmid) MIN (RHat -RHmid) /S_stepspeed FOR sp =0 TO servopulses LHat =LHat MIN s_stepspeed -S_stepspeed MIN LHmid 'left leg in RHat =RHat MIN s_stepspeed -S_stepspeed MIN RHmid 'right leg in GOSUB tellservos PAUSE framepause NEXT RETURN '-------------- standon2fromRU: 'DEBUG" RUs2" servopulses =(LHat -LHmid) MIN (RHmid -RHat) /S_stepspeed FOR sp =0 TO servopulses LHat =LHat MIN s_stepspeed -S_stepspeed MIN LHmid 'left leg in RHat =RHat +S_stepspeed MAX RHmid 'right leg out GOSUB tellservos PAUSE framepause NEXT 'DEBUG" LHat=",dec LHat," RHat=",dec RHat,cr RETURN '-------------- standon2fromLU: 'DEBUG" LUs2" servopulses =(LHmid -LHat) MIN (RHat -RHmid) /S_stepspeed FOR sp =0 TO servopulses LHat =LHat +S_stepspeed MAX LHmid 'left leg out RHat =RHat MIN s_stepspeed -S_stepspeed MIN RHmid 'right leg in GOSUB tellservos PAUSE framepause NEXT 'DEBUG" LHat=",dec LHat," RHat=",dec RHat,cr RETURN '-------------- standonleft: 'DEBUG" sL" servopulses =ABS(LHwd -LHat) MIN ABS(RHat -RHwu) /S_stepspeed FOR sp =0 TO servopulses LHat =LHat +S_stepspeed MAX LHwd 'left leg out RHat =RHat MIN s_stepspeed -S_stepspeed MIN RHwu 'right leg in GOSUB tellservos PAUSE framepause NEXT 'DEBUG" LHat=",dec LHat," RHat=",dec RHat,cr RETURN '-------------- standonright: servopulses =ABS(LHat -LHwu) MIN ABS(RHwd -RHat) /S_stepspeed FOR sp =0 TO servopulses'standfootpulses LHat =LHat MIN s_stepspeed -S_stepspeed MIN LHwu 'left leg in RHat =RHat +S_stepspeed MAX RHwd 'right leg out GOSUB tellservos PAUSE framepause NEXT 'DEBUG" LHat=",dec LHat," RHat=",dec RHat,cr RETURN '-------------- toesright: DEBUG" tR" servopulses =ABS(LAat -LAwbl) MIN ABS(RAwfr -RAat) /S_turnspeed 'LAwbl> LAat=LAtbl in BK after a turn FOR sp =0 TO servopulses LAat =LAat MIN s_turnspeed -s_turnspeed MIN LAwbl 'left toe in RAat =RAat +s_turnspeed MAX RAwfr 'right toe out GOSUB tellservos PAUSE framepause NEXT 'DEBUG" LAat=",dec LAat," RAat=",dec RAat,cr RETURN '-------------- toesleft: DEBUG" tL" servopulses =ABS(LAwfl -LAat) MIN ABS(RAat -RAwbr) /S_turnspeed 'RAwbr> RAat=RAtbr in BK after a turn FOR sp =0 TO servopulses 'DEBUG" LAat=",dec LAat," RAat=",dec RAat,cr LAat =LAat +s_turnspeed MAX LAwfl 'left toe out RAat =RAat MIN s_turnspeed -s_turnspeed MIN RAwbr 'right toe in GOSUB tellservos PAUSE framepause NEXT RETURN '-------------------------------------------------------------- '****copy this section into other progs using the servos**** TellServos: 'convert logical direction to physical PULSOUT s_LA,basepulse +255 -LAat *pscale 'left foot rotate -out / +in PULSOUT s_RA,basepulse +RAat *pscale 'right foot rotate +out / -in TellHipServos: PULSOUT s_LH,basepulse +255 -LHat *pscale 'left leg +in / -out PULSOUT s_RH,basepulse +RHat *pscale 'right leg -in / +out 'DEBUG DEC (basepulse +255 -LHat *8)," " 'DEBUG DEC (basepulse +RHat *8),CR RETURN '--------------------------------------------------------------