'{$STAMP BS2sx} ' {$PBASIC 2.5} '>Frea7_2.bsx '============================================================== 'BRANCH cmnd,[,_setlegs,_GetUp] '============================================================== '****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_turnspeeds 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 'LHmid 2300 'feet flat =500+1810 1810=>226 '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 226 ' 226=>2304 '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,"P2" 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,_setlegs,_GetUp] 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 '-------------------------------------------------------------- _setlegs: 'use pot to find actual servo values 'HIGH potpin 'discharge condenser 'PAUSE 1 'discharge time 'RCTIME potpin,1,W_tempPOT 'W_tempPOT =W_tempPOT /5 MAX 255 'DEBUG "pot=",DEC W_tempPOT," ",CR 'LAat =W_tempPOT 'PULSOUT s_LA,basepulse +LAat *pscale 'left foot rotate -out / +in 'RAat =W_tempPOT 'PULSOUT s_RA,basepulse +RAat *pscale 'right foot rotate +out / -in 'LHat =W_tempPOT 'PULSOUT s_LH,basepulse +LHat *pscale 'left leg +in / -out 'RHat =W_tempPOT 'PULSOUT s_RH,basepulse +RHat *pscale 'right leg -in / +out 'PAUSE 20 'GOTO setlegs '-------------------------------------------------------------- _GetUp: DEBUG" GetUp" 'DEBUG?tipF_,?tipB_ 'test if fallen upside down PAUSE 500 'give time for Hg to settle IF tipF_=0 AND tipB_=0 THEN standupfromUd 'stand from upside down 'test if fallen forward IF tipF_=0 THEN standupfromlieF 'stand from fallen forward 'test if fallen backward IF tipB_=0 THEN standupfromlieB 'stand from fallen backwards IF tipF_=1 AND tipB_=1 THEN _DONE 'all DONE GOTO _GetUp 'just in case sensors change midway '-------------------------------------------------------------- standupfromUd: 'STAND UP FROM UPSIDE DOWN DEBUG"Ud" f_stood =0 servopulses =ABS(LHlieout -LHat) MIN ABS(RHlieout -RHat) /3 +1 FOR sp =1 TO servopulses'100 LHat =LHat +3 MAX LHlieout 'legs out RHat =RHat +3 MAX RHlieout GOSUB tellservos PAUSE framepause NEXT servopulses =ABS(LAoutstand -LAat) MIN ABS(RAoutstand -RAat) /3 +1 FOR sp =1 TO servopulses'60 LAat =LAat +3 MAX LAoutstand 'toes out to outstand RAat =RAat +3 MAX RAoutstand GOSUB tellservos PAUSE framepause NEXT ' DEBUG"stop" ' STOP PAUSE 500 'give time for Hg to settle IF tipF_=0 AND tipB_=0 THEN standupfromUd GOTO Stndub 'else standupfromlieB/Stndub: '-------------------- standupfromlieB: 'STAND UP FROM LYING ON BACK DEBUG"Ub" f_stood =0 servopulses =ABS(LHwd -LHat) MIN ABS(RHwd -RHat) /3 +1 'DEBUG ?LHat,?LHwd,?RHat,?RHwd IF LHat>LHwd THEN _ulbin _ulbout: FOR sp =1 TO servopulses'40 LHat =LHat +3 MAX LHwd 'legs out so toes miss RHat =RHat +3 MAX RHwd GOSUB tellservos PAUSE framepause NEXT GOTO _ulbtin _ulbin: 'legs already out so in again FOR sp =1 TO servopulses'40 LHat =LHat -3 MIN LHwd 'legs out so toes miss RHat =RHat -3 MIN RHwd GOSUB tellservos PAUSE framepause NEXT _ulbtin: servopulses =ABS(LAoutstand -LAat) MIN ABS(RAoutstand -RAat) /5 +1 FOR sp =1 TO servopulses'50 LAat =LAat +5 MAX LAoutstand 'toes out to outstand RAat =RAat +5 MAX RAoutstand GOSUB tellservos PAUSE framepause NEXT servopulses =ABS(LHlieout -LHat) MIN ABS(RHlieout -RHat) /3 +1 FOR sp =1 TO servopulses'50 LHat =LHat +3 MAX LHlieout 'legs out RHat =RHat +3 MAX RHlieout GOSUB tellservos PAUSE framepause NEXT Stndub: servopulses =ABS(LAat -LAinstand) MIN ABS(RAat -RAinstand) /5 +1 FOR sp =1 TO servopulses'35 LAat =LAat -5 MIN LAinstand 'toes to in RAat =RAat -5 MIN RAinstand GOSUB tellservos PAUSE framepause NEXT servopulses =ABS(LAmid -LAat) MIN ABS(RAmid -RAat) /4 +1 'DEBUG ?servopulses FOR sp =1 TO servopulses'100'60 LAat =LAat +4 MAX LAmid 'toes to mid RAat =RAat +4 MAX RAmid GOSUB tellservos PAUSE framepause NEXT 'DEBUG ?LHat,?LHlieout,?RHat,?RHlieout PAUSE 500 'give time for Hg to settle IF tipF_=0 OR tipB_=0 THEN _GetUp GOTO upfromlie 'else '-------------------- standupfromlieF: 'STAND UP FROM LYING ON FRONT DEBUG"Uf" f_stood =0 servopulses =ABS(LHwd -LHat) MIN ABS(RHwd -RHat) /3 +1 IF LHat>LHwd THEN _ulfin FOR sp =1 TO servopulses'60 LHat =LHat +3 MAX LHwd 'legs out so toes miss RHat =RHat +3 MAX RHwd GOSUB tellservos PAUSE framepause NEXT GOTO _ulftin _ulfin: 'legs already out so in again FOR sp =1 TO servopulses'40 LHat =LHat -3 MIN LHwd 'legs out so toes miss RHat =RHat -3 MIN RHwd GOSUB tellservos PAUSE framepause NEXT _ulftin: 'debug"uf1" servopulses =ABS(LAat -LAinstand) MIN ABS(RAat -RAinstand) /5 +1 FOR sp =1 TO servopulses'60 LAat =LAat -5 MIN LAinstand 'toes in to instand RAat =RAat -5 MIN RAinstand GOSUB tellservos PAUSE framepause NEXT 'debug"uf2" servopulses =ABS(LHlieout -LHat) MIN ABS(RHlieout -RHat) /3 +1 FOR sp =1 TO servopulses'60 LHat =LHat +3 MAX LHlieout 'legs out RHat =RHat +3 MAX RHlieout GOSUB tellservos PAUSE framepause NEXT 'debug"uf3" servopulses =ABS(LAmid -LAat) MIN ABS(RAmid -RAat) /5 +1 FOR sp =1 TO servopulses'60 LAat =LAat +5 MAX LAmid 'left foot rotate back RAat =RAat +5 MAX RAmid 'right foot rotate back GOSUB tellservos PAUSE framepause NEXT PAUSE 500 'give time for Hg to settle IF tipF_=0 OR tipB_=0 THEN _GetUp 'not DONE else drop through to upfromlie '-------------------- upfromlie: 'debug"upfromlie" servopulses =ABS(LHat -LHwd) MIN ABS(RHat -RHwd) /2 +1 FOR sp =1 TO servopulses'150 LHat =LHat -2 MIN LHwd 'legs in RHat =RHat -2 MIN RHwd GOSUB tellservos PAUSE framepause NEXT 'debug"uf5" servopulses =ABS(LAat -LAwbl) MIN ABS(RAwfr -RAat) /2 +1 FOR sp =1 TO servopulses'30 LAat =LAat -2 MIN LAwbl 'toes to walk position RAat =RAat +2 MAX RAwfr GOSUB tellservos PAUSE framepause NEXT 'debug"uf6" servopulses =ABS(LHat -LHmid) MIN ABS(RHat -RHmid) /2 +1 FOR sp =1 TO servopulses'30 LHat =LHat -2 MIN LHmid 'legs to stand position RHat =RHat -2 MIN RHmid GOSUB tellservos PAUSE framepause NEXT GOTO _GetUp 'DONE '-------------------------------------------------------------- '****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 '--------------------------------------------------------------