'>TF-Condor.BS2 '>dynamic.BS2 dynamic walking '>TecFoot 27 July 2000 Milford Instruments code by David Buckley 'Control program for TecFoot with Stamp2 and two co-processor units. '{$STAMP BS2} '{$PBASIC 2.5} DEBUG CLS,"2.9",CR 'Operation '--------- 'if !FixedTip jumper is on then P13 is pulled LOW and tip auto adjusts. 'Two modes alternate at poweron: ' 0 stand still and respond to sensors; ' 1 walk and respond to sensors. 'IR can be disabled by holding in LeftToe onpowerup. 'US can be disabled by holding in RightToe onpowerup. 'Both Toes in disable both IR and US. 'BigRedLED is off for mode-0 and on for mode-1. 'PicAxe rear RedLED=on if IR on, YellowLED=on if US on. 'PicAxe rear GreenLEDs are on if IR reading is within stepup/down range set by BS2. ' Left-GreenLED is Left-IR, Right-GreenLED is Right-IR. 'PicAxe auto zeros IR to IR distance seen at poweron. 'PicAxe front RedLED is on while waiting for input from BS2 and when transmitting to BS2. 'PicAxe monitors Servo Battery against value sent from BS2 at Poweron (225=4.5v) and if good ' lights PowerGoodGreenLED. 'BS2 uses manual flow control to tell PicAxe when it is ready, auto didn't really work. 'If running an old version (2.6) to test without IR then PAx checks ReadyIn at poweron and ' if HIGH (pulled high by 10k) then jumps to just monitor Servo Battery and switches PowerGoodLED ' and toggles LIRgreenLED. 'Notes '----- 'There is one output(O5) and one input(I7) left on PicAxe18x. 'History '------- '01May11 0A1 Status,StatusL,StatusR renamed f4_Status,f_ClearL,f_ClearR ' ObMax changed to ObMaxL=4,ObMaxR=15 ' - now Map increments more for R and can get out of corners better ' ObAvoid was not used, deleted '11Nov10 09B changed sense of IRfeelers and amalgamated code for feelers and toes '29Oct10 03E added mode jumper for P13 fixedtip. ' Removed the series 2AA cells and went back to ' 4AA cells for the servos and 6AA cells for the electronics. '29Oct10 2.9 '28Oct10 015 added separate Walk and Turn foottip as in 2.6 '25Oct10 06A added PicAxe18x to read SharpIR sensor analog values ' current ~30mA average per device - too much for original 78L05 5v 100mA regulator!! ' added 7805 1A 5v regulator for IR ' added message to PAx with stepup/down and cmndmode (from toes) for LEDs ' added IR moves code ' 18F removed pad code and pad connector - pins used for PicAxe ' removed mode jumper on p13 '03Oct10 14B B_stop: RTB=0, LTB=0 commented out, done at START '03Oct10 2.8 '?????10 fitted two Sharp GP2Y0A21YK IR analog sensors looking down. '31Jan10 Aux connector 2 pin out diagram corrected '16Nov09 Speaker put on and 6AA removed and 2AA put in series with servo 4AA '14Nov09 MIN addded to FootTipX, otherwise can't get going as feet come off but not enough tip. ' manage FootTipX so foot leaves the floor '06Nov09 test for foot up and dynamic foottip values added adjfoottipL:, adjfoottipR: '06Nov09 foottip values put in DATA '06Nov09 2.7 '>TF-Condor2.BS2 '06Nov09 IsXOff: changed to QXtipped: - better describes code '06Nov09 footsensX changed to footsoleX, pins 8,15 '06Nov09 2.6 '04Nov09 changed the beepbeep at the end of Init: '01Nov09 reordered ReadPot:, AutoOrManual:, DecayMap:, ActOnMap: '28Oct09 2.5 '28Oct09 #1 sense every R or L pace instead of every R+L pace '30Mar09 added DefaultSspeed CON '02Oct08 2.4 reordered program introduced Cmnd and Default Cmdn in DATA '08Sep08 changed B prefix to B_ '13Jul08 2.3 changed to Getcmnd->Walk->Getcmnd... instead of Walk->Gosub(Getcmnd) ' Paces added '26Jun08 prefixed setup names with B(ehavior) '26Jun08 added Obstacle vars to remember R/L obstacles, to get out of corners '26Jun08 added usXin<6 then sbk, now can go backwards away from objects '26Jun08 ver 2.2 '03Mar08 removed RockPause and added 80mS footdown pause for really foot down '02Mar08 added balanceR and balanceL variables for feet, and pot speed control '14Feb08 added RevEd-LCD to see what US distances are '13Feb08 added toe sensors '------------------------------------------- 'BS2, BS2e EEPROM 10million writes 'BS2sx, BS2p EEPROM 100thousand writes ' 3,600 seconds/hr ' 86,400 seconds/day '31,536,000 seconds/year '------------------------------------------- 'Sharp GP2Y0A21YK 'about 51cm to ground => Vo=~0.6v 'turn on power 'wait 50ms 'read Vo (Vo updated about every 8ms) '------------------------------------------- 'Servo co-processor info: 'Send two bytes, 'Command byte format: 'XXXXYYZZ 'XXXX is speed 0 to 15 - extra increments per frame (0=slow,15=fast) ' ZZ is servo number - 0,1,2 ' 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 'T2400 (or T9600 with jumper)(or inverted with jumper) 'jumpers - 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 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 PingR PIN 4 PingL PIN 5 spkrLED PIN 6 'Piezo speaker via 0u1 parallel with LED+resistor pot CON 7 '220R pin to (10K pot in parallel with 0u1) to 0v [0-603] 'Pins 8 - 15 each pulled up to BS2+5v by 10K footsoleR PIN 8 '100K pullup, 0u1 to 0v, switched to 0v by ground force footToeR PIN 9 '10K pullup 100Uf+100uF to 0v, toe uswitch to 0v across caps PAxIRdata PIN 10 'data from PicAxe18x PAxIRcmnd PIN 11 'cmnd to PicAxe18x LCD CON 12 'use no welcome msg jumper1 then no cls needed fixedtip PIN 13 'low for auto adjust tip footToeL PIN 14 '10K pullup, 100Uf+100uF to 0v, toe uswitch to 0v across caps footsoleL PIN 15 '100K pullup, 0u1 to 0v, switched to 0v by ground force 'Pot/D7 as servo o 0v o 0v ' o 6v servo supply o 5v from 78L05 regulator ' o BS2 D7 pin o BS2 D7 pin ' 'Ping same as servo o 0v ' o 5v from 7805 regulator each Ping takes 22mA quiescent ' o BS2 pin ' 'LCD same as servo o 0v ' o 5v from 7805 regulator ' o BS2 pin ' 'Mode jumper ' o BS2-Pin13 closed=low for autoadjust tip, open=high for fixed tip ' o 0v ' 'Aux connector 2 ' E=0v Pins - all pulled up to BS2-5v by 10K 5v is from 78L05 regulator ' 100mA 5v o o 8 - footsoleR ' 100mA 5v o o 9 - footToeR ' Proto 9v o o 10 - BS2 serial from PicAxe81x_out7 ' type 9v o o 11 - side serial to PicAxe81x_in8 ' side Servo 6v o o 12 - serial to LCD ' Servo 6v o o 13 - jumper autoadjust ' 0v o o 14 - footToeL ' 0v o o 15 - footsoleL 'Servos from 4xAA cells switched =4.8v - 6v 'other elctronics 5v through 78L05 (100mA), from 9v '9v switched from battery 'ByteBus connector at front of board ' 1 ' MR_o ' 0v o ' D0 o BS2 Pin8 footsoleR ' D1 o BS2 Pin9 footToeR ' D2 o BS2 Pin10 from PicAxe81x_out7 ' D3 o BS2 Pin11 to PicAxe81x_in8 ' D4 o BS2 Pin12 serial to LCD ' D5 o BS2 Pin13 autoadjust tip jumper ' D6 o BS2 Pin14 footToeL ' D7 o BS2 Pin15 footsoleL ' 5v o ' 9v o ' 'Shoulder pads and Feet ' right left ' foot Reg-5v 0v foot 5v - 330R - LED -O- switch - 0v ' switches D8 D15 switches ^ ' D2 D3 D9 D14 D7 D6 1N914 (so 330R doesn't charge 0u1) ' D10 D13 | ' D0 D1 D11 D12 D5 D4 5v -100K -O- 0u1 - 0v ' front | ' Pin - 1N914 >-- 220R - ' (diode OR) 'Left foot diode ORed to P15 (D1.7) 'Right foot diode ORed to P8 (D1.0) ' 'Foot sense cable ' 0v 0v ' 5v 5v ' D8 D15 Sole ' D9 D14 Toe ' front ' 'LCD RevEd T9600 CON 84 'jumper2 9600 N9600 CON 16468 'jumper2 9600 i96n CON 16468 'jumper2 9600 'LCD CON 10 'use no welcome msg jumper1 then no cls needed '------------------------------------------- 'Ping 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 '------------------------------------------- 'PicAxe18x =PA18 see program below T4800 CON 188 N4800 CON 17197 T2400 CON 396 N2400 CON 16780 '------------------------------------------- 'SHARP IR irRin VAR usRin 'from PAx only used for local debug irLin VAR usLin 'from PAx only used for local debug irStepUpDiff CON 5 'edit - sent to PicAxe18 on Init irDropDiff CON 8 'edit - sent to PicAxe18 on Init '------------------------------------------- Sbatok CON 225 '4.5v 5v=255 Sbaud CON 16468 '9600 Sbaud, 8 bit, inverted Sservset CON 0 Sservread CON %00000100 SQstatus CON %00001000 'command byte to get status of all servos Scmnd VAR Byte Sspeed VAR Scmnd.HIGHNIB Sservo VAR Scmnd.LOWNIB Sstatus VAR Byte 'Servo status flags Spos VAR Sstatus SVbat VAR Sstatus f4_Status VAR Nib f_ClearR VAR f4_Status.BIT0 f_ClearL VAR f4_Status.BIT1 ' f_BatOk VAR f4_Status.BIT2 'not used ' f_spare VAR f4_Status.Bit3 'not used footup CON 1 'all switches ORed open read high foot CON 0 'servo number leg CON 1 'servo number splay CON 2 'servo number SQueryleg CON %0101 'command value to query servo 1 = leg true CON 1 false CON 0 IsHigh CON 1 ' for PULSOUT IsLow CON 0 ' for PULSOUT ObstaclesMap VAR Byte ObL VAR ObstaclesMap.HIGHNIB ObR VAR ObstaclesMap.LOWNIB ObMaxL CON 4 '0 f_pot VAR Bit '1=>pot in circuit and not set to zero f_leanR VAR Bit '1=>lean R AND move LeftFoot f_US VAR Bit 'US on f_IR VAR Bit 'IR on f_turn VAR Bit 'turning, for selection of FootTip fdlean VAR Byte lstep VAR Byte '1/2 of a stride Rtb VAR Byte 'RightTurnBy Ltb VAR Byte 'LeftTurnBy foottipR VAR Byte 'offset from flat, outside down +ve max 255-Rfootflat = 255-125 =130 foottipL VAR Byte 'offset from flat, outside down +ve max Lfootflat =125 footbalanceR VAR Byte 'offset from flat, outside down +ve footbalanceL VAR Byte 'offset from flat, outside down +ve waitcount VAR Byte RlegAt VAR Byte LlegAt VAR Byte upwaitmax CON 50 'upwaitmin CON 20 Cmnd VAR Nib mode VAR Cmnd 'only used in Init: to tell PAx i VAR Cmnd 'FOR NEXT counter in RELAX potval VAR Word IRthreshL VAR potval.HIGHBYTE IRthreshR VAR potval.LOWBYTE paces VAR Byte 'scope VAR Byte 'wandering range max for paces 'scope =10 'or set this by Pot 'servos 'pins mirror 'servo =0 rfoot, 255=tip, + out down 'servo =1 rleg, 0=fd, 255=bk 'servo =2 splay, 0=in 'servo =3 lfoot, 0=tip - out down 'servo =4 lleg, 255=fd, 0=bk 'servo =5 spare DefaultSspeed CON 12 'used by checkpot: DefaultFootTipW CON 115'110'90'88 DefaultFootTipT CON 95 '90'88 foottipmax CON 120 '<125L <130R see above foottipmin CON 90 DefaultCmnd DATA 0 'Stop,Rock,Fd,Bk,Rt,Lt DfoottipWL DATA DefaultFootTipW DfoottipWR DATA DefaultFootTipW DfoottipTL DATA DefaultFootTipT DfoottipTR DATA DefaultFootTipT '-------------------- Program ----------------------------------- Init: 'READ DfoottipWL,foottipL :DEBUG "tipW ",DEC FoottipL 'READ DfoottipWR,FoottipR :DEBUG " ",DEC FoottipR,CR 'READ DfoottipTL,foottipL :DEBUG "tipT ",DEC FoottipL 'READ DfoottipTR,FoottipR :DEBUG " ",DEC FoottipR,CR READ DefaultCmnd,Cmnd Cmnd =Cmnd.BIT1^1 *2 'toggle between 0 and 2 DEBUG "C",DEC Cmnd,CR WRITE DefaultCmnd,Cmnd f_go =Cmnd.BIT1 'if Cmnd=2 then f_go =1 'Condor either stands-still(0) or walks Fd(2), 'either mode is responsive to US,IR depending on toe state at poweron LOW 11 'BS2 ready for serin/serout pin pulled high by 10k 'mode msg GOSUB beep1 'one beep stop PAUSE 100 IF f_go THEN GOSUB beep1 'two beeps walk PAUSE 200 'for LCD to initialise SEROUT LCD,T9600,[254,1] PAUSE 2 'for LCD to cls SEROUT LCD,T9600,["TecFoot"] SEROUT LCD,T9600,[" usL usR"] 'get cmnd mode from Toe sensors 'mode =3 'IR + US on 'mode =2 'right toe pressed => US off 'mode =1 'left toe pressed => IR off mode =0 'default => SU+IR off 'PicAxe not ready earlier PAUSE 2000 'allow toe capacitors to charge so we can read toes otherwise =0 'f_US =0 '0 at powerup 'f_IR =0 '0 at powerup IF footToeR=1 THEN mode.BIT0 =1 :f_US =1 'high if not pressed IF footToeL=1 THEN mode.BIT1 =1 :f_IR =1 'high if not pressed 'DEBUG "m",DEC mode,DEC footToeL,DEC footToeR,CR 'tell PicAxe18x cmdn mode for LEDs, and threshold differences for IR sensors SEROUT PAxIRcmnd,T4800,[mode,irStepUpDiff,irDropDiff,Sbatok] HIGH 11 'BS2 not ready for serin PAin: 'sync BS2 and PAx LOW 11 'BS2 ready for serin 'truebaud so PAxIRcmnd low=ready, idle=high, flow didn't work here SERIN PAxIRdata,T4800,[WAIT(255)] HIGH 11 'BS2 not ready for serin DEBUG "M",DEC mode,CR 'Sspeedmsg: 'SEROUT LCD,T9600,[254,192,"speed="] 'GOSUB beepbeep checkpot: 'only called by manual_setup: if required, sets f_pot and Sspeed HIGH 7 PAUSE 1 RCTIME 7,1,potval IF potval=0 OR potval>800 THEN f_pot =0 Sspeed =DefaultSspeed 'no pot ELSE f_pot =1 'use pot ENDIF DEBUG ? potval,? f_pot 'DEBUG CRSRXY,0,1,? potval,? f_pot 'use when debugging pot reading, writes to same place on screen 'SEROUT LCD,T9600,[254,192,DEC potval] 'SEROUT LCD,T9600,[254,198,DEC Sspeed," "] 'GOTO checkpot 'Development options '------------------- 'GOTO showSen 'GOTO sway 'GOTO manual_setup '-------------------------------------------------------------------- Start: DEBUG CR,"S" 'GOSUB report 'DEBUG "gC" Rtb =0 Ltb =0 'ReadPot: IF f_pot THEN 'ELSE use DefaultSspeed set in checkpot: HIGH 7 PAUSE 1 RCTIME 7,1,potval Sspeed =potval /40 MAX 15 ENDIF 'DEBUG ?Sspeed 'SEROUT LCD,T9600,[254,198,DEC Sspeed," "] SEROUT LCD,T9600,[254,192,"d",DEC3 foottipL,",",DEC3 foottipR] DEBUG "d",DEC3 foottipL,",",DEC3 foottipR,CR 'AutoOrManual: ' IF PadOrUS_ THEN ReadPad 'DecayMap: obR =obR MIN obdecay -obdecay obL =obL MIN obdecay -obdecay 'ActOnMap: IF ObL+ObR>1 THEN B_Bk 'if obstacles L and R IF ObL>ObR THEN B_Rt IF ObR>0 THEN B_Lt '---------------------------- ReadSensors: '## serout ">,PinsHighByte" to neocortex, so it knows footstate '## if serin highlevel cmnd here from neocortex, can allow or disallow Toe, Sonar '## if serin highlevel cmnd set DefaultCmnd =0 '## if Not allowToes then _ReadSonar 'GOTO _ReadToes 'GOTO START _ReadToes: f_ClearL =footToeL f_ClearR =footToeR IF f_ClearL=1 AND f_ClearR=1 THEN _GetPA18Status '1 = toe clear DEBUG " toe-" GOTO _CaseStatus _GetPA18Status: IF f_IR=0 THEN _ReadSonar DEBUG " ir-" 'truebaud so PAxIRcmnd low=ready WAIT(255), 'SERIN PAxIRdata\PAxIRcmnd,T4800,200,_ReadSonar,[WAIT(255),irReport,irLin,irRin,IRthreshL,IRthreshR,SVbat] 'DEBUG HEX2 irReport," ",DEC irLin," ",DEC irRin," ",DEC IRthreshL," ",DEC IRthreshL," ",CR,"SV",DEC SVbat,CR LOW 11 'BS2 ready for serin SERIN PAxIRdata,T4800,200,_ReadSonar,[WAIT(255),f4_Status,irLin,irRin,IRthreshL,IRthreshR] HIGH 11 'BS2 not ready for serin DEBUG BIN3 f4_Status," ",DEC irLin," ",DEC irRin," ",DEC IRthreshL," ",DEC IRthreshL," ",CR _CaseStatus: 'toe hit IF f_ClearL=0 AND f_ClearR=0 THEN DEBUG "bk" ObL =ObL +obup MAX ObMaxL ObR =ObR +obup MAX ObMaxR GOTO B_Bk ENDIF IF f_ClearR=0 THEN DEBUG "lt" :ObR =ObR +obup MAX ObMaxR :GOTO B_Lt IF f_ClearL=0 THEN DEBUG "rt" :ObL =ObL +obup MAX ObMaxL :GOTO B_Rt _ReadSonar: IF f_US=0 THEN DefaultBehaviour ' DEBUG "us " GOSUB Get_Sonar usRin =usRin MAX 90 'allows mid to be 90 - 164 ie 127 +-37 (!!??!! what is 164) usLin =usLin MAX 90 '##if sensors serout sensors to neocortex _DisplaySonar: SEROUT LCD,T9600,[254,201,DEC3 usLin," ",DEC3 usRin] 'DEBUG CRSRXY,0,3,"sonar ",DEC usLin,",",DEC usRin," =>",CLREOL DEBUG CR,"sonar ",DEC usLin,",",DEC usRin," =>" _CaseSonar: IF usRin<6 THEN DEBUG "s-bk," :ObR =ObR +obup MAX ObMaxR :GOTO B_Bk IF usLin<6 THEN DEBUG "s-bk," :ObL =ObL +obup MAX ObMaxL :GOTO B_Bk IF usRin<15 THEN DEBUG "s-lt," :ObR =ObR +obup MAX ObMaxR :GOTO B_Lt IF usLin<15 THEN DEBUG "s-rt," :ObL =ObL +obup MAX ObMaxL :GOTO B_Rt '##if serin cmnd then BehaviourSetup IF usRin>usLin THEN RTb =usRin -usLin *128 /usRin MAX 100 IF usLin>usRin THEN LTb =usLin -usRin *128 /usLin MAX 100 'SEROUT LCD,T9600,[254,201,DEC3 usLin," ",DEC3 usRin] 'SEROUT LCD,T9600,[254,201,DEC3 LTb," ",DEC3 RTb] ' sturn =Ltb -Rtb 'DEBUG CRSRXY,0,4,"Ltb ",DEC Ltb," ","Rtb ",DEC Rtb," ",CR DEBUG "Ltb ",DEC Ltb," ","Rtb ",DEC Rtb," ",CR ' DEBUG 1,CR,"Tb ",DEC Ltb,",",DEC RTb," ",CR DefaultBehaviour: READ DefaultCmnd,Cmnd 'Bstop or Bfd alternates on Reset/PowerUp 'GOTO BehaviourSetup '---------------------------- 'BehaviourSetup: 'leaves IF fixedtip THEN WRITE DfoottipWL,DefaultFootTipW WRITE DfoottipWR,DefaultFootTipW WRITE DfoottipTL,DefaultFootTipT WRITE DfoottipTR,DefaultFootTipT ENDIF f_turn=0 BRANCH Cmnd,[B_Stop,B_Rock,B_Fd,B_Bk,B_Rt,B_Lt] 'set up behaviour for walk GOTO Start 'for Cmnd>5, ie illegal key presses =6,7 'lstep + fdlean must be less than mid to 0 and mid to 255 or wraps! B_Stop: foottipL =0 foottipR =0 footbalanceR =0 'offset from flat, outside down +ve footbalanceL =0 'offset from flat, outside down +ve lstep =0 fdlean =0 'RTb =0 'zeroed at START 'LTb =0 'zeroed at START GOTO Walk B_Rock: DEBUG "R" foottipL =80 foottipR =80 footbalanceR =30 'offset from flat, outside down +ve footbalanceL =25 'offset from flat, outside down +ve lstep =0 fdlean =10 GOTO Walk B_Fd: DEBUG "F" READ DfoottipWL,foottipL '=88 READ DfoottipWR,foottipR '=88 footbalanceR =25 'offset from flat, outside down +ve footbalanceL =24 'offset from flat, outside down +ve lstep =90 fdlean =35'30 'max lstep+fdlean 128 GOTO Walk B_Bk: DEBUG "B" READ DfoottipWL,foottipL '=88 READ DfoottipWR,foottipR '=88 footbalanceR =20 'offset from flat, outside down +ve footbalanceL =20 'offset from flat, outside down +ve lstep =-60 fdlean =-50 GOTO Walk B_Rt: DEBUG "R" footbalanceR =15 'offset from flat, outside down +ve footbalanceL =15 'offset from flat, outside down +ve Rtb =120 GOTO _B_t B_Lt: DEBUG "L" footbalanceR =15 'offset from flat, outside down +ve footbalanceL =15 'offset from flat, outside down +ve Ltb =120 _B_t: READ DfoottipTL,foottipL '=88 READ DfoottipTR,foottipR '=88 lstep =0 fdlean =20 f_turn=1 GOTO Walk '----------------------- Dynamic Walk ------------------------------- walk: 'dynamic walk 'DEBUG CRSRY,5,CLREOL,"W"'alk" DEBUG "W" IF f_leanR =1 THEN moveR '#1 moveL: f_leanR =1 '#1 GOSUB waitLon 'can't push off unless it's down 'PAUSE 100 'bad place for a pause, kills rocking, won't start walking tiptoR: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat-footbalanceR] SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat-foottipL] 'DEBUG DEC Rfootflat,"," 'DEBUG DEC (Rfootflat-footbalanceR) 'DEBUG "Rtip Ld=",DEC foottipL,"," QLtipped: GOSUB qmovedL IF Sstatus.LOWNIB<>%1111 THEN QLtipped 'wait till servo move done 'foot level left SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat] '+ out down oscillates and falls over, + out up won't oscillate ??????what 'PAUSE 50 'jerky walking ' and swing left forward Sservo =leg SEROUT rtx\rflow,Sbaud,[Scmnd,Rlegmid+lstep+fdlean] SEROUT ltx\lflow,Sbaud,[Scmnd,Llegmid+lstep-fdlean] Sservo =splay SEROUT rtx\rflow,Sbaud,[Scmnd,splay0 +Ltb MAX 255] '+ve => splay out PAUSE 20 'allow time for foot to lift, if it is going to 'bigger PAUSE disrupts walking adjfoottipL: IF fixedtip THEN movedL 'IF foottipL=0 THEN START READ DfoottipWL,foottipL IF f_turn THEN READ DfoottipTL,foottipL 'overwrite wth turn value IF footsoleL<>1 THEN 'adjust foottip if not off ground 'foot starts off @LFootFlat and tips down to 0 by foottipL, (Lfootflat-foottipL) so don't go through 0 foottipL =foottipL +3 MAX foottipmax 'LFootFlat 'make sure foottipL is within bounds ELSE foottipL =foottipL -1 MIN foottipmin 'don't let it go too low to oscillate ENDIF IF f_turn THEN WRITE DfoottipTL,foottipL ELSE WRITE DfoottipWL,foottipL ENDIF movedL: DEBUG "Ld=>",DEC foottipL,"," paces =paces +1 '#1 GOTO Start '#1 mid: '------------------ mid swing -------------- moveR: f_leanR =0 '#1 GOSUB waitRon 'can't push off unless it's down 'PAUSE 100 'bad place for a pause, kills rocking, won't start walking tiptoL: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat+foottipR] SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat+footbalanceL] 'DEBUG "Ltip Rd=",DEC foottipR,"," QRtipped: GOSUB qmovedR IF Sstatus.LOWNIB<>%1111 THEN QRtipped 'wait till servo move done 'foot level right SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat] 'PAUSE 50 'jerky walking 'and swing right forward Sservo =leg SEROUT rtx\rflow,Sbaud,[Scmnd,Rlegmid-lstep+fdlean] '-ve=forward SEROUT ltx\lflow,Sbaud,[Scmnd,Llegmid-lstep-fdlean] '+ve=forward Sservo =splay SEROUT rtx\rflow,Sbaud,[Scmnd,splay0 +Rtb MAX 255] '+ve => splay out PAUSE 20 'allow time for foot to lift, if it is going to 'bigger PAUSE disrupts walking adjfoottipR: IF fixedtip THEN movedR 'IF foottipR=0 THEN START READ DfoottipWR,foottipR IF f_turn THEN READ DfoottipTR,foottipR 'overwrite wth turn vaqlue IF footsoleR<>1 THEN 'adjust foottip if not off ground 'foot starts off @LFootFlat and tips down to 255 by foottipL, (Lfootflat+foottipL) so don't go through 255 foottipR =foottipR +3 MAX foottipmax'(255-RFootFlat)'130 'make sure foottipR is within bounds ELSE foottipR =foottipR -1 MIN foottipmin 'don't let it go too low to oscillate ENDIF IF f_turn THEN WRITE DfoottipTR,foottipR ELSE WRITE DfoottipWR,foottipR ENDIF movedR: DEBUG "Rd=>",DEC foottipR,"," paces =paces +1 '#1 'paces =paces +2 '#1 GOTO Start '------------------- Subroutines --------------------------------- report: 'DEBUG DEC foottipL," ",DEC waitcountL," ",DEC foottipR," ",DEC waitcountR,CR DEBUG BIN4 footsoleL," ",BIN4 footsoleR,CR RETURN '---------------------------- 'showSen: DEBUG 1,BIN1 IN12,BIN1 IN13," ",BIN1 IN9, BIN1 IN8,CR DEBUG BIN1 IN14,BIN1 IN15," ",BIN1 IN11,BIN1 IN10,CR ' goto showSen RETURN '---------------------------- Get_Sonar: 'max PULSIN is 18.5mS (Ping Documentation) 'ie 18500uS =>9250 counts@2us 'ie max target is 9250uS =>125.5inch PAUSE 10 PingR = IsLow ' make trigger 0-1-0 PULSOUT PingR,5 ' 10uS pulse, activate sensor PULSIN PingR, IsHigh, rawDist ' measure echo pulse usRin =rawDist ** uS2inch ' use inches then fits in a byte PAUSE 10 PingL = IsLow ' make trigger 0-1-0 PULSOUT PingL,5 ' 10uS pulse, activate sensor PULSIN PingL, IsHigh, rawDist ' measure echo pulse usLin =rawDist ** uS2inch ' use inches then fits in a byte RETURN '---------------------------- waitRon: 'DEBUG"wR" waitcount =0 wR: IF footsoleR<>footup THEN endwR 'footup=all off floor waitcount =waitcount+1 PAUSE 10 IF waitcountfootup THEN endwL 'footup=all off floor waitcount =waitcount+1 PAUSE 10 IF waitcount%1111 THEN waitfinished waitfinishedL: SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,waitfinishedl,[Sstatus] ' debug bin result,"left",cr IF Sstatus.LOWNIB<>%1111 THEN waitfinishedl RETURN '---------------------------- QwhereR: 'Query position servo has reached *****NOT USED***** SEROUT rtx\rflow,Sbaud,[Scmnd,0] '?status SERIN rtx,Sbaud,50,QwhereR,[Spos] RETURN '---------------------------- QwhereL: 'Query position servo has reached *****NOT USED***** SEROUT ltx\lflow,Sbaud,[Scmnd,0] '?status SERIN ltx,Sbaud,50,QwhereL,[Sstatus] RETURN '---------------------------- QmovedR: 'QueryMovedRight SEROUT rtx\rflow,Sbaud,[SQstatus,0] '?status SERIN rtx,Sbaud,50,Qmovedr,[Sstatus] RETURN '---------------------------- QmovedL: 'QueryMovedLeft SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,Qmovedl,[Sstatus] RETURN '---------------------------- beep1: 'from Init: FREQOUT spkrLED, 500, 2000, 3500 GOTO beepLED beepbeep: 'from ActOnMap: FREQOUT spkrLED, 100, 2500, 3000 PAUSE 100 FREQOUT spkrLED, 100, 2500, 3000 beepLED: OUTPUT spkrLED :spkrLED =f_go 'turn on LED if Go, Frequout leaves pin input RETURN '---------------------------------------------------------------- 'isLoff 'IF footsoleL<>footup AND Sstatus.LOWNIB<>%1111 THEN isLoff 'if tired this could wait forever 'isRoff: 'IF footsoleR<>footup AND Sstatus.LOWNIB<>%1111 THEN isRoff 'if tired this could wait forever '---------------------------------------------------------------- 'sway: 'simple sway to check servo limits ' DEBUG"sway" 'Sspeed =8 ''tip ' Sservo =0 ' SEROUT rtx\rflow,Sbaud,[Scmnd,170] 'rfoot,255=tip, ' SEROUT ltx\lflow,Sbaud,[Scmnd,180] 'lfoot, 0=tip,185,210 ' GOSUB waitfinished ''pause 1000 ''foot level right ' Sservo =0 ' SEROUT rtx\rflow,Sbaud,[Scmnd,90] 'rfoot,255=tip, 135 ' SEROUT ltx\lflow,Sbaud,[Scmnd,180] 'lfoot, 0=tip,185,210 ' GOSUB waitfinished ' PAUSE 100 '' gosub STANDUP ''tip ' Sservo =0 ' SEROUT rtx\rflow,Sbaud,[Scmnd,70] 'rfoot,255=tip, 35, 27 ' SEROUT ltx\lflow,Sbaud,[Scmnd,80] 'lfoot, 0=tip, ' GOSUB waitfinished ''pause 1000 ''foot level left ' Sservo =0 ' SEROUT rtx\rflow,Sbaud,[Scmnd,70] 'rfoot,255=tip, 35, 27 ' SEROUT ltx\lflow,Sbaud,[Scmnd,160] 'lfoot, 0=tip, 120 ' GOSUB waitfinished ' PAUSE 100 '' gosub STANDUP ' GOTO sway 'END '---------------------------------------------------------------- 'manual_setup: 'edit position values to check stances ''Rfootflat CON 125 'us49=124 '+ is down ''Lfootflat CON 131 'us51=120 '+ is up ''Rlegmid CON 128 'us50=128 '-ve fd ''Llegmid CON 128 'us50=128 '+ve fd ''splay0 CON 59 'us23=59 '+ve out ' Sspeed =8 ' Sservo =0 '' SEROUT rtx\rflow,Sbaud,[Scmnd,0] '=1018-1020us 'rfoot, 255=tip, 35, 27 'to check pulse length '' SEROUT rtx\rflow,Sbaud,[Scmnd,255] '=2042us 'rfoot, 255=tip, 35, 27 'to check pulse length ''END ' SEROUT rtx\rflow,Sbaud,[Scmnd,124] 'rfoot, 255=tip, 35, 27 '' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'rfoot, 255=tip, 35, 27 ' SEROUT ltx\lflow,Sbaud,[Scmnd,120] 'lfoot, 0=tip,185,220 '' SEROUT ltx\lflow,Sbaud,[Scmnd,potval] 'lfoot, 0=tip,185,220 ' Sservo =1 ' SEROUT rtx\rflow,Sbaud,[Scmnd,128] 'rleg, 0=fd, 255=bk '' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'rleg, 0=fd, 255=bk ' SEROUT ltx\lflow,Sbaud,[Scmnd,128] 'lleg, 255=fd, 0=bk '' SEROUT ltx\lflow,Sbaud,[Scmnd,potval] 'lleg, 255=fd, 0=bk ' Sservo =2 ' SEROUT rtx\rflow,Sbaud,[Scmnd,94] 'splay, 0,255=out '' SEROUT rtx\rflow,Sbaud,[Scmnd,potval] 'splay, 0,255=out ' ' SEROUT ltx\lflow,Sbaud,[Scmnd,127] 'nothing '' GOTO checkpot ' ' DEBUG"endtest" 'END '================================================================ '********** PicAxe18x for Sharp IR *********** ''>CondorIR-18x 26 October 2010, 00:26:36 ''PicAxe18x ''reads Sharp GP2Y0A02YK IR distance sensors and serials to BS2 '(There is one output(O5) and one input(I7) left on PicAxe18x.) ' ''cmnd sent from BS2 depends on Toe states '' default no toes pressed =3 '' right toe pressed bit0=low '' left toe pressed bit1=low ''desired voltage threshold for 'Servo Battery voltage OK' is sent from BS2 on Powerup. ''History ''------- ''11Nov10 changed sense of ir reports, added statusBat ' ''outputs 'symbol L_IRokpin =0 'GreenLED through 390R to 0v 'symbol R_IRokpin =1 'GreenLED through 390R to 0v 'symbol IR_LEDpin =2 'RedLED through 390R to 0v 'symbol US_LEDpin =3 'YellowLED through 390R to 0v 'symbol VSbatokpin=4 'GreenLED through 330R to 0v ''pin5 spare 'symbol waitRxLEDpin =6 'RedLED through 390R to 5v 'symbol ToBS2pin =7 ''inputs 'symbol VSbatpin =0 '1u0 to ground, V servo battery to pin through 1M 'symbol LIRpin =1 '1u0 to ground, input to pin through 100K 'symbol RIRpin =2 '1u0 to ground, input to pin through 100K 'symbol FromBS2pin=6 'symbol BS2ready_ =pin6 ''pin7 spare ' ''b0 'W0 ''b1 'W0 'symbol cmnd =B0 'W0 'Init then START 'symbol status =B0 'W0 'Init then START 'symbol statusR =BIT0 '1=ok 0=obstacle or cliff 'symbol statusL =BIT1 '1=ok 0=obstacle or cliff 'symbol statusBat =BIT2 '1=ok 0=low 'symbol Rdist =B2 'W1 'symbol Ldist =B3 'W1 'symbol accumR =W2 'ZeroIR: ''symbol =B4 'W2 ' ''symbol =B5 'W2 ' 'symbol accumL =W3 'ZeroIR: 'symbol threshR =B6 'W3 'START 'symbol threshL =B7 'W3 'START 'symbol drop =B8 'W4 'symbol stepup =B9 'W4 'symbol VSbat =B10 'W5 'START 'symbol VSbatok =b11 'W5 ' ' 'Init: ' High ToBS2pin 'prime pin high ready for T4800_4, T idle is high, N idle is low ' if BS2ready_=1 then SVokLED2 'for Condor2.6 ' ' low waitRxLEDpin 'LED on indicate waiting for serin cmnd ' serin FromBS2pin,T4800_4,cmnd,stepup,drop,VSbatok ' high waitRxLEDpin 'LED off indicate got serin cmnd ' HIGH 2 'do IR on ' HIGH 3 'do US on ' IF cmnd=0 THEN ' LOW 2 ' LOW 3 ' ENDIF ' IF cmnd=2 THEN 'US off ' LOW 3 ' ENDIF ' IF cmnd=1 THEN 'IR off ' LOW 2 ' ENDIF ' 'ZeroIR: ' readadc RIRpin,Rdist ' readadc LIRpin,Ldist ' AccumR =Rdist ' AccumL =Ldist ' PAUSE 50 ' readadc RIRpin,Rdist ' readadc LIRpin,Ldist ' AccumR =AccumR +Rdist ' AccumL =AccumL +Ldist ' PAUSE 50 ' readadc RIRpin,Rdist ' readadc LIRpin,Ldist ' AccumR =AccumR +Rdist ' AccumL =AccumL +Ldist ' PAUSE 50 ' readadc RIRpin,Rdist ' readadc LIRpin,Ldist ' AccumR =AccumR +Rdist ' AccumL =AccumL +Ldist ' ' AccumR =AccumR /4 ' AccumL =AccumL /4 ' ' WRITE 0,AccumR 'R average ' WRITE 1,AccumL 'L average ' WRITE 3,VSbatok 'threshold for good battery ' 'Sync: ' SEROUT ToBS2pin,T4800_4,(255) ' IF BS2ready_=0 THEN Sync 'goes high when BS2 gets 255 ' 'START: ' HIGH 6 'RedLED off ' IF BS2ready_=0 THEN sendit ' 'VSokLED: ' readadc VSbatpin,VSbat ' VSbat =VSbat max 254 '255 is sync ' ' IF Vsbat>VSbatok THEN ' HIGH VSbatokpin ' statusBat =1 ' ELSE ' LOW VSbatokpin ' statusBat =0 ' ENDIF ' '_R: ' readadc RIRpin,Rdist ' statusR =1 ' READ 0,threshR ' threshR =threshR +stepup ' IF Rdist>threshR THEN ' statusR=0 ' ENDIF ' READ 0,threshR ' threshR =threshR MIN drop -drop ' IF RdistthreshL THEN ' statusL=0 ' ENDIF ' READ 1,threshL ' threshL =threshL MIN drop -drop ' IF LdistVSbatok THEN ' HIGH VSbatokpin ' ELSE ' LOW VSbatokpin ' ENDIF ' PAUSE 100 ' TOGGLE 0 ' LOOP '================================================================