davidbuckley.net
home  >  Bambino  >  Programs 1 to 16  |  Programs 17 to 21  |  Programs 22 to 28 14 January 2012

Bambino Programming 17 - 21 by David Buckley

Developer Programming instructions

Programs 22 to 28

[page top]

'Bambino-17
Sertxd("B17",cr)		'report program number

#PicAxe 20M2
#no_data

'Bambino-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values and put in EEPROM
' Bambino-02cX	'Calibrate Eye light values
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions and put in EEPROM.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'            	
            	
'Creaton of sections ACTIONS, BEHAVIOURS, ACTS
'---------------------------------------------

'Introduction of ACT parameter 'Ado'
'-----------------------------------

'modified
'--------
'start
'i renamed iA:

'New
'---
'B_fast
'Ado
'ACT RockAdo
'ACT FdAdo
'ACT BkAdo


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 whiskerLedL_ =c.3    'low =>whisker LED on
symbol whiskerLedR_ =b.3    'low =>whisker LED on
symbol iIRin        =c.6
symbol oIRout       =b.2

symbol spulse 	=b2
symbol iA 		=b3
symbol rollat 	=b4
symbol paceat 	=b5

symbol rollC	=150	'100 is roll to the left
symbol rollR      =rollC+15
symbol rollL      =rollC-15
symbol rollspeed  =b6   '4 is too fast, 0 won't move
	rollspeed =1  	'4 is too fast, 0 won't move
symbol paceC	=150	'100 is left foot back
symbol paceFR	=paceC-20	'Right foot forward
symbol paceFL	=paceC+20	'Left  foot forward
symbol paceFRmax	=100	'Right foot forward
symbol paceFLmax	=190	'Left  foot forward
symbol sframe     =20	'time between servo pulses
symbol pacespeed  =b7   '6 is about max for servos to keep up
	pacespeed =2   	'6 is about max for servos to keep up  
symbol Ado		=b8	'parameter for ACTs




initialise:
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	

start:

	gosub B_fast
	Ado =4
	gosub RockAdo
	Ado =8
	gosub FdAdo
	Ado =8
	gosub BkAdo
	goto start


'-----------------------------------------
'Acts
'----

RockAdo:
	for iA=1 to Ado  
 	  gosub _rollR
	  gosub _rollL
	 next iA 
	return
	
FdAdo:
	for iA=1 to Ado 
	  gosub Fd
	 next iA 
	return
	
BkAdo:
	for iA=1 to Ado 
	  gosub Bk
	 next iA 
	return
'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =4 
	return
'-----------------------------------------
'Actions
'-------

Fd:
sertxd (" Fd")
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
sertxd (" Bk")
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC then gosub Fd	'need left foot forward
	gosub _RF
	return
	
Lt:
	if paceat>paceC then gosub Fd	'need right foot forward
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
	

Stand:	'stand with both feet flat
sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for Spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------

[page top]

'Bambino-17a
Sertxd("B17a",cr)		'report program number

#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-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values and put in EEPROM
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions and put in EEPROM.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test _Rt, _Lt
'Bambino-16	'Test _Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	

'modified
'--------
'start:

'New
'---
'Beep150

symbol servopace    =c.0    'left connector
symbol servoroll    =c.4    'right connector
symbol VoiceLed     =c.5
symbol eyeLedL_     =c.2    'low =>LED on 
symbol eyeLedR_     =b.1    'low =>LED on
symbol whiskerLedL_ =c.3    'low =>whisker LED on
symbol whiskerLedR_ =b.3    'low =>whisker LED on
symbol eyeR         =c.7		  	
symbol eyeL         =c.1		  	
symbol iIRin        =c.6
symbol oIRout       =b.2


symbol spulse	=b2
symbol iA 		=b3
symbol rollat 	=b4
symbol paceat 	=b5

symbol rollC	=155	'100 is roll to the left
symbol rollR      =173'176
symbol rollL      =135'130
symbol rollspeed  =b6   '4 is too fast, 0 won't move
symbol paceC	=145	'100 is left foot back
symbol paceFR	=130'125'120'105	'Right foot forward
symbol paceFL	=160'165'170'185	'Left  foot forward
symbol paceFRmax	=100	'Right foot forward
symbol paceFLmax	=190	'Left  foot forward
symbol sframe     =20'35'time between servo pulses
symbol pacespeed  =b7   '6 is about max for servos to keep up
symbol Ado		=b8	'parameter for ACTs

'symbol rollCoffset	=rollC -150
'symbol paceCoffset	=paceC -150


'IRin cmnds PicAxe controller
'these are actual tested values 
'"AXE040=I-R UPGRADE .pdf" is wrong for 20M2
'-------------------------------------------
'Mute(20)      Power(21)
'1(0)    2(1)      3(2)
'4(3)    5(4)      6(5)
'7(6)    8(7)      9(8)
'P+(16)  0(9)     V+(18)
'P-(17) 10(12)    V-(19)

symbol IRkeyMute  =20
symbol IRkeyPower =21
symbol IRkey1     =0
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 IRkey0     =9
symbol IRkey10p   =12	'10+ 10plus
symbol IRkeyPp    =16	'P-  Pplus
symbol IRkeyPm    =17	'P+  Pminus
symbol IRkeyVp    =18	'V-  Vminus
symbol IRkeyVm    =19	'V+  Vminus


'All startX run automatically on PowerOn
'suspend 1	
initialise:
	high eyeLedR_	'make output
	high eyeLedL_	'make output
	gosub _iStand
	rollspeed =1  	'4 is too fast, 0 won't move
	pacespeed =2   	'6 is about max for servos to keep up  

'resume 1

start:	
	gosub beep150
'#rem
	Ado =4
	gosub RockAdo
	gosub Stand
	Ado =8
	gosub FdAdo
	Ado =8
	gosub BkAdo

'	goto start
'#endrem	 
	gosub ToggleEyes  'alter eyeR so we know program is running
	gosub B_fast
	gosub Fd
	gosub Fd
	gosub Fd
	gosub Fd
	gosub EyeRon
	gosub B_turn
	gosub Rt
	gosub Rt
	gosub Rt
	gosub Rt
	gosub EyesON
	gosub B_fast
	gosub Fd
	gosub Fd
	gosub Fd
	gosub Fd
	gosub EyeLon
	gosub B_turn
	gosub Lt
	gosub Lt
	gosub Lt
	gosub Lt

	gosub EyesON
	Ado =12
	gosub BkAdo

	goto start


'-----------------------------------------
'Acts
'----
RockAdo:
	for iA=1 to Ado  
 	  gosub _rollR
	  gosub _rollL
	 next iA 
	return
	
FdAdo:
	for iA=1 to Ado  
	  gosub Fd
	 next iA 
	return

BkAdo:
	for iA=1 to Ado 
	  gosub Bk
	 next iA 
	return

ToggleEyes:
	toggle eyeLedR_	'alter eyeR so we know program is running
	toggle eyeLedL_	'alter eyeR so we know program is running
	return

EyesON:
	low eyeLedR_
	low eyeLedL_
	return
EyeRON:
	high eyeLedR_
	low eyeLedL_
	return
EyeLON:
	low eyeLedR_
	high eyeLedL_
	return

'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =4 
	return
B_medium:
	rollspeed =2
	pacespeed =3 
	return
B_slow:
	rollspeed =1
	pacespeed =2 
	return
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return

'-----------------------------------------
'Actions
'-------
beep150:
	sound VoiceLed,(150,10)
	return

Fd:
sertxd (" Fd")
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
sertxd (" Bk")
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC then gosub Fd	'need left foot forward
	gosub _RF
	return
	
Lt:
	if paceat>paceC then gosub Fd	'need right foot forward
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 

Stand:	'stand with both feet flat
sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------
'start1:
togeyes:
	gosub ToggleEyes  'alter eyeR so we know program is running
	pause 500
	goto togeyes
'-----------------------------------------

[page top]

'Bambino-18
Sertxd("B18",cr)		'report program number

#PicAxe 20M2
#no_data

'Bambino-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a


            	
'Walk Fd with obstacle avoidance using eyes from 02a
'---------------------------------------------------            	
            	
'modified
'--------
'all variable moved up by 2 bytes to leave all the bit variables spare for flags, etc
'all variable moved up by another byte for temp
'start
'Fd	for defaultB
'Bk	for defaultB
'Rt	for defaultB
'Lt	for defaultB


'New
'---
'beep150
'EyesON
'defaultB  (defaultBehaviour) 1=use default allows Fd, Bk, Rt, Lt to use specific speed settings
'B_turn
'lightL
'lightR
'temp
'obsdetectL
'obsdetectR
'QEwhiskerL		Query Eye whisker Left
'QEwhiskerR		Query Eye whisker Right


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 whiskerLedL_ =c.3      'low =>whisker LED on
symbol whiskerLedR_ =b.3      'low =>whisker LED on
symbol eyeR         =c.7		  	
symbol eyeL         =c.1		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

'symbol general 	 =b0
symbol B_flags     =b3
symbol defaultB    =bit31	'defaultBehaviour 1=use default
symbol temp        =b4
symbol spulse 	 =b5
symbol iA 		 =b6
symbol rollat 	 =b7
symbol paceat 	 =b8

symbol rollC  	 =150	'100 is roll to the left
symbol rollR       =rollC+15
symbol rollL       =rollC-15
symbol rollspeed   =b9   '4 is too fast, 0 won't move
	rollspeed =1  	'4 is too fast, 0 won't move
symbol paceC	 =150	'100 is left foot back
symbol paceFR	 =paceC-20	'Right foot forward
symbol paceFL 	 =paceC+20	'Left  foot forward
symbol paceFRmax 	 =100	'Right foot forward
symbol paceFLmax	 =190	'Left  foot forward
symbol sframe      =20	'time between servo pulses
symbol pacespeed   =b10   '6 is about max for servos to keep up
	pacespeed =2   	'6 is about max for servos to keep up  
symbol Ado		 =b11	'parameter for ACTs
symbol obstacle    =b12  '%11 bitmap of obstacles  %LeftRight
symbol lightL      =b13  'light level Left
symbol lightR      =b14  'light level Right
'depending on the sensors obsdetectL and obsdetectR may need to be different
symbol obsdetectL	 =120	'scaled light level difference for obstacle
symbol obsdetectR	 =120	'scaled light level difference for obstacle



initialise:
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	defaultB =1		'defaultBehaviour On
	
start:


	obstacle =0
	gosub EyesOFF
QEwhiskerL:
	readadc eyeL,lightL
	low eyeLedL_			'eyeL on
	readadc eyeL,temp
	high  eyeLedL_			'eyeL off
	if lightL>temp then			'why this happens I don't know
	  temp =lightL				
	 endif	
	temp =temp -lightL *256 /temp 'find difference and scale it
	sertxd("L=",#temp," ")
	if temp<obsdetectL then QEwhiskerR	'ignore
	obstacle =obstacle or %10 

QEwhiskerR:
	readadc eyeR,lightR
	low eyeLedR_			'eyeR on
	readadc eyeR,temp
	high  eyeLedR_			'eyeR off
	if lightR>temp then		'why this happens I don't know
	  temp =lightR				
	 endif	
	temp =temp -lightR *256 /temp 'find difference and scale it
	sertxd("R=",#temp,cr)
	if temp<obsdetectR then Walk	'ignore
	obstacle =obstacle or 1 
	

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start


'-----------------------------------------
'Acts
'----

RockAdo:
	for iA=1 to Ado  
 	  gosub _rollR
	  gosub _rollL
	 next iA 
	return
	
FdAdo:
	for iA=1 to Ado 
	  gosub Fd
	 next iA 
	return
	
BkAdo:
	for iA=1 to Ado 
	  gosub Bk
	 next iA 
	return
	

'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6 
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'-----------------------------------------
'Actions
'-------

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
		

Fd:
sertxd (" Fd")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
sertxd (" Bk")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC then gosub Fd	'need left foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _RF
	return
	
Lt:
	if paceat>paceC then gosub Fd	'need right foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
	

Stand:	'stand with both feet flat
sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for Spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------

[page top]

'Bambino-18a
Sertxd("B18a",cr)		'report program number

#PicAxe 20M2
#no_data

'Bambino-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values and put in EEPROM
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions.and put in EEPROM
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a
'Bambino-18a	'Walk Fd with drop-off avoidance using whiskers values from 02b


            	
'Walk Fd with drop-off avoidance using whiskers values from 02b
'--------------------------------------------------------------           	
            	
'modified
'--------


'New
'---
'temp0
'holedetectL
'holedetectR
'QGwhiskerL		Query Ground whisker Left
'QGwhiskerR		Query Ground whisker Right


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 whiskerLedL_ =c.3      'low =>whisker LED on
symbol whiskerLedR_ =b.3      'low =>whisker LED on
symbol eyeR         =c.7	'adc	  	
symbol eyeL         =c.1	'adc	  	
symbol whiskerL     =b.6      'adc whisker Down Left		  	
symbol whiskerR     =b.0      'adc whisker Down Right		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

symbol general 	=b0
symbol temp0	=b0
symbol B_flags    =b3
symbol defaultB   =bit31	'defaultBehaviour 1=use default
symbol temp       =b4
symbol spulse 	=b5
symbol iA 		=b6
symbol rollat 	=b7
symbol paceat 	=b8

symbol rollC	=150	'100 is roll to the left
symbol rollR      =rollC+15 
symbol rollL      =rollC-15
symbol rollspeed  =b9   '4 is too fast, 0 won't move
	rollspeed =1  	'4 is too fast, 0 won't move
symbol paceC	=150	'100 is left foot back
symbol paceFR	=paceC-20	'Right foot forward
symbol paceFL	=paceC+20	'Left  foot forward
symbol paceFRmax	=100	'Right foot forward
symbol paceFLmax	=190	'Left  foot forward
symbol sframe     =20	'time between servo pulses
symbol pacespeed  =b10   '6 is about max for servos to keep up
	pacespeed =2   	'6 is about max for servos to keep up  
symbol Ado		=b11	'parameter for ACTs
symbol obstacle   =b12  '%11 bitmap of obstacles  %LeftRight
symbol lightL     =b13  'light level Left
symbol lightR     =b14  'light level Right
'depending on the sensors holedetectL and holedetectR may need to be different
symbol holedetectL=50	'scaled light level difference for holes
symbol holedetectR=50	'scaled light level difference for holes
'edit holedetect values so Bambino can see table edges

symbol EEholedetectR =252
symbol EEholedetectL =253
write EEholedetectR,holedetectR
write EEholedetectL,holedetectL

initialise:
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	defaultB =1		'defaultBehaviour On
	
start:


	obstacle =0
	gosub EyesOFF
QGwhiskerL:
	readadc whiskerL,lightL
	low whiskerLedL_			'eyeL on
	readadc whiskerL,temp
	high whiskerLedL_			'eyeL off
	if lightL>temp then		'why this happens I don't know
	  temp =lightL				
	 endif	
	temp =temp -lightL *256 /temp 'find difference and scale it
  sertxd("L=",#temp," ")
	read EEholedetectL,temp0
	if temp>temp0 then QwhiskerR	'ignore - not a hole
	obstacle =obstacle or %10 

QGwhiskerR:
	readadc whiskerR,lightR
	low whiskerLedR_			'eyeR on
	readadc whiskerR,temp
	high  whiskerLedR_		'eyeR off
	if lightR>temp then		'why this happens I don't know
	  temp =lightR				
	 endif	
	temp =temp -lightR *256 /temp 'find difference and scale it
  sertxd("R=",#temp,cr)
	read EEholedetectR,temp0
	if temp>temp0 then Walk		'ignore - not a hole
	obstacle =obstacle or 1 
	

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start


'-----------------------------------------
'Acts
'----

RockAdo:
	for iA=1 to Ado  
 	  gosub _rollR
	  gosub _rollL
	 next iA 
	return
	
FdAdo:
	for iA=1 to Ado 
	  gosub Fd
	 next iA 
	return
	
BkAdo:
	for iA=1 to Ado 
	  gosub Bk
	 next iA 
	return
	

'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6 
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'-----------------------------------------
'Actions
'-------

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
		

Fd:
  sertxd (" Fd")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
  sertxd (" Bk")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC then gosub Fd	'need left foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _RF
	return
	
Lt:
	if paceat>paceC then gosub Fd	'need right foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
	

Stand:	'stand with both feet flat
'  sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for Spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------

[page top]

'Bambino-18b
Sertxd("B18b",cr)		'report program number

#PicAxe 20M2
#no_data

'Bambino-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values and put in EEPROM
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions.and put in EEPROM
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a
'Bambino-18a	'Walk Fd with drop-off avoidance using whiskers values from 02b
'Bambino-18b	'rewritten with autocalibration of down whiskers and reading of whiskers as subroutines


            	
'Walk Fd with drop-off avoidance
'-------------------------------
            	
'modified
'--------
'calibrateholewhiskers:
'QGwhiskerL:		Query whisker Left, scaling now done in calibrateholewhiskers:
'QGwhiskerR:		Query whisker Right, scaling now done in calibrateholewhiskers:
'temp now temp0

'New
'---
'tempw
'tempHi_wLg		'whisker Left ground reading
'tempLo_wRg		'whisker Left ground reading

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 whiskerLedL_ =c.3      'low =>whisker LED on
symbol whiskerLedR_ =b.3      'low =>whisker LED on
symbol eyeR         =c.7	'adc	  	
symbol eyeL         =c.1	'adc	  	
symbol whiskerL     =b.6      'adc whisker Down Left		  	
symbol whiskerR     =b.0      'adc whisker Down Right		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

symbol general 	=b0
symbol temp0	=b0
symbol B_flags    =b3
symbol defaultB   =bit31	'defaultBehaviour 1=use default
'symbol           =b4
symbol spulse 	=b5
symbol iA 		=b6
symbol  i 		=b6	'calibrateholewhiskers:	
symbol rollat 	=b7
symbol paceat 	=b8

symbol rollC	=150	'100 is roll to the left
symbol rollR      =rollC+15 
symbol rollL      =rollC-15
symbol rollspeed  =b9   '4 is too fast, 0 won't move
	rollspeed =1  	'4 is too fast, 0 won't move
symbol paceC	=150	'100 is left foot back
symbol paceFR	=paceC-20	'Right foot forward
symbol paceFL	=paceC+20	'Left  foot forward
symbol paceFRmax	=100	'Right foot forward
symbol paceFLmax	=190	'Left  foot forward
symbol sframe     =20	'time between servo pulses
symbol pacespeed  =b10   '6 is about max for servos to keep up
	pacespeed =2   	'6 is about max for servos to keep up  
symbol Ado		=b11	'parameter for ACTs
symbol obstacle   =b12  '%11 bitmap of obstacles  %LeftRight
symbol lightL     =b13  'light level Left
symbol lightR     =b14  'light level Right
symbol tempw       =w8
symbol  tempLo_wRg  =b16
symbol  tempHi_wLg  =b17


'depending on the sensors holedetectL and holedetectR may need to be different
symbol holedetectL=50	'scaled light level difference for holes
symbol holedetectR=50	'scaled light level difference for holes
'edit holedetect values so Bambino can see table edges

symbol EEholedetectR =252
symbol EEholedetectL =253
write EEholedetectR,holedetectR
write EEholedetectL,holedetectL

initialise:
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	defaultB =1		'defaultBehaviour On

	gosub calibrateholewhiskers

	
start:


	obstacle =0
	gosub EyesOFF

	gosub QwhiskerL	'and update obstacle map
	gosub QwhiskerR	'and update obstacle map

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start


'-----------------------------------------
'Acts
'----

RockAdo:
	for iA=1 to Ado  
 	  gosub _rollR
	  gosub _rollL
	 next iA 
	return
	
FdAdo:
	for iA=1 to Ado 
	  gosub Fd
	 next iA 
	return
	
BkAdo:
	for iA=1 to Ado 
	  gosub Bk
	 next iA 
	return
	

'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6 
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'-----------------------------------------
'Actions
'-------

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
		

Fd:
  sertxd (" Fd")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
  sertxd (" Bk")
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC then gosub Fd	'need left foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _RF
	return
	
Lt:
	if paceat>paceC then gosub Fd	'need right foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
	

Stand:	'stand with both feet flat
'  sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for Spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------
'Sense
'-----
calibrateholewhiskers:
'take averages of 8 readings per whisker
	w8 =0	'clear tempHi,tempLo
	low whiskerLedL_				'eyeL on
	for i=0 to 7
'		low whiskerLedL_			'eyeL on
		readadc whiskerL,temp0
'		high whiskerLedL_			'eyeL off		
		sertxd("L",#temp0)
		temp0 =temp0 /8
		tempHi_wLg =tempHi_wLg +temp0
		sertxd(" ",#tempHi_wLg,cr)
	  next i	
	high whiskerLedL_				'eyeL off
	low whiskerLedR_				'eyeL on
	for i=0 to 7
'		low whiskerLedR_			'eyeL on
		readadc whiskerR,temp0
'		high whiskerLedR_			'eyeL off
		sertxd("R",#temp0)
		temp0 =temp0 /8
		tempLo_wRg =tempLo_wRg +temp0
		sertxd(" ",#tempLo_wRg,cr)
	  next i	
	high whiskerLedR_				'eyeL off
'set threshold at 60%
	tempHi_wLg =tempHi_wLg *3 /5
	tempLo_wRg =tempLo_wRg *3 /5
  sertxd("holethresh L",#tempHi_wLg," R",#tempLo_wRg,cr)
	write EEholedetectL,tempHi_wLg
	write EEholedetectR,tempLo_wRg
	return 

QGwhiskerL:
	low whiskerLedL_			'LwLED on
	readadc whiskerL,tempHi_wLg
	high whiskerLedL_			'LwLED off
	readadc whiskerL,lightL
	if lightL>tempHi_wLg then	'why this happens I don't know
		 lightL =tempHi_wLg	
	  endif	
	read EEholedetectL,temp0
	if tempHi_wLg<temp0 then 		'a hole, no ground detected
		obstacle =obstacle or %10 
	  endif
sertxd(cr,"wL",#tempHi_wLg)  
	return 

QGwhiskerR:
	low whiskerLedR_			'RwLED on
	readadc whiskerR,tempLo_wRg
	high  whiskerLedR_		'RwLED off
	readadc whiskerR,lightR		'ambient light
	if lightR>tempLo_wRg then	'why this happens I don't know
		 lightL =tempLo_wRg	
	  endif	
	read EEholedetectR,temp0
	if tempLo_wRg<temp0 then 		'a hole, no ground detected
		obstacle =obstacle or %01 
	  endif
  sertxd(" wR",#tempLo_wRg)
	return 
'-----------------------------------------

[page top]

'Bambino-19
Sertxd("B19",cr)		'report program number @4800

#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-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a
'Bambino-18a	'Walk Fd with drop-off avoidance using whiskers values from 02b
'Bambino-18b	'rewritten with autocalibration of down whiskers and reading of whiskers as subroutines
'Bambino-19	'Wait for IR command then do it otherwise WANDER

            	
'If IR command then do it otherwise walk and avoid
'-------------------------------------------------            	
            	
'modified
'--------
'start
'Fd		'test for direction
'Bk		'test for direction
'Rt		'test for direction
'Lt		'test for direction
'RockAdo	'default behaviour, added Stand
'beep50

'New
'---
'Rest:
'direction  =bit30	'1=Fd, 0=Bk:
'=================================================================
'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 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		'Rest (Sleep is a reserved word)

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
Symbol KEY_TENT   = 54
Symbol KEY_VERT_CROSS = 37
Symbol KEY_VCROSS = 37		'synonym for easier coding
Symbol KEY_DIAG_CROSS = 20
Symbol KEY_XCROSS = 20		'synonym for easier coding
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
Symbol KEY_PLUS   = 11
'=================================================================

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 whiskerLedL_ =c.3    'low =>whisker LED on
symbol whiskerLedR_ =b.3    'low =>whisker LED on
symbol eyeR         =c.7		  	
symbol eyeL         =c.1		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

'symbol general 	=b0
symbol B_flags    =b3
symbol f_direction=bit30	'1=forwards, 0=backwards    
symbol defaultB   =bit31	'defaultBehaviour 1=use default
symbol temp       =b4
symbol spulse 	=b5
symbol iA 		=b6
symbol rollat 	=b7
symbol paceat 	=b8

symbol rollC	=150	'100 is roll to the left
symbol rollR      =rollC+15
symbol rollL      =rollC-15
symbol rollspeed  =b9   '4 is too fast, 0 won't move
	rollspeed =1  	'4 is too fast, 0 won't move
symbol paceC	=150	'100 is left foot back
symbol paceFR	=paceC-20	'Right foot forward
symbol paceFL	=paceC+20	'Left  foot forward
symbol paceFRmax	=100	'Right foot forward
symbol paceFLmax	=190	'Left  foot forward
symbol sframe     =20	'time between servo pulses
symbol pacespeed  =b10   '6 is about max for servos to keep up
	pacespeed =2   	'6 is about max for servos to keep up  
symbol Ado		=b11	'parameter for ACTs
symbol obstacle   =b12  '%11 bitmap of obstacles  %LeftRight
symbol lightL     =b13  'light level Left
symbol lightR     =b14  'light level Right
symbol cmnd		=b15  'cmnd from IRremote


initialise:
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	defaultB =1		'defaultBehaviour On

start:
	Ado =4				'for RockAdo
	irin [50,noIR],iIRin,cmnd	'irin with timeout
	'						if no IR then WANDER
	gosub beep50                  'so we know IR received
DoIR:
	gosub EyesON
	'map keys
	'0= KEY_UP    (16)	'Step Forward
	'1= KEY_DOWN  (17)	'Step Backward
	'2= KEY_RIGHT (18)	'Turn Right
	'3= KEY_LEFT  (19)	'Turn Left
	'4= IRkey1    (0)		'RockADo
	'5= KEY_POWER (21)	'Rest
	'trap some other keys, if not one of the above then Start
	'              0  1  2  3  4 5  6 7 8 9 10 11 12 13 14 15
	lookdown cmnd,(16,17,18,19,0,21,9,1,2,3,4, 5, 6, 7, 8, 9),cmnd
	if cmnd>5 then Start
	'             0  1  2  3  4       5
	on cmnd gosub Fd,Bk,Rt,Lt,RockAdo,Rest
	irin iIRin,cmnd 
	gosub beep50                  'so we know IR received
	goto DoIR
	
noIR:			'WANDER	
	obstacle =0
	gosub EyesOFF
EwhiskerR:
	readadc c.7,lightR
	low eyeLedR_			'eyeR on
	readadc c.7,temp
	high  eyeLedR_			'eyeR off
	if lightR>temp then		'why this happens I don't know
	  temp =lightR				
	 endif	
	temp =temp -lightR
	if temp<2 then EwhiskerL		'ignore
	obstacle =obstacle or 1 
	
EwhiskerL:
	readadc c.1,lightL
	low eyeLedL_			'eyeL on
	readadc c.1,temp
	high  eyeLedL_			'eyeL off
	if lightL>temp then			'why this happens I don't know
	  temp =lightL				
	 endif	
	temp =temp -lightL
	if temp<5 then Walk		'ignore
	obstacle =obstacle or %10 

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start


'-----------------------------------------
'Acts
'----

RockAdo:
	if defaultB=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

Rest:
	gosub _rollL
	gosub _iStand
	pwmout eyeLedR_,100,400 'active low so high Dutycycle for dim
	pwmout eyeLedL_,100,400 'active low so high Dutycycle for dim
	high VoiceLed_
_RestIRin:
	irin iIRin,cmnd		'irin with no timeout
	if cmnd=5 then _RestIRin
	pwmout eyeLedR_,off	
	pwmout eyeLedL_,off
	return
'-----------------------------------------
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6 
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'-----------------------------------------
'Actions
'-------

beep50:
	sound VoiceLed_,(50,10)
	return

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
		

Fd:
sertxd (" Fd")
	f_direction =1
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
	

Bk:
sertxd (" Bk")
	f_direction =0
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return


Rt:
	if paceat<paceC and f_direction=1 then gosub Fd	'need left foot forward
	if paceat<paceC and f_direction=0 then gosub Bk	'need left foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _RF
	return
	
Lt:
	if paceat>paceC 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 defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _LF
	return

_RF:
'sertxd ("_RF")
	for spulse=paceat to paceFR step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	for spulse=paceat to paceFL step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
	

Stand:	'stand with both feet flat
sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 


_rollL:
'sertxd ("_rL")
	for spulse=rollat to rollL step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	for Spulse=rollat to rollR step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 

_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------

[page top]

'Bambino-20
Sertxd("B20",cr)		'report program number @4800

#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-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions and put values in EEPROM.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a
'Bambino-18a	'Walk Fd with drop-off avoidance using whiskers values from 02b
'Bambino-18b	'rewritten with autocalibration of down whiskers and reading of whiskers as subroutines
'Bambino-19	'Wait for IR command then do it otherwise WANDER
'Bambino-20	'rewrite to incorporate rollC and paceC in EEPROM 254,255
           	
'Rewrite of 19 - If IR command then do it otherwise walk and avoid
'-----------------------------------------------------------------            	
'modified
'--------
'symbol allocations
'sframe increased to 22 from 20, Bambino4 fell over @20
'initialise	updates rollC and paceC from EEPROM if not =255
'DoIR:
'_RestIRin:
'_RF:		incorporate paceC in EEPROM 255
'_LF:		incorporate paceC in EEPROM 255
'_rollL:	incorporate rollC in EEPROM 254
'_rollR:	incorporate rollC in EEPROM 254

'New
'---
'=================================================================
'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 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		'Rest (Sleep is a reserved word)

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
Symbol KEY_TENT   = 54
Symbol KEY_VERT_CROSS = 37
Symbol KEY_VCROSS = 37		'synonym for easier coding
Symbol KEY_DIAG_CROSS = 20
Symbol KEY_XCROSS = 20		'synonym for easier coding
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
Symbol KEY_PLUS   = 11
'=================================================================
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 whiskerLedL_ =c.3    'low =>whisker LED on
symbol whiskerLedR_ =b.3    'low =>whisker LED on
symbol eyeR         =c.7		  	
symbol eyeL         =c.1		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

'symbol general 	=b0
symbol flags	=w1
symbol B_flags     =b3
symbol f_direction  =bit30	'1=forwards, 0=backwards    
symbol defaultB     =bit31	'defaultBehaviour 1=use default
symbol servosAt	=w2
symbol  rollat 	 =b4
symbol  paceat 	 =b5
symbol ServosTo	=w3
symbol  rollTo	 =b6	   
symbol  paceTo	 =b7
symbol Smoveby	=w4
symbol  rollby	 =b8
symbol  paceby     =b9
symbol Scentre	=w5
symbol  rollC	 =b10
symbol  paceC      =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 counters	=w7
symbol  spulse 	 =b14
symbol  iA 		 =b15
symbol temp       =w8
symbol  tempL      =b16
'symbol tempH      =b17

	
symbol sframe     =22	'time between servo pulses
symbol Ado		=b18	'parameter for ACTs
symbol obstacle   =b19  '%11 bitmap of obstacles  %LeftRight
symbol lightL     =b20  'light level Left
symbol lightR     =b21  'light level Right
symbol cmnd		=b22  'cmnd from IRremote

'Cmnds 
'-----
'Sleep,Fd, Bk, Rt, Lt RockAdo
'0     1   2   3   4  5
'=================================================================
initialise:
	rollby    =15	'+ve right 
	paceby    =20	'+ve Left  foot forward
	rollspeed =1  	'4 is too fast, 0 won't move
	pacespeed =2   	'6 is about max for servos to keep up  
	rollC	    =150	'Default, 100 is roll to the left
	paceC	    =150	'Default, 100 is left foot back
	read 254,temp	'set in prog5
	if temp>130 and temp<170 then	'so servo is always between 100,200
	  rollC =temp
	 endif 
	read 255,temp	'set in prog5
	if temp>120 and temp<180 then	'so servo is always between 100,200
	  paceC =temp
	 endif 
	sertxd("rollC=",#rollC," paceC=",#paceC,cr)
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	defaultB =1		'defaultBehaviour On

start:
	Ado =4				'for RockAdo
	irin [50,noIR],iIRin,cmnd	'irin with timeout
	'						if no IR then WANDER
	gosub beep50                  'so we know IR received
DoIR:
	gosub EyesON
	'map keys
	'0= KEY_UP    (16)	'Step Forward
	'1= KEY_DOWN  (17)	'Step Backward
	'2= KEY_RIGHT (18)	'Turn Right
	'3= KEY_LEFT  (19)	'Turn Left
	'4= IRkey1    (0)		'RockADo
	'5= KEY_POWER (21)	'Rest
	'trap some other keys, if not one of the above then Start
	'              0  1  2  3  4 5  6 7 8 9 10 11 12 13 14 15
	lookdown cmnd,(16,17,18,19,0,21,9,1,2,3,4, 5, 6, 7, 8, 9),cmnd
	if cmnd>5 then Start
	'             0  1  2  3  4       5
	on cmnd gosub Fd,Bk,Rt,Lt,RockAdo,Rest
	irin iIRin,cmnd 
	gosub beep50                  'so we know IR received
	goto DoIR
	
noIR:			'WANDER	
	obstacle =0
	gosub EyesOFF
	obstacle =0
	gosub EyesOFF
EwhiskerR:
	readadc c.7,lightR
	low eyeLedR_			'eyeR on
	readadc c.7,tempL
	high  eyeLedR_			'eyeR off
	if lightR>tempL then		'why this happens I don't know
	  tempL =lightR				
	 endif	
	tempL =tempL -lightR
	if tempL<2 then EwhiskerL	'ignore
	obstacle =obstacle or 1 
	
EwhiskerL:
	readadc c.1,lightL
	low eyeLedL_			'eyeL on
	readadc c.1,tempL
	high  eyeLedL_			'eyeL off
	if lightL>tempL then			'why this happens I don't know
	  tempL =lightL				
	 endif	
	tempL =tempL -lightL
	if tempL<5 then Walk		'ignore
	obstacle =obstacle or %10 

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start
'=================================================================
'Acts
'----

RockAdo:
	if defaultB=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

Rest:
	gosub _rollL
	gosub _iStand
	pwmout eyeLedR_,100,400 'active low so high Dutycycle for dim
	pwmout eyeLedL_,100,400 'active low so high Dutycycle for dim
	high VoiceLed_
_RestIRin:
	irin iIRin,cmnd		'irin with no timeout
	if cmnd=5 then _RestIRin
	pwmout eyeLedR_,off	
	pwmout eyeLedL_,off
	return
'=================================================================
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'=================================================================
'Actions
'-------

beep50:
	sound VoiceLed_,(50,10)
	return

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
'-----------------------------------------
Fd:
sertxd (" Fd")
	f_direction =1
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
'-----------------------------------------
Bk:
sertxd (" Bk")
	f_direction =0
	if defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return
'-----------------------------------------
Rt:
	if paceat<paceC and f_direction=1 then gosub Fd	'need left foot forward
	if paceat<paceC and f_direction=0 then gosub Bk	'need left foot forward
	if defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _RF
	return
'-----------------------------------------
Lt:
	if paceat>paceC 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 defaultB=1 then gosub beep150
	if defaultB=1 then gosub B_turn
	gosub _LF
	return
'-----------------------------------------
_RF:
'sertxd ("_RF")
	paceto =paceC -paceby
	for spulse=paceat to paceto step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	paceto =paceC +paceby
	for spulse=paceat to paceto step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
Stand:	'stand with both feet flat
sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
_rollL:
'sertxd ("_rL")
	rollto =rollC -rollby
	for spulse=rollat to rollto step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	rollto =rollC +rollby
	for Spulse=rollat to rollto step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------
'=================================================================

[page top]

'Bambino-21
Sertxd("B21",cr)		'report program number @4800

#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-01	'Test eyeLEDs, whiskerLEDs and speaker
'Bambino-02	'Test InfraRed input
'Bambino-02a	'Test Eye light values
'Bambino-02b	'Test Whisker light values
'Bambino-03	'Play the inbuilt tunes using keys 1 to 4 and beep on 5
'Bambino-04	'Set servos to mid position	
'Bambino-05	'Calibrate servo mid positions and put values in EEPROM.
'Bambino-06	'Introduce _iStand:- initialise with feet in mid position
'Bambino-07	'Set rollR
'Bambino-08	'Set rollL
'Bambino-09	'Check rollR, rollL
'Bambino-10	'Introduction of _Stand
'Bambino-11	'Investigate effect of rollspeed;	watch roll for all three speeds
'Bambino-12	'Investigate effect of rollspeed;	watch Roll and Stand for all three speeds
'Bambino-13	'Check paceFL paceFR;	Investigate effect of pacespeed
'Bambino-14	'Check Fd
'Bambino-15	'Test Rt, Lt
'Bambino-15a	'Test T2L Turn to Light, T2D Turn to Dark
'Bambino-16	'Test Bk
'Bambino-17	'Creaton of sections ACTIONS, BEHAVIOURS, ACTS;
            	'Introduction of ACT parameter 'Ado'
'Bambino-17a 'Demo routine            	
'Bambino-18	'Walk Fd with obstacle avoidance using eyes from 02a
'Bambino-18a	'Walk Fd with drop-off avoidance using whiskers values from 02b
'Bambino-18b	'rewritten with autocalibration of down whiskers and reading of whiskers as subroutines
'Bambino-19	'If IR command then do it otherwise walk and avoid
'Bambino-20	'rewrite to incorporate rollC and paceC in EEPROM 254,255
'Bambino-21	'rewrite to clearer code
            	
'-----------------------------------------------------------------            	
'modified
'--------
'Start:
'DoIR -> DoIRcmnd
'NoIR changed to Walkabout:

'New
'---
'=================================================================
'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 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
Symbol KEY_TENT   = 54
Symbol KEY_VERT_CROSS = 37
Symbol KEY_VCROSS = 37		'synonym for easier coding
Symbol KEY_DIAG_CROSS = 20
Symbol KEY_XCROSS = 20		'synonym for easier coding
symbol IRkey1     =  0
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
'=================================================================
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 whiskerLedL_ =c.3    'low =>whisker LED on
symbol whiskerLedR_ =b.3    'low =>whisker LED on
symbol eyeR         =c.7		  	
symbol eyeL         =c.1		  	
symbol iIRin        =c.6
symbol oIRout       =b.2

'symbol general 	=b0
symbol flags	=w1
symbol B_flags     =b3
'symbol f_doIRcmnds  =bit29
symbol f_direction  =bit30	'1=forwards, 0=backwards    
symbol f_defaultB   =bit31	'defaultBehaviour 1=use default
symbol servosAt	=w2
symbol  rollat 	 =b4
symbol  paceat 	 =b5
symbol ServosTo	=w3
symbol  rollTo	 =b6	   
symbol  paceTo	 =b7
symbol Smoveby	=w4
symbol  rollby	 =b8
symbol  paceby     =b9
symbol Scentre	=w5
symbol  rollC	 =b10
symbol  paceC      =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 counters	=w7
symbol  spulse 	 =b14
symbol  iA 		 =b15
symbol temp       =w8
symbol  tempL      =b16
'symbol tempH      =b17

	
symbol sframe     =22	'time between servo pulses
symbol Ado		=b18	'parameter for ACTs
symbol obstacle   =b19  '%11 bitmap of obstacles  %LeftRight
symbol lightL     =b20  'light level Left
symbol lightR     =b21  'light level Right
symbol IRcmnd	=b22  'cmnd from IRremote
symbol cmnd		=b23  'Action to be done

'=================================================================
initialise:
	rollby    =15	'+ve right 
	paceby    =20	'+ve Left  foot forward
	rollspeed =1  	'4 is too fast, 0 won't move
	pacespeed =2   	'6 is about max for servos to keep up  
	rollC	    =150	'Default, 100 is roll to the left
	paceC	    =150	'Default, 100 is left foot back
	read 254,temp	'set in prog5
	if temp>130 and temp<170 then	'so servo is always between 100,200
	  rollC =temp
	 endif 
	read 255,temp	'set in prog5
	if temp>130 and temp<170 then	'so servo is always between 100,200
	  paceC =temp
	 endif 
	sertxd("rollC=",#rollC," paceC=",#paceC,cr)
	low eyeLedR_	'make output LED on
	low eyeLedL_	'make output LED on
	gosub _iStand
	f_defaultB =1	'defaultBehaviour On

start:
sertxd("S")
	Ado =4				'for RockAdo
	irin [50,Walkabout],iIRin,IRcmnd	'irin with timeout, no IR =>Walkabout
	gosub beep50                  'so we know IR received
	goto DoIRcmnd
'=================================================================
'Activities
'----------
DoIRcmnd:	
sertxd("IR")
	select IRcmnd
	  case KEY_POWER
	  	gosub rest
		gosub EyesON
		goto DoIRcmnd		'have already got IRcmnd
	  case KEY_UP
	  	gosub Fd
	  case KEY_DOWN
	  	gosub Bk
	  case KEY_RIGHT
	  	gosub Rt
	  case KEY_LEFT
	  	gosub Lt
	  case IRkey1
	  	gosub RockAdo
	  case KEY_0
		goto Start	
	 end select

	irin iIRin,IRcmnd 		'Get IRcmnd
	gosub beep50                  'so we know IR received
	goto DoIRcmnd			'on other codes

'-----------------------------------------	
Walkabout:
sertxd("W")
	obstacle =0
	gosub EyesOFF
EwhiskerR:
	readadc c.7,lightR
	low eyeLedR_			'eyeR on
	readadc c.7,tempL
	high  eyeLedR_			'eyeR off
	if lightR>tempL then		'why this happens I don't know
	  tempL =lightR				
	 endif	
	tempL =tempL -lightR
	if tempL<2 then EwhiskerL	'ignore
	obstacle =obstacle or 1 
	
EwhiskerL:
	readadc c.1,lightL
	low eyeLedL_			'eyeL on
	readadc c.1,tempL
	high  eyeLedL_			'eyeL off
	if lightL>tempL then		'why this happens I don't know
	  tempL =lightL				
	 endif	
	tempL =tempL -lightL
	if tempL<5 then Walk		'ignore
	obstacle =obstacle or %10 

Walk:	
	gosub B_fast
	b0 =obstacle 			'to use bits
	if bit0=0 then
	  low eyeLedR_
	 endif 
	if bit1=0 then
	  low eyeLedL_
	 endif 
	on obstacle gosub Fd,Lt,Rt,Bk 
	goto start                    'loop back to start for unknown
'=================================================================
'Behaviours
'----------
B_fast:
	rollspeed =3
	pacespeed =6
	return
	
B_turn:	
	rollspeed =1	'slow so foot doesn't bounce
	pacespeed =5 
	return	
'=================================================================
'Acts
'----

RockAdo:
	if f_defaultB=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

Rest:
	gosub _rollL
	gosub _iStand
	pwmout eyeLedR_,100,400 'active low so high Dutycycle for dim
	pwmout eyeLedL_,100,400 'active low so high Dutycycle for dim
_RestIRwait:
	high VoiceLed_
	irin iIRin,IRcmnd		'irin with no timeout
	gosub beep50            'so we know IR received
	if IRcmnd=KEY_POWER then _RestIRwait	'wait until different IRcmnd
	pwmout eyeLedR_,off	
	pwmout eyeLedL_,off
	return
	
'=================================================================
'Actions
'-------

Fd:
sertxd ("Fd")
	f_direction =1
	if f_defaultB=1 then gosub B_fast
	if paceat<paceC then _FdL 'else _FdL
_FdR:
'sertxd ("_FdR")
	gosub _rollL
	gosub _RF
	gosub Stand
	return
_FdL:
'sertxd ("_FdL")
	gosub _rollR
	gosub _LF
	gosub Stand
	return
'-----------------------------------------
Bk:
sertxd ("Bk")
	f_direction =0
	if f_defaultB=1 then gosub B_fast
	if paceat<paceC then _BkR 'else _BkL
_BkL:
	gosub _rollR
	gosub _RF
	gosub Stand
	return
_BkR:
	gosub _rollL
	gosub _LF
	gosub Stand
	return
'-----------------------------------------
Rt:
sertxd ("Rt")
	if paceat<paceC and f_direction=1 then gosub Fd	'need left foot forward
	if paceat<paceC and f_direction=0 then gosub Bk	'need left foot forward
	if f_defaultB=1 then gosub beep150
	if f_defaultB=1 then gosub B_turn
	gosub _RF
	return
'-----------------------------------------
Lt:
sertxd ("Lt")
	if paceat>paceC 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_defaultB=1 then gosub beep150
	if f_defaultB=1 then gosub B_turn
	gosub _LF
	return
'=================================================================
_RF:
'sertxd ("_RF")
	paceto =paceC -paceby
	for spulse=paceat to paceto step -pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
_LF:
'sertxd ("_LF")
	paceto =paceC +paceby
	for spulse=paceat to paceto step pacespeed
	  paceat =spulse
	  pulsout servoroll,rollat	'pulse servo hold in position
	  pulsout servopace,paceat	'pulse servo
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
Stand:	'stand with both feet flat
'sertxd (" Sd")
	if rollat<rollC then _StandR 'else _StandL
_StandL:
'sertxd ("_SL")
	for spulse=rollat to rollC step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
_StandR:
'sertxd ("_SR")
	for spulse=rollat to rollC step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
_rollL:
'sertxd ("_rL")
	rollto =rollC -rollby
	for spulse=rollat to rollto step -rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
	 
_rollR:
'sertxd ("_rR")
	rollto =rollC +rollby
	for Spulse=rollat to rollto step rollspeed
	  rollat =spulse
	  pulsout servoroll,rollat	'pulse servo
	  pulsout servopace,paceat	'pulse servo hold in position
	  pause sframe
	 next spulse
	return 
'-----------------------------------------
_iStand:
'initialise with feet in mid position
	for spulse=0 to 10
	  pulsout servopace,paceC	'pulse servo paceC
	  pulsout servoroll,rollC	'pulse servo rollC
	  pause 70				'pause 70ms - do it slowly
	 next spulse
	rollat =rollC
	paceat =paceC 
	return 
'-----------------------------------------
beep50:
	sound VoiceLed_,(50,10)
	return

beep150:
	sound VoiceLed_,(150,10)
	return

EyesOFF:
	high eyeLedR_
	high eyeLedL_
	return
	
EyesON:
	low eyeLedR_
	low eyeLedL_
	return
'=================================================================

[page top]
Revisions