' =========================================================================
'
'   File...... explorer.sxb
'   Purpose... explorer robot
'   Author.... David Mcanulty
'   E-mail.... dave000@hellspark.com
'
' =========================================================================


' -------------------------------------------------------------------------
' Program Description
' -------------------------------------------------------------------------


' -------------------------------------------------------------------------
' Device Settings
' -------------------------------------------------------------------------

DEVICE 		SX28,TURBO,BANKS8,OSCHS3,SYNC,OPTIONX
`DEVICE         SX28, OSC4MHZ, TURBO, STACKX, OPTIONX
FREQ            50_000_000


' -------------------------------------------------------------------------
' IO Pins
' -------------------------------------------------------------------------

Left_Motor_A    	VAR     RA.0
Left_Motor_B    	VAR     RA.1
Right_Motor_A   	VAR     RA.2
Right_Motor_B   	VAR     RA.3
IR_emit_Left		VAR     RB.0
IR_emit_Right		VAR     RB.1
Front_IR_Detect		VAR	RB.4

Left_Motor_A_Status	VAR	RC.5
Left_Motor_B_Status	VAR	RC.4
Right_Motor_A_Status	VAR	RC.3
Right_Motor_B_Status	VAR	RC.2
IR_emit_Left_Status	VAR     RC.1
IR_emit_Right_Status	VAR     RC.0
Front_IR_Detect_Left	VAR	RB.7
Front_IR_Detect_Center	VAR	RB.6
Front_IR_Detect_Right	VAR	RB.5


' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------

Yes             CON     1
No              CON     0
Motor_Delay	CON	100		'#ms long to leave the motors on 

' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------


' -------------------------------------------------------------------------
  INTERRUPT
' -------------------------------------------------------------------------

' =========================================================================
  PROGRAM Start
' =========================================================================

Pgm_ID:
  DATA  "Explorer", 0


' -------------------------------------------------------------------------
' Subroutine Declarations
' -------------------------------------------------------------------------


' -------------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------------

Start:
  ' initialization code here
  TRIS_A = %0000                            ' make input or output (0=output 1=input)
  PLP_A  = %1111                            ' 0=pull-up unused pins (unused should be inputs)
  TRIS_B = %00011100                        ' 76543210
  PLP_B  = %11110011
  TRIS_C = %11000000
  PLP_C  = %00111111

  'Default coast mode
  Left_Motor_A = 0
  Left_Motor_B = 0
  Right_Motor_A = 0
  Right_Motor_B = 0

  Left_Motor_A_Status = 0
  Left_Motor_B_Status = 0
  Right_Motor_A_Status = 0 
  Right_Motor_B_Status = 0
  IR_emit_Left_Status = 0
  IR_emit_Right_Status = 0
  Front_IR_Detect_Left = 0
  Front_IR_Detect_Center = 0
  Front_IR_Detect_Right = 0


Main:
  DO
     IR_emit_Left_Status = 1
     FREQOUT IR_emit_Left, 100, 38000	    ' FREQOUT (Pin, Duration in ms, Freq in hz 1-65335) square wave output
     IR_emit_Left_Status = 0
     IF Front_IR_Detect = Yes THEN          ' yes is 0, see constants
	Front_IR_Detect_Left = 1
        Left_Motor_A = 1
	Left_Motor_B = 0
	pause Motor_Delay
	Left_Motor_A = 0    
        Left_Motor_B = 0  
	Front_IR_Detect_Left = 0
     ENDIF
     pause 500
     
     IR_emit_Right_Status = 1
     FREQOUT IR_emit_Right, 100, 38000	    ' FREQOUT (Pin, Duration in ms, Freq in hz 1-65335) square wave output
     IR_emit_Right_Status = 0
     IF Front_IR_Detect = Yes THEN          ' yes is 0, see constants
	Front_IR_Detect_Right = 1
        Right_Motor_A = 1
	Right_Motor_B = 0     
	pause Motor_Delay
	Right_Motor_A = 0    
        Right_Motor_B = 0  
	Front_IR_Detect_Right = 0
     ENDIF
     pause 500
  Loop
GOTO Main


' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------