'>TF-Condor.BS2 '{$STAMP BS2} '{$PBASIC 2.5} DEBUG CLS,"3.6",CR 'Operation '--------- 'Two modes alternate at poweron: ' 0 stand still and respond to sensors; ' 1 walk and respond to sensors. ' switched from TVR010 ' KEY_0 W(wander) =>1 ' KEY_POWER H(halt) =>0 ' KEY_1 =>Rock ' KEY_UP Fd ' KEY_DOWN Bk ' KEY_RIGHT Rt ' KEY_LEFT Lt 'Toe sensors can not be disabled '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. ''if f_fixedtip is low(=default) tip auto adjusts. 'Bugs '---- 'Init: f_go =Cmnd 'for beep AND LED control '?? only works for 0,1 'Notes '----- 'There is one io (ioC.7) left on PicAxe18M2. iC.5 used for IRin 'BS2pin13 spare 'BS2 Pot pin7 could do a compass 'Foot processors tranparent, do no processing other than read switches and light LEDs 'old - There is one output(O5) and one input(I7) left on PicAxe18x. 'History '------- '21Jan15 107 added f_react because Do3Bk4Rt always re-entered! Altered Do3Bk4Rt to reAct6Bk6Rt '21Jan15 113 added Vv and added it at start of Do3Bk4Rt so can tell when it triggers '29Jul14 11D added in Do3Bk4Rt '25Aug13 141 commented f_fixed tip statements, moved _GetPA18data: IF f_IR=0 THEN _ReadSonar to end ' decoded IR from TVR10controller '09Jul13 15F moved 'ActOnMap: so IRcmnd gets preference, cmnd Byte, made UpdateMap, deleted Avoid: '09Jul13 v3.6 '09Jul13 added IRcmnd in from PicAxe18m2 '31Jan13 136 GetDo: changed to SetDo:, getDo: put after DoCmnd: (faster - no sensor test) '12Dec12 13F changed round sensor tests and moved Avoid: (was _CaseStatus:) '08Dec12 v3.5 '08Dec12 124 added pDo, DefaultDo DATA, pDo DATA, reorganised variables ' moved ActOnMap: back, IRthreshX changed to floordistX '08Dec12 v3.4 '09Dec12 11F moved ActOnMap: but doesn't work properly there, don't use this '08Dec12 v3.3 '30Jul12 119 removed 'pin13 =footsoleX' from adjfoottipX: why was it there? ' changed topcase of rightleg servo - lugs broken after a fall at MakerFaire ' made new leftleg tendon - broken ' removed 'IF f_fixedtip THEN....' from BehaviourSetup: to Init: '30Jul12v3.2 '01Jul12 changed left stride servo (with Jet2 tilt) ' changed round stride servos as on WildEagle so lead extensions not needed ' made extension plates for feet instead of making longer tendons. ' added 100k pullup to toes so can run program without toes connected '11Apr12 removed 78L05, All logic now uses 7805 ' removed 10k pullups on BS2pinsH ' removed WAIT 255 from PicAxe Serin ' moved IRthreshX to Init, no point in getting it every cycle, saves time. ' Stop changed to Halt so can debug 'H', 'S' was used for Start: ' waitfinishedX: changed to waitmovedX ' reading tip in adjust was already done in Behave, if tip=0 don't adjust ' mode in init: replaced with cmnd '28Mar12 Changed PicAxe18X To 18M2, added three pin headers for IRcode(PicAxe) and compass(not connected) '28Mar12 v3.1 ' removed pin13 autotip jumper,changed fixedtip from pin13 to f_fixedtip_ '26Mar12 0B5 added Frustration and A- CON x '25Mar12 leg pushrod is in inside hole in servo-horn, ' tried with rod in middle hole to lengthen stride but didn't walk as well, ' even tried Fd lean at 5,10,15,20,25 so put everything back ' to inside hole, Fdlean 35, lstep 90 '22Mar12 0CF removed PicAxe18x Sync code, not needed, B_Stop: RTb=0 LTb=0 added '19Feb12 0C6 changed ObupL, ObupR, back to Obup - didn't make any difference in bad corners ' removed duplicate debug tip '19Feb12 09B changed Obup to ObupL, ObupR, preferentialy avoid R '19Feb12 09B changed (worn out) foot switches to microswitches which have less springiness -> tip range needs to be bigger ' changed foot flat positions from 1.5ms To R1.25ms and L1.75ms' ' To allow bigger tip range, altered default tip To 140 '17Feb12 09B moved LOW 11 to start, code only worked because of PA18x delay on startup. '27Nov11 09B added fixed 18M2-OLED@2400, (won't work at 9600) ' changed text of display so down values clearer ' Pings stop working if <5v, so added test for USXin=0 (ie US not working) to avoid walking back, ignores US if either=0 '27Nov11 v3.0 '27Nov11 0A0 changed baud rates to be indirect to CONs (why the extra byte?) '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 v2.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 v2.8 'Oct 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 v2.7 '>TF-Condor2.BS2 '06Nov09 IsXOff: changed to QXtipped: - better describes code '06Nov09 footsensX changed to footsoleX, pins 8,15 '06Nov09 v2.6 '04Nov09 changed the beepbeep at the end of Init: '01Nov09 reordered ReadPot:, AutoOrManual:, DecayMap:, ActOnMap: '28Oct09 v2.5 '28Oct09 #1 sense every R or L pace instead of every R+L pace '30Mar09 added DefaultSspeed CON '02Oct08 v2.4 reordered program introduced Cmnd and Default Cmdn in DATA '08Sep08 changed B prefix to B_ '13Jul08 v2.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 vver 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 '>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. '================================================================= 'BS2, BS2e EEPROM 10million writes 'BS2sx, BS2p EEPROM 100thousand writes ' 3,600 seconds/hr ' 86,400 seconds/day '31,536,000 seconds/year '================================================================= TVR010_PicAxe_IR_controller: '--------------------------- 'Before use, the transmitter must be programmed with the ‘Sony’ transmit code. '1. Insert 2 AAA size batteries, preferably alkaline. '2. Press ‘S’ and ‘B’ at the same time. S is in the centre of the arrows. ' The top left red LED should light. '3. Press ‘0’. The LED should flash. '4. Press ‘1’. The LED should flash. '5. Press ‘3’. The LED should go out. '6. Press the red power button (top right). '------------------------------------------- 'IRin cmnds PicAxe controller 'DO NOT PRESS OTHER KEYS 'ie [A] [B] [C] [D] [E] [F] [G] 'They change the Mode and [B] has to be pressed to change back. '[square] [triangle] [()] [L] [X] [backwards F] have no effect key_power CON 21 'Stop -> command mode key_up CON 16 'Step Forward key_down CON 17 'Step Backward key_right CON 18 'Turn Right key_left CON 19 'Turn Left 'x not used on Condor key_bar CON 96 'x'learn an Act, next [key] selects a Act key_tent CON 54 'x'next [key] selects a Coded-Act key_vert_cross CON 37 'x'use synonym for easier coding key_vcross CON 37 key_diag_cross CON 20 'x'use synonym for easier coding key_xcross CON 20 'x'transfer EEPROM to PC for including in programs ' 'Coded Acts KEY_1 CON 0 'Rock KEY_2 CON 1 KEY_3 CON 2 KEY_4 CON 3 KEY_5 CON 4 KEY_6 CON 5 KEY_7 CON 6 KEY_8 CON 7 KEY_9 CON 8 key_minus CON 98 key_0 CON 9 'QUIT command mode and Wander key_plus CON 11 '================================================================= '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, servoTo ' 01 - report servo position, servoAt ' 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-FRM010-serialLCDfirmware 'T2400 (or T9600 with jumper)(or inverted with jumper3) 'jumpers 1,2,3,4 - 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 line for left leg servo co-processor lflow CON 1 'flow control from left leg servo co-processor 'pin=0 to prevent BS2tx rtx CON 2 'serial line for right leg servo co-processor rflow CON 3 'flow control from right leg servo co-processor 'pin=0 to prevent BS2tx PingR PIN 4 PingL PIN 5 spkrLED PIN 6 'Piezo speaker via 0u1 parallel with LED+resistor pot CON 7 'see below 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 PicAxe18m2 PA18rdy_ PIN 10 'Ready_ from PicAxe18m2 PAxIRcmnd PIN 11 'cmnd to PicAxe18m2 BS2ready_ PIN 11 'BS2 ready for serin/serout pin pulled high by 10k LCD CON 12 'use no welcome msg jumper1 then no cls needed pin13 PIN 13 'spare - was high for auto adjust tip, now a flag 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 7805 regulator ' o BS2 D7 pin o BS2 D7 pin 'Pot Board ' 220R pin to {10K pot in parallel with 0u1} to 0v [0-603] ' free end of pot to 5v by jumper for A/D use ' 220R To LED To 0v ' '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 - removed '{ o BS2-Pin13 closed=low for autoadjust tip, open=high for fixed tip '{ o 0v ' '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. ' E=0v Pins ^ pulled up to BS2-5v by 100K (other logic 5v is from 7805 regulator) ' 100mA 5v o o 8 footsoleR ' 100mA 5v o o 9 ^ footToeR ' Proto 9v o o 10 BS2 serial from PicAxe18M2_B.7 ' type 9v o o 11 side serial to PicAxe18M2_C.6 ' side Servo 6v o o 12 serial to LCD ' Servo 6v o o 13 ' 0v o o 14 ^ footToeL ' 0v o o 15 footsoleL 'Servos from 4xAA cells switched =4.8v - 6v 'Servo CoPro from BS2 5v 'other elctronics 5v through 7805 (1A), 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 PicAxe18M2_B.7 ' D3 o BS2 Pin11 to PicAxe18M2_C.6 ' D4 o BS2 Pin12 serial to LCD ' D5 o BS2 Pin13 ' D6 o BS2 Pin14 footToeL ' D7 o BS2 Pin15 footsoleL ' 5v o ' 9v o ' 'Shoulder connectors and Feet ' left right ' foot foot 5v - 330R - LED -O- switch - 0v ' switches 0v toe D9 switches ^ ' S2 S1 5v sole D8 S1 S2 1N914 (so 330R doesn't charge 0u1) ' D15 sole 5v | ' S4 S3 D14 toe 0v S3 S4 5v -100K -O- 0u1 - 0v ' | ' Pin - 1N914 >-- 220R - ' (diode OR) 'Left foot diode ORed to P15 (D1.7) 'Right foot diode ORed to P8 (D1.0) ' 'Foot sense cable ' D14 D9 Toe ' D15 D8 Sole ' 5v 5v ' 0v 0v ' 'Servo connectors ' 0 1 2 2 1 0 ' ' 2 Splay ' 1 Leg ' 0 Foot '================================================================= #IF ($STAMP = BS2SX) OR ($STAMP = BS2P) #THEN T2400 CON 1021 'RevEd-OLED-18M2 N2400 CON 17405 ' T4800 CON 500 ' N4800 CON 16884 ' ' T9600 CON 240 'PAxBaud idle high ' T9600 CON 240 'jumper2 9600 RevEd-LCD-FRM010-serialLCDfirmware ' N9600 CON 16624 'jumper2 9600; jumper3 inverted RevEd-LCD-FRM010-serialLCDfirmware i96n CON 16624 'Sbaud servo-copro ms100 CON 100 *5/2 'beep ms500 CON 500 *5/2 'beep Hz2000 CON 2000 *5/2 'beep Hz2500 CON 2500 *2/5 'beep Hz3000 CON 3000 *2/5 'beep Hz3500 CON 3500 *2/5 'beep #ELSE 'BS2 T2400 CON 396 'RevEd-OLED-18M2 N2400 CON 16780 ' T4800 CON 188 ' N4800 CON 16572 ' T9600 CON 84 'PAxBaud idle high ' T9600 CON 84 'jumper2 9600 RevEd-LCD-FRM010-serialLCDfirmware ' N9600 CON 16468 'jumper2 9600; jumper3 inverted RevEd-LCD-FRM010-serialLCDfirmware i96n CON 16468 'Sbaud servo-copro ms100 CON 100 'beep ms500 CON 500 'beep Hz2000 CON 2000 'beep Hz2500 CON 2500 'beep Hz3000 CON 3000 'beep Hz3500 CON 3500 'beep #ENDIF '----------------- 'true CON 1 'false CON 0 'scope VAR Byte 'wandering range max for paces 'scope =10 'or set this by Pot 'RlegAt VAR Byte 'LlegAt VAR Byte 'paces VAR Byte IsHigh CON 1 ' for PULSOUT IsLow CON 0 ' for PULSOUT Rin VAR Byte ' general R in irRin, usRin Lin VAR Byte ' general L in irLin, usLin tempWord VAR Word 'part used potval_tmp VAR tempWord 'not used temp1 VAR tempWord.HIGHBYTE 'not used temp0 VAR tempWord.LOWBYTE 'not used tempIRcmnd VAR tempWord.LOWBYTE temp VAR tempWord.LOWBYTE waitcount_tmp VAR temp cmnd VAR Byte i VAR cmnd 'counter in RELAX: 'Scmnd VAR Byte 'servo cmnd - see below at 'Servos pDo VAR Byte f4_Status VAR Nib f_ObsR VAR f4_Status.BIT0 '0=ok 1=obstacle or cliff, sethigh by toe hit or PA18IR f_ObsL VAR f4_Status.BIT1 '0=ok 1=obstacle or cliff, sethigh by toe hit or PA18IR f_SBatOk_ VAR f4_Status.BIT2 '0=ok 1=low 'not used ' f_spare VAR f4_Status.Bit3 'not used f8_flags VAR Byte f_go VAR f8_flags.BIT0 'not stopped, cmnd<>0 for beep and LED control f_react VAR f8_flags.BIT1 '1=doing a reAct 'f_pot VAR f8_flags.BIT1 '1=>pot in circuit and not set to zero 'f_fixedtip VAR f8_flags.BIT2 '1=>fixedtip, 0=>auto, defaults to 0 on powerup, never set to 1 f_US VAR f8_flags.BIT3 'US on f_IR VAR f8_flags.BIT4 'IR on f_leantR VAR f8_flags.BIT5 '1=>leant R AND moved LeftFoot f_turn VAR f8_flags.BIT6 'turning, for selection of FootTip f_MtL VAR f8_flags.BIT7 'last turn was Lt =>1 'stepping fdlean VAR Byte lstep VAR Byte '1/2 of a stride Rby VAR Byte 'RightBy - steering Lby VAR Byte 'LefttBy - steering TurnByMax CON 100 'see splay0 above foottip VAR Byte 'offset from flat, outside down +ve 'foottipR VAR Byte 'offset from flat, outside down +ve max 255-Rfootflat = 255-58 =197 'foottipL VAR Byte 'offset from flat, outside down +ve max Lfootflat =183 footbalanceR VAR Byte 'offset from flat, outside down +ve footbalanceL VAR Byte 'offset from flat, outside down +ve ObstaclesMap VAR Byte ObL VAR ObstaclesMap.HIGHNIB ObR VAR ObstaclesMap.LOWNIB upwaitmax CON 50 'at pause 10 =>500ms 'upwaitmin CON 20 ObMaxL CON 15 ' TurnByMax =255 -59 =196 'DefaultSspeed CON 12 'Init DefaultSspeed CON 15 'Init DefaultFootTipW CON 140 '125 DefaultFootTipT CON 100 '95 foottipmax CON 170 'MAX Lfootflat MAX (255 -Rfootflat ) foottipmin CON 90 '-------------------------------------------- z CON 0 H CON 1 ' Halt Rock CON 2 F CON 3 B CON 4 R CON 5 L CON 6 E CON 7 ' Stand Easy - RELAX D CON 8 ' do Do in next byte W CON 9 ' Wander S CON 10 Vv CON 11 DfoottipW DATA DefaultFootTipW '0 'DfoottipWR DATA DefaultFootTipW 'DfoottipWL DATA DefaultFootTipW DfoottipT DATA DefaultFootTipT '1 'DfoottipTR DATA DefaultFootTipT 'DfoottipTL DATA DefaultFootTipT Dos DATA DoH,DoF '2,3 DefaultDoi DATA 0 '4 '0 or 1, first two entries only for Reset state-toggle DefaultDo DATA DoF '5 'dummy, overwritten in Init DoH DATA H,E '6 DoE DATA D,DoE '7,8 DoF DATA F,z '9,10 'Do4TurnR DATA A_Rt,A_Rt,A_Rt,A_Rt,z reAct6Bk6Rt DATA Vv,B,B,B,B,B,B,R,R,R,R,R,R,Vv,Vv,z '================================================================= '-------------------- Program Description ----------------------- 'Init: ' update Default cmnd ' get current Do ' beep to indicate mode ' check Toes to see if US, IR are to be disabled ' 'Note even if IR are not used by BS2 they can't be turned off to save current ' talk to PicAxe18 '' check to see if pot fitted and read it 'Start: ' 'ReadPot: ' 'DecayMap: ' 'ActOnMap: ' avoid ' 'ReadSensors: ' '_ReadToes: ' _GetPA18Status: ' _CaseStatus: IR obstacle ' avoid ' _ReadSonar: ' _DisplaySonar: ' _CaseSonar: 'inches ' avoid ' steer ' DefaultBehaviour: ' 'BehaviourSetup: 'leaves ' B_Stop: B_Rock: B_Fd: B_Bk: B_Rt: B_Lt: B_t: 'walk: 'dynamic walk ' moveL: ' moveR: ' tiptoR: ' tiptoL: ' QLtipped: ' QRtipped: ' adjfoottipR: ' adjfoottipL: ' movedL: ' movedR: '================================================================= 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 LOW 11 'BS2 ready for PA18 serin/serout pin pulled high by 10k 'PA18 checks this to see if older software is running 'At power on PA18 waits ~37ms checking serial port for download before running program 'so 11 is low before PA18 is running READ DefaultDoi,Cmnd Cmnd =Cmnd.BIT0^1 'toggle between 0 and 1 DEBUG "C",DEC Cmnd," " WRITE DefaultDoi,Cmnd f_go =Cmnd 'Condor either stands-still(0) or walks Fd(1), 'either mode is responsive to US,IR depending on toe state at poweron 'f_fixedtip =1 'default on poweron =0 'IF f_fixedtip THEN ' WRITE DfoottipW,DefaultFootTipW ' WRITE DfoottipT,DefaultFootTipT 'WRITE DfoottipWR,DefaultFootTipW 'WRITE DfoottipWL,DefaultFootTipW 'WRITE DfoottipTR,DefaultFootTipT 'WRITE DfoottipTL,DefaultFootTipT 'ENDIF 'ModeGoBeep: GOSUB beep500 '500ms 'one beep stop PAUSE 100 IF f_go THEN GOSUB beep500 'two beeps walk PAUSE 100 'OLED needs 500ms to initialise but one beep1 is not enough (manual 500ms total) SEROUT LCD,LCDbaud,[254,1] PAUSE 10 'for OLED to cls (6.2ms) '254,2, is needed for OLED - according to Manual not needed! SEROUT LCD,LCDbaud,[254,2,"Tip usL usR"] SEROUT LCD,LCDbaud,[254,192,"Ob "] 'GetSensorMode: '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 cmnd =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 cmnd.BIT0 =1 : f_US =1 'high if not pressed IF footToeL=1 THEN cmnd.BIT1 =1 : f_IR =1 'high if not pressed DEBUG "L-IR,R-US %",BIN2 cmnd,CR initPA18: DEBUG"PA18" 'tell PicAxe18m2 cmnd mode for LEDs, and threshold differences for IR sensors 'after poweron PicAxe is waiting for this 'If PicAxe18m2 is already running - say BS2 was reset - ' then PA18rdy_ will be high=IDLE it will ignore the Serout setup information ' floordistL/R will be read as cmnd,f4_Status ie wrong ' but it doesn't matter the PicAxe will use data from when it had power on ' the BS2 program will run, just the floordist DEBUG message will be wrong. IF PA18rdy_=1 THEN initPA18 'wait ready SEROUT PAxIRcmnd,PAxBaud,[cmnd,irStepUpDiff,irDropDiff,Sbatok] LOW 11 'BS2 ready for serin SERIN PAxIRdata,PAxBaud,1000,Start,[floordistL,floordistR] HIGH 11 'BS2 not ready for serin DEBUG " IR2floorLR ",DEC floordistL," ",DEC floordistR," ",CR 'floordist not used again 'Sspeedmsg: 'SEROUT LCD,LCDbaud,[254,192,"speed="] 'GOSUB beepbeep 'REM------------------- 'checkpot: 'only called by manual_setup: if required, sets f_pot and Sspeed ' HIGH 7 ' PAUSE 1 ' RCTIME 7,1,potval_tmp ' IF potval_tmp=0 OR potval_tmp>800 THEN 'f_pot =0 'Sspeed =DefaultSspeed 'no pot ' ELSE ' f_pot =1 'use pot ' ENDIF ' DEBUG ? potval_tmp,? f_pot 'DEBUG CRSRXY,0,1,? potval_tmp,? f_pot 'use when debugging pot reading, writes to same place on screen 'SEROUT LCD,LCDbaud,[254,192,DEC potval_tmp] 'SEROUT LCD,LCDbaud,[254,198,DEC Sspeed," "] 'GOTO checkpot 'ENDREM------------------- SetDo: READ DefaultDoi,cmnd READ Dos +cmnd,pDo WRITE DefaultDo,pDo DEBUG "D",DEC pDo Sspeed =DefaultSspeed 'Development options '------------------- 'GOTO showSen 'GOTO sway 'GOTO manual_setup '================================================================= Start: DEBUG CR,CR,"S " 'GOSUB report Rby =0 Lby =0 'ReadPot: 'Pot not fitted, f_pot =0 in Init ' IF f_pot THEN 'ELSE use DefaultSspeed set in checkpot: ' HIGH 7 ' PAUSE 1 ' RCTIME 7,1,potval_tmp ' Sspeed =potval_tmp /40 MAX 15 ' ENDIF 'DEBUG ?Sspeed 'SEROUT LCD,LCDbaud,[254,198,DEC Sspeed," "] ''SEROUT LCD,LCDbaud,[254,2,"T",DEC3 foottip] '254,2, is needed for OLED - according to Manual not needed! DEBUG "Tip",DEC3 foottip," " '",Rd",DEC3 foottipR,CR 'DecayMap: obR =obR MIN obdecay -obdecay obL =obL MIN obdecay -obdecay DEBUG "m",DEC ObL,",",DEC ObR," " SEROUT LCD,LCDbaud,[254,195,DEC2 ObL," ",DEC2 ObR] '---------------------------- '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 _GetPA18data: 'truebaud so PAxIRcmnd low=ready idle=high WAIT(255), WAIT(255)=old????? LOW 11 'BS2 ready for serin SERIN PAxIRdata,PAxBaud,100,_ReadSonar,[tempIRcmnd,f4_Status,irLin,irRin] 'min wait 50 HIGH 11 'BS2 not ready for serin 'f4_Status = f_spare,f_SBatOK_,f_ObsL,f_ObsR DEBUG "?",DEC tempIRcmnd,",","%",BIN3 f4_Status,",",DEC irLin,",",DEC irRin IF tempIRcmnd=255 THEN _NoIRcmnd cmnd =255 'translate IR codes LOOKDOWN tempIRcmnd,[254 ,254, key_1, key_up,key_down,key_right,key_left,254,254,key_0,key_power],cmnd ' 0 1 2 3 4 5 6 7 8 9 10 ' getDo,B_Halt,B_Rock,B_Fd, B_Bk, B_Rt, B_Lt 254 254 _W _Halt IF cmnd<>255 THEN Docmnd 'do valid cmnd, ignore other codes _NoIRcmnd: IF f_IR=0 THEN _ReadToes IF f_ObsL=1 OR f_ObsR=1 THEN DEBUG " floor " :GOTO UpdateMap _ReadToes: f_ObsL =~footToeL f_ObsR =~footToeR IF f_ObsL=1 OR f_ObsR=1 THEN DEBUG " toe" :GOTO UpdateMap _ReadSonar: IF f_US=0 THEN ReadDo 'DEBUG "us " GOSUB Get_Sonar 'inches usRin =usRin MAX USmaxRange '90 usLin =usLin MAX USmaxRange '90 '##if sensors serout sensors to neocortex _DisplaySonar: SEROUT LCD,LCDbaud,[254,201,DEC3 usLin," ",DEC3 usRin] 'DEBUG CRSRXY,0,3,"sonar ",DEC usLin,",",DEC usRin," =>",CLREOL DEBUG " s",DEC usLin,",",DEC usRin," =>" IF usRin=0 AND usLin=0 THEN ReadDo 'don't act on sonar if a Sonar not working ie =0 _CaseSonar: 'inches 6=close IF usLinusLin THEN Rby =usRin -usLin *TurnByMax /usRin 'MAX TurnByMax 'can't be >TurnByMax even if usLin=0 IF usLin>usRin THEN Lby =usLin -usRin *TurnByMax /usLin 'MAX TurnByMax 'can't be >TurnByMax even if usRin=0 'SEROUT LCD,LCDbaud,[254,201,DEC3 usLin," ",DEC3 usRin] 'SEROUT LCD,LCDbaud,[254,201,DEC3 LTb," ",DEC3 RTb] DEBUG "Lby",DEC Lby,",","Rby",DEC Rby UpdateMap: IF f_ObsL THEN ObL =ObL +obup MAX ObMaxL '15 IF f_ObsR THEN ObR =ObR +obup MAX ObMaxR '15 IF f_react THEN GOTO ReadDo ActOnMap: IF ObL+ObR>6 THEN pDo =reAct6Bk6Rt : f_react =1 : GOTO ReadDo 'if obstacles L and R IF ObL+ObR>mapClose THEN B_Bk 'if obstacles L and R IF ObL>ObR THEN B_Rt IF ObR>0 THEN B_Lt ReadDo: READ pDo,Cmnd pDo =pDo +1 DEBUG " c",DEC Cmnd 'GOTO Docmnd '---------------------------- Docmnd: f_turn=0 ' 0 1 2 3 4 5 6 7 8 9 10 11 ' z H Rock F B R L E D W S Vv BRANCH Cmnd,[getDo,B_Halt,B_Rock,B_Fd,B_Bk,B_Rt,B_Lt,B_Easy,_D,_W,_Halt,_Vv] 'fall through for illegal cmnds GOTO Start getDo: 'DefaultDo set in init, read P18irin to modify f_react =0 READ DefaultDo,pDo GOTO ReadDo _D: 'do Do in next byte READ pDo,pDo GOTO START _W: 'Wander WRITE DefaultDo,DoF f_go =1 GOSUB beepLED cmnd =0 GOTO Docmnd _Halt: WRITE DefaultDo,DoH f_go =0 GOSUB beepLED cmnd =0 GOTO Docmnd _Vv: GOSUB bipbip GOTO ReadDo '---------------------------- BehaviourSetup: 'leaves B_Easy: ' Stand Easy - RELAX GOSUB RELAX GOTO START 'lstep + fdlean must be less than mid to 0 and mid to 255 or wraps! B_Halt: DEBUG "H" foottip =0 footbalanceR =0 'offset from flat, outside down +ve footbalanceL =0 'offset from flat, outside down +ve lstep =0 fdlean =0 Rby =0 'stops the hips jittering Lby =0 'stops the hips jittering GOTO Walk B_Rock: DEBUG "R" foottip =80 footbalanceR =30 'offset from flat, outside down +ve footbalanceL =25 'offset from flat, outside down +ve fdlean =10 lstep =0 GOTO Walk B_Fd: DEBUG "F" READ DfoottipW,foottip '90,140,170 footbalanceR =25 'offset from flat, outside down +ve footbalanceL =24 'offset from flat, outside down +ve fdlean =20'35'30 lstep =90 'max lstep+fdlean 128 GOTO Walk B_Bk: DEBUG "B" READ DfoottipW,foottip '90,140,170 footbalanceR =20 'offset from flat, outside down +ve footbalanceL =20 'offset from flat, outside down +ve fdlean =-50 lstep =-60 GOTO Walk B_Rt: DEBUG "R" footbalanceR =20'15 'offset from flat, outside down +ve footbalanceL =20'15 'offset from flat, outside down +ve Rby =120 f_MtL =0 GOTO _B_t B_Lt: DEBUG "L" footbalanceR =20'15 'offset from flat, outside down +ve footbalanceL =20'15 'offset from flat, outside down +ve Lby =120 f_MtL =1 _B_t: READ DfoottipT,foottip '90,100,170 fdlean =20 lstep =0 f_turn=1 GOTO Walk '----------------------- Dynamic Walk ------------------------------- walk: 'dynamic walk 'DEBUG CRSRY,5,CLREOL,"W"'alk" DEBUG "W" 'For 88" (2235mm) takes seconds 30 => 176"/min ~15fpm, 4.47 m/min ~1/4 kmph 'no waitfinished, no US 30.2 short strides 'no waitfinished, US 31.2 longer strides ' waitfinished, no US 27.7 full strides ' waitfinished, US 31.7 full strides ' waitfinished, US 29.5 full strides - with serout and Debug sonar info disabled ' waitXon US 39.3 ponderous walk - with serout and Debug sonar info disabled ' waitXon no US 29.8 ponderous walk IF f_leantR =1 THEN moveR '#1 'at poweron f_leantR =0 so left moves first '----------------------- move Left Leg moveL: 'GOSUB waitmovedR 'wait till R moved 'if was moving R means already standing on L, so only useful if Condor picked up. 'waiton never did anything 'GOSUB waitLon 'can't push off unless it's down 'HIGH 7 'GOSUB waitRon 'can't standon it unless it's down Gives a ponderous walk 'LOW 7 'PAUSE 100 'bad place for a pause, kills rocking, won't start walking tiptoR: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat-footbalanceR] ' + is down SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat-foottip] ' + is up 'DEBUG DEC Rfootflat,"," 'DEBUG DEC (Rfootflat-footbalanceR) 'DEBUG "Rtip Ld=",DEC foottipL,"," QLtipped: GOSUB waitmovedL 'wait till servo move done 'foot level left SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat] 'outside_edge down oscillates and falls over, outside_edge up won't oscillate 'PAUSE 50 'jerky walking ' and swing left 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 +Lby MAX 255] '+ve => splay out PAUSE 20 'allow time for foot to lift, if it is going to 'bigger PAUSE disrupts walking adjfoottipL: 'IF f_fixedtip THEN movedL IF foottip=0 THEN movedL 'don't adjust if not tipping IF footsoleL=0 THEN 'adjust foottip if still on ground 'foot starts off @LFootFlat and tips down to 0 by foottip, (Lfootflat-foottip) so don't go through 0 foottip =foottip +3 MAX foottipmax 'LFootFlat 'make sure foottip is within bounds ELSE foottip =foottip -1 MIN foottipmin 'don't let it go too low to oscillate ENDIF IF f_turn THEN WRITE DfoottipT,foottip ELSE WRITE DfoottipW,foottip ENDIF movedL: f_leantR =1 '#1 'paces =paces +1 '#1 GOTO Start '#1 '----------------------- move Right Leg moveR: 'GOSUB waitmovedR 'wait till R moved 'GOSUB waitRon 'can't push off unless it's down 'HIGH 7 'GOSUB waitLon 'can't standon it unless it's down 'LOW 7 'PAUSE 100 'bad place for a pause, kills rocking, won't start walking tiptoL: Sservo =foot SEROUT rtx\rflow,Sbaud,[Scmnd,Rfootflat+foottip] ' + is down SEROUT ltx\lflow,Sbaud,[Scmnd,Lfootflat+footbalanceL] ' + is up 'DEBUG "Ltip Rd=",DEC foottipR,"," QRtipped: GOSUB waitmovedR '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 +Rby MAX 255] '+ve => splay out PAUSE 20 'allow time for foot to lift, if it is going to 'bigger PAUSE disrupts walking adjfoottipR: 'IF f_fixedtip THEN movedR IF foottip=0 THEN movedR 'don't adjust if not tipping IF footsoleR=0 THEN 'adjust foottip if still on ground 'foot starts off @LFootFlat and tips down to 255 by foottip, (Lfootflat+foottipL) so don't go through 255 foottip =foottip +3 MAX foottipmax'(255-RFootFlat)'130 'make sure foottipR is within bounds ELSE foottip =foottip -1 MIN foottipmin 'don't let it go too low to oscillate ENDIF IF f_turn THEN WRITE DfoottipT,foottip ELSE WRITE DfoottipW,foottip ENDIF movedR: f_leantR =0 '#1 'paces =paces +1 '#1 GOTO Start '================================================================= '------------------- Subroutines --------------------------------- report: 'DEBUG DEC foottipL," ",DEC waitcount_tmpL," ",DEC foottipR," ",DEC waitcount_tmpR,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 5 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_tmp =0 wR: IF footsoleR<>footup THEN endwR 'footup=all off floor waitcount_tmp =waitcount_tmp+1 PAUSE 10 IF waitcount_tmpfootup THEN endwL 'footup=all off floor waitcount_tmp =waitcount_tmp+1 PAUSE 10 IF waitcount_tmp%1111 THEN waitmovedR RETURN '---------------------------- waitmovedL: SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,waitmovedL,[Sstatus_tmp] IF Sstatus_tmp.LOWNIB<>%1111 THEN waitmovedl RETURN '---------------------------- QwhereR: 'Query position servo has reached *****NOT USED***** SEROUT rtx\rflow,Sbaud,[Scmnd,0] '?status SERIN rtx,Sbaud,50,QwhereR,[Spos_tmp] RETURN '---------------------------- QwhereL: 'Query position servo has reached *****NOT USED***** SEROUT ltx\lflow,Sbaud,[Scmnd,0] '?status SERIN ltx,Sbaud,50,QwhereL,[Spos_tmp] RETURN '---------------------------- QmovedR: 'QueryMovedRight *****NOT USED***** SEROUT rtx\rflow,Sbaud,[SQstatus,0] '?status SERIN rtx,Sbaud,50,QmovedR,[Sstatus_tmp] RETURN '---------------------------- QmovedL: 'QueryMovedLeft *****NOT USED***** SEROUT ltx\lflow,Sbaud,[SQstatus,0] '?status SERIN ltx,Sbaud,50,QmovedL,[Sstatus_tmp] RETURN '---------------------------- 'beep100: 'from Start: ' FREQOUT spkrLED, ms100, Hz2000, Hz3500 ' GOTO beepLED beep500: 'from Init: FREQOUT spkrLED, ms500, Hz2000, Hz3500 GOTO beepLED bipbip: 'not used FREQOUT spkrLED, ms100, Hz2500, Hz3000 PAUSE 100 FREQOUT spkrLED, ms100, Hz2500, Hz3000 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_tmp] 'rfoot, 255=tip, 35, 27 ' SEROUT ltx\lflow,Sbaud,[Scmnd,120] 'lfoot, 0=tip,185,220 '' SEROUT ltx\lflow,Sbaud,[Scmnd,potval_tmp] 'lfoot, 0=tip,185,220 ' Sservo =1 ' SEROUT rtx\rflow,Sbaud,[Scmnd,128] 'rleg, 0=fd, 255=bk '' SEROUT rtx\rflow,Sbaud,[Scmnd,potval_tmp] 'rleg, 0=fd, 255=bk ' SEROUT ltx\lflow,Sbaud,[Scmnd,128] 'lleg, 255=fd, 0=bk '' SEROUT ltx\lflow,Sbaud,[Scmnd,potval_tmp] 'lleg, 255=fd, 0=bk ' Sservo =2 ' SEROUT rtx\rflow,Sbaud,[Scmnd,94] 'splay, 0,255=out '' SEROUT rtx\rflow,Sbaud,[Scmnd,potval_tmp] 'splay, 0,255=out ' ' SEROUT ltx\lflow,Sbaud,[Scmnd,127] 'nothing '' GOTO checkpot ' ' DEBUG"endtest" 'END '================================================================ ''>CondorIR-18m2 ''>CondorIR-18x 26 October 2010, 00:26:36 '#PicAxe18m2 ''PicAxe18m2 2.B '#no_data ''reads Sharp GP2Y0A02YK IR distance sensors and serials to BS2 ''Memory used = 435 bytes out of 2048 ' ''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. ''Note even if IR are not used by BS2 they can't be turned off to save current '' ''PicAxe18m2 28vars +96ram 256data 2kprogram-DATA ''PicAxe18x 14vars +96ram 256data 2kprogram ' ''History ''------- ''08Jul13 setfreq m16 so now can do 9600, ''08Jul13 added irin[0],pi_IR,cmnd at START: and cmnd at sendit: ''10Feb13 revised IRdistX to IRfX ''30Aug12 revised names ''10Apr12 status =0 if Xdist=0, and TOGGLE pinIR_LED ''10Apr12 ReportIRthresh: ''10Apr12 status changed to 0=ok ''10Apr12 sendit: number of vars reduced ''10Apr12 sertxd DEBUG messages ''10Apr12 floorthreshX, removed write/read floordistX ''10Apr12 changed to 18m2 ''09Apr12 revised names ''22mar12 changed threshX to floordistX, removed Sync code, not needed ''11Nov10 changed sense of ir reports, added statusBat ' ' ____ ' (DAC/Touch/ADC/Out/In) C.2 1|18M2|18 C.1 (In/Out/ADC/Touch) ' (SRQ/Out) SerialOut / C.3 2| |17 C.0 (In/Out/ADC/Touch) ' (in) SerialIn / C.4 3| |16 C.7 (In/Out) ' (in) C.5 4| |15 C.6 (In/Out) ' 0v 5| |14 5v ' (SRI/Out/In) B.0 6| |13 B.7 (In/Out/ADC/Touch) '(i2c-sda/Touch/ADC/Out/In) B.1 7| |12 B.6 (In/Out/ADC/Touch/PWM) ' (hserin/Touch/ADC/Out/In) B.2 8| |11 B.5 (In/Out/ADC/Touch/hserout) ' (PWM/Touch/ADC/Out/In) B.3 9|____|10 B.4 (In/Out/ADC/Touch/i2c-scl) ' ' ''outputs pins 'symbol po_IRobsL =B.0 '6 'GreenLED through 390R to 0v 'symbol po_IRobsR =B.1 '7 'GreenLED through 390R to 0v 'symbol po_IR_LED =B.2 '8 'RedLED through 390R to 0v 'symbol po_US_LED =B.3 '9 'YellowLED through 390R to 0v 'symbol po_VSbatokLED =B.4 '10 'GreenLED through 330R to 0v ''symbol pA_Vbat B.5 '11 '9v test 'symbol po_WaitRxLED_ =B.6 '12 'RedLED through 390R to 5v 'symbol po_SerToBS2 =B.7 '13 'symbol po_RdyToBS2_ =B.7 '13 T4800_4, T idle is high, data pulses low ' 'N idle is low =>ready ''inputs 'symbol pA_VSbat =C.0 '17 'A/D '1u0 to ground, V servo battery to pin through 1M 'symbol pA_LIR =C.1 '18 'A/D '1u0 to ground, input to pin through 100K 'symbol pA_RIR =C.2 '1 'A/D '1u0 to ground, input to pin through 100K ''po_Serial Out C.3 '2 ''pi_Serial In C.4 '3 'symbol pi_IR =C.5 '4 'symbol pi_FromBS2 =C.6 '15 'symbol pi_BS2ready_ =pinC.6 '15 ''pin spare =C.7 '16 ' ''b0 'W0 ''b1 'W0 ' ''Vars B1 - B13 'symbol cmnd_status =W0 'symbol status =B0 'W0 'Init then START 'symbol statusR_ =BIT0 '0=ok 1=obstacle or cliff 'symbol statusL_ =BIT1 '0=ok 1=obstacle or cliff 'symbol statusBat_ =BIT2 '0=ok 1=low 'symbol cmnd =B1 'W0 'Init then START 'symbol IRf =W1 'symbol IRfR =B2 'W1 'IR distance to floor Right 'symbol IRfL =B3 'W1 'IR distance to floor Left 'symbol accumR =W2 'ZeroIR: 'symbol floordistR =B4 'W2 'START: 'symbol floorthreshR =B5 'W2 ' 'symbol accumL =W3 'ZeroIR: 'symbol floordistL =B6 'W3 'START: 'symbol floorthreshL =B7 'W2 ' 'symbol ObstaclesAt =W4 'symbol drop =B8 'W4 'symbol stepup =B9 'W4 'symbol battery =W5 'symbol VSbat =B10 'W5 'START 'symbol VSbatok =B11 'W5 ' ''Note - At power on PA18x waits ~37ms checking serial port for download before running program ' 'symbol BS2baud =T9600_16 ' ''goto testboard ' ' 'Init: 'setfreq m16 'sertxd @19200 ' sertxd("I",CR) ' LOW po_WaitRxLED_ 'redLED on indicate waiting for serin cmnd ' LOW po_RdyToBS2_ ' SERIN po_FromBS2,T4800_4,cmnd,stepup,drop,VSbatok 'sertxd (#cmnd," ",#stepup," ",#drop," ",#VSbatok,CR) ' ' HIGH po_RdyToBS2_ 'make sure it is on idle, otherwise errors below ' HIGH po_WaitRxLED_ 'readLED off indicate got serin cmnd ' ' 'ZeroIR: ' readadc pA_RIR,IRfR ' readadc pA_LIR,IRfL ' AccumR =IRfR ' AccumL =IRfL ' 'sertxd (#floordistL," ",#floordistR,cr) ' PAUSE 50 ' readadc pA_RIR,IRfR ' readadc pA_LIR,IRfL ' AccumR =AccumR +IRfR ' AccumL =AccumL +IRfL ' 'sertxd (#floordistL," ",#floordistR,cr) ' PAUSE 50 ' readadc pA_RIR,IRfR ' readadc pA_LIR,IRfL ' AccumR =AccumR +IRfR ' AccumL =AccumL +IRfL ' 'sertxd (#floordistL," ",#floordistR,cr) ' PAUSE 50 ' readadc pA_RIR,IRfR ' readadc pA_LIR,IRfL ' AccumR =AccumR +IRfR ' AccumL =AccumL +IRfL ' 'sertxd (#floordistL," ",#floordistR,cr) ' ' AccumR =AccumR /4 ' AccumL =AccumL /4 'sertxd ("Av ",#floordistL," ",#floordistR,CR) ' ' 'ReportIRthresh: ' LOW po_WaitRxLED_ 'RedLED on - wait ready ' TOGGLE po_US_LED 'show we are waiting here ' IF pi_BS2ready_=1 THEN ReportIRthresh 'wait BS2ready ' LOW po_WaitRxLED_ 'RedLED off - BS2 ready ' PAUSE 1 ' SEROUT po_SerToBS2,T4800_4,(floordistL,floordistR) 'sertxd ("sent",CR) ' ''SetLEDs ' HIGH po_IR_LED 'do IR on ' HIGH po_US_LED 'do US on ' ' IF cmnd=0 THEN ' LOW po_IR_LED 'IR off ' LOW po_US_LED 'US off ' ENDIF ' IF cmnd=2 THEN 'US off ' LOW po_US_LED ' ENDIF ' IF cmnd=1 THEN 'IR off ' LOW po_IR_LED ' ENDIF ' ' 'START: 'sertxd("S") ' HIGH po_WaitRxLED_ 'RedLED off ' IF pi_BS2ready_=0 THEN sendit ' cmnd =255 ' irin[30],pi_IR,cmnd '20 minimum 'sertxd(#cmnd) ' IF cmnd<>255 THEN START 'SVokLED: ' readadc pA_VSbat,VSbat ' sertxd("VSbat",#VSbat) ' VSbat =VSbat MAX 254 '255 is sync ' 'sertxd("VSbat",#VSbat,cr) ' IF VSbat<200 THEN '< ~4v ' TOGGLE po_VSbatok 'problem with reading VSbat ' statusBat_ =1 ' GOTO _L ' ENDIF ' IF VSbat>VSbatok THEN ' HIGH po_VSbatokLED 'LED on ' statusBat_ =0 ' ELSE ' LOW po_VSbatokLED ' statusBat_ =1 ' ENDIF ' 'sertxd("bat ",#VSbat," ",#VSbatok," ",#statusBat,cr) ' '_L: ' readadc pA_LIR,IRfL ' sertxd(" IRfL",#IRfL) ' statusL_ =0 ' IF IRfL>0 THEN 'got IR reading, 0,1 noise level ' LOW po_IRobsL ' floorthreshL =floordistL +stepup MAX 255 ' IF IRfL>floorthreshL THEN ' statusL_=1 ' HIGH po_IRobsL ' ENDIF ' floorthreshL =floordistL MIN drop -drop ' IF IRdfL0 THEN 'got IR reading, 0,1 noise level ' LOW po_IRobsR ' floorthreshR =floordistR +stepup MAX 255 ' IF IRfR>floorthreshR THEN ' statusR_=1 ' HIGH po_IRobsR ' ENDIF ' floorthreshR =floordistR MIN drop -drop ' IF IRfR") ' PAUSE 1 'give time for BS2 to unset ready ' GOTO START ' ' ' 'SVokLED2: 'for Condor2.6 ' READ 3,VSbatok ' IF VSbatok=0 THEN ' VSbatok =225 '4.5v default if not got value from BS2 ' ENDIF ' DO 'forever when IR not used ' readadc pA_VSbat,VSbat ' VSbat =VSbat MAX 254 '255 is sync ' IF VSbat>VSbatok THEN ' HIGH po_VSbatokLED ' ELSE ' LOW po_VSbatokLED ' ENDIF ' PAUSE 100 ' TOGGLE 0 ' LOOP ' ' 'testboard: 'sertxd("I") 'toggle B.0'pinL_IRok 'toggle B.1'pinR_IRok 'toggle B.2'pinR_IRok 'toggle B.3'pinR_IRok 'toggle B.4'pinR_IRok 'toggle B.5'pinR_IRok 'toggle B.6'pinR_IRok 'toggle B.7'pinR_IRok ''pause 100 ''goto init 'B13 =B13 +1 'sertxd(#b13," ") 'readadc C.0,B1 ' sertxd("bat",#b1) 'readadc C.1,B1 ' sertxd(" L",#b1) 'readadc C.2,B1 ' sertxd(" R",#b1,CR) 'goto testboard '================================================================ '================================================================