'------------------------------------------------------
'  Name     : Serpentronic.bas                        
'  Compiler : PicBasic Pro - MicroEngineering Labs
'  Notes    : Complete control Program for the robot 
'           : snake. Mode select push-button switch
'           : allows the infrared sensor to be easily
'           : calibrated. The robot will stop and turn
'           : if an obstacle is encountered.                
'------------------------------------------------------

' PortA set as outputs
trisa = %00000000

' PortB set as outputs. pins 0-1 inputs   
trisb = %00000011   

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

led_left        VAR portA.2
led_right       VAR portA.3
piezo           VAR portA.4

cal_switch      VAR portB.0
ir_input        VAR portB.1
servo_1         VAR PORTB.2
servo_2         VAR PORTB.3
servo_3         VAR PORTB.7
servo_4         VAR PORTB.6
servo_5         VAR PORTB.5
servo_6         VAR PORTB.4


ir_count        VAR byte
temp            VAR byte
object_detect   VAR byte
num_samples     VAR byte
threshold       VAR byte
rand            VAR word
timer           VAR byte
temp1           VAR byte
i               VAR byte

look_right      VAR byte
look_left       VAR byte
turn_count      VAR byte

servo1          VAR BYTE
servo2          VAR BYTE
servo3          VAR BYTE
servo4          VAR BYTE
servo5          VAR BYTE
servo6          VAR BYTE

low led_left
low led_right

Low servo1
Low servo2
Low servo3
Low servo4
Low servo5
Low servo6

turn_count = 0
num_samples = 40
threshold = 25

'--------------------------------------------------
' create randon noises and flash LED's

       For temp1 = 1 to 5
         High led_left
         Low led_right 
         GoSub randomize
         Pause 50
       
         Low led_left
         High led_right
         GoSub randomize
         Pause 50 
       Next temp1       

       Low led_right
       
'--------------------------------------------------
' start main execution

start:

      If cal_switch = 1 then
         pause 50   
         release_calibrate:
         If cal_switch = 1 then 
            goto release_calibrate
         else   
            Sound piezo,[120,4,90,2,100,2,110,4]
            pause 50
            goto ir_cal
         endif              
      endif

gosub infrared

     if object_detect = 1 then
        high led_left  
        high led_right
        Sound piezo,[100,4,90,2]
        servo1 = 180
        gosub servo
        servo1 = 120
        gosub servo
        turn_count = turn_count + 1
           if turn_count.0 = 1 then
              gosub slide_right
           else         
              gosub slide_left
           endif
                         
     endif
          
        low led_left        
        low led_right
        
gosub forward

goto start

'Subroutines start here

'--------------------------------------------------
' slither forward routine in a sine wave pattern

forward:
   servo1 = 157
   servo2 = 210
   servo3 = 143
   servo4 = 100
   servo5 = 157
   servo6 = 210
   GoSub servo
   servo1 = 143
   servo2 = 100
   servo3 = 157
   servo4 = 210
   servo5 = 143
   servo6 = 100
   GoSub servo
return

'--------------------------------------------------
' right turn movement routine

slide_right:

For temp1 = 1 to 3
   servo1 = 150
   servo2 = 210
   servo3 = 150
   servo4 = 100
   servo5 = 150
   servo6 = 210
   GoSub servo
   servo1 = 190
   servo2 = 100
   servo3 = 190
   servo4 = 210
   servo5 = 190
   servo6 = 100
   GoSub servo
next temp1
return

'--------------------------------------------------
' left turn movement routine

slide_left:

For temp1 = 1 to 3
   servo1 = 150
   servo2 = 210
   servo3 = 150
   servo4 = 100
   servo5 = 150
   servo6 = 210
   GoSub servo
   servo1 = 100
   servo2 = 100
   servo3 = 100
   servo4 = 210
   servo5 = 100
   servo6 = 100     
   GoSub servo
Next temp1
return

'--------------------------------------------------
' random sound generator subroutine

randomize:

Random rand
i = rand & 31 + 64
Sound piezo,[i,4]
Return

'--------------------------------------------------
' infrared detection subroutine 

infrared:

     ir_count = 0
     object_detect = 0

     for temp = 1 to num_samples             
        if ir_input = 0 then          
        ir_count = ir_count + 1    
        endif
     next
         
     if ir_count >= threshold then  
     object_detect = 1       
     endif

return     

'--------------------------------------------------
' subroutine to calibrate I.R. sensors

ir_cal:                       
                       
      If ir_input = 0 then     
          high led_left       
          high led_right
      endif
          low led_left
          low led_right   
      
      If cal_switch = 1 then
         pause 50
         button_release:
         If cal_switch = 1 then 
            goto button_release
         else   
            Sound piezo,[120,4,90,2,100,2,110,4]
            pause 50
            goto start
         endif
      endif
      
goto ir_cal

'--------------------------------------------------
' subroutine to set servos

servo:                    

       For timer = 1 to 20
       PulsOut servo_1,servo1
       PulsOut servo_2,servo2
       PulsOut servo_3,servo3
       PulsOut servo_4,servo4
       PulsOut servo_5,servo5
       PulsOut servo_6,servo6
       Pause 12
       Next timer

Return