'>CondorIR-18m2 from 10Apr12 '>CondorIR-18x 26 October 2010, 00:26:36 #PicAxe18m2 #no_data 'Memory used = 526 bytes out of 2048 'setfreq m16 sertxd @19200 'reads Sharp GP2Y0A02YK IR distance sensors and serials to BS2 'status 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 '------- '22Jan15 added set f_b_IR, set f_b_US so IR can set them on or off '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 - f_Obj '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.1 8| |11 B.5 (In/Out/ADC/Touch/hserout) ' (pwm/Touch/ADC/Out/In) B.1 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_IRrLED =B.2 '8 'RedLED through 390R to 0v symbol po_USyLED =B.3 '9 'YellowLED through 390R to 0v symbol po_VSbatokgLED =B.4 '10 'GreenLED through 330R to 0v 'symbol pA_Vbat B.5 '11 '9v test symbol po_WaitRxrLED_ =B.6 '12 'RedLED through 390R to 5v symbol po_SerToBS2 =B.7 '13 symbol po_RdyToBS2_ =B.7 '13 T9600_16, 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 'sertxd @19200_16 '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 f_ObsR =BIT0 '0=ok 1=obstacle or cliff symbol f_ObsL =BIT1 '0=ok 1=obstacle or cliff symbol f_b_US =BIT2 '1=doUS symbol f_b_IR =BIT3 '1=doIR symbol statusBat_ =BIT4 '0=ok 1=low 'ignored by BS2 symbol cmnd =B1 'W0 'START:IRin 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 'W3 ' 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 'T idle is high, data pulses low SYMBOL KEY_7 = 6 '[7]' [9][7]=disable Whiskers test SYMBOL KEY_8 = 7 '[8]' [9][8]=disable Ultrasonic test SYMBOL KEY_9 = 8 '[9] set Behaviour modes [9][9]=set US behaviour to User SYMBOL KEY_MINUS = 98 '[-] enable & test Whisker sensors and re-act, R_Whiskers: SYMBOL KEY_PLUS = 11 '[+] enable & test Ping sensor and re-act, R_Ping: 'goto testboard '--------------------------------------------------------- Init: setfreq m16 'sertxd @19200 sertxd("I",CR) LOW po_WaitRxrLED_ 'redLED on indicate waiting for serin cmnd LOW po_RdyToBS2_ SERIN pi_FromBS2,BS2baud,status,stepup,drop,VSbatok 'cmnd for LEDs sertxd (#status," ",#stepup," ",#drop," ",#VSbatok,CR) HIGH po_RdyToBS2_ 'make sure it is on idle, otherwise errors below HIGH po_WaitRxrLED_ 'redLED 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_WaitRxrLED_ 'RedLED on - wait ready TOGGLE po_USyLED 'show we are waiting here IF pi_BS2ready_=1 THEN ReportIRthresh 'wait BS2ready LOW po_WaitRxrLED_ 'RedLED off - BS2 ready PAUSE 1 'give BS2 time to be ready SEROUT po_SerToBS2,BS2baud,(floordistL,floordistR) sertxd ("floordist>",cr) 'SetLEDs HIGH po_IRrLED :IF f_b_IR=0 THEN :LOW po_IRrLED :ENDIF 'do IR on HIGH po_USyLED IF f_b_US=0 THEN :LOW po_USyLED :ENDIF 'do US on '--------------------------------------------------------- START: sertxd("S") HIGH po_WaitRxrLED_ 'RedLED off IF pi_BS2ready_=0 THEN sendit cmnd =255 irin[30],pi_IR,cmnd '20 minimum sertxd(#cmnd) if cmnd=KEY_MINUS then :HIGH po_IRrLED :f_b_IR =1 :endif 'do IR on if cmnd=KEY_PLUS then :HIGH po_USyLED :f_b_US =1 :endif 'do US on if cmnd=KEY_9 then GOSUB CaseKey9 if cmnd<>255 then START 'If IRcmnd received then loop until BS2 ready, 'if finger off button then IRcmnd =255 SVokLED: readadc pA_VSbat,VSbat sertxd("VSbat",#VSbat,"/",#VSbatok) VSbat =VSbat MAX 254 '255 is sync 'sertxd("VSbat",#VSbat,cr) IF VSbat<200 THEN '< ~4v TOGGLE po_VSbatokgLED 'problem with reading VSbat statusBat_ =1 GOTO _L ENDIF IF VSbat>VSbatok THEN HIGH po_VSbatokgLED 'LED on statusBat_ =0 ELSE LOW po_VSbatokgLED statusBat_ =1 ENDIF 'sertxd("bat ",#VSbat," ",#VSbatok," ",#statusBat,cr) _L: readadc pA_LIR,IRfL sertxd(" IRfLR ",#IRfL) f_ObsL =0 IF IRfL>0 THEN 'got IR reading, 0,1 noise level LOW po_IRobsL floorthreshL =floordistL +stepup MAX 255 IF IRfL>floorthreshL THEN f_ObsL=1 HIGH po_IRobsL ENDIF floorthreshL =floordistL MIN drop -drop IF IRfL0 THEN 'got IR reading, 0,1 noise level LOW po_IRobsR floorthreshR =floordistR +stepup MAX 255 IF IRfR>floorthreshR THEN f_ObsR=1 HIGH po_IRobsR ENDIF floorthreshR =floordistR MIN drop -drop IF IRfR") pause 1 'give time for BS2 to unset ready GOTO START '========================================================= CaseKey9: sertxd("key9") GOSUB nokey IRIN pi_IR,cmnd 'Get cmnd if cmnd=KEY_7 then :LOW po_IRrLED :f_b_IR =0 :endif 'do IR on if cmnd=KEY_8 then :LOW po_USyLED :f_b_US =0 :endif 'do US on cmnd =cmnd +200 MAX 254 nokey: IRIN [50,_nokeyend],pi_IR,cmnd'IRIN timeout =>reurn GOTO nokey _nokeyend: RETURN '========================================================= 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_VSbatokgLED else low po_VSbatokgLED 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 '=========================================================