; Template file for a PIC16C84 SIL program
; Version 0.806


#INCLUDE PIC16C71.H

VAR PORTA[5], PORTB[6], PCLATH[0A] : BYTE

; Detector LED Definitions
CONST ON_TABLE = b'0'
CONST OFF_TABLE = b'1'
VAR PORTB[5] BITS RIGHT_LED[7], CENTER_LED[6], LEFT_LED[5], NA0[4], NA1[3], Freq_Pin[2],  Servo_Drive[1], Servo_Steer[0] : BYTE
VAR Led_Jmp BITS RLED[2], CLED[1], LLED[0] : BYTE

; Defines Timer 0 for RANDOM number generator
VAR TIM0[1] : BYTE

; Steering Servo Definitions
 ;NOTE: Each delay is 3MicroSec, so we do each of these three times
        ; (or we miltiply them by 9)
	;300 Microseconds
CONST LEFT_90 = d'33'
	;2160 Microseconds
CONST RIGHT_90 = d'240'
	;750 Microseconds
CONST LEFT_45 = d'83'
	;1680 Microseconds
CONST RIGHT_45 = d'187'
	;1210 Microseconds
CONST STRAIGHT = d'134'
	;2295 Microseconds
CONST TURN_TIME = d'255'
VAR Steer_Angle : BYTE

; Drive Motor Definitions
 ;NOTE: Each delay is 3MicroSec, so we do each of these three times
        ; (or we miltiply them by 9)
	;2000 Microseconds
CONST FWD_FAST = d'222'
	;1780 Microseconds
CONST FWD_SLOW = d'198'
	;300 Microseconds
CONST REV_FAST = d'33'
	;1700 Microseconds
CONST REV_SLOW = d'188'
	;1730 Microseconds
CONST STOP_HARD = d'192'
VAR Drive_Speed : BYTE

; Random Number Stuff
VAR Random_Number : BYTE

; General Counter
VAR Counter : BYTE
VAR Wait_Ctr1, Wait_Ctr2 : BYTE

; -------------------------------------------------
; These are used by the interrupt service routine
; -------------------------------------------------
; Counts the number of times through the Interrupt Service Routine
; CONST Set us up to go through the interrupt routine x times
VAR Int_Counter : BYTE
CONST INT_DIVIDER = d'5'

; Delay Temp Variable
VAR Delay_Temp : BYTE



MAIN()
; Disable Interrupts 
; Inititalize Variables
; Set inputs and outputs up, and deassert outputs (relay)
INIT_CODE:
	DISABLE_INTERRUPTS
        DIRECTION_A(b'11111')
        DIRECTION_B(b'11110000')
	DISABLE_EXTIRQ
	DISABLE_PORTBIRQ
	CLRWDT
	SELECT_TIMER
	PRESCALER_TMR0(1/16)
        SELECT_TIMER_PRESCALER
	Servo_Drive := 0
	Servo_Steer := 0
	Freq_Pin := 0
	Int_Counter := INT_DIVIDER
	Steer_Angle := STRAIGHT
	Drive_Speed := STOP_HARD
	ENABLE_T0IRQ
	ENABLE_INTERRUPTS
  	Call Wait_1Sec

READ_SWITCHES
        Call Get_Leds
	JUMP_TABLE Led_Jmp 
; make sure jump is checked for valid
	GOTO Go_Straight
	GOTO Go_Right_90
	GOTO Go_Straight
	GOTO Go_Right_90
	GOTO Go_Left_90
	GOTO Go_Backup
	GOTO Go_Left_90
	GOTO Go_Backup
	GOTO READ_SWITCHES

Go_Straight
  Drive_Speed := STOP_HARD
  Steer_Angle := STRAIGHT
  Call Wait_Half_Sec
  Drive_Speed := FWD_FAST
  Call Get_Leds
  WHILE Led_Jmp = 0 DO
    Call Get_Leds
  ENDWHILE
  GOTO Quick_Abort

Go_Left_90
  IF LEFT_LED = ON_TABLE THEN
  ELSE
    GOTO Quick_Abort
  ENDIF
  Drive_Speed := STOP_HARD
  Steer_Angle := LEFT_90
  Call Wait_Half_Sec
  Call Get_Random
  Drive_Speed := FWD_SLOW
  WHILE Random_Number # 0 DO
    Counter := h'ff'
    WHILE Counter > 0 DO
      IF LEFT_LED = ON_TABLE THEN
      ELSE
        GOTO Quick_Abort
      ENDIF
      DEC(Counter)
    ENDWHILE
    DEC(Random_Number)
  ENDWHILE
  GOTO READ_SWITCHES

	
Go_Right_90
   IF RIGHT_LED = ON_TABLE THEN
   ELSE
     GOTO Quick_Abort
   ENDIF
  Drive_Speed := STOP_HARD
  Steer_Angle := RIGHT_90
  Call Wait_Half_Sec
  Call Get_Random
  Drive_Speed := FWD_SLOW
  WHILE Random_Number # 0 DO
    Counter := h'ff'
    WHILE Counter > 0 DO
      IF RIGHT_LED = ON_TABLE THEN
      ELSE
        GOTO Quick_Abort
      ENDIF
      DEC(Counter)
    ENDWHILE
    DEC(Random_Number)
  ENDWHILE
  GOTO READ_SWITCHES

	
Go_Backup
  Drive_Speed := STOP_HARD
  Steer_Angle := STRAIGHT
  Call Wait_Half_Sec
  Drive_Speed := REV_SLOW
  Counter := d'2'
  Call Wait_1Sec
  Call Wait_1Sec
  GOTO Go_Left_90
   


Quick_Abort
  Drive_Speed := STOP_HARD
  Call Wait_1Sec
  GOTO READ_SWITCHES

ENDMAIN

PROCEDURE Wait_Half_Sec()
  Counter := d'255'
  WHILE Counter > 0 DO
    DEC(Counter)
    Wait_Ctr1 := h'ff'
    WHILE Wait_Ctr1 > 0 DO
       DEC(Wait_Ctr1)
    ENDWHILE
  ENDWHILE
  Counter := d'63'
  WHILE Counter > 0 DO
    DEC(Counter)
    Wait_Ctr1 := h'ff'
    WHILE Wait_Ctr1 > 0 DO
       DEC(Wait_Ctr1)
    ENDWHILE
  ENDWHILE
ENDPROC

PROCEDURE Wait_1Sec()
  Wait_Ctr2 := 2
  WHILE Wait_Ctr2 > 0
     DEC(Wait_Ctr2)
     CALL Wait_Half_Sec
  ENDWHILE
ENDPROC

PROCEDURE Get_Leds()
  Led_Jmp := 0
  RLED := RIGHT_LED
  CLED := CENTER_LED
  LLED := LEFT_LED
ENDPROC

; This is a random number generator used to give
; Random 8 bit numbers for movement variation
; (random based on random number in the timer)
PROCEDURE Get_Random
  Random_Number := TIM0
ENDPROC


T0IRQ_PROC
; Later this will be used to constantly drive the servos
; This does the drive pulse width
  IF Int_Counter = 1 THEN
    Servo_Drive := 1 
    Delay_Temp := Drive_Speed
    DELAY(Delay_Temp)
    Delay_Temp := Drive_Speed
    DELAY(Delay_Temp)
    Delay_Temp := Drive_Speed
    DELAY(Delay_Temp)
    Servo_Drive := 0
    XOR(PORTB, b'00000100')
; This does the steering pulse width
    Servo_Steer := 1 
    Delay_Temp := Steer_Angle
    DELAY(Delay_Temp)
    Delay_Temp := Steer_Angle
    DELAY(Delay_Temp)
    Delay_Temp := Steer_Angle
    DELAY(Delay_Temp)
    Servo_Steer :=  0
    Int_Counter := INT_DIVIDER
    GOTO TOIRQ_END
  ELSE
    DEC(Int_Counter)
  ENDIF
TOIRQ_END
ENDT0IRQ

EXTIRQ_PROC
; Body of ext. INT interrupt service routine
; Delete if not needed to save memory.
ENDEXTIRQ

PORTBIRQ_PROC
; Body of PORTB change interrupt service routine
; Delete if not needed to save memory.
ENDPORTBIRQ