Bambino_43: SERTXD("B43",CR) 'report program number @4800 '2017 bytes #PicAxe 20M2 #no_data 'Choose #defines '#define SRF05 'use a PicAxe SRF05 ultrasonic sensor 'if no echo SRF05 times out at 32ms #define HCSR04 'use a HC-SR04 ultrasonic sensor 'if no echo HC-SR04 never times out #define centimeters 'distances in centimeters cm '#define inches 'distances in inches '(takes two extra bytes over distance in cm) '#define testUS '================================================================= '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 ' save rollbyL and rollbyR in EEPROM '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, ' remember holes and obstacles 'Bambino-37 PicAxe SRF05 Ultrasonic sensor added ' and facility to adjust ' USawareAt and UStoonear from IR-handset ' Adjust RollC PaceC removed to make room. 'Bambino-38 Added code to use either PicAxe SRF05 or an HC-SR04 ' Ultrasonic sensor with #defines for sensor type, ' cm or inches, and testUS. 'Bambino-39 added KEY_PLUS to test Ping sensor and act on result, ' can be put in learned routines. 'Bambino-40 minor rewrites - see modified, better use of R_Ping ' added poweron test of US, if <10 then disable US 'Bambino-41 added CASE KEY_9 '[9] Behaviour to select behaviours ' B_slow,B_medium,B_fast, ' B_UStimid,B_USnormal,B_USbold, ' B_noWhiskers,B_noUS ' B_USxxx now write to RAM 'Bambino-42 moved whisker testing and reaction to R_Whiskers ' added KEY_MINUS to test Whiskers and act on result ' removed test of US in INITIALISE:, not needed now on keys[9][8] ' initialise EEPROM with DUStoonear, DUSawareat, DUSconfigOK 'Bambino-43 Act#8 made autorun ' On poweron to WANDER do [f|][8][0][Power] ' enter command mode do [f|][8][Power] ' WANDER B_slow do [f|][8][9][1][0][Power] ' run Act1 do [f|][8][1][Power] ' do SAct1 then Act1 do [f|][8][f^][1][1][Power] ' etc '----------------------------------------------------------------- Rationale: 'To save room FdAdo: and BkAdo: deleted as not currently used. 'The initialisation of EEPROM USvalues has been removed. 'And DUSconfigOK=251 is no longer used. ' 'Previously Bambino always started to WANDER after power-on, now 'Act#8 autoruns and so you can get Bambino to do any Act or SAct 'and set the behaviours you want. '================================================================= Modified_and_New: Modified: 'initMIND: cmnd =8 :GOTO DoCmnd 'beep255: renamed squak: 'because that is what it sounds like 'DoCmnd: CASE KEY_0 GOSUB calibrateGwhiskers 'R_Ping: GOSUB squak instead of beep255 'FdAdo: and BkAdo: deleted as not currently used 'SYMBOL DUSconfigOK=251 deleted as no longer used 'SYMBOL OK =224 deleted as no longer used 'SYMBOL slot7E =251 'SYMBOL B_USnormaltoonear =10 deleted as no longer used 'SYMBOL B_USnormalawareat =20 deleted as no longer used New: '================================================================= Note: '================================================================= ProgramOverview: 'Bambino_Suite: 'Modified_and_New: 'ProgramOverview: 'BambinoCommands: 'TVR010_PicAxe_IR_controller: 'PIN_MAPPING: 'VARIABLES_and_CONSTANTS: 'Movement commands 'EEPROM Acts, start addresses 'EEPROM config data addresses 'RAM data addresses 'PingSensors: '------------------------------------------ 'INITIALISE: initialise Bambino 'init_rollCpaceC: 'initBODY: 'defaultMIND 'initMIND: ' set cmnd to 8 to run Act8 and goto Docmnd '------------------------------------------ 'ACTIVITIES: 'NewCmnd: 'Docmnd: ' select action on cmnd ' includes all cmnds listed under [KEY_xxx] ' Play and Record set playing and recording flags ' some actions alter cmnd and goto Docmnd to reprocess cmnd ' key Acts: could just drop through to IRcmnd but does a goto instead, ' saves testing the Play and Record flags ' 'RecordAct: ' if recording then ' if end of slot ' cmnd=end ' goto Docmnd to execute it ' write cmnd to selected act in eeprom ' 'PlayAct: ' if playing then ' read cmnd of selected act from eeprom ' if cmnd <> end of list ' goto Docmnd to execute it ' else drop through to IRcmnd: ' 'IRcmnd: ' wait for IR cmnd ' beep to acknowlede IR ' goto Docmnd ' '------------------------------------------ '[defaultMIND: ] '[ calibrate hole sensors] 'WANDER: ' DecayMemory: decay obstacle count in memory ' R_Whiskers ' R_Ping ' Fd ' Qwander: listen for IRcmnd(with timeout) ' if no cmnd goto WANDER ' if cmnd goto NewCmnd: (and then Docmnd) '================================================================= 'BEHAVIOURS: 'B_fast: 'B_medium: 'B_slow: 'B_turn 'B_USbold: 'B_USnormal: 'B_UStimid: '================================================================= 'REACTIONS: 'R_Ping: 'R_Whiskers '================================================================= 'PROGRAMED_ACTS: 'SUBROUTINE_ACTS: 'RockAdo: 'SAct#100 '(whee: - HMI) '(whoa: - HMI) '--------------------------- 'BkBkOtMt: 'Rest: '_RestIRin: '================================================================= 'ACTIONS: 'Fd: '_FdR: '_FdL: '----------------------------------------- 'BkBk: 'Bk: '_BkL: '_BkR: '----------------------------------------- 'Mt: '----------------------------------------- 'Ot: '----------------------------------------- 'Rt: '----------------------------------------- 'Lt: '----------------------------------------- 'Stand: 'stand with both feet flat '_StandL: '_StandR: '================================================================= 'MOVEMENTS: '_RF: '_LF: '----------------------------------------- '_rollL: '_rollR: '----------------------------------------- 'pulseservos: '----------------------------------------- '_iStand: '================================================================= 'COMMUNICATION: 'HMI - Human Machine Interface 'beep50: 'squak: 'beep255: 'whee: 'whoa: 'EyesOFF: 'EyesON: 'EyesDimmest: '================================================================= 'UTILITIES: 'IRgetAct_cmnd: 'nokey: 'badkey: 'Z: 'do nothing '================================================================= 'SENSE: 'QEwhiskerL: 'QEwhiskerR: 'calibrateholewhiskers: 'QGwhiskerL: 'QGwhiskerR 'setUSawares: 'QPing: '================================================================= BambinoCommands: 'using TVR010_PicAxe_IR_controller ' '[KEY_POWER] stops a learned Act and enters Command mode. ' ends modify mode, ends learning ' it can't stop a coded Act ' 'Direction Keys '-------------- '[KEY_UP] ie [Fd] Step Forward '[KEY_DOWN] ie [Bk] Step Backward '[KEY_RIGHT] ie [Rt] Step Right '[KEY_LEFT] ie [Lt] Step Left ' 'Function Keys '------------- '[KEY_BAR] ie [f|] enters learn mode to record key presses ' 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 coded Acts 'Acts can chain to other Acts, eg Fd,Bk,2 - chain to Act2 'A learned Act which chains itself repeats forever, 'eg [f|][1][Fd][1][POWER] Act1 will do Fd forever '[KEY_TENT] ie [f^] selects coded Acts = subroutines, ' has to be followed by a number key 1-8 ' plays coded Acts, coded Acts play until the end. ' [KEY_1]=>RockAdo '[KEY_VCROSS] ie [f+] modify USawareAt, UStoonear ' [+] set USAwareAt in EEPROM DUSawareAt ' [-] set UStoonear in EEPROM DUStoonear ' [Key_POWER] to quit '[KEY_XCROSS] ie [fX] Dump EEPROM Acts DATA to PC, including Config Data ' 'Act Keys '-------- '[1] to [8] Play an Act '[f^][1] to [f^][8] Play a coded Act 'When Recording an Act, ' [1]-[8] record a jump to a new Act ' if [1],[2],[3],[4],[5],[6],[7].[8],[0] are used to jump to ' 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 the recording; ' [f^] then [1]-[8] to do and include a Coded Act; ' when the memory slot is full Bambino will beep and recording will end. 'Act8 Autoruns on PowerOn ' On poweron to WANDER do [f|][8][0][Power] ' enter command mode do [f|][8][Power] ' WANDER B_slow do [f|][8][9][1][0][Power] ' run Act1 do [f|][8][1][Power] ' do SAct1 then Act1 do [f|][8][f^][1][1][Power] ' etc 'Act0 is WANDER and cannot be changed ' 'Behavior Key '------------ '[9] Behaviour mode eg [9][1] set speed slow ' [1] set speed slow ' [2] set speed medium ' [3] set speed fast ' [4] set US behaviour to timid ' [5] set US behaviour to normal ' [6] set US behaviour to bold ' [7] disable Whiskers test ' [8] disable Ultrasonic test ' 'Sensor Keys '----------- '[-] enable and test Whisker sensors and re-act, R_Whiskers: '[+] enable and test Ping sensor and re-act, R_Ping: ' 'Activity Key '------------ '[0] WANDER ' ' 'Quirks '------ ' If you are recoeding Act1 and press [f|][2] then you will start 'to record Act2 and Act1 will be left without placing an end marker. 'If Act1 is then played the new moves will be played then whatever 'moves remained there from before. 'If you enter record mode but press [POWER] before pressing a valid 'Act key [1] - [8] then until you press [POWER] a second time and 'Bambino's eyes glow dim any commands will be recorded in Act #6. 'There is no room left to correct this! '[KEY_POWER]=21 @32bytes per slot =>$2A0 =>$A0 =160 =slot #6 '================================================================= 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 '[Fd] Step Forward SYMBOL KEY_DOWN = 17 '[Bk] Step Backward SYMBOL KEY_RIGHT = 18 '[Rt] Step Right SYMBOL KEY_LEFT = 19 '[Lt] Step Left SYMBOL KEY_BAR = 96 '[f|] learn an Act, next [key] selects an Act SYMBOL KEY_TENT = 54 '[f^] next [key] selects a Coded-Act SYMBOL KEY_VERT_CROSS = 37 'use synonym for easier coding SYMBOL KEY_VCROSS = 37 '[f+] Modify Mode {when routine included} ' [+] set USAwareAt in DUSawareAt ' [-] set UStoonear in DUStoonear ' {[Fd] alter PaceCentre in DpaceC} ' {[Bk] alter PaceCentre in DpaceC} ' {[Rt] alter RollCentre in DrollC} ' {[Lt] alter RollCentre in DrollC} ' { [-][+] to adjust } ' [Key_POWER] to quit SYMBOL KEY_DIAG_CROSS = 20 'use synonym for easier coding SYMBOL KEY_XCROSS = 20 '[fX] transfer EEPROM to PC for including in programs ' --Coded Acts and Commands-- SYMBOL KEY_1 = 0 '[1] [f^][1]=>RockADo [9][1]=set speed slow SYMBOL KEY_2 = 1 '[2] [9][2]=set speed medium SYMBOL KEY_3 = 2 '[3] [9][3]=set speed fast SYMBOL KEY_4 = 3 '[4] [9][4]=set US behaviour to timid SYMBOL KEY_5 = 4 '[5]' [9][5]=set US behaviour to normal SYMBOL KEY_6 = 5 '[6]' [9][6]=set US behaviour to bold 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] [1]-[8] set Behaviour modes SYMBOL KEY_MINUS = 98 '[-] enable & test Whisker sensors and re-act, R_Whiskers: SYMBOL KEY_0 = 9 '[0] WANDER SYMBOL KEY_PLUS = 11 '[+] enable & test Ping sensor and re-act, R_Ping: '================================================================= CommandMapping: ' 0 - 99 direct keys '100 - 199 behaviours '200 - 254 subroutine Acts - 255 is end marker '================================================================= PIN_MAPPING: 'SYMBOL GwhiskerR =b.0 'adc whisker Down Right 'SYMBOL eyeLedR_ =b.1 'low =>LED on 'SYMBOL outIR =b.2 'SYMBOL GwhiskerLedR_ =b.3 'low =>whisker LED on 'b.4 'compass/user 'SYMBOL GwhiskerL =b.5 'adc whisker Down Left 'SYMBOL HSIcmnd =b.6 'hardware serial in 'SYMBOL pingpin =b.7 'Ultrasonic sensor SRF05 in single pin mode ' or HC-SR04 on a single pin 'SYMBOL servopace =c.0 'left connector 'SYMBOL eyeL =c.1 'SYMBOL eyeLedL_ =c.2 'low =>LED on 'SYMBOL GwhiskerLedL_ =c.3 'low =>whisker LED on 'SYMBOL servoroll =c.4 'right connector 'SYMBOL VoiceLed_ =c.5 'low =>LED on 'SYMBOL inIR =c.6 'SYMBOL eyeR =c.7 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 SYMBOL pingpin =b.7 'Ultrasonic sensor SRF05 in single pin mode ' or HC-SR04 on a single pin 'SYMBOL HSIcmnd =b.6 'hardware serial in '----------------------------------------- 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_nowhiskers =bit25 '1=ignore Eye and Ground Whiskers SYMBOL f_noUS =bit26 '1=ignore US SYMBOL f_turnedL =bit27 '1=turned Left, 0= turned Right 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 temp3cmnd =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 SYMBOL BeHave =B19 'W10 SYMBOL Apend =B20 'Penultimate end of record slot SYMBOL eptr =B21 'pointer to EEPROM 'W11 SYMBOL Ado =B22 'parameter for Acts SYMBOL usdist =B23 'US distance to object 'b24 - b27 '=== 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 SYMBOL slot7E =251 'slot 7 End, don't overwrite Config Data 'EEPROM Config DATA addresses SYMBOL DUStoonear =252 'holds user UStoonear, BkBkOtMt if usdist <= UStoonear SYMBOL DUSawareAt =253 'holds user USawareAt, noaction if usdist>USawareAt ELSE Mt 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 obsclearL and obsclearR 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 artificial light then Bambino may need an eye shade or a neb 'to shield the ground from the artificial 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 'set by calibrateholewhiskers: % light level difference for hole SYMBOL RAMholedetectL =31 'set by calibrateholewhiskers: % light level difference for hole SYMBOL RAMobstaclesR =32 'map SYMBOL RAMobstaclesL =33 'map SYMBOL RAMlightR =34 'eye ambientlight level Right SYMBOL RAMlightL =35 'eye ambientlight level Left SYMBOL RAMUStoonear =36 SYMBOL RAMUSawareAt =37 '================================================================= PingSensors: '----------- ' At sea level sound travels through air at 1130 feet per second. ' This equates to 1 inch in 73.746 uS, or 1 cm in 29.034 uS. ' ' Since the Ping sensor measures the time required for the sound wave ' to travel from the sensor and back. The result is divided by two to ' remove the return portion of the echo pulse, then multiplied by 10 ' to convert PicAxe20M2 10uS resolution to uS. ' The final raw result is converted to cm.or inches ' uS * 10/2/73.746in => * 0.01356 => * 4443/65536 => ** 4443 ' uS * 10/2/29.034cm => * 0.03444 => * 11286/65536 => ** 11286 SYMBOL uS10tocm =11286 ' 10 / 147.492 (with **) 'SYMBOL uS10toinch =4443 ' 10 / 58.068 (with **) SYMBOL uS10tocm4mHz =11286 ' 10 / 147.492 (with **) SYMBOL cmtoinch =25802 ' /2.54 -> *25802 /65536 -> **25802 #ifdef SRF05 SYMBOL USscalecm =uS10tocm4mHz 'pulsin units 10uS SYMBOL pingtrig =2 '20uS @4Mhz #endif 'The HC-SR04 starts the return pulse too quickly for a PicAxe20m2 at 4Mhz 'to detect the start of the pulse. It needs to run at least at 8MHz. 'Also if the sensor never gets a return echo from an obstacle the 'return pulse never ends and the PULSIN timer has to overflow before 'it returns a value. At 8MHz the timer resolution is 5us. The timer 'is 16 bits which means each reading takes '65,536 * 5us =327680 'which is about 1/3 of a second. This is a long time to wait between 'steps and makes walking very slow. 'Increasing the clock speed to 32MHz means the timer resolution is '1.25us and it times out in 65536 *1.25us = 81920us or about 0.08 'of a second which is much better. 'So SETFREQ m32 is used before triggering the sensor #ifdef HCSR04 SYMBOL USscalecm =uS10tocm4mHz /8 'pulsin units 1.25uS SYMBOL pingtrig =12 '15uS @32Mhz #endif '================================================================= #ifdef testUS goto testping #endif INITIALISE: init_Servos_rollCpaceC: 'get values from EEPROM set by program #8 or #8+ READ DrollC,rollC READ DpaceC,paceC SERTXD("rollC=",#rollC," paceC=",#paceC,CR) initBODY: GOSUB _iStand 'stand and initialise rollat, paceat defaultMIND: 'f_noUS =1 'disable US, poweron default is 0 'f_noWhiskers =1 'disable Whiskers, poweron default is 0 GOSUB B_fast GOSUB B_USnormal 'set RAMUS distances to B_USnormal 'if US distances are valid, overwrite B_USnormal: values READ DUStoonear,temp0 IF temp0>0 AND temp0<255 THEN POKE RAMUStoonear,temp0 ENDIF READ DUSawareAt,temp1 IF temp1>0 AND temp1<255 THEN POKE RAMUSawareAt,temp1 ENDIF '---------------- initMIND: GOSUB calibrateGwhiskers GOSUB squak 'so we know Bambino started 'leaves VoiceLED on f_record =0 Ado =4 'for RockAdo 'RAMobstaclesL, RAMobstaclesR always 0 on poweron cmnd =KEY_8 GOTO DoCmnd '================================================================= ACTIVITIES: NewCmnd: GOSUB beep50 'so we know IR received DoCmnd: SERTXD(CR,#cmnd," @",#eptr) SELECT cmnd '[POWER],[UP],[DOWN],[RIGHT],[LEFT],[0] CASE KEY_POWER IF f_record=0 AND f_play=0 THEN GOSUB rest GOTO NewCmnd '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 'ignore [0] if recording 'so it can be learned at recordAct: GOSUB calibrateGwhiskers GOTO WANDER ENDIF '[1]-[8] Acts CASE cmnd 0-7 Acts 1-8 IF f_record<>1 THEN f_play=1 'play eptr =32 *cmnd 'set to start SERTXD("p@",#eptr,CR) ENDIF '[9] Behaviour CASE KEY_9 GOSUB IRgetKey1to8 cmnd =cmnd +100 GOSUB beep50 GOTO DoCmnd '[+] do Pingsensor CASE KEY_PLUS 'Test Ping sensor and act on result f_noUS =0 GOSUB R_Ping '[-] do Whiskers CASE KEY_MINUS 'Test Whiskers and act on result f_noWhiskers =0 GOSUB R_Whiskers '[mode|] record 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 slot7E 'to avoid overwriting EEPROM config DATA GOSUB nokey SERTXD("r@",#eptr,CR) GOTO WaitIRcmnd 'get cmnds '[mode^] coded Acts CASE KEY_TENT 'coded acts GOSUB IRgetAct_cmnd cmnd =cmnd +200 '+200 for subroutine Acts SERTXD(" A",#cmnd,CR) GOTO DoCmnd 'already beeped '[mode+] Adjust CASE KEY_VCROSS 'modify USawareAt, UStoonear IRIN inIR,cmnd 'Get cmnd IF cmnd=KEY_PLUS OR cmnd=KEY_MINUS THEN GOSUB setUSawares ELSE GOSUB badkey ENDIF '[modeX] Dump 'Remember internal Slots/Acts are 0-7 keys [1]-[8] 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) 'do Coded Act subroutines CASE >=200 Act =cmnd -200 SERTXD("C",#Act,CR) ON Act GOSUB RockAdo,whee,whoa 'add others 'do Behaviours CASE >=100 behave =cmnd -100 ' [1] [2] [3] [4] [5] [6] [7] [8] ON behave GOSUB B_slow,B_medium,B_fast,B_UStimid,B_USnormal,B_USbold,B_noWhiskers,B_noUS 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 WaitIRcmnd: SERTXD(">") GOSUB LEDswaitkey 'eyes dimmest, whiskers off IF f_record=1 THEN LOW VoiceLED_ GOSUB EyesOFF ENDIF 'mouth lit IRIN inIR,cmnd 'Get cmnd GOTO NewCmnd '----------------------------------------- WANDER: SERTXD(CR,"W") 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 (CR,"obL",#temp1," obR",#temp0) GOSUB R_whiskers GOSUB R_Ping GOSUB Fd Qwander: irin [50,WANDER],inIR,cmnd 'irin with timeout, no IR =>WANDER GOTO NewCmnd '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 f_Bfast =0 RETURN B_slow: rollspeed =1 pacespeed =1 f_Bfast =0 RETURN B_turn: 'only called if B_fast rollspeed =1 'slow so foot doesn't bounce pacespeed =5 RETURN B_USbold: POKE RAMUStoonear,5 POKE RAMUSawareAt,10 RETURN B_USnormal: POKE RAMUStoonear,10 POKE RAMUSawareAt,20 RETURN B_UStimid: POKE RAMUStoonear,15 POKE RAMUSawareAt,30 RETURN B_noWhiskers: f_noWhiskers =1 RETURN B_noUS: f_noUS =1 RETURN '================================================================= REACTIONS: '--------- R_Ping: 'react to Ping information IF f_noUS=1 THEN RETURN ENDIF GOSUB Qping 'read US -> usdist PEEK RAMUStoonear,temp0 PEEK RAMUSawareAt,temp1 SERTXD("US",#usdist,"<=",#temp0,"|",#temp1) SELECT usdist CASE <=temp0 GOSUB squak GOSUB BkBkOtMt 'Bk Bk Ot Mt CASE <=temp1 GOSUB beep50 GOSUB Mt 'MemoryTurn, turn same way as last time ENDSELECT RETURN '----------------------------------------- R_whiskers: IF f_noWhiskers=1 THEN RETURN ENDIF obstacleMap =0 GOSUB EyesOFF 'disable PWM otherwise can't turn them on or off Qholes: 'holes have priority GOSUB QGwhiskerL GOSUB QGwhiskerR IF obstacleMap<4 THEN Qobstacles 'no holes f_direction =0 'turn direction back if hole 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 temp3cmnd =obstacleMap /4 'shift down 2 bits 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 temp3cmnd =obstacleMap ShowMind: SERTXD("=",#f_Lefthole,#f_Righthole,#temp1,#temp0,">") '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 Wavoid: ' % 00 01 10 11 ON temp3cmnd GOSUB Z, Lt,Rt,BkBkOtMt RETURN '================================================================= PROGRAMED_ACTS: '----------- Subroutine_ACTS: RockAdo: 'SAct#100 IF f_Bfast=1 THEN GOSUB B_fast FOR iA=1 TO Ado GOSUB _rollR GOSUB _rollL NEXT iA GOSUB Stand RETURN '--------------------------- 'whee: - HMI '--------------------------- 'whoa: - HMI '--------------------------- BkBkOtMt: 'Bk Bk Ot Mt GOSUB BkBk GOSUB Ot GOSUB Mt 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 ("F") 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 B_turn GOSUB _LF IF f_Bfast=1 THEN GOSUB B_fast f_turnedL =1 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_ Z: 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 '---------------------------------------- calibrateGwhiskers: 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 '---------------------------------------- setUSawares: tempW0 =0 _setUSawares: GOSUB Qping SERTXD("US",#usdist,CR) IF usdist=0 THEN GOSUB badkey GOTO setUSawares ENDIF 'SERTXD("cmnd",#cmnd,CR) SELECT cmnd CASE KEY_PLUS 'SERTXD("KEY_PLUS",CR) WRITE DUSawareAt,usdist 'SERTXD("AwareAt=",#usdist,CR) CASE KEY_MINUS 'SERTXD("KEY_MINUS",CR) WRITE DUStoonear,usdist 'SERTXD("TooClose=",#usdist,CR) CASE KEY_POWER 'Quit 'SERTXD("KEY_POWER",CR) GOSUB beep50 'done RETURN 'RETURN ELSE GOSUB badkey ENDSELECT _setUSawaresIRin: GOSUB beep50 'done cmnd IRIN inIR,cmnd GOTO _setUSawares Qping: 'Query Ultrasonic Sensor, read into USdist LOW pingpin ' make trigger 0-1-0 #ifdef HCSR04 SETFREQ m32 ' pulse resolution 1.25uS #endif PULSOUT pingpin,pingtrig ' activate sensor PULSIN pingpin,1,tempw0 ' measure high(=1) echo pulse in units, ' 10us@4MHz, 1.25us@32MHz ' pulsin rolls over at 65536counts =>0 ' ie 0.65536sec @ 4MHz(10uS) too long to wait if nothing there ' 0.081926sec @32MHz(1.25uS) tolerable ' so use 32MHz for HCSR04 ' SRF05 auto times out at 36ms and returns 0, good. tempw0 =tempw0 ** USscalecm 'scale time to cm #ifdef inches tempw0 =tempw0 ** cmtoinch #endif IF tempw0=0 THEN ' if timeout then usdist =255 ' set to max distance ELSE usdist =tempw0 MAX 255 ENDIF #ifdef HCSR04 SETFREQ m4 ' restore clock frequency #endif RETURN #ifdef testUS 'save code space if not testing testping: GOSUB Qping SERTXD(#USdist,CR) TOGGLE VoiceLed_ PAUSE 50 GOTO testping #endif '=================================================================