' {$STAMP BS2} ' {$PBASIC 2.5} '>TecFoot 27 July 2000 Milford Instruments 'code by David Buckley DEBUG CLS,"2.02" 'Problems '-------- 'P15 now stopped working ' 'todo '---- 'need to introduce Acts2, but no room ' 'History '------- 01C '26Nov09 2.02 changed LCD to P14. only 4 bytes left so D_ moved from $100 to end of Acts '26Nov09 changed LCD to P14 '11May09 Got Mode working, now on Reset toggles between DoUSstand,DoUSwalk '30Mar09 tested ServoCP pulses and added note, recalculated servo mids '08Jan09 _D moved '07Sep08 I dropped it and had to repair left hip! walking changed a lot, new mids needed '07Sep08 added US DeadBand of 10% distance '26Jun08 added BakR and usXin<6 test, so can go backwards from obstacles, falls over! '10Mar08 changed FbStep to StepBy, dFb to dStepBy, vFb to vSb '07Mar08 added PotSpeed facility, 8-13 useable, better if it is not used '04Mar08 1B5 altered tendon attachment hole in foot servo horns from 2nd to 4th ' to use all servo range and increase torque, ' changed foot servo mid, up and down to suit. ' altered servo horn for leg to 'cross' and used hole nearest centre, ' smaller radius than 'disc', more torque. Changed mid to 1500uS '28Feb08 17A corrected scaling of Rtb, Ltb '14Feb08 added RevEd-LCD to see what US distances are '09Feb08 Sls - ServoLegtSplay changed to Sfs - ServoFeetSplay ' added vRTb,vLTb and FbStep '08Feb08 corrected servo values from 10us to coprocessor scale of 4uS ' and got rid of junk. '06Feb08 added piezo speaker, pot and 2 headers for Ping us sensors '05Feb08 changed SplayServoDisc so Splayout is +ve and altered FSO, FSI ' also changed Manual to suit '04Feb08 moved Do to second page. Allows Pose and Act names in Dos ' Can't pass cmnds with params down a level - too hard to pick up params. '25Jan08 there was no way of reading param for Pose adjust, corrected '07Jan08 294 added _D: to chain Dos and allow Dos to be in second page, saved bytes as well! '27Nov07 25A F was Dc1+1 instead of =Dc1 '12Nov07 copmpletely rewritten from TecFoot1.BS2 to use Do,Act,Pose '06Nov07 each data line had an entry 'items' for number of data pairs, ' removed 'items' value, now test for end of list by servo=15 ' 'Control program for TecFoot with Stamp2 and two co-processor units. 'Servo values are held in the Data Area, edit values to fine tune 'operation of TecFoot. '------------------------------------------- 'Circuit board '------------- 'Ping same as servo o 0v ' o 5v from 78L05 regulator each Ping takes 22mA quiescent ' o BS2 pin ' 'LCD same as servo o 0v ' o 5v from 78L05 regulator ' o BS2 pin ' 'Aux connector 1 now routed to spkr, pot and 2 Pings ' Pins ' o 7 pot 220R to (10K pot in parallel with 0u1) to Ov ' o 6 spkr 0u1 to piezo to 0v ' o 5 Lping ' o 4 Rping ' 'Aux connector 2 Modified ' if on a 16 way ribbon cable then is same as Sinclair Projects (except for 4v8) but reversed ' ie a 16way DIL header on same cable will have SinclairProjects pinout, except the data bits are reversed! ' E=0v Pins - all pulled up to BS2-5v by 10K ' 0v o o 8 - ' 0v o o 9 - ' Proto Servo 6v o o 10 - BS2 ' type Servo 6v o o 11 - side ' side 9v o o 12 - ' 9v o o 13 - ' 100mA 5v o o 14 - ' 100mA 5v o o 15 - ' 'Servos from 4xAA cells =4.8v - 6v 'other elctronics 5v through 78L05 (100mA) '9v switched from battery ' 'ByteBus connector at front of board (adopted when BS1 first came out and have plug in boards) ' 1 ' MR_o ' 0v o ' D0 o BS2 Pin8 ' D1 o BS2 Pin9 ' D2 o BS2 Pin10 ' D3 o BS2 Pin11 ' D4 o BS2 Pin12 ' D5 o BS2 Pin13 ' D6 o BS2 Pin14 ' D7 o BS2 Pin15 ' 5v o ' 9v o ' 'SinclairProjects pinout, 16 pin 0.3 DIL ' 1 ' D0 o o 5v ' D1 o o 5v ' D2 o o 9v ' D3 o o 9v ' D4 o o ' D5 o o ' D6 o o 0v ' D7 o o 0v '(Why Sinclair Projects pinout?) '(Because I invented it for Sinclair Projects magazine and have other equipment using that pinout.) ' 'BS2 used is 2nd hand, damaged - no output on 8,9,10; inputs 9,10,11 shorted, 15 stuck high '------------------------------------------- 'Ping Sensors '------------ ' At sea level sound travels through air at 1130 feet per second. This ' equates to 1 inch in 73.746 uS, or 1 cm in 29.034 uS. ' ' Since the Ping sensor measures the time required for the sound wave to ' travel from the sensor and back. The result is divided by two to ' remove the return portion of the echo pulse, then multiplied by 2 to convert ' BS2 2uS resolution to uS. ' The final raw result is converted to cm. ' uS2in CON 889 ' 1 / 73.746 (with **) ' uS2cm CON 2257 ' 1 / 29.034 (with **) ' us2cm * 1/29.034 => * 0.03444 => * 2257/65536 => ** 2257 ' uS2in * 1/73.746 => * 0.01356 => * 889/65536 => ** 889 '------------------------------------------- 'Servo co-processor info: '----------------------- 'Initialises all servos to 127 ' Send two bytes, ' Command byte format: XXXXYYZZ ' ZZ is servo number - 0,1,2 ' XXXX is speed 0 to 15 - extra increments per frame (0=slow,15=fast) ' YY is command : ' 00 - set servo end position ' 01 - report servo position ' XXXX ignored, ZZ =servo number ' response =0 to 255 =current servo position. ' 10 - report all servos ' XXXX ignored, ZZ must be %00 ' response = %STUVWXYZ ' where S,T,U,V,W,X,Y and Z may be 0 or 1 ' STUV ' S - (servo 4) always 1 on 3 servo chip ' T - servo 2 ' U - servo 1 ' V - servo 0 ' 0 => relaxed ' 1 => being pulsed ' WXYZ ' W - (servo 4) always 1 on 3 servo chip ' X - servo 2 ' Y - servo 1 ' Z - servo 0 ' 0 => not reached end point ' 1 => has reached end point ' (ie a quick way to read all servos) ' 11 - relax servo, ie stop pulsing it until 00 sent ' byte is from 0 to 255 '[pulse as measured by BS2, 0=>1018us, 255=>2042us ie 4us resolution] '------------------------------------------- 'SERIAL LCD RevEd-LCD 'T2400 (or T9600 with jumper)(or inverted with jumper) 'jumpersL-R - open/closed = welcome-message/none, 2400/9600, true/inverted, 2-lines/1-line 'precede all codes by 254 =command character ' 1 clear display and move to start of first line ' 2 move cursor and display 'window' to the start of the first line ' 3 seems to be same as 2 ' 4 set 'right to left' printing mode - not useful ' 5 set 'scroll printing to the left' mode - not useful ' 6 set 'left to right' printing mode ' 7 set 'scroll printing to the right' mode ' 8,9 blanks screen, crash, needs power off to reset ' 10 turn visual LCD screen off ' 11 blanks screen, crash, needs power off to reset ' 12 hide cursor ' 13 make cursor flash ' 14 turn visual LCD screen (and cursor) on ' 15 turn on flashing block cursor ' 16 move cursor left one position ' 17,18,19 no discernable effect ' 20 move cursor right one position ' 21,22,23 no discernable effect ' 24 scroll display 'window' left one position, window wraps at 40 chars ' 25,26,27 same as 28 ' 28 scroll display 'window' right one position, window wraps at 40 chars ' 128 move cursor to start of first line, 129 position 1 etc (window 0-39) ' 192 move cursor to start of second line, 193 position 1 etc (window 0-39) '------------------------------------------- 'Stamp2 connections Lrtx CON 0 'serial pin to left leg servo co-processor lflow CON 1 'flow control for left leg servo co-processor 'pin=0 to prevent tx Rrtx CON 2 'serial pin for right leg servo co-processor rflow CON 3 'flow control for right leg servo co-processor 'pin=0 to prevent tx 'pins 4 - 7 to 4 pin header Aux1 and other peripherals Rping PIN 4 ' ping connector 0v 5v ping Lping PIN 5 ' ping connector 0v 5v ping spkr CON 6 '0u1 to piezo spkr to 0v pot CON 7 '220R pin to (10K pot in parallel with 0u1) to 0v 'pins 8 - 15 to pads and 16 pin header, each pulled up to BS2+5v by 10K 'DEBUG LEDs for use with LEDboard on bytebus 'LEDmain CON 8 'LEDsonar CON 13 'pins 8 - 11 bad, 8,9,10 - no output; 9,10,11 - inputs common 'P15 now stuck LOW LEDR CON 12 'only used in Do TurnR LEDL CON 13 'only used in Do TurnL LEDwaitm CON 15 'only used in waitfinished: 'P15 now stuck LOW LCD CON 14 'use no welcome msg jumper1 then no cls needed 'LCD RevEd T9600 CON 84 'jumper2 9600 'N9600 CON 16468 'jumper2 9600 'i96n CON 16468 'jumper2 9600 '------------------------------------------- 'Stamp2 constants 'DATA area - Servo numbers Srf CON 0 'ServoRightFoot Srl CON 1 'ServoRightLeg Sfs CON 2 'ServoFeetSplay Sspare CON 3 'it is just more convenient for plugging if this is spare Sll CON 4 'ServoLeftLeg physically connectors mirror right Slf CON 5 'ServoLeftFoot physically connectors mirror right 'co-processor SCbaud CON 16468 'BS2 9600 baud, 8 bit, inverted 'SCbaud con 16624 'BS2sx 9600 baud, 8 bit, inverted SCQstatus CON %00001000 'SCcmnd byte to get status of all servos SCcmnd VAR Byte SCspeed VAR SCcmnd.HIGHNIB SCservo VAR SCcmnd.LOWNIB SCstatus VAR Byte 'Servo status flags servpos VAR SCstatus 'Ping IsHigh CON 1 ' for Ping PULSOUT IsLow CON 0 ' for Ping PULSOUT rawDist VAR Word 'for Ping uS2inch CON 889 ' 1 / 73.746 (with **) uS2cm CON 2257 ' 1 / 29.034 (with **) usRin VAR Byte 'U/S range R in inches, so fits in a byte usLin VAR Byte 'U/S range L in inches, so fits in a byte 'other i VAR Nib 'i VAR Byte '------------------------------------------- 'Servo Positions actual 10uS with SuperPulser 8Feb08, feet 4Mar08 usRLm CON 150 '0=forward usLLm CON 150 '255=forward usRFm CON 146 '255=down usLFm CON 152 '0=down usFSm CON 123 '255=out more out than in so toes don't bang usRFb CON 121 'just balancing, nearly overbalancing out @100 usLFb CON 182 'just balancing, nearly overbalancing out @200 'usRFu CON 100 'for max tilt right 'usRFd CON 200 'for max tilt left 'usLFu CON 200 'for max tilt left 'usLFd CON 100 'for max tilt right 'Coprocessor 0=1018us 255=2042us 4us resolution (logical conversion is below) 'so deduct us102 and multiply by 2.5 for CoPro values sRLm CON 135'120 'us48=120 120 fd, 135 bk '135 evens out walking better 28Oct09 sLLm CON 140'120 'us48=120 135 fd, 120 bk '140 evens out walking better sRFm CON 118'110 'us46=110 110 up, 145 down, enough up to balance sLFm CON 108'125 'us50=125 130 up, 125 down, enough up to balance sFSm CON 59 'us21=53 53 in, 202 out 'sRFb CON 54 'just balancing, sRFm-62, nearly overbalancing out @0 'sLFb CON 45 'just balancing, sLFm-77, nearly overbalancing out @0 'sRFu CON 0 'for max tilt right 'sRFd CON 255 'for max tilt left 'sLFu CON 255 'for max tilt left 'sLFd CON 0 'for max tilt right '---------------------------------------------------------------- 'Flags flags VAR Byte f_usTbOn VAR flags.BIT0 'not used yet U/S-TurnByOn f_usOn VAR flags.BIT1 'U/S-rangingOn f_PS VAR flags.BIT2 'PotSpeed potval VAR Word pPose VAR Byte 'pointer to Pose in DATA pAct VAR Byte 'pointer to Act in DATA pDo VAR Byte 'pointer to Do in DATA cmnd VAR Byte 'general cmnd read from DATA param VAR Byte 'temp pRL VAR Byte 'pos_RightLeg pLL VAR Byte 'pos_LeftLeg 'pRF VAR Byte 'pos_RightFoot 'pLF VAR Byte 'pos_LeftFoot pFS VAR Byte 'pos_FeetSplay pFSm VAR Byte 'pos_FeetSplayMid '**** othe mids are CON **** 'Logical 255=forward 255=down 255=out RLm CON 255 -sRLm 'RightLegMid LLm CON sLLm 'LeftLegMid RFm CON sRFm 'RightFootMid LFm CON 255 -sLFm 'LeftFootMid FSm CON sFSm 'FeetSplayMid, splay angle mid dStepBy CON 80 'DefaultStepBy 120 MAX, 100 OK StepBy VAR Byte 'StepBy StepBy =dStepBy vSb CON 255 'variableStepBy as parameter =>use StepBy RTb VAR Byte 'RightTurnBy 10us resolution, from pFSM LTb VAR Byte 'LeftTurnBy 10us resolution, from pFSM RTb =0 'f_TbOn VAR flags.BIT0 enables=1, disables=0, variable update LTb =0 'f_TbOn VAR flags.BIT0 enables=1, disables=0, variable update vRTb CON 255 'variable name pointer for in DATA vLTb CON 254 'variable name pointer for in DATA LeanF VAR Byte 'Pose cmnds '---------- '[_RFd,_RFu,_RLf,_RLb,_LFd,_LFu,_LLf,_LLb,_FSo,_FSi] z CON 0 'end of list 0 zilch 'vSb CON 255'value Sb, as parameter =>use Variable StepBy Pc1 CON 1 'first Pose cmnd RFd CON 1 'RightFootDown 4us resolution, from pRFM RFu CON 2 'RightFootUp 4us resolution, from pRFM RLF CON 3 'RightLegForward 4us resolution, from pRLM RLb CON 4 'RightLegBack 4us resolution, from pRLM LFd CON 5 'LeftFootDown 4us resolution, from pLFM LFu CON 6 'LeftFootUp 4us resolution, from pLFM LLf CON 7 'LeftLegForward 4us resolution, from pLLM LLb CON 8 'LeftLegBack 4us resolution, from pLLM FSo CON 9 'FeetOut 4us resolution, from pFSM FSi CON 10 'FeetIn 4us resolution, from pFSM ' 'Act cmnds '--------- 'z CON 0 'end of list 0 zilch Ac1 CON 11 'first Act cmnd 'Pose adjust - behavioral cmnds, would be used following sensor reading 'RFdb CON 11 'RightFootDownBy 4us resolution, from pRF 'RFub CON 12 'RightFootUpBy 4us resolution, from pRF 'RLFb CON 13 'RightLegForwardBy 4us resolution, from pRL 'RLbb CON 14 'RightLegBackBy 4us resolution, from pRL 'LFDb CON 15 'LeftFootDownBy 4us resolution, from pLF 'LFUb CON 16 'LeftFootUpBy 4us resolution, from pLF 'LLfb CON 17 'LeftLegForwardBy 4us resolution, from pLL 'LLbb CON 18 'LeftLegBackBy 4us resolution, from pLL 'FSOb CON 19 'FeetOutBy 4us resolution, from pFS 'FSIb CON 20 'FeetInBy 4us resolution, from pFS 'Behavior cmnds Bc1 CON 11 'first Behavior cmnd SS CON 11 'ServoSpeed increments per frame 10us resolution P CON 12 ' Pause 100ms resolution E CON 13 'end bLeanF CON 14 'posture control, lean forward +- max about 20 'Do cmnds '-------- 'intrinsic Acts 'z CON 0 'end of list 0 zilch Dc1 CON 15 'first Do cmnd F CON 15 'Forward one step B CON 16 'Backward one step R CON 17 'Right turn one step L CON 18 'Left turn one step D CON 19 'chain Do, next byte is LowByte of pDo, 'D is necessary because DATA is bytes and pDo goes over 255 Sb CON 20 'StepBy +ve forward, -ve backward V CON 21 'Beep PH CON 22 'PinHigh pins 8 - 15 PL CON 23 'PinLow pins 8 - 15 PsL CON 24 'PinsLow all 8 -15 USE WITH CARE IF LCD, messes with display 'behavior control 'usTbOn CON 23 'u/s-TurnBy On 'usTbOff CON 24 'u/s-TurnBy Off PSon CON 25 'PotSpeed set SCpeed from Pot on PSoff CON 26 'PotSpeed set SCpeed from Pot off USon CON 27 'US ranging On, set flag f_US =1 USoff CON 28 'US ranging Off, set flag f_US =0 lastcmnd CON 28 FREQOUT spkr, 500, 2500, 3000 ' announcement for DEBUG DEBUG "-- Tecfoot --", CR ' a heading for the following DEBUGs 'Pose '---- 'This must not overlap Act-cmnds as an Action may be a Pose or Act-cmnd DATA @lastcmnd,"P_" Pnull DATA z 'standeasy DATA LFU,10,RFu,10,LLf,0,RLF,0,z stand DATA LFd,0,RFd,0,LLf,0, RLf,0,z 'paceStop DATA LLf,0,RLf,0,0,z 'leanFD DATA LLb,100,RLb,100,z 'leanBK DATA LLf,100,RLf,100,z leanR DATA RFu,40,LFd,160,z ' leanL DATA LFu,40,RFd,159,z 'LFu,39 leanTR DATA RFu,40,LFd,160,z ' leanTL DATA LFu,40,RFd,158,z 'LFu,38 standR DATA LFd,0,z standL DATA RFd,0,z 'noleanside DATA LFd,0,RFd,0,z Rfootup DATA RFu,90,z Lfootup DATA LFu,90,z paceRF DATA RFd,0,LLb,vSb,RLf,vSb,FSo,vRTb,z 'vSb,vRTb => use variable paceRB DATA RFd,0,LLf,vSb,RLb,vSb,FSo,vLTb,z 'vSb,vLTb => use variable paceLF DATA LFd,0,LLf,vSb,RLb,vSb,FSo,vLTb,z 'vSb,vLTb => use variable paceLB DATA LFd,0,LLb,vSb,RLf,vSb,FSo,vRTb,z 'vSb,vRTb => use variable splayout DATA FSo,120,z 'splayoutL DATA LFd,0,FSo,120,z splayin DATA FSi,120,z nosplay DATA FSo,0,z 'Rstep Rstep DATA DEBUG ?Pnull DEBUG ?leanR DEBUG ?paceRF DEBUG ?paceRB DEBUG ?paceLF DEBUG ?paceLB DEBUG ?splayout DEBUG ?nosplay 'Act '--- DATA "A_" '{speed,pose} is an Action Anull DATA z 'if Anull done then only USsensors/USbehaviours are tested S4 DATA SS,4,z S12 DATA SS,12,z FL DATA bLeanF,20,SS,12,leanR,paceLF,z FR DATA bLeanF,20,SS,12,leanL,paceRF,z BL DATA bLeanF, 0,SS,12,leanR,paceLB,z BR DATA bLeanF, 0,SS,12,leanL,paceRB,z TR DATA bLeanF, 0,SS,12,leanTL,standL,splayout,leanTR,standR,nosplay,stand,z TL DATA bLeanF, 0,SS,12,leanTR,standR,splayout,leanTL,standL,nosplay,stand,z 'notip DATA SS,2,stand,z tipRL DATA SS,3,nosplay,SS,2,leanR,SS,3,standR,SS,2,leanL,SS,3,standL,tipRL Astand DATA stand,z Aend DATA DEBUG HEX Anull,"$=",?Anull DEBUG ?S12 DEBUG ?FL DEBUG ?FR DEBUG ?BL DEBUG ?tipRL 'Do '-- ' DATA @256, "D_" DATA "D_" Dcmnd DATA 0 'toggles 0,1 on reset 'Does DATA Dowalk,Dowalk' 'always does Dowalk, use for Stance testing 'Does DATA Dostand,Dowalk' 'toggles between first two on reset, use when no Pings fitted Does DATA DoUSstand,DoUSwalk 'toggles between first two on reset 'Dnull DATA z 'D_Do DATA DoT 'D_Do DATA DoTip 'D_Do DATA PH,LEDmain,DoFB 'D_Do DATA 'Dostand DATA S4,Astand,z 'use when no Ping sensors fitted DoUSstand DATA S4,USon,Astand,z DoUSwalk DATA S4,USon,PSoff 'fall through to Dowalk Dowalk DATA F,D,Dowalk 'Dotip DATA stand,tipRL,z 'tipRL never ends 'DoFB DATA Sb,dStepBy,FL,FR,FL,FR,Sb,45,BR,BL,BR,BL,BR,BL,BR,BL,z 'DoT DATA Sb,0,TR,TR,TR,TL,TL,TL,z Bnull DATA "B_" 'Behavioral Dos TurnR DATA V,V,PH,LEDR,usOff,R,PL,LEDL,z TurnL DATA V,V,V,PH,LEDL,usOff,L,L,PL,LEDL,z BakR DATA V,usOff,B,B,R,z '-------------------- Program ----------------------------------- 'GOTO manual_setup 'manual test code '-------------------- Main ------------------- Init: READ Dcmnd,cmnd cmnd =cmnd^1 WRITE Dcmnd,cmnd DEBUG ? cmnd 'TecFoot either stands still or walks Fd, 'either mode is responsive TO US, toggles ON RESET f_usOn =0 'U/S-ranging Off in case no Pings fitted PAUSE 200 'for LCD to initialise SEROUT LCD,T9600,[254,1] PAUSE 2 'for LCD to cls SEROUT LCD,T9600,["TecFoot usL usR",254,192,"speed="] DEBUG "flags=",BIN8 flags,CR Initbeep: 'beep V see above ParamError: FOR i=0 TO cmnd 'if i is a Nib then program doesn't work properly, compile bug? FREQOUT spkr, 500, 2000, 3500 'beep PAUSE 100 NEXT defaultDo: READ Dcmnd,cmnd 'default Do READ Does +cmnd,pDo 'get pointer GOTO ReadDo '------------------------------------------------------------------------ DoDo: IF cmndpRL THEN cmnd =BL GOTO Act _R: DEBUG "R" cmnd =TR : GOTO Act _L: DEBUG "L" cmnd =TL : GOTO Act _D: 'chain a Do 'READ pDo +256,pDo READ pDo,pDo 'DEBUG ?pDo GOTO ReadDo _Sb: DEBUG "Sb" GOSUB ReadDoParam StepBy =param 'DEBUG ?StepBy GOTO ReadDo _V: DEBUG "V" FREQOUT spkr, 100, 2500, 3000 GOTO ReadDo _PH: GOSUB ReadDoParam HIGH param GOTO ReadDo _PL: GOSUB ReadDoParam LOW param GOTO ReadDo _PsL: OUTH =0 GOTO ReadDo _PSon: f_PS =1 GOTO ReadDo _PSoff: f_PS =0 GOTO ReadDo _usOn: 'DEBUG "us1" f_usOn =1 GOTO ReadDo _usOff: 'DEBUG "us0" f_usOn =0 GOTO ReadDo ReadDoParam: READ pDo +256,param 'DEBUG ?param pDo =pDo +1 RETURN '------------------------------------------------------------------------ Act: DEBUG CR,"A",DEC cmnd," " IF cmndusLin THEN RTb =usRin -usLin *256 /usRin MAX 100 IF usLin>usRin THEN LTb =usLin -usRin *256 /usLin MAX 100 ENDIF GOSUB waitfinished IF cmnd<>0 THEN Act GOTO ReadDo _SS: READ pAct,param pAct =pAct +1 'increment past param ' DEBUG "SS", DEC param SCspeed =param MAX 15 GOTO ReadAct _P: READ pAct,param pAct =pAct +1 PAUSE param*100 GOTO ReadAct _E: DEBUG"end" END _bLeanF: READ pAct,LeanF pAct =pAct +1 GOTO ReadAct 'PoseAdjust: ' READ pAct,param ' ' pAct =pAct +1 ' BRANCH cmnd-Ac1,[_RFdb,_RFub,_RLfb,_RLbb,_LFdb,_LFub,_LLFb,_LLbb,_FSob,_FSib] ' 'can't ever get here ' GOTO ParamError 'just in case ' '_RFdb: ' pRF =pRF +param MAX 255 : servpos =pRF : SCservo =Srf : GOTO DoServos '_RFub: ' pRF =pRF MIN param -param : servpos =pRF : SCservo =Srf : GOTO DoServos '_RLFb: ' pRL =pRL MIN param -param : servpos =pRL : SCservo =Srl : GOTO DoServos '_RLBb: ' pRL =pRL +param MAX 255 : servpos =pRL : SCservo =Srl : GOTO DoServos '_LFDb: ' pLF =pLF MIN param -param : servpos =pLF : SCservo =Slf : GOTO DoServos '_LFUb: ' pLF =pLF +param MAX 255 : servpos =pLF : SCservo =Slf : GOTO DoServos '_LLFb: ' pLL =pLL +param MAX 255 : servpos =pLL : SCservo =Sll : GOTO DoServos '_LLbb: ' pLL =pLL MIN param -param : servpos =pLL : SCservo =Sll : GOTO DoServos '_FSOb: ' pFS =pFS MIN param -param : servpos =pLS : SCservo =Sls : GOTO DoServos '_FSIb: ' pFS =pFS +param MAX 255 : servpos =pLS : SCservo =Sls : GOTO DoServos '------------------------------------------------------------------------ Pose: DEBUG "P"," " DoPose: IF cmnd=z THEN ReadAct 'check cmnds BRANCH cmnd-Pc1,[_RFd,_RFu,_RLf,_RLb,_LFd,_LFu,_LLf,_LLb,_FSo,_FSi] IF cmnd use variable pRL =RLM +param -LeanF MAX 255 : GOTO _RLs _RLb: 'RightLegBack IF param=vSb THEN param =StepBy 'v => use variable pRL =100 +RLm MIN param -param +LeanF MIN 100 -100 '100 is zero safety _RLs: servpos =255 -pRL : SCservo =Srl : GOTO DoServos _LLf: 'LeftLegForward IF param=vSb THEN param =StepBy 'v => use variable pLL =LLm +param -LeanF MAX 255 : GOTO _LLs _LLb: 'LeftLegBack IF param=vSb THEN param =StepBy 'v => use variable pLL =100 +LLm MIN param -param +LeanF MIN 100 -100 '100 is zero safety _LLs: servpos =pLL : SCservo =Sll : GOTO DoServos _FSo: 'FeetOut IF param=vRTb THEN param =RTb 'v => use variable IF param=vLTb THEN param =LTb 'v => use variable pFS =FSm +param MAX 255 : GOTO _FSs _FSi: 'FeetIn pFS =FSm MIN param -param _FSs: servpos =pFS : SCservo =Sfs : GOTO DoServos '------------------------------------------------------------------------ DoServos: IF SCservo>2 THEN _LeftServos _RighttServos: SEROUT Rrtx\rflow,SCbaud,[SCcmnd,servpos] GOTO _ToldServos _LeftServos: SCservo =SCservo -3 SEROUT Lrtx\lflow,SCbaud,[SCcmnd,servpos] _ToldServos: IF pPose<>0 THEN ReadPose 'for Pose adjust cmnds from Act: GOTO ReadAct '------------------- Subroutines --------------------------------- waitfinished: 'loop here until moves finished DEBUG"w" HIGH LEDwaitm SEROUT Rrtx\rflow,SCbaud,[SCQstatus,0] '?status SERIN Rrtx,SCbaud,50,waitfinished,[SCstatus] 'DEBUG bin SCstatus,"right",cr IF SCstatus.LOWNIB<>%1111 THEN waitfinished waitfinishedL: SEROUT Lrtx\lflow,SCbaud,[SCQstatus,0] '?status SERIN Lrtx,SCbaud,50,waitfinishedl,[SCstatus] 'DEBUG bin result,"left",cr IF SCstatus.LOWNIB<>%1111 THEN waitfinishedL LOW LEDwaitm RETURN '---------------------------------------------------- Get_Sonar: 'HIGH LEDsonar PAUSE 10 Rping = IsLow ' make trigger 0-1-0 PULSOUT Rping,5 ' 10uS pulse, activate sensor PULSIN Rping, IsHigh, rawDist ' measure echo pulse usRin =rawDist ** uS2inch 'use inches then fits in a byte PAUSE 10 Lping = IsLow ' make trigger 0-1-0 PULSOUT Lping,5 ' 10uS pulse, activate sensor PULSIN Lping, IsHigh, rawDist ' measure echo pulse usLin =rawDist ** uS2inch 'use inches then fits in a byte 'LOW LEDsonar RETURN '---------------------------------------------------- 'RELAX: ' FOR i=0 TO 2 ' SCcmnd =%1100 +i ' SEROUT Rrtx\rflow,SCbaud,[SCcmnd,0] ' SEROUT Lrtx\lflow,SCbaud,[SCcmnd,0] ' NEXT ' RETURN '---------------------------- manual_setup: 'edit position values to check stances SCspeed =8 SCservo =2 SEROUT Lrtx\lflow,SCbaud,[SCcmnd,152] 'Lfoot 255=lean, 152=flat, 0=tip SCservo =1 SEROUT Lrtx\lflow,SCbaud,[SCcmnd,150] 'Lleg 255=fd, 150=mid, 0=bk SCservo =0 SEROUT Lrtx\lflow,SCbaud,[SCcmnd,127] 'spare SCservo =0 SEROUT Rrtx\rflow,SCbaud,[SCcmnd,146] 'Rfoot 0=lean 146=flat 255=tip SCservo =1 SEROUT Rrtx\rflow,SCbaud,[SCcmnd,150] 'Rleg 0=fd, 150=mid, 255=bk SCservo =2 SEROUT Rrtx\rflow,SCbaud,[SCcmnd,123] 'splay 255=out, 123=mid(so 255 is max splay), 0=in GOTO manual_setup DEBUG"endtest" END '---------------------- End Program -------------------------------------