'=================================================================================== 
' EV8 Hexapod demo 
' Code developed by Michael Gruber www.roboteshop.com 
' Date: 8th August, 2011 
' Function: 
'    1. If left and right IR sensor detect no obstacles - walk forward
'    2. If left and right IR sensor detect obstacles - walk backward
'    3. If only left IR sensor detect obstacles - turn right
'    4. If only right IR sensor detect obstacles - turn left
'=================================================================================== 


Const tempo_SPD	As Byte = 50    'tempo  
Const NOTE_NUM	As Byte = 10    '11 notes (size of array), adjust size for more notes
Const ServoC	As Byte = 0     'Servo I/O pin number (left)
Const IR		As Byte = 1     'IR LED I/O pin number
Const IRin_L	As Byte = 2     'Left IR sensor I/O pin number 
Const IRin_R	As Byte = 3     'Right IR sensor I/O pin number
Const ServoL    As Byte = 4     'Servo I/O pin number (right)
Const ServoR    As Byte = 5     'Servo I/O pin number (center)
Const LS		As Byte = 6     'Light Sensor (LS) I/O pin numbe
Const Buzzer	As Byte = 7     'Buzzer I/O pin number
'array of notes, increase the size of array for more notes 
Const Rhythm(NOTE_NUM) As Byte = {10,0,10,0,10,0,8,0,10,0,12}  
'array of note length, increase the size of array for more notes 
Const Duration(NOTE_NUM) As Byte = {2,1,2,4,2,4,2,4,2,1,5}

Dim time,gait,IR_L,IR_R,IRSelect,i As Byte
Dim Cpos,Lpos,Rpos,pulsepause,tempo As Word
Dim pulsecount As Float

Sub Main()
	Pause 600		'Wait 3 seconds
	Play_Notes()	'Play tune
'---------------------------------------------------------------------------
' main Loop 
'--------------------------------------------------------------------------- 
    Do
    	'If In(LS) = 1 Then						'check if ambient light is dark - if dark go to sleep.
        'Else	
        	Sound(IR, 5, 38500)					'sound command for 38.5kHz IR carrier  
        	IR_R = In(IRin_R)					'get right side IR sensor status 
        	IR_L = In(IRin_L)					'get left side IR sensor status
        	If IR_R = 1 And IR_L = 1 Then		'1 = no reflection, 0 = IR reflected off surface		
            	Forward()
            	'High ServoL
            	'High ServoR
        	Elseif IR_R = 0 And IR_L = 0 Then	'both sensors blocked, reverse
            	Backward()
            	'Low ServoL
            	'Low ServoR            	
        	Elseif IR_R = 0 And IR_L = 1 Then	'right sensor blocked, turn left            
            	Left()
            	'High ServoL
            	'Low ServoR  
        	Elseif IR_R = 1 And IR_L = 0 Then	'left sensor blocked, turn right            
            	Right()
            	'Low ServoL
            	'High ServoR 
        	End If
        'End If
    Loop
End Sub

Sub Forward()
	Initialise()
	SOUND (Buzzer,250,523)	'Do 1 C
	For gait = 1 To 2
		Cpos = 340
		Servo(Cpos, Lpos, Rpos)
		Lpos = 325
		Rpos = 325
		Servo(Cpos, Lpos, Rpos)
		Cpos = 260
		Servo(Cpos, Lpos, Rpos)
		Lpos = 275
		Rpos = 275
		Servo(Cpos, Lpos, Rpos)				
	Next gait
End Sub

Sub Backward()
	Initialise()
	SOUND (Buzzer,250,1047)	'Do High C
	For gait = 1 To 2
		Cpos = 340
		Servo(Cpos, Lpos, Rpos)
		Lpos = 275
		Rpos = 275
		Servo(Cpos, Lpos, Rpos)
		Cpos = 260
		Servo(Cpos, Lpos, Rpos)
		Lpos = 325
		Rpos = 325
		Servo(Cpos, Lpos, Rpos)				
	Next gait
End Sub

Sub Left()
	Initialise()
	SOUND (Buzzer,250,659)	'Mi 3 E
	For gait = 1 To 2
		Cpos = 340
		Servo(Cpos, Lpos, Rpos)
		Lpos = 275
		Rpos = 325
		Servo(Cpos, Lpos, Rpos)
		Cpos = 260
		Servo(Cpos, Lpos, Rpos)
		Lpos = 325
		Rpos = 275
		Servo(Cpos, Lpos, Rpos)				
	Next gait
End Sub

Sub Right()
	Initialise()
	SOUND (Buzzer,250,785)	'Sol 5 G
	For gait = 1 To 2
		Cpos = 340
		Servo(Cpos, Lpos, Rpos)
		Lpos = 325
		Rpos = 275
		Servo(Cpos, Lpos, Rpos)
		Cpos = 260
		Servo(Cpos, Lpos, Rpos)
		Lpos = 275
		Rpos = 325
		Servo(Cpos, Lpos, Rpos)				
	Next gait
End Sub

Sub Initialise()
	Cpos = 300
    Lpos = 300
    Rpos = 300
    Servo(Cpos, Lpos, Rpos) 
End sub

Sub Servo(Cpos As Word,Lpos As Word, Rpos As Word)
	For time = 0 To 10
        Pulseout ServoC, Cpos
        Pulseout ServoL, Lpos
        Pulseout ServoR, Rpos
        '1 pulse = 5 microseconds
        'servos are positioned un 20 millisecond cycles
        '1 cycle = 20 milliseconds - 3x servo pulses)
        ' 300 pluses = 1500 microseconds = 1.5 milliseconds
       	pulsecount = (Cpos + Lpos + Rpos)*5
       	pulsecount = pulsecount / 1000
       	pulsecount = 20 - pulsecount
       	pulsepause = Float2word(pulsecount)
        Pause pulsepause 'pause to complete a 20ms cycle 
    Next time 	
End Sub

'--------------------------------------------------------------------------- 
' Routine to use the buzzer to play the notes 8
' The Select Case table contains frequency of notes for 2 octaves.  
' Add more notes in the table, if needed. 
'--------------------------------------------------------------------------- 
Sub Play_Notes() 
         
    For i=0 To NOTE_NUM             'number of notes to play 
        tempo = Duration(i)         'load the note length 
     
        Select Case Rhythm(i)       'play notes through Select Case table 
            Case 0
                Pause tempo_SPD*tempo                   'rest 
            Case 1
                Sound(Buzzer, tempo_SPD*tempo, 523)     'Do (C5) 
            Case 2
                Sound(Buzzer, tempo_SPD*tempo, 587)     'Re (D5) 
            Case 3
                Sound(Buzzer, tempo_SPD*tempo, 659)     'Mi (E5) 
            Case 4
                Sound(Buzzer, tempo_SPD*tempo, 698)     'Fa (F5) 
            Case 5   
                Sound(Buzzer, tempo_SPD*tempo, 784)     'Sol (G5) 
            Case 6
                Sound(Buzzer, tempo_SPD*tempo, 880)     'La (A5) 
            Case 7
                Sound(Buzzer, tempo_SPD*tempo, 987)     'Ti (B5)  
            Case 8
                Sound(Buzzer, tempo_SPD*tempo, 1047)    'Do (C6) 
            Case 9
                Sound(Buzzer, tempo_SPD*tempo, 1175)    'Re (D6) 
            Case 10
                Sound(Buzzer, tempo_SPD*tempo, 1319)    'Mi (E6) 
            Case 11
                Sound(Buzzer, tempo_SPD*tempo, 1397)    'Fa (F6) 
            Case 12
                Sound(Buzzer, tempo_SPD*tempo, 1568)    'Sol (G6) 
            Case 13
                Sound(Buzzer, tempo_SPD*tempo, 1760)    'La (A6) 
            Case 14
                Sound(Buzzer, tempo_SPD*tempo, 1976)    'Ti (B6) 
            Case 15
                Sound(Buzzer, tempo_SPD*tempo, 2093)    'Do (C7) 
        End Select
    Next i 
End Sub