'>LokiCerebP28X2v1.bas 'circuit has 4MHz resonator #PicAxe 28X2 'PIC18F25K22 'defaults to 8MHz internal resonator setfreq m16 'sertxd 19200 'setfreq m8 'sertxd 9600 #No_data '#com 1 'still need BS2 debug window closed #No_table SERTXD(CR,CR,"LokiCerebP28X2v1.bas",CR) 'Memory used = 1126 bytes out of 4096 '21Apr18 Eye LEDS fitted on p2,p3 previously for Sharp IRrangers '----------------------------------------------------------- 'Bugs ' f_CPAdj =1 [Cerebellum]AdjPose: walking doesn't work if 0 !!! '----------------------------------------------------------- 'ReProgram Transmitter "IR REMOTE CONTROL PPM060.pdf" 'Press C LED lights 'Press 2 LED flash 'Press 1 LED flash 'Press 2 LED flash and goes out 'PicAxe 28X2 '----------- 'bit0-bit31 32 'b0-b55 56 'wo-w27 28 'EEPROM 256 0-255 'PEEK/POKE 200 56-255 'Scratchpad @ptr 1024 0-1023 'default 8MHz internal, setfreq 4,8,16, external 16,32MHz '-----LCD----------- see end pgm for commands '"C= s= " C=#cmnd from BS2 s=#S0 Servo 0 speed when set from BS2 '"76543210 " sensor bits '================================================================= PIN_MAPPING: #rem PICAXE-28X2 18F25K22 ___ ___ RESET | 1 V 28| B.7 IN OUT COMP1- TOUCH ADC0 IN OUT A.0 | 2 27| B.6 IN OUT COMP2- TOUCH ADC1 IN OUT A.1 | 3 26| B.5 IN OUT ADC13 TOUCH PWM DAC VREF- COMP2+ TOUCH ADC2 IN OUT A.2 | 4 25| B.4 IN OUT ADC11 TOUCH HPWMD VREF+ COMP1+ TOUCH ADC3 IN OUT A.3 | 5 24| B.3 IN OUT ADC9 TOUCH SI | 6 23| B.2 IN OUT ADC8 TOUCH HINT2 HPWMB SRNQ OUT SO A.4 | 7 22| B.1 IN OUT ADC10 TOUCH HINT1 HPWMC 0V | 8 21| B.0 IN OUT ADC12 TOUCH HINT0 PWM SRI XI | 9 20| V+ XO |10 19| 0V TIMER IN OUT C.0 |11 18| C.7 IN OUT ADC19 TOUCH KBD HSERIN PWM IN OUT C.1 |12 17| C.6 IN OUT ADC18 TOUCH KBC HSEROUT PWM HPWMA TOUCH ADC14 IN OUT C.2 |13 16| C.5 IN OUT ADC17 TOUCH SDO SCL SCK TOUCH ADC4 IN OUT C.3 |14___15| C.4 IN OUT ADC16 TOUCH SDI SDA Connectors: LCD o signal Eye | o Servos 5v0v NoseWiskers o iris o 5v | o 5v 3 o o o o PWM o 0v | o 0v 2 o o o o IR | o ER 1 o o o o PWM | o EL 0 o o o o 5v | o 0v o 0v |_______ ----------------------- C.7 hserin mm,,,,,,,,, C.6 hserout C.4 hi2c sda C.3 hi2c sck ----------------------- ----- Pins -------- 'portA.0 readadc 0, SYMBOL ReyeLED =A.0 'portA.1 readadc 1, SYMBOL LeyeLED =A.1 portA.2 readadc 2, LDR IRiris 10K|1K pullup =>~900R (still goes low in bright light) portA.2 tipFdpin Hg switch 10K|1K pullup (dual use with iris) portA.3 tipBkpin Hg switch 10K pullup SYMBOL IRwhisker =pinC.0 'p11 infra SYMBOL IRwhiskerpwmR =C.1 'p12 [pwm1] SYMBOL IRwhiskerpwmL =C.2 'p13 [pwm2] SYMBOL SerFromBS2 =C.3 'p14 \BS2 comms SYMBOL BS2readypin =pinC.3 'p14 / SYMBOL SerToBS2 =B.7 'p28 BS2 comms SYMBOL SerToLCD =B.6 'p27 LCD comms SYMBOL grnLED =B.5 'p26 high=on, 200R symbol spare =B.4 'p25 SYMBOL servo3 =B.3 'p24 \ SYMBOL servo2 =B.2 'p23 |Servos SYMBOL servo1 =B.1 'p22 | SYMBOL servo0 =B.0 'p21 / SYMBOL leftfrontheel =C.7 'p18 \ SYMBOL leftfronttoe =C.6 'p17 |hole sensors SYMBOL rightfrontheel =C.5 'p16 | SYMBOL rightfronttoe =C.4 'p15 / #endrem SYMBOL ReyeLED =A.0 'p2 SYMBOL LeyeLED =A.1 'p3 SYMBOL SerToBS2 =B.7 'p28 SYMBOL P28ready =B.7 'p28 SYMBOL SerToLCD =B.6 'p27 SYMBOL grnLED =B.5 'p26 high=on, 200R 'symbol spare =B.4 'p25 SYMBOL servo3 =B.3 'p24 SYMBOL LHsp =B.3 'p24 LH - Left Hip SYMBOL servo2 =B.2 'p23 SYMBOL LAsp =B.2 'p23 LA - Left Ankle SYMBOL servo1 =B.1 'p22 SYMBOL RHsp =B.1 'p22 RH - Right Hip SYMBOL servo0 =B.0 'p21 SYMBOL RAsp =B.0 'p21 RA - Right Ankle 'INC - pins 8,9,10,11 when foot is in air switches are closed and Dx are switched to 0v, ' when foot is down switches are open and Dx pulled to 5v by 100k SYMBOL leftfrontheel =C.7 'p18 | SYMBOL leftfronttoe =C.6 'p17 |hole sensors SYMBOL rightfrontheel =C.5 'p16 | SYMBOL rightfronttoe =C.4 'p15 | SYMBOL SerFromBS2 =C.3 'p14 SYMBOL BS2readypin =pinC.3 'p14 SYMBOL IRwhiskerpwmL =C.2 'p13 [pwm2] SYMBOL IRwhiskerpwmR =C.1 'p12 [pwm1] SYMBOL IRwhisker =pinC.0 'p11 infra 'symbol IRinfrain2 =C.0 'p11 GetIRcode: irin ' 'infra 28x firmware only allows infra 'infrain2 'on I0==C0 to special variable infra 0-127 'infrain 'on I0==C0 to special variable infra 0-15 - for early chip 'readadc 0,IRrangeR 'Sharp IR rangers 'readadc 1,IRrangeL 'Sharp IR rangers 'readadc 2,IRiris 'LDR SYMBOL tipFdpin =pinA.2 'Hg switch SYMBOL tipBkpin =pinA.3 'Hg switch 'DATA 0,(Sspeed0) 'default from last time 'DATA 1,(Sspeed1) 'default from last time 'DATA 2,(Sspeed2) 'default from last time 'DATA 3,(Sspeed3) 'default from last time '************************* END Machine Dependent ************************* '-------------------------------------------------------------- '----- Vars/Cons -------- 'symbol BS2baud =N4800 '28X 'symbol BS2baud =N9600_8 SYMBOL BS2baud =N9600_16 'doesn't work at 16MHz without the _16 'symbol LCDbaud =T2400 'symbol LCDbaud =T9600_8 SYMBOL LCDbaud =T9600_16 'doesn't work at 16MHz without the _16 'Servo flags SYMBOL Command = B0 '$flags$cmnd GetCommnd: Serin from BS2 SYMBOL f_CSpulse = BIT4 '1 =>pulse servos, 0 =>no pulses only set in init: SYMBOL f_CtfG = BIT5 '1=> Test Foot Sensors, 0=>OFF, SYMBOL f_CPAdj = BIT6 '1=> Adjust servo values SYMBOL f_CsStep = BIT7 '1=> singlestep => display servovalues on LCD SYMBOL f8_Sensrs = B1 'f4_Gnd VAR f8_Sensrs.LOWNIB 'symbol f_GRtoe = BIT8 '0=hole, 1=on ground LED on 'symbol f_GRheel = BIT9 '0=hole, 1=on ground LED on 'symbol f_GLtoe = BIT10 '0=hole, 1=on ground LED on 'symbol f_GLHeel = BIT11 '0=hole, 1=on ground LED on SYMBOL f_tipBk = BIT12 '1=tipped SYMBOL f_tipFd = BIT13 '1=tipped SYMBOL f_whiskerR = BIT14 '1=sensed SYMBOL f_whiskerL = BIT15 '1=sensed SYMBOL f8_status = B2 SYMBOL f_RF = BIT16 SYMBOL f_RO = BIT17 SYMBOL f_LO = BIT18 '19,20,21,22,23 SYMBOL f4_Sdo = B3 'lowNIB 'if =0 then all moved and at Pose, used for cmnd from BS2 SYMBOL f_S0do = BIT24 '1 =>servo not yet at Pose SYMBOL f_S1do = BIT25 SYMBOL f_S2do = BIT26 SYMBOL f_S3do = BIT27 SYMBOL f_midPosed = BIT28 'pose inserted by AdjPose '29,30,31 SYMBOL temp = B4 SYMBOL IRcmnd = B5 SYMBOL cmnd = B6 SYMBOL waitcount = B7 SYMBOL BS2offcount = B7 'b0-b55 56 SYMBOL Sat0 = B8 'Servo At LOGICAL SYMBOL SatRA = B8 'Servo At LOGICAL SYMBOL Sat1 = B9 'Servo At LOGICAL SYMBOL SatRH = B9 'Servo At LOGICAL SYMBOL Sat2 = B10 'Servo At LOGICAL SYMBOL SatLA = B10 'Servo At LOGICAL SYMBOL Sat3 = B11 'Servo At LOGICAL SYMBOL SatLH = B11 'Servo At LOGICAL SYMBOL Spose0 = B12 'Servo To LOGICAL 'symbol SposeRA = B12 'Servo To LOGICAL SYMBOL Spose1 = B13 'Servo To LOGICAL 'symbol SposeRH = B13 'Servo To LOGICAL SYMBOL Spose2 = B14 'Servo To LOGICAL 'symbol SposeLA = B14 'Servo To LOGICAL SYMBOL Spose3 = B15 'Servo To LOGICAL 'symbol SposeLH = B15 'Servo To LOGICAL SYMBOL IRiris = B16 'only used before IRrange SYMBOL IRduty = B17 'only used before IRrange 'SYMBOL IRrangeR = B18 'only used before move got Sharp rangers not fitted 'SYMBOL IRrangeL = B19 'only used before move got Sharp rangers not fitted SYMBOL S0 = B20 'BS2 input parameter Servo 0 SYMBOL S1 = B21 'BS2 input parameter Servo 1 SYMBOL S2 = B22 'BS2 input parameter Servo 2 SYMBOL S3 = B23 'BS2 input parameter Servo 3 SYMBOL Sspeed0 = B24 'Servo Speed RA synonym SYMBOL Sspeed1 = B25 'Servo Speed RH synonym SYMBOL Sspeed2 = B26 'Servo Speed LA synonym SYMBOL Sspeed3 = B27 'Servo Speed LH synonym SYMBOL SspeedRA = B24 'Servo0 Speed synonym SYMBOL SspeedRH = B25 'Servo1 Speed synonym SYMBOL SspeedLA = B26 'Servo2 Speed synonym SYMBOL SspeedLH = B27 'Servo3 Speed synonym SYMBOL hframes = B28 'halt-time in servo frames SYMBOL Sto0 = B29 'Servo To RA LOGICAL SYMBOL Sto1 = B30 'Servo To RH LOGICAL SYMBOL Sto2 = B31 'Servo To LA LOGICAL SYMBOL Sto3 = B32 'Servo To LH LOGICAL SYMBOL StoRA = B29 'Servo0 To LOGICAL synonym SYMBOL StoRH = B30 'Servo1 To LOGICAL synonym SYMBOL StoLA = B31 'Servo2 To LOGICAL synonym SYMBOL StoLH = B32 'Servo3 To LOGICAL synonym SYMBOL tempW = W19 SYMBOL BS2offcountmax =50 'was 10 before eyeLEDs, gives about 40 blinks per minute SYMBOL feetdownwait =15'30 'Logical servo positions are about scale mid of 1500us =150 SYMBOL jmid = 150 'jscale VAR Byte 'jmidS VAR Byte 'actual mid range (logical) 'For PULSOUT section 'symbol scale =1 'picAxe 4MHz 10us to 10us do *sscale 'symbol sscale =2 'picAxe 8MHz 10us to 5us do *sscale SYMBOL sscale =4 'picAxe 16MHz 10us to 2.5us do *sscale 'symbol pscale =1 'picAxe 4MHz 10us to 10us do *pscale 'symbol pscale =2 'picAxe 8MHz 10us to 5us do *pscale SYMBOL pscale =4 'picAxe 16MHz 10us to 2.5us do *pscale 'pscale CON 5 'BS2 10us to 2us do *pscale 'pscale CON 3200 'BS2sx 10us to 0.8us do */pscale this multiplies by 3200 and divides by 256 =12.5 'potscale CON 43 'BS2 'potscale CON 90 'BS2sx 'symbol framepause = 8 '25ms 40Hz frame PicAxe28x2@8MHz 'symbol framepause = 3 '20ms 50Hz frame PicAxe28x2@8MHz SYMBOL framepause = 5'20'36'45 '??ms 50Hz frame PicAxe28x2@16MHz 'symbol framepause = 35 '??ms 50Hz frame PicAxe28x2@16MHz 'symbol framepause = 25 '??ms 50Hz frame PicAxe28x2@16MHz 'symbol framepause = 17 '20ms 50Hz frame PicAxe28x2@16MHz 'symbol framepause = 15 '??ms 50Hz frame PicAxe28x2@16MHz 'symbol framepause = 8 '16ms 60Hz frame PicAxe28x2@16MHz 'symbol framepause = 10 '16ms 60Hz frame PicAxe28x2@16MHz '--------------------------------------------------------- 'All pose data must follow this order SYMBOL Rlift = 0 SYMBOL Rstride = 1 SYMBOL Llift = 2 SYMBOL Lstride = 3 'toe down is +ve 'toe forward is +ve '************************* Machine Dependent ***************************** 'Mapping - joints to pose(0 - 3) SYMBOL RA =Rlift 'Right Ankle SYMBOL RH =Rstride 'Right Hip SYMBOL LA =Llift 'Left Ankle SYMBOL LH =Lstride 'Left Hip '************************* END Machine Dependent ************************* 'ACTUAL values from PotSetRanges - values are in 10us units 'S refers to servo 'assemble joints to as near as 150 as possible, hips straight ahead, feet flat 'ht615 Ankle servos changed for S06 18Sep06, RA ht615 servo failed 'S06 servo rotates opposite to ht615 so RA needs reversing at pulse out time 'RA,LH need reversing at pulse out time 'RA [55]65 79 144 76 220[226]in 'RH [55]60 92 152 93 245[245]out 'LA [49]55 76 131 89 220[226]out realign 'LH [55]60 97 157 73 230[234]in SYMBOL S_RAmin = 65 '55 parts hit out 50 servo stop S06 SYMBOL S_RAmid = 144 'flat 255-S_RAmid =111 S06 SYMBOL S_RAmax = 220 '226 servostop S06 SYMBOL S_RHmin = 60 '55 servo stop SYMBOL S_RHmid = 152 SYMBOL S_RHmax = 245 '245 pot electronics max SYMBOL S_LAmin = 55 '49 hit S06 SYMBOL S_LAmid = 131 'flat S06 SYMBOL S_LAmax = 220 '226 out hit S06 SYMBOL S_LHmin = 60 '55 servo stop SYMBOL S_LHmid = 157 SYMBOL S_LHmax = 230 '234 pot electronics max 'LOGICAL THIS SECTION IS NOT RIGHT ??!! when it is add limits to Jpose '[with S03 Hips and ht615 Ankles Left servos go contra to logical direction] 'with S03 Hips and S06 Ankles RA and LH servos go contra to logical direction 'toe down is +ve 'toe forward is +ve 'RAmin CON 255 -S_RAmax =35 'note max min reversals 'RAmid CON 255 -S_RAmid '255-S_RAmid =111 35 76 111 79 190 'RAmax CON 255 -S_RAmin =190 'RHmin CON S_RHmin 'RHmid CON S_RHmid '154 'RHmax CON S_RHmax 'LAmin CON S_LAmax 'LAmid CON S_LAmid '159 'LAmax CON S_LAmin 'LHmin CON 255 -S_LHmax 'note max min reversals 'LHmid CON 255 -S_LHmid '255-S_LHmid =102 'LHmax CON 255 -S_LHmin 'wuclear CON jmid -Fuclear SYMBOL FuclearR = 20 'minimum up for toes to miss when walking SYMBOL FuclearL = 20 'minimum up for toes to miss when walking SYMBOL Fsclear = 8 'minimum difference between toe positions to miss when walking SYMBOL Cposition =0 SYMBOL Csetspeed =1 SYMBOL Cgetsensrs =2 SYMBOL CGetIRcode =3 'depreciated except for robot to robot, use 08m2cmnder 'can do irout to another robot via IRwhiskers 'CGetIfrain CON 4 'depreciated ' CASE 8 SEROUT SerToLCD,LCDbaud,(254,192,"No reason") ' CASE 9 SEROUT SerToLCD,LCDbaud,(254,192,"No Tasker") ' CASE 10 SEROUT SerToLCD,LCDbaud,(254,192,"No IREar") '----------------------------------------------------------- Init: LOW P28ready '/ready HIGH B.6 'set LCD pin to idle otherwise first cmnd doesn't work HIGH grnLED Sat0 =jmid 'initial position Sat1 =jmid 'initial position Sat2 =jmid 'initial position Sat3 =jmid 'initial position Spose0 =jmid 'no move Spose1 =jmid 'no move Spose2 =jmid 'no move Spose3 =jmid 'no move Sto0 =jmid 'no move Sto1 =jmid 'no move Sto2 =jmid 'no move Sto3 =jmid 'no move #rem doesn't give the right pulse length servo B.0,150'Spose0 'start servos servo B.1,150'Spose1 'servo B.2,150'Spose2 'servo B.3,150'Spose3 servopos B.0,150 servopos B.1,150 'servopos B.2,150 'servopos B.3,150 #endrem READ 0,Sspeed0 'default from last time READ 1,Sspeed1 'default from last time READ 2,Sspeed2 'default from last time READ 3,Sspeed3 'default from last time f_midPosed =0 f8_Sensrs =pinsC /16 'Query hole Sensors and shift to lownib PAUSE 1000 '>200 for LCD, and give time for BS2 to be !ready SEROUT B.6,LCDbaud,(254,1) 'cls SEROUT B.6,LCDbaud,(254,128,"Loki Cerebellum") SEROUT B.6,LCDbaud,(254,192,"ready") PAUSE 1000 SEROUT SerToLCD,LCDbaud,(254,1) 'cls SEROUT SerToLCD,LCDbaud,(128,"C= s=") 'line 1,0 eSTART: sertxd("!") 'indicate serin timeout '----------------------------------------------------------- sSTART: f_CtfG =1 'Sensors will now be read and displayed until BS2 turns them off=BS2default 'So I can see sensors on LCD until BS2 ready BS2offcount =0 HIGH ReyeLED 'eye on HIGH LeyeLED 'eye on '----------------------------------------------------------- START: BS2offcount =BS2offcount +1 IF BS2offcount>BS2offcountmax THEN sStart: 'to turn on sensor display LOW ReyeLED 'eye off, this gives a shrt blink on after sSTART LOW LeyeLED 'eye off, this gives a shrt blink on after sSTART sertxd("S",#f_CtfG) '1=> Test Foot Sensors, 0=>OFF HIGH P28ready 'indicate ready IF f_CtfG=0 THEN CalcFlags 'BS2offcount =0 'high grnled '--------------------------------------- 'ReadSensors: QhS: f8_Sensrs =pinsC /16 'Query hole Sensors and shift to lownib Qtip: QtipFd: 'also A/D IRiris adcsetup =0' set all A/D to digital, readadc automatically changes to A/D f_tipFd =0 'BIT12 'not tipped IF tipFdpin=1 THEN QtipBk 'Hg switch open f_tipFd =1 'tipped, Hg switch closed QtipBk: f_tipBk =0 'BIT13 'not tipped IF tipBkpin=1 THEN Qwhiskers 'Hg switch open f_tipBk =1 'tipped, Hg switch closed '------------------------ Qwhiskers: 'pwmout C.1, 104, 211 '38KHz@16MHz 'pwmout C.1, 52, 208 99% 8MHz 'pwmout C.1, 52, 105 50% 8MHz 'pwmout C.1, 52, 2: 1% 8MHz 'IRwhisker = pin0 = portC 0 readadc 2,IRiris 'read light level, 60 - 249=dark 'sertxd("iris",#IRiris,cr) :goto Qwhiskers IRduty =15 'if light IF IRiris<220 THEN _pwmR IRduty =8 'if dark _pwmR: setfreq m8 pwmout C.1,52,IRduty '38KHz @8MHz 'pwmout C.1,104,5 '38KHz @16MHz PAUSE 1 '0 minimum f_whiskerR =NOT IRwhisker pwmout C.1,off' 0, 0 'stop pwm setfreq m16 LOW C.1 'and turn off LEDpwmR IRduty =16 'if light IF IRiris<220 THEN _pwmL IRduty =11 'if dark _pwmL: setfreq m8 pwmout C.2,52,IRduty '38KHz @8MHz 'pwmout C.1,104,5 '38KHz @16MHz PAUSE 1 '0 minimum f_whiskerL =NOT IRwhisker pwmout C.2,off' 0, 0 'stop pwm setfreq m16 LOW C.2 'and turn off LEDpwmR '_QwhiskerR: IF f_whiskerR=0 THEN _QwhiskerL HIGH C.1 'turn on LED for obstacle _QwhiskerL: IF f_whiskerL=0 THEN _whiskersdone HIGH C.2 'turn on LED for obstacle _whiskersdone: '------------------------ 'QIRrangers: ' readadc 0,IRrangeR 'Sharp IR rangers ' readadc 1,IRrangeL 'Sharp IR rangers '------------------------ SensorsToLCD: SEROUT SerToLCD,LCDbaud,(254,192,#bit15,#bit14,#bit13,#bit12,#bit11,#bit10,#bit9,#bit8) CalcFlags: IF SatRH>SatLH THEN 'this is right as used by MetaAct F f_RF =1 ELSE f_RF =0 ENDIF IF SatRH>jmid THEN f_RO =1 ELSE f_RO =0 ENDIF IF SatLH>jmid THEN f_LO =1 ELSE f_LO =0 ENDIF '--------------------------------------- QBS2ready: IF BS2readypin=0 THEN Start 'redo sensors if enabled BS2offcount =0 'BS2 no longer off 'sertxd(cr) LOW P28ready PAUSE 1'4 'so line is at idle, longer than a STOP bit, 1 is ok '------------------------ SensorsToBS2: SEROUT SerToBS2,BS2baud,(f8_Sensrs,f8_status) sertxd(CR,"Sen>BS2",CR) IF BS2readypin=1 THEN SensorsToBS2 '------------------------ GetCmnd: LOW grnLED 'off HIGH P28ready 'B0.lownib=cmnd, B0.highnib=flags SERIN [100,eStart],SerFromBS2,BS2baud,Command,S0,S1,S2,S3,hframes LOW P28ready 'indicate got data and not ready HIGH grnLED 'on cmnd =Command &%1111 'sertxd("Command",#Command,"-",#S3," ",#S2," ",#S0," ",#S1," ",#hframes,cr) 'display order GOSUB BlinkOn '------------------------ CmndToLCD: SEROUT SerToLCD,LCDbaud,(254,130,#cmnd) GOSUB BlinkOff IF cmnd=Cposition THEN SetPose IF cmnd=Csetspeed THEN SetSpeed IF cmnd=Cgetsensrs THEN START IF cmnd=CGetIRcode THEN GetIRcode 'depreciated, use 08m2, see note above at Constants GOTO ShowError SetPose: HIGH ReyeLED 'eye on HIGH LeyeLED 'eye on 'somehow Sx to Stox doesn't work here!!!! 'xgosub S2T ' xStoLH =S3 ' xStoLA =S2 ' xStoRH =S1 ' xStoRA =S0 ' xSto3 =S3 ' xSto2 =S2 ' xSto1 =S1 ' xSto0 =S0 ' Spose3 =S3 ' Spose2 =S2 ' Spose1 =S1 ' Spose0 =S0 'xgosub P2T 'xSto3 =Spose3 'xSto2 =Spose2 'xSto1 =Spose1 'xSto0 =Spose0 GOTO AdjPose '------------------- test #rem P2T: Sto3 =Spose3 Sto2 =Spose2 Sto1 =Spose1 Sto0 =Spose0 RETURN S2T: Sto3 =S3 Sto2 =S2 Sto1 =S1 Sto0 =S0 RETURN #endrem '------------------- /test '------------------------ SetSpeed: Sspeed0 =S0 MIN 1 'hangs if 0 WRITE 0,Sspeed0 'save Sspeed1 =S1 MIN 1 'hangs if 0 WRITE 1,Sspeed1 'save Sspeed2 =S2 MIN 1 'hangs if 0 WRITE 2,Sspeed2 'save Sspeed3 =S3 MIN 1 'hangs if 0 WRITE 3,Sspeed3 'save SEROUT SerToLCD,LCDbaud,(254,134,#S0) GOTO START '------------------------ GetIRcode: 'Depreciated except for robot to robot FOR temp=0 TO 2 'blink LEDs so we can tell something is being done HIGH C.1 'pwmpinR_LED on HIGH C.2 'pwmpinL_LED on PAUSE 50 LOW C.1 'pwmpinR_LED off LOW C.2 'pwmpinL_LED off PAUSE 50 NEXT irin C.0, IRcmnd 'infrain 'WAITS HERE FOR BUTTON PRESS GOTO START '------------------------ ShowError: sertxd("error",#Command) SEROUT SerToLCD,LCDbaud,(254,1) PAUSE 1 'without pause 1 misses first char SEROUT SerToLCD,LCDbaud,("Error") SELECT cmnd CASE 8 'nothing sets this SEROUT SerToLCD,LCDbaud,(254,192,"No reason") CASE 9 SEROUT SerToLCD,LCDbaud,(254,192,"No Tasker") CASE 10 SEROUT SerToLCD,LCDbaud,(254,192,"No IREar") ENDSELECT FOR temp =0 TO 50 GOSUB Blink NEXT SEROUT SerToLCD,LCDbaud,(254,1) 'cls GOTO START '************************************************************************* BlinkOn: TOGGLE c.1 'pwmpinR_LED on TOGGLE c.2 'pwmpinL_LED on RETURN BlinkOff: TOGGLE c.1 'pwmpinR_LED off TOGGLE c.2 'pwmpinL_LED off RETURN Blink: HIGH c.1 'pwmpinR_LED on HIGH c.2 'pwmpinL_LED on PAUSE 30 'less than 25 and can't see blinks LOW c.1 'pwmpinR_LED off LOW c.2 'pwmpinL_LED off RETURN '************************* Machine Dependent ***************************** AdjPose: IF f_CPAdj=0 THEN MoveCalcs 'f_CPAdj always 1 from BS2 'sertxd("Adj",#f_midPosed) 'sertxd( CR," LH "," LA "," RA "," RH ",CR) 'display order 'sertxd(#StoLH," ",#StoLA," ",#StoRA," ",#StoRH," ",CR) 'display order 'sertxd(#StoLH," ",#StoLA," ",#StoRA," ",#StoRH," ",#f_Spulse,CR) 'display order 'symbol Fuclear = 19 'minimum up for toes to miss when walking 'symbol Fsclear = 8 'minimum difference between toe positions to miss when walking 'symbol wuclear = jmid -Fuclear 'gosub P2T 'gosub S2T Sto3 =S3 Sto2 =S2 Sto1 =S1 Sto0 =S0 ' Sto3 =Spose3 ' Sto2 =Spose2 ' Sto1 =Spose1 ' Sto0 =Spose0 'sertxd("AdjP") 'SERTxd (cr,"Sat ",#Sat3," ",#Sat2," ",#Sat0," ",#Sat1) 'SERTxd (cr,"Sto ",#Sto3," ",#Sto2," ",#Sto0," ",#Sto1,cr) '_AclearW: 'make sure feet don't hit each other '_ACu: 'sertxd("_ACu") IF f_midPosed=1 THEN _sC 'now do stride end IF StoLH=SatLH AND StoRH=SatRH THEN _sC 'hips no change 'on fast the toes ALWAYS banged - why??? ' IF SatLHSatRH AND StoLH>=StoRH THEN _sC 'toes not going to cross _RuC: IF StoRA>=StoLA THEN _LuC 'right toes down, if equal then lift left 'sertxd("_RuC") temp =StoLA -FuclearR 'left down so adjust right ankle StoRA =StoRA MAX temp temp =SspeedRA *3 'SspeedRA for slugish servos StoRA =StoRA -temp 'sertxd(#StoRA) GOTO _uC _LuC: sertxd("_LuC") temp =StoRA -FuclearL 'right down so adjust left ankle StoLA =StoLA MAX temp temp =SspeedLA *3 'SspeedRA for slugish servos StoLA =StoLA -temp sertxd(#StoLA) _uC: 'sertxd("_uC") IF StoLH=StoRH THEN _Sc 'only do mid pose if stride goes past mid f_midPosed =1 'sertxd(#f_midPosed) temp =StoLH +StoRH /2 StoLH =temp StoRH =temp sertxd(#StoRH) GOTO _DonePose _sC: 'move front foot out of the way, back has preference 'sertxd("_sC") 'sertxd("SE") IF StoLH>StoRH THEN temp =StoRH +Fsclear StoLH =StoLH MIN temp elseif StoLH=StoRH THEN 'except if = then R forward StoRH =StoRH +Fsclear else temp =StoLH +Fsclear StoRH =StoRH MIN temp endif f_midPosed =0 _DonePose: 'sertxd("DP",#f_midPosed) '************************* END Machine Dependent ************************* '------------- MoveCalcs: 'sertxd("moveC") waitcount =0 f_S0do =1 'doing servo f_S1do =1 'doing servo f_S2do =1 'doing servo f_S3do =1 'doing servo '------------- DisplayMove: 'only display if SingleStepping IF f_CsStep=0 THEN Move SEROUT SerToLCD,T2400,(254,128, #Sto3," ",#Sto2," ",#Sto0," ",#Sto1)',CR'" : " '---------------------------------------------------- MOVE: IF BS2readypin=1 THEN sertxd("interupt") GOTO Start 'BS2 interupt endif 'sertxd("ma",#Sat3," ",#Sat2," ",#Sat0," ",#Sat1,cr) 'display order 'sertxd("mt",#Sto3," ",#Sto2," ",#Sto0," ",#Sto1,cr) 'display order 'SERTxd (cr,"s",#Sspeed0," to ",#Sto3," ",#Sto2," ",#Sto0," ",#Sto1," h",#hframes) 'SERTxd (" at ",#Sat3," ",#Sat2," ",#Sat0," ",#Sat1," end") 'with no Lpause loops at 9 - 19 ms see above, 'MIN Sspeedx is not needed because from Servo limits ' STox is always > (max Sspeedx=15) '------------- _S0: IF Sat0=Sto0 THEN _endS0 IF Sto0>Sat0 THEN _incS0 _decS0: Sat0 =Sat0 MIN Sspeed0 -Sspeed0 MIN Sto0 GOTO _S1 _incS0: Sat0 =Sat0 +Sspeed0 MAX Sto0 GOTO _S1 _endS0: f_S0do =0 'sertxd("0") '------------- _S1: IF Sat1=Sto1 THEN _endS1 IF Sto1>Sat1 THEN _incS1 _decS1: Sat1 =Sat1 MIN Sspeed1 -Sspeed1 MIN Sto1 GOTO _S2 _incS1: Sat1 =Sat1 +Sspeed1 MAX Sto1 GOTO _S2 _endS1: f_S1do =0 'sertxd("1") '------------- _S2: IF Sat2=Sto2 THEN _endS2 IF Sto2>Sat2 THEN _incS2 _decS2: Sat2 =Sat2 MIN Sspeed2 -Sspeed2 MIN Sto2 GOTO _S3 _incS2: Sat2 =Sat2 +Sspeed2 MAX Sto2 GOTO _S3 _endS2: f_S2do =0 'sertxd("2") '------------- _S3: IF Sat3=Sto3 THEN _endS3 IF Sto3>Sat3 THEN _incS3 _decS3: Sat3 =Sat3 MIN Sspeed3 -Sspeed3 MIN Sto3 GOTO Spulse _incS3: Sat3 =Sat3 +Sspeed3 MAX Sto3 GOTO Spulse _endS3: f_S3do =0 'sertxd("3") '------------- Spulse: 'Servo pulse IF f_CSpulse=0 THEN _Spause 'test takes 0.3ms '************************* Machine Dependent *** Servo direction ********* ' =(255 -(SatLH -jmid)) +S_LHmid 'shift to servo mid and reverse servo temp =255 +jmid -SatLH +S_LHmid 'shift to servo mid and reverse servo tempW =temp *pscale PULSOUT LHsp,tempW 'sertxd("LH",#tempw," ") temp =SatLA -jmid +S_LAmid 'shift to servo mid tempW =temp *pscale PULSOUT LAsp,tempW 'sertxd("LA",#tempw," ") ' =(255 -(SatRA -jmid)) +S_RAmid temp =255 +jmid -SatRA +S_RAmid 'shift to servo mid and reverse servo tempW =temp *pscale PULSOUT RAsp,tempW 'sertxd("RA",#tempw," ") temp =SatRH -jmid +S_RHmid 'shift to servo mid tempW =temp *pscale PULSOUT RHsp,tempW 'sertxd("RH",#tempw,cr) '************************* END Machine Dependent ************************* _Spause: 'Pulse pause ' PAUSE 0 'loops at ~45Hz 'all sums in _Spulse with Sto(),Sat() logical values ' PAUSE 8 'loops at ~50Hz, 60-70Hz without any pause, with Sto(),Sat() actual values PAUSE framepause 'sertxd("f_Sdo=",#f4_Sdo,cr) temp =f4_Sdo &%1111 IF temp<>0 THEN MOVE 'toggle B.6 'to test frame rate 'goto move _Qhdo: 'halt for frames IF hframes=0 THEN Qmoved hframes =hframes -1 GOTO MOVE Qmoved: 'skip footdown check if ignoring sensors or no holes IF f_CtfG=0 THEN donemove 'foot hole sensors 0=>hole, temp =pinsC /16 'Query hole Sensors and shift to lownib 'sertxd("*",#temp,"*") IF temp=%1111 THEN donemove 'foot hole sensors 0=>hole, waitcount =waitcount +1 'SEROUT SerToLCD,LCDbaud,(254,136,#waitcount," ") IF waitcount=15 then is FootOnHole donemove: sertxd(CR) waitcount =0 'clear it for sensor waitcount at START 'sertxd("doneMove",#f_midPosed) IF f_midPosed=1 THEN AdjPose 'do commanded Pose LOW ReyeLED 'eye off LOW LeyeLED 'eye off GOTO Start '------------- PotSetRanges: 'NO POT 'DEBUG"psr" ' servo =RAsp 'un-comment for desired servos ' servo =RHsp 'un-comment for desired servos 'servonum =LAsp 'un-comment for desired servos ' servo =LHsp 'un-comment for desired servos 'comment following to save space 'HIGH potpin ' PAUSE 1 'RCTIME potpin,1,tempw ' tempw =tempw /2 '1 - 633 => 0 - 317; /3=>211 actual 18Sep06 '1 - 745 => 1 - 370; /3=>230 actual 'DEBUG 1,DEC servo," ",DEC tempw," " '*5 to get 10us resolution 'PULSOUT servonum,tempw*/pscale ' PAUSE 20 ' GOTO PotSetRanges '========================================================================= 'RevEdSerialLCD: '{$STAMP BS1} 'PAUSE 2000 'SEROUT 6,T9600,10,[254,1] 'cls -needs a pace time here of at least 2 'PAUSE 1000 'SEROUT 6,T9600,["hello"] 'GOTO RevEdSerialLCD 'END 'GOTO demo1 'Scroll in 40chars from right hand side 'GOTO demo2 'Print 40 chars then window to the right to see last chars '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) '---------------------------------------------------------