' {$STAMP BS2} '>TecFoot 27 July 2000 Milford Instruments 'code by David Buckley info@milinst.demon.co.uk 'ORIGINAL TecFoot program '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. 'Nomenclature '------------ 'RELAX - Stop pulsing all servos, servos relaxed, reduces current drain 'LEANBK - Lean back 'LEANFD - Lean forward 'LEANR - Lean right 'LEANL - Lean left 'STANDUP 'FD - ForwarD requires a value in paces, 0 on exit 'BK - BacKward requires a value in paces, 0 on exit 'RT - RightTurn requires a value in turn, 0 on exit 'LT - LeftTurn requires a value in turn, 0 on exit 'R - Right 'L - Left 'LB - LeftBack move left leg backwards and right leg forward 'RF - RightForward move left leg backwards and right leg forward 'LF - LeftForward move left leg forwards and right leg backwards 'RB - RightBack move left leg forwards and right leg backwards 'TURNR - TURNRight one cycle of turn, angle controlled by splay 'TURNL - TURNLeft one cycle of turn, angle controlled by splay 'noleanside - alter feet to stand vertically but leave legs alone ' feet under tension to cotrol sway from one foot to the other 'splay - angle between feet 'stand - stand with feet nearly under tension to resist rocking 'standeasy - stand with feet relaxed 'leanpause - wait for TecFoot to tip onto foot when leaning '------------------------------------------- 'Servo co-processor info: '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 'Position byte is from 0 to 255 '------------------------------------------- 'Stamp2 connections and constants: ltx CON 0 'serial out pin to left leg servo co-processor lflow CON 1 'flow control for left leg servo co-processor 'pin=0 to prevent tx rtx CON 2 'serial line 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 by pad 'pins 8 - 15 to pads and 16 pin header baud CON 16468 'BS2 9600 baud, 8 bit, inverted 'baud con 16624 'BS2sx 9600 baud, 8 bit, inverted servset CON 0 servread CON %00000100 Qstatus CON %00001000 'command byte to get status of all servos true CON 1 false CON 0 'DATA area - Servo numbers Srfoot CON 0 Srleg CON 1 Ssplay CON 2 Sspare CON 3 Slleg CON 4 'change in order from right to left because servo Slfoot CON 5 'leads to feet servos won't reach otherwise. command VAR Byte speed VAR command.HIGHNIB servo VAR command.LOWNIB servpos VAR Byte Sstatus VAR Byte 'Servo status flags flow VAR Nib 'data line tx VAR Nib 'flow control line D_pos VAR Byte 'data pointer D_items VAR Nib 'number of items in record items VAR Nib 'loop counter i VAR items 'loop counter turn VAR Byte 'parameter for doturnL/R paces VAR Byte 'parameter for FD/BK domoveflag VAR Bit 'flag domoveflag =false '---------------------------------------------------------------- 'Data Area 'Notes - lfoot 0=down, lleg 0=BK, rfoot 0=up, rleg 0=FD, ' lleg 110, rleg 170, mean of swing for pace 'D_pos speed items data - edit to fine tune '----- ----- ----- ------------------------ D_standeasy DATA 2, 8, Slfoot, 180, Srfoot, 80 DATA Slleg, 100, Srleg, 160 D_stand DATA 2, 8, Slfoot, 150, Srfoot, 95 DATA Slleg, 100, Srleg, 160 D_paceStop DATA 5, 4, Slleg, 100, Srleg, 160 D_leanFD DATA 2, 4, Slleg, 0, Srleg, 255 D_leanBK DATA 2, 4, Slleg, 255, Srleg, 0 D_leanR DATA 2, 4, Slfoot, 0, Srfoot, 60 D_leanL DATA 2, 4, Slfoot, 215, Srfoot, 255 D_noleanside DATA 3, 4, Slfoot, 150, Srfoot, 95 D_Rfootup DATA 15, 2, Srfoot, 50 D_Lfootup DATA 15, 2, Slfoot, 150 D_paceLB DATA 'dummy address same as RF D_paceRF DATA 4, 4, Slleg, 70, Srleg, 90 D_paceRB DATA 'dummy address same as LF D_paceLF DATA 4, 4, Slleg, 190, Srleg, 210 D_splayout DATA 15, 2, Ssplay, 0 D_splayin DATA 15, 2, Ssplay, 255 D_nosplay DATA 15, 2, Ssplay, 150 leanpause CON 800 'wait for TecFoot to tip onto foot '-------------------- Program ----------------------------------- 'GOTO manual_setup 'manual test code 'GOTO AlignHorns 'Align Servo Horns '-------------------- Main ------------------- START: 'Sample routine DEBUG"SSstart",CR GOSUB STANDUP PAUSE 1000 paces =10 :GOSUB FD ' gosub STANDUP turn =2 :GOSUB LT paces =10 :GOSUB FD GOSUB STANDUP turn =2 :GOSUB RT ' paces =1 :gosub FD ' paces =1 :gosub BK ' turn =8 :gosub RT ' paces =6 :gosub BK ' gosub STANDUP ' gosub RELAX GOTO start DEBUG"end" END '------------------- Subroutines --------------------------------- RELAX: FOR i=0 TO 2 command =%1100 +i SEROUT rtx\rflow,baud,[command,0] SEROUT ltx\lflow,baud,[command,0] NEXT RETURN '---------------------------- LEANFD: D_pos =D_leanfd :GOSUB domove RETURN '---------------------------- LEANBK: D_pos =D_leanbk :GOSUB domove RETURN '---------------------------- STANDUP: D_pos =D_nosplay :GOSUB domove D_pos =D_standeasy :GOSUB domove RETURN '---------------------------- FD: IF paces=0 THEN endfd againfd: IF paces=0 THEN endfdr DEBUG ?paces GOSUB RF paces =paces-1 IF paces=0 THEN endfdl GOSUB LF paces =paces-1 GOTO againfd endfdl: GOSUB LEANR GOTO fdstop endfdr: GOSUB LEANL fdstop: D_pos =D_pacestop :GOSUB domove D_pos =D_noleanside :GOSUB domove endfd:RETURN '---------------------------- BK: IF paces=0 THEN endbk againbk: IF paces=0 THEN endbkr DEBUG ?paces GOSUB RB paces =paces-1 IF paces=0 THEN endfdl GOSUB LB paces =paces-1 GOTO againbk endbkl: GOSUB LEANR GOTO bkstop endbkr: GOSUB LEANL bkstop: D_pos =D_pacestop :GOSUB domove D_pos =D_noleanside :GOSUB domove endbk: RETURN '---------------------------- RF: GOSUB LEANL D_pos =D_paceRF :GOSUB domove D_pos =D_noleanside :GOSUB domove RETURN '---------------------------- RB: GOSUB LEANL D_pos =D_paceRB :GOSUB domove D_pos =D_noleanside :GOSUB domove RETURN '---------------------------- LF: GOSUB LEANR D_pos =D_paceLF :GOSUB domove D_pos =D_noleanside :GOSUB domove RETURN '---------------------------- LB: GOSUB LEANR D_pos =D_paceLB :GOSUB domove D_pos =D_noleanside :GOSUB domove RETURN '---------------------------- RT: IF turn=0 THEN endrt GOSUB TURNR turn =turn-1 GOTO RT endrt: RETURN '---------------------------- LT: IF turn=0 THEN endlt GOSUB TURNL turn =turn-1 GOTO LT endlt: RETURN '---------------------------- TURNR: D_pos =D_stand :GOSUB domove GOSUB LEANL D_pos =D_splayout :GOSUB domove D_pos =D_stand :GOSUB domove GOSUB LEANR D_pos =D_nosplay :GOSUB domove D_pos =D_stand :GOSUB domove RETURN '---------------------------- TURNL: D_pos =D_stand :GOSUB domove GOSUB LEANR D_pos =D_splayout :GOSUB domove D_pos =D_stand :GOSUB domove GOSUB LEANL D_pos =D_nosplay :GOSUB domove D_pos =D_stand :GOSUB domove RETURN '---------------------------- LEANR: D_pos =D_leanR :GOSUB domove PAUSE leanpause D_pos =D_Lfootup :GOSUB domove RETURN '---------------------------- LEANL: D_pos =D_leanL :GOSUB domove PAUSE leanpause D_pos =D_Rfootup :GOSUB domove RETURN '---------------------------------------------------- domove: domoveflag =true tellservos: 'if entered here with domoveflag=false 'then we don't go into waitfinished 'DEBUG"tell",cr READ D_pos,speed READ D_pos+1,D_items FOR items=1 TO D_items STEP 2 READ D_pos+items+1,servo READ D_pos+items+2,servpos 'DEBUG ?servo,?servpos IF servo>2 THEN tellLeft SEROUT rtx\rflow,baud,[command,servpos] GOTO nextpos tellLeft: servo =servo -3 SEROUT ltx\lflow,baud,[command,servpos] nextpos: NEXT IF domoveflag=true THEN waitfinished RETURN '---------------------------- waitfinished: 'loop here until moves finished domoveflag=false 'DEBUG"waitr",cr SEROUT rtx\rflow,baud,[Qstatus,0] '?status SERIN rtx,baud,50,waitfinished,[Sstatus] 'DEBUG bin Sstatus,"right",cr IF Sstatus.LOWNIB<>%1111 THEN waitfinished waitfinishedl: SEROUT ltx\lflow,baud,[Qstatus,0] '?status SERIN ltx,baud,50,waitfinishedl,[Sstatus] 'DEBUG bin result,"left",cr IF Sstatus.LOWNIB<>%1111 THEN waitfinishedl RETURN '---------------------------------------------------- manual_setup: 'edit position values to check stances speed =2 'D_leanR data 2, 4, Slfoot, 0, Srfoot, 20 'D_leanL data 2, 4, Slfoot, 225, Srfoot, 255 servo =2 SEROUT ltx\lflow,baud,[command,236] 'lfoot 235=lean 180=flat 0=tip servo =1 SEROUT ltx\lflow,baud,[command,100] 'lleg[100] 255=fd, 0=bk servo =0 SEROUT ltx\lflow,baud,[command,127] 'spare servo =0 SEROUT rtx\rflow,baud,[command,51] 'rfoot 51=lean 80=flat 255=tip servo =1 SEROUT rtx\rflow,baud,[command,160] 'rleg[160] 0=fd, 255=bk servo =2 SEROUT rtx\rflow,baud,[command,150] 'splay[150] 255=in DEBUG"end manual_setup" END '---------------------------------------------------- AlignHorns: servo =2 SEROUT ltx\lflow,baud,[command,255] 'lfoot servo =1 SEROUT ltx\lflow,baud,[command,127] 'lleg servo =0 SEROUT ltx\lflow,baud,[command,127] 'spare servo =0 SEROUT rtx\rflow,baud,[command,0] 'rfoot servo =1 SEROUT rtx\rflow,baud,[command,127] 'rleg servo =2 SEROUT rtx\rflow,baud,[command,150] 'splay DEBUG"end AlignHorns" END '---------------------- End Program -------------------------------------