Bambino_40: SERTXD("B40",CR) 'report program number @4800 '1905 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 '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 '----------------------------------------------------------------- Rationale: 'minor rewrites - see modified, better use of R_Ping 'Also if the battery voltage gets low then sometimes the Ultrasonic 'sensor stops working and indicates an obstacle when there isn't one 'Bambino then keeps walking backwards away from the non existant object 'so a test has been put in INITIALISE: to disable the sensor if the initial 'reading on power-on is less than 10. '================================================================= Modified_and_New: 'modified '-------- 'init_rollCpaceC: renamed init_Servos_rollCpaceC: 'BT: deleted - BkBkOtMt: written 'Deleted 'Movement commands SYMBOLs cFd =0,cLt,cRt,cBT,cMt 'QUS: deleted 'R_Ping: now calls QPing, does BkBkOtMt: 'DoCmnd: CASE KEY_PLUS now calls R_Ping 'Walk: - BkBkOtMt instead of BT ' 'New '--- 'f_noUS 'ignore US if 0 'BkBkOtMt: '================================================================= 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_Servos_rollCpaceC: 'initBODY: 'initMIND: 'GOTO Qwander and listen for IR cmnd(with timeout) ' if cmnd then NewCmnd else WANDER: '------------------------------------------ '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 ' '------------------------------------------ '[initMIND: ] '[ calibrate hole sensors] 'WANDER: ' DecayMemory: decay obstacle count in memory ' Qholes: Query holes - read Gwhiskers ' Qobstacles Query obstacles - read Ewhiskers ' ObstacleMemory: update memory obstacle counts ' AddMemoryToMap: ' Qfrustrated: Query obstacle counts, if >Mfru_obs then back ' ShowMind: set LEDs so we knw what is happening ' Walk: act on cmnd - Fd,Lt,Rt,Bk2OtMt ' 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: '================================================================= 'PROGRAM_ACTS: 'RockAdo: '#100 'FdAdo: 'BkAdo '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: 'beep255: 'whee: 'whoa: 'EyesOFF: 'EyesON: 'EyesDimmest: '================================================================= 'UTILITIES: 'IRgetAct_cmnd: 'nokey: 'badkey: '================================================================= 'SENSE: 'QEwhiskerL: 'QEwhiskerR: 'calibrateholewhiskers: 'QGwhiskerL: 'QGwhiskerR 'setUSawares: 'QPing: '================================================================= 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 subroutines Acts (SAct), '[KEY_TENT] has to be followed by a number key 1-8 ' [key1]=>RockAdo 'SActs play until the end. ' '[KEY_VCROSS] 'modify USawareAt, UStoonear '[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 subroutine 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 '[v] Step Backward SYMBOL KEY_RIGHT = 18 '[>] Step Right SYMBOL KEY_LEFT = 19 '[<] Step Left SYMBOL KEY_BAR = 96 'f[|] learn an Act, next [key] selects an Act SYMBOL KEY_TENT = 54 'f[^] next [key] selects a Subroutine-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 ' {[^] alter PaceCentre in DpaceC} ' {[v] alter PaceCentre in DpaceC} ' {[>] alter RollCentre in DrollC} ' {[<] alter RollCentre in DrollC} ' { [-][+] to adjust } ' [Key_POWER] to quit SYMBOL KEY_DIAG_CROSS = 20 'use synonym for easier coding SYMBOL KEY_XCROSS = 20 'f[X] transfer EEPROM to PC for including in programs ' ' -- Subroutine-Act and Commands -- SYMBOL KEY_1 = 0 '[1] RockADo SYMBOL KEY_2 = 1 '[2] SYMBOL KEY_3 = 2 '[3] SYMBOL KEY_4 = 3 '[4] SYMBOL KEY_5 = 4 '[5] SYMBOL KEY_6 = 5 '[6] SYMBOL KEY_7 = 6 '[7] SYMBOL KEY_8 = 7 '[8] SYMBOL KEY_9 = 8 '[9] SYMBOL KEY_MINUS = 98 '[-] SYMBOL KEY_0 = 9 '[0] QUIT command mode SYMBOL KEY_PLUS = 11 '[+] Test Ping sensor and re-act, R_Ping: '================================================================= CommandMapping: ' 0 - 99 direct keys '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 'b.6 'hserin '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 '----------------------------------------- VARIABLES_and_CONSTANTS: SYMBOL OK =224 'arbitrary value 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_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 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 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 =250 'slot 7 End, don't overwrite Config Data 'EEPROM Config DATA addresses SYMBOL DUSconfigOK=251 '224=ok 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 act0s =0 ' 0-31 SYMBOL act1s =32 '32-63 SYMBOL act2s =32 *2 '64-95 SYMBOL act3s =32 *3 '96-127 SYMBOL act4s =32 *4 '128-159 SYMBOL act5s =32 *5 '160-191 SYMBOL act6s =32 *6 '192-223 SYMBOL act7s =32 *7 '224-250 - EEPROM config DATA 251,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 '================================================================= 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 SYMBOL USoff =10 'on PowerOn if >= then enable US #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) 'check if US distances have been initialised, if not use B_USnormal: READ DUSconfigOK,temp0 IF temp0<>OK THEN GOSUB B_USnormal initBODY: HIGH eyeLedR_ 'make output and OFF HIGH eyeLedL_ 'make output and OFF GOSUB _iStand 'stand and initialise rollat, paceat GOSUB Qping 'Query Ultrasonic IF USdist 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 '[+] do Pingsensor CASE KEY_PLUS 'Test Ping sensor and act on result GOSUB R_Ping '[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("rec@",#eptr,CR) GOTO WaitIRcmnd 'get cmnds '[mode^] subroutine Acts CASE KEY_TENT 'subroutine acts GOSUB IRgetAct_cmnd cmnd =cmnd +200 '+200 for subroutine Acts SERTXD(" Act",#cmnd,CR) GOTO DoCmnd 'already beeped '[mode+] Adjust CASE KEY_VCROSS 'modify USawareAt, UStoonear _case_VCROSS: 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 subroutine Acts 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 WaitIRcmnd: SERTXD(">") GOSUB EyesDimmest IF f_record=1 THEN LOW VoiceLED_ GOSUB EyesOFF ENDIF 'mouth lit IRIN inIR,cmnd 'Get cmnd GOTO NewCmnd '----------------------------------------- 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 (CR,"obL",#temp1," obR",#temp0) 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 IF cmnd=0 THEN GOSUB R_Ping 'Query Ultrasonic and react if obstacle Walk: ' % 00 01 10 11 ON cmnd GOSUB Fd,Lt,Rt,BkBkOtMt 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 ' RETURN 'B_slow: ' rollspeed =1 ' pacespeed =1 ' RETURN B_turn: rollspeed =1 'slow so foot doesn't bounce pacespeed =5 RETURN 'B_USbold: ' WRITE DUStoonear,5 ' WRITE DUSawareAt,10 ' GOTO DUSok B_USnormal: WRITE DUStoonear,10 WRITE DUSawareAt,20 GOTO DUSok 'B_UStimid: ' WRITE DUStoonear,15 ' WRITE DUSawareAt,30 DUSok: WRITE DUSconfigOK,OK RETURN '================================================================= REACTIONS: '--------- R_Ping: 'react to Ping information, no action if no obstacle IF f_noUS=1 THEN RETURN ENDIF GOSUB Qping 'read US -> usdist READ DUStoonear,temp0 READ DUSawareAt,temp1 SERTXD("US",#usdist,"<=",#temp0,"|",#temp1) SELECT usdist CASE <=temp0 GOSUB beep255 GOSUB BkBkOtMt CASE <=temp1 GOSUB beep50 GOSUB Mt 'MemoryTurn, turn same way as last time ENDSELECT 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 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 ("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 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_ 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("USdist",#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 '=================================================================