'------------------------------------------------------
'  Name     : Snake-test.bas                        
'  Compiler : PicBasic Pro - MicroEngineering Labs
'  Notes    : Program to test the main controller 
'           : board by flashing the LED's, producing 
'           : sounds and setting each of the servos to
'           : their middle positions               
'------------------------------------------------------

' 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
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
rand            VAR WORD
timer           VAR BYTE 
temp1           VAR BYTE
i               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

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

       For temp1 = 1 to 7
         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:

   servo1 = 150
   servo2 = 150
   servo3 = 150
   servo4 = 150
   servo5 = 150
   servo6 = 150
   
   GoSub servo

goto start

'Subroutines start here

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

randomize:

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

'--------------------------------------------------
' 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