Bambino_36: SERTXD("B36",CR) 'report program number @4800 '1893 bytes #PicAxe 20M2 #no_data '================================================================= 'PICAXE IC Memory ext I/O Outputs Inputs ADC Memory Starts Polled Resonator ' £ Type pins bytes slot pins Vars. RAM Spad Data Table* Int. def. opt. '2.28 PICAXE-20M2 20 2k 18 1-16 1-17 4 28 512 256 512 8 Yes 4 -32 'PICAXE-20M2 circuit board with LEDs off takes 1mA 'PICAXE-20M2 circuit board with LEDs off and servos connected takes 10mA '================================================================= Bambino_Suite: 'Bambino-01 Test eyeLEDs, whiskerLEDs and speaker 'Bambino-02 Test PWM of eyeLEDs 'Bambino-03 Test InfraRed input 'Bambino-04 Test Eye light values 'Bambino-05 Test Whisker light values 'Bambino-06 Play the inbuilt tunes using keys 1 to 4 and beep on 5 'Bambino-07 Set servos to mid position 'Bambino-08 Calibrate servo mid positions and put in EEPROM. 'Bambino-08+ Adjust servo mid positions from Handset and put in EEPROM. 'Bambino-09 Introduce _iStand:- initialise with feet in mid position 'Bambino-10 Set rollby, rollbymax for Roll Right 'Bambino-11 Check rollby, rollbymax for Roll Left 'Bambino-12 Check rollby and rollbymax for rollL, rollR 'Bambino-13 Introduction of _Stand 'Bambino-14 Investigate effect of rollspeed; ' watch roll for all three speeds 'Bambino-15 Investigate effect of rollspeed; ' watch Roll and Stand for all three speeds 'Bambino-16 Check paceFL paceFR; Investigate effect of pacespeed ' Introduction of pulseservos: 'Bambino-17 Check Fd 'Bambino-18 Test Rt, Lt 'Bambino-19 Test Bk 'Bambino-20 Test T2l: TurntoLight: T2d: TurntoDark: Wl: Wd: 'Bambino-21 Creation of sections ACTIONS, BEHAVIOURS, ACTS; ' Introduction of ACT parameter 'Ado' 'Bambino-22 Demo routine + example use of Start1: to flash eyes 'Bambino-23 Walk Fd with obstacles avoidance using eyes from 04, ' introduction of Default behaviour 'Bambino-24 Walk Fd with obstacles avoidance as 23, with Act BT 'Bambino-25 Walk Fd with drop-off avoidance using whiskers from 05 'Bambino-26 rewritten with reading of whiskers as subroutines 'Bambino-27 rewritten with autocalibration of down whiskers 'Bambino-28 Test maximum PWM and hence minimum brightness of eyeLEDs, ' needed in Bambino-29 'Bambino-29 Wait for IR command then do it otherwise WANDER as B24 'Bambino-30 rewrite to clearer code 'Bambino-31 remember a sequence of key presses on IR-controller 'Bambino-32 remember up to 7 sequences of key presses in RAM, ' forgotten on power off ' Adjust RollC and PaceC added, easier to fine tune. ' Execute subroutines from IR-controller added, RockAdo. 'Bambino-33 remember up to 8 sequence of key presses in EEPROM, ' now not forgotten. ' Data can now be sent to a PC. 'Bambino-34 obstacles memory made deeper for better obstacle avoidance, ' and put in RAM, Mfrus_obs introduced 'Bambino-35 key [0] can now be learned 'Bambino-36 incorporate drop-off avoidance from B25, ' avoid holes and obstacles '----------------------------------------------------------------- Rationale: 'Now we have figured out how to get Bambino to WANDER avoiding obstacles 'it is time to add looking for and avoiding holes or edges. '================================================================= Modified_and_New: 'modified '-------- 'DecayMemory: 'New '--- 'synonyms of XLedX_pin added to PIN_MAPPING 'Qholes: 'ShowMind '================================================================= BambinoCommands: '[KEY_BAR] enters learn mode '[KEY_BAR] has to be followed by a number key 1-8 ' = the Act to be remembered 'up to 8 Acts can be learned 'learned Acts can call subroutine Acts (SAct) 'a learned Act which chains itself repeats forever, eg #1 = Fd,1 'Acts can chain to other Acts, eg Fd,Bk,2 - chain to Act2 '[KEY_POWER] stops the playing Act. '[KEY_TENT] selects SActs, '[KEY_TENT] has to be followed by a number key 1-8 ' [key1]=>RockAdo 'SActs play until the end. ' '[KEY_XCROSS] 'Dump EEPROM Acts DATA to PC ' 'When recording an Act, ' if [1],[2],[3],[4],[5],[6],[7] ' are used to chain another Act there is no point in recording more moves ' because they will never be replayed since control is transfered to the new Act, ' just press [KEY_POWER] to end recording. ' or [1]-[8] to jump to the new Act ' or [KEY_TENT] then [1]-[8] to do a SAct before finishing the original Act ' when the memory slot is full Bambino will beep and recording will end. '================================================================= 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 using 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 SYMBOL KEY_POWER = 21 'Sleep SYMBOL KEY_UP = 16 'Step Forward SYMBOL KEY_DOWN = 17 'Step Backward SYMBOL KEY_RIGHT = 18 'Turn Right SYMBOL KEY_LEFT = 19 'Turn Left SYMBOL KEY_BAR = 96 'learn an Act, next [key] selects an Act SYMBOL KEY_TENT = 54 'next [key] selects a Subroutine-Act SYMBOL KEY_VERT_CROSS = 37 'use synonym for easier coding SYMBOL KEY_VCROSS = 37 SYMBOL KEY_DIAG_CROSS = 20 'use synonym for easier coding SYMBOL KEY_XCROSS = 20 'transfer EEPROM to PC for including in programs ' ' -- Subroutine-Act and Commands -- SYMBOL IRkey1 = 0 ' RockADo SYMBOL IRkey2 = 1 SYMBOL IRkey3 = 2 SYMBOL IRkey4 = 3 SYMBOL IRkey5 = 4 SYMBOL IRkey6 = 5 SYMBOL IRkey7 = 6 SYMBOL IRkey8 = 7 SYMBOL IRkey9 = 8 SYMBOL KEY_MINUS = 98 SYMBOL KEY_0 = 9 'QUIT command mode SYMBOL KEY_PLUS = 11 '================================================================= PIN_MAPPING: SYMBOL servopace =c.0 'left connector SYMBOL servoroll =c.4 'right connector SYMBOL VoiceLed_ =c.5 'low =>LED on SYMBOL eyeLedL_ =c.2 'low =>LED on SYMBOL eyeLedR_ =b.1 'low =>LED on SYMBOL eyeLedL_pin =pinc.2 'low =>LED on SYMBOL eyeLedR_pin =pinb.1 'low =>LED on SYMBOL GwhiskerLedL_ =c.3 'low =>Ground whisker LED on SYMBOL GwhiskerLedR_ =b.3 'low =>Ground whisker LED on SYMBOL GwhiskerLedL_pin =pinc.3 'low =>Ground whisker LED on SYMBOL GwhiskerLedR_pin =pinb.3 'low =>Ground whisker LED on SYMBOL eyeR =c.7 'adc SYMBOL eyeL =c.1 'adc SYMBOL GwhiskerL =b.5 'adc Ground whisker Down Left SYMBOL GwhiskerR =b.0 'adc Ground whisker Down Right SYMBOL inIR =c.6 SYMBOL outIR =b.2 '----------------------------------------- VARIABLES_and_CONSTANTS: SYMBOL tempW0 =W0 SYMBOL temp0 =B0 'initialise,calibrateholewhiskers,QGwhiskerX SYMBOL temp0dark =B0 'QGwhiskerX SYMBOL temp1 =B1 'initialise,QGwhiskerX SYMBOL temp1lit =B1 'QGwhiskerX 'W1_flags SYMBOL obstacleMap =B2 '%1111 bitmap of obstacles SYMBOL f_Rightobstacle =bit16 SYMBOL f_Leftobstacle =bit17 SYMBOL f_Righthole =bit18 SYMBOL f_Lefthole =bit19 SYMBOL B_flags =B3 SYMBOL f_play =bit28 '1=playing SYMBOL f_record =bit29 '1=recording SYMBOL f_direction =bit30 'stepping direction in turns Fd=1, Bk=0 SYMBOL f_Bfast =bit31 'speed Behaviour 1=use B_fast: 'W2 SYMBOL temp2 =B4 'calibrateholewhiskers,QGwhiskerR SYMBOL temp2diff =B4 SYMBOL temp3 =B5 SYMBOL Scentre =W3 SYMBOL rollC =B6 '100 is roll to the left, 200 to the right SYMBOL paceC =B7 '100 is left foot back, 200 left foot forward SYMBOL Sat =W4 'current servo positions SYMBOL rollat =B8 SYMBOL paceat =B9 SYMBOL Sto =W5 'desired servo positions SYMBOL rollto =B10 SYMBOL paceto =B11 SYMBOL Sspeed =W6 SYMBOL rollspeed =B12 '4 is too fast, 0 won't move SYMBOL pacespeed =B13 '6 is about max for servos to keep up 'SYMBOL Smoveby =W7 'SYMBOL rollby =B14 '+ve right 15 good for walking 'SYMBOL paceby =B15 '+ve Left foot forward 20@speed 3; 25@speed 1 SYMBOL counters =W8 SYMBOL i =B16 '_iStand: SYMBOL spulse =B16 'FOR NEXT counter for servo pulses SYMBOL iA =B17 'counter for Acts 'W9 SYMBOL cmnd =B18 SYMBOL IRcmnd =B18 'cmnd from IRremote SYMBOL Act =B19 'W10 SYMBOL Apend =B20 'Penultimate end of record slot SYMBOL eptr =B21 'pointer to EEPROM 'W11 SYMBOL Ado =B22 'parameter for Acts '=== Your Bambino ================ SYMBOL rollby =15 '15 good for walking, edit to value in program11 SYMBOL paceby =25 '+ve Left foot forward 20@speed 3; 25@speed 1 SYMBOL sframe =20 'time between servo pulses SYMBOL rollbymax =20 '@speed 3; 25@speed 1 'rollmax absolute max is 40, above that the tilt rods hit the body SYMBOL pacebymax =30 'pacebymax absolute max is 30, above that the tilt rods hit the rear femur 'and the horn screws hit the leg body 'EEPROM DATA SYMBOL DrollC =254 'holds rollC, 100 is roll to the left, 150 is nominal centre SYMBOL DpaceC =255 'holds paceC, 100 is left foot back, 150 is nominal centre '================================= 'depending on the sensors obsdetectL and obsdetectR may need to be different SYMBOL obsclearL =20 '% light level difference for obstacle SYMBOL obsclearR =20 '% light level difference for obstacle 'If there is close tungsten light then Bambino may need an eye shade or a neb 'to shield the ground from the tungsten light. 'whisker LEDs on, 82R 1.414v =>17.2mA @4.02V SYMBOL Minc_obs =2 'Mind increment for obstacle SYMBOL Mmax_obs =4 'Mind limit to obstacle memory SYMBOL Mfru_obs =2 'Mind level above which back direction is selected for turns SYMBOL Minc_hole =4'6'8 'Mind memory increment for holes SYMBOL Mmax_holes =6'8 'Mind max memory increment for holes '----------------------------------------- 'EEPROM Acts, start addresses SYMBOL act0 =0 ' 0-31 SYMBOL act1 =32 '32-63 SYMBOL act2 =32 *2 '64-95 SYMBOL act3 =32 *3 '96-127 SYMBOL act4 =32 *4 '128-159 SYMBOL act5 =32 *5 '160-191 SYMBOL act6 =32 *6 '192-223 SYMBOL act7 =32 *7 '224-251 - EEPROM config DATA 252,253,254,255 'If [KEY_XCROSS] is used to dump to a PC EEPROM Acts DATA from another Bambino 'then paste it here '----------------------------------------- 'RAM data addresses 28 - 511 SYMBOL RAMtemp0 =28 'first non variable RAM location SYMBOL RAMtemp1 =29 SYMBOL RAMholedetectR =30 'holds % light level difference for hole SYMBOL RAMholedetectL =31 'holds % light level difference for hole SYMBOL RAMobstaclesR =32 SYMBOL RAMobstaclesL =33 SYMBOL RAMlightR =34 'eye ambientlight level Right SYMBOL RAMlightL =35 'eye ambientlight level Left '================================================================= INITIALISE: init_rollCpaceC: 'get values from EEPROM set by program #8 or #8+ READ DrollC,rollC READ DpaceC,paceC SERTXD("rollC=",#rollC," paceC=",#paceC,CR) HIGH eyeLedR_ 'make output and OFF HIGH eyeLedL_ 'make output and OFF GOSUB _iStand 'stand and initialise rollat, paceat initmind: GOSUB B_fast f_record =0 'RAMobstaclesL, RAMobstaclesR always 0 on poweron GOSUB calibrateholewhiskers GOSUB beep255 'so we know Bambino started, leaves VoiceLED on START: SERTXD("S") Ado =4 'for RockAdo IRIN [50,WANDER],inIR,cmnd'IRIN with timeout, no IR =>Walkabout GOTO DoCmnd '================================================================= ACTIVITIES: WaitIRcmnd: SERTXD(">") GOSUB EyesDimmest IF f_record=1 THEN LOW VoiceLED_ GOSUB EyesOFF ENDIF 'mouth lit IRIN inIR,cmnd 'Get cmnd GotCmnd: GOSUB beep50 'so we know IR received DoCmnd: SERTXD(CR,#cmnd," @",#eptr) SERTXD(" f_p",#f_play," f_r",#f_record,",") SELECT cmnd CASE KEY_POWER IF f_record=0 AND f_play=0 THEN GOSUB rest GOTO GotCmnd 'have already got IRcmnd in rest: ELSE f_play =0 IF f_record=1 THEN f_record =0 WRITE eptr,255 'end marker ENDIF 'GOSUB dump 'debug ENDIF CASE KEY_UP GOSUB Fd CASE KEY_DOWN GOSUB Bk CASE KEY_RIGHT GOSUB Rt CASE KEY_LEFT GOSUB Lt CASE KEY_0 IF f_record=0 THEN initmind 'ignore [0] if recording so it can be learned at recordAct: CASE cmnd 0-7 Acts 1-8 IF f_record<>1 THEN f_play=1 'play eptr =32 *cmnd 'set to start SERTXD("play@",#eptr,CR) ENDIF CASE KEY_BAR f_play =0 F_record =1 LOW VoiceLED_ 'mouth lit for record GOSUB IRgetAct_cmnd eptr =32 *cmnd Apend =eptr +31 MAX 253 'to avoid overwriting EEPROM config DATA GOSUB nokey SERTXD("rec@",#eptr,CR) GOTO WaitIRcmnd 'get cmnds CASE KEY_TENT 'subroutine acts GOSUB IRgetAct_cmnd cmnd =cmnd +200 '+200 for subroutine Acts SERTXD(" Act",#cmnd,CR) GOTO DoCmnd 'already beeped CASE KEY_VCROSS 'modify RollC PaceC IRIN inIR,cmnd 'Get cmnd SELECT cmnd CASE KEY_UP, KEY_DOWN GOSUB adjustpaceC CASE KEY_LEFT, KEY_RIGHT GOSUB adjustrollC ELSE GOSUB badkey ENDSELECT CASE KEY_XCROSS 'Dump sends DATA to PC, include Config Data FOR Act=0 TO 7 SERTXD(CR,"'Slot ",#Act,CR,"DATA(") FOR eptr=0 TO 30 temp0 =Act *32 +eptr READ temp0,cmnd SERTXD(#cmnd,",") NEXT eptr temp0 =Act *32 +31 READ temp0,cmnd SERTXD(#cmnd,")") NEXT Act SERTXD(CR) CASE >=200 SERTXD("case >200",CR) Act =cmnd -200 ON Act GOSUB RockAdo,whee,whoa 'add others END SELECT '------------- recordAct: IF f_record=1 THEN IF eptrbreak _playon: GOSUB EyesOFF READ eptr,cmnd Eptr =eptr +1 IF cmnd<>255 THEN DoCmnd '255=end f_play =0 ENDIF GOTO WaitIRcmnd 'get IR '----------------------------------------- WANDER: SERTXD(CR,"W") GOSUB EyesOFF 'disable PWM otherwise can't turn them on or off DecayMemory: PEEK RAMobstaclesR,temp0 'read memory PEEK RAMobstaclesL,temp1 'read memory temp0 =temp0 MIN 1 -1 'decay memory temp1 =temp1 MIN 1 -1 'decay memory POKE RAMobstaclesR,temp0 'save POKE RAMobstaclesL,temp1 'save SERTXD (" obL",#temp1," obR",#temp0,CR) obstacleMap =0 Qholes: 'holes have priority GOSUB QGwhiskerL GOSUB QGwhiskerR IF obstacleMap<4 THEN Qobstacles 'no holes f_direction =0 'turn direction back if hole cmnd =obstacleMap /4 'shift down 2 bits temp0 =Minc_hole *f_Righthole MAX Mmax_obs 'if f_Righthole=0 then temp0=0 temp1 =Minc_hole *f_Lefthole MAX Mmax_obs 'if f_Lefthole=0 then temp1=0 POKE RAMobstaclesR,temp0 'reinitialise if a hole POKE RAMobstaclesL,temp1 'reinitialise if a hole GOTO ShowMind 'ignore obstacles till holes cleared Qobstacles: GOSUB QEwhiskerL GOSUB QEwhiskerR ObstacleMemory: PEEK RAMobstaclesR,temp0 temp0 =Minc_obs *f_Rightobstacle +temp0 MAX Mmax_obs 'if f_Rightobstacle=0 then no change to temp0 POKE RAMobstaclesR,temp0 'update R memory PEEK RAMobstaclesL,temp1 temp1 =Minc_obs *f_Leftobstacle +temp1 MAX Mmax_obs 'if f_Leftobstacle=0 then no change to temp1 POKE RAMobstaclesL,temp1 'update L memory AddMemoryToMap: IF temp1<>0 THEN f_Leftobstacle =f_Leftobstacle OR 1 ENDIF IF temp0<>0 THEN f_Rightobstacle =f_Rightobstacle OR 1 ENDIF Qfrustrated: temp1 =temp1 +temp0 IF temp1>Mfru_obs THEN f_direction =0 'frustrated so turn direction is back ENDIF cmnd =obstacleMap ShowMind: 'alter LEDs so we can tell what Bambino is thinking '1 XOR (1^) inverts the logic value 'LEDs are high for off and obstacleMap bits are high for an obstacle eyeLedL_pin =1^f_Leftobstacle 'indicate obstacle eyeLedR_pin =1^f_Rightobstacle 'indicate obstacle GwhiskerLedL_pin =1^f_Lefthole 'indicate hole GwhiskerLedR_pin =1^f_Righthole 'indicate hole Walk: ' % 00 01 10 11 ON cmnd GOSUB Fd,Lt,Rt,BT Qwalk: irin [50,WANDER],inIR,cmnd 'irin with timeout, no IR =>WANDER GOTO GotCmnd 'QUIT if IRin '================================================================= BEHAVIOURS: '---------- B_fast: pacespeed =6 '6 is about max for servos to keep up rollspeed =3 'half of pacespeed is about right for walking f_Bfast =1 RETURN 'B_medium: ' rollspeed =2 ' pacespeed =3 ' RETURN 'B_slow: ' rollspeed =1 ' pacespeed =1 ' RETURN B_turn: rollspeed =1 'slow so foot doesn't bounce pacespeed =5 RETURN '================================================================= PROGRAM_ACTS: '----------- RockAdo: '#100 IF f_Bfast=1 THEN GOSUB B_fast FOR iA=1 TO Ado GOSUB _rollR GOSUB _rollL NEXT iA GOSUB Stand RETURN FdAdo: FOR iA=1 TO Ado GOSUB Fd NEXT iA RETURN BkAdo: FOR iA=1 TO Ado GOSUB Bk NEXT iA RETURN BT: GOSUB Bk GOSUB Bk GOSUB Rt GOSUB Rt RETURN Rest: GOSUB _rollL GOSUB _iStand GOSUB EyesDimmest HIGH GwhiskerLedL_ HIGH GwhiskerLedR_ HIGH VoiceLed_ _RestIRin: IRIN inIR,cmnd 'IRIN with no timeout RETURN '================================================================= ACTIONS: '------- Fd: SERTXD ("Fd") f_direction =1 IF paceatpaceC AND f_direction=1 THEN GOSUB Fd 'need right foot forward IF paceat>paceC AND f_direction=0 THEN GOSUB Bk 'need right foot forward IF f_Bfast=1 THEN GOSUB beep255 IF f_Bfast=1 THEN GOSUB B_turn GOSUB _LF IF f_Bfast=1 THEN GOSUB B_fast RETURN '================================================================= MOVEMENTS: '--------- _RF: 'SERTXD ("_RF") paceto =paceC -paceby FOR spulse=paceat TO paceto STEP -pacespeed paceat =spulse GOSUB pulseservos NEXT spulse RETURN _LF: 'SERTXD ("_LF") paceto =paceC +paceby FOR spulse=paceat TO paceto STEP pacespeed paceat =spulse GOSUB pulseservos NEXT spulse RETURN '----------------------------------------- Stand: 'stand with both feet flat 'SERTXD (" Sd") IF rollat 0-7 IF cmnd<8 THEN _IRgetActEnd IF cmnd=KEY_POWER THEN RETURN ENDIF 'quit GOSUB badkey GOTO IRgetAct_cmnd 'repeat until valid act _IRgetActEnd: GOSUB beep50 RETURN nokey: IRIN [50,_nokeyend],inIR,cmnd'IRIN timeout =>reurn GOTO nokey _nokeyend: RETURN badkey: LOW GwhiskerLedR_ PAUSE 100 HIGH GwhiskerLedR_ LOW GwhiskerLedL_ PAUSE 100 HIGH GwhiskerLedL_ RETURN '================================================================= SENSE: '----- QEwhiskerL: READADC eyeL,temp0dark 'read ambient light level POKE RAMlightL,temp0dark 'save it LOW eyeLedL_ 'eyeL on PAUSE 1 READADC eyeL,temp1lit 'read light level with LED on HIGH eyeLedL_ 'eyeL LED off temp2 =255 -temp0dark 'possible change 'find difference as % of possible change temp2diff =temp1lit MIN temp0dark -temp0dark *100 /temp2 'SERTXD(" Lw=",#temp2diff," ") IF temp2diff>obsclearL THEN f_Leftobstacle =f_Leftobstacle OR 1 ENDIF RETURN QEwhiskerR: READADC eyeR,temp0dark 'read ambient light level POKE RAMlightR,temp0dark 'save it LOW eyeLedR_ 'eyeR on PAUSE 1 READADC eyeR,temp1lit 'read light level with LED on HIGH eyeLedR_ 'eyeR LED off temp2 =255 -temp0dark 'possible change 'find difference as % of possible change temp2diff =temp1lit MIN temp0dark -temp0dark *100 /temp2 'SERTXD("Rw=",#temp2diff,CR) IF temp2diff>obsclearR THEN f_Rightobstacle =f_Rightobstacle OR 1 ENDIF RETURN '------------------------------------------ calibrateholewhiskers: GOSUB QGwhiskerL 'set threshold at 60% temp2 =temp2 *3 /5 '60% POKE RAMholedetectL,temp2 GOSUB QGwhiskerR 'set threshold at 60% temp2 =temp2 *3 /5 '60% POKE RAMholedetectR,temp2 RETURN QGwhiskerL: temp1lit =0 'initialise illuminated light level LOW GwhiskerLedL_ 'LED on FOR i = 1 TO 6 'the FOR NEXT loop takes 10ms READADC GwhiskerL,temp2 'sensorval in temp2 temp1lit =temp1lit MIN temp2 'keep maximum NEXT i temp0dark =0 'initialise ambient light level HIGH GwhiskerLedL_ 'LED off FOR i = 1 TO 6 'the FOR NEXT loop takes 10ms READADC GwhiskerL,temp2 'sensorval in temp2 temp0dark =temp0dark MIN temp2 'keep maximum NEXT i temp2 =255 -temp0dark 'possible change 'find difference as % of possible change temp2diff =temp1lit -temp0dark *100 /temp2 ' SERTXD(" L=",#temp0dark,"/",#temp2diff," ") PEEK RAMholedetectL,temp0 IF temp2diff<=temp0 THEN '- a hole f_Lefthole =f_Lefthole OR 1 ENDIF RETURN QGwhiskerR: temp1lit =0 'initialise illuminated light level LOW GwhiskerLedR_ 'LED on FOR i = 1 TO 6 'the FOR NEXT loop takes 10ms READADC GwhiskerR,temp2 'sensorval in temp2 temp1lit =temp1lit MIN temp2 'keep maximum NEXT i temp0dark =0 'initialise ambient light level HIGH GwhiskerLedR_ 'LED off FOR i = 1 TO 6 'the FOR NEXT loop takes 10ms READADC GwhiskerR,temp2 'sensorval in temp2 temp0dark =temp0dark MIN temp2 'keep maximum NEXT i temp2 =255 -temp0dark 'possible change 'find difference as % of possible change temp2diff =temp1lit -temp0dark *100 /temp2 ' SERTXD("R=",#temp0dark,"/",#temp2diff) PEEK RAMholedetectR,temp0 IF temp2diff<=temp0 THEN '- a hole f_Righthole =f_Righthole OR 1 ENDIF RETURN '================================================================= ADJUST_RollC_PaceC: '------------------ adjustrollC: temp0 =DrollC LOW GwhiskerLedR_ 'LED on, Right servo is Roll GOTO adjustC adjustpaceC: temp0 =DpaceC LOW GwhiskerLedL_ 'LED on, Left servo is Pace ' GOTO _adjustC adjustC: rollat =RollC paceat =PaceC waitIR: GOSUB pulseservos: IRIN [50,waitIR],inIR,cmnd 'irin with timeout, no IR =>_waitIR SELECT cmnd CASE KEY_POWER 'finished SELECT temp0 CASE DrollC WRITE DrollC,rollC 'save for other programs HIGH GwhiskerLedR_ CASE DpaceC WRITE DpaceC,paceC 'save for other programs HIGH GwhiskerLedL_ ENDSELECT SERTXD("rollC=",#rollC," paceC=",#paceC,CR) GOTO ACTIVITIES 'back to wait IR cmnd CASE KEY_MINUS SELECT temp0 CASE DrollC rollC =rollC -1 SERTXD("rollC=",#rollC,CR) CASE DpaceC paceC =paceC -1 SERTXD("paceC=",#paceC,CR) ENDSELECT CASE KEY_PLUS SELECT temp0 CASE DrollC rollC =rollC +1 SERTXD("rollC=",#rollC,CR) CASE DpaceC paceC =paceC +1 SERTXD("paceC=",#paceC,CR) ENDSELECT ELSE GOSUB beep255 ENDSELECT GOTO adjustC '=================================================================