'-----------------------------------------------------------------
'  Name     : Frogbotic.bas                        
'  Compiler : PicBasic Pro MicroEngineering Labs
'  Notes    : Program to coordinate the jumping of a robotic frog
'-----------------------------------------------------------------                 

' set porta to inputs
trisa = %11111111

' set portb pins 2 & 3 to inputs 
trisb = %00001100                   

'----------------------------------------------------------------- 
' initialize variables

servo_pos_l   VAR BYTE
servo_pos_r   VAR BYTE
timer1        VAR BYTE
timer2        VAR BYTE
timer3        VAR BYTE
temp1         VAR BYTE

servo_r       VAR PORTB.5
servo_l       VAR PORTB.6
switch_r      VAR PORTA.4
switch_l      VAR PORTA.3
led_l         VAR PORTB.1
led_r         VAR PORTB.0
piezo         VAR PORTB.4

low servo_l
low servo_r

'----------------------------------------------------------------- 

start:

for temp1 = 1 to 5
   SOUND piezo, [80,4,100,2]
   low led_l
   low led_r
   pause 50
   high led_l
   high led_r
   next temp1
   SOUND piezo, [100,4,120,2,80,2,90,2]
   low led_l
   low led_r

right:

if switch_r = 1 then 
   high led_r
   servo_pos_r = 158
   gosub right_servo
else
   low led_r
   servo_pos_r = 200
   gosub right_servo
endif

left:

if switch_l = 1 then
   high led_l
   servo_pos_l = 152
   gosub left_servo
else
   low led_l
   servo_pos_l = 100
   gosub left_servo
endif

if switch_l = 1 and switch_r = 1 then
   for temp1 = 1 to 6
   servo_pos_l = 150
   servo_pos_r = 159
   gosub both_servo
   next temp1
   servo_pos_l = 100
   servo_pos_r = 200
   gosub both_servo
endif

goto right

'----------------------------------------------------------------- 
' subroutines to set servos

both_servo:                           
       for timer1 = 1 to 15
       pulsout servo_l,servo_pos_l
       pulsout servo_r,servo_pos_r
       pause 6
       next timer1
return

left_servo:
      for timer2 = 1 to 10
      pulsout servo_l,servo_pos_l
      pause 6
      next timer2
return

right_servo
     for timer3 = 1 to 10
     pulsout servo_r,servo_pos_r
     pause 6
     next timer3
return

end