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


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


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

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


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

Left_Motor_F    	VAR     RA.0
Left_Motor_R    	VAR     RA.1
Right_Motor_F   	VAR     RA.2
Right_Motor_R   	VAR     RA.3

Front_IR_Emit_L		VAR     RB.0
Front_IR_Emit_R		VAR     RB.1

'inputs
Auto_Enable		VAR	RB.2
Front_IR_Detect		VAR	RB.4

' Status leds for debug
Left_Motor_F_Status	VAR	RC.5
Left_Motor_R_Status	VAR	RC.4
Right_Motor_F_Status	VAR	RC.3
Right_Motor_R_Status	VAR	RC.2

IR_Emit_Status		VAR     RC.1

Front_IR_Detect_Left_Status	VAR	RB.7
Front_IR_Detect_Center_Status	VAR	RB.6
Front_IR_Detect_Right_Status	VAR	RB.5


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

Yes             CON     0		' ir sensor logic is flipped 0v is detect
No              CON     1
Motor_Delay	CON	5000		'#ms long to leave the motors on 
Loop_Delay	CON	1000		'#ms to delay logic loop
'Motor_Delay	CON	1		'for sxsim #ms long to leave the motors on 
'Loop_Delay	CON	1		'for sxsim #ms to delay logic loop
'
' -------------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------------
ir_in_left		VAR	Word
ir_in_right		VAR	Word
ir_in_center		VAR	Word
Front_IR_Emit	        VAR     Word
tmpW1			VAR	Word
' -------------------------------------------------------------------------
'  INTERRUPT
' -------------------------------------------------------------------------
INTERRUPT NOPRESERVE 76_000
  IF Front_IR_Emit=1 THEN
      Front_IR_Emit_L = ~Front_IR_Emit_L   'Toggle pin state
  ENDIF
  IF Front_IR_Emit=2 THEN
  	Front_IR_Emit_R = ~Front_IR_Emit_R   'Toggle pin state
  ENDIF
RETURNINT

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

Pgm_ID:
  DATA  "Explorer", 0


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

DELAY_MS           SUB     1, 2                    ' delay in milliseconds

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

Start:
  ' initialization code here
  TRIS_A = %0000                            ' make input or output (0=output 1=input)
  PLP_A  = %1111                            ' 0=pulls-up unused pins (unused should be inputs)
  TRIS_B = %00010100                        ' 2,4 = input, the rest are outputs
  PLP_B  = %11111111			    ' no pullups
  TRIS_C = %00000000			    ' all outputs
  PLP_C  = %11111111			    ' no pullups

  'Default coast mode
  Left_Motor_F = 0
  Left_Motor_R = 0
  Right_Motor_F = 0
  Right_Motor_R = 0

  Left_Motor_F_Status = 0
  Left_Motor_R_Status = 0
  Right_Motor_F_Status = 0 
  Right_Motor_R_Status = 0
  IR_emit_Status = 0

  Front_IR_Detect_Left_Status = 0
  Front_IR_Detect_Center_Status = 0
  Front_IR_Detect_Right_Status = 0


Main:
  DO
     DELAY_MS Loop_Delay

     ' clear all status leds
     Left_Motor_F_Status = 0
     Left_Motor_R_Status = 0  
     Right_Motor_F_Status = 0
     Right_Motor_R_Status = 0  
     Front_IR_Detect_Center_Status = 0
     Front_IR_Detect_Left_Status = 0
     Front_IR_Detect_right_Status = 0

     ' read IR detector status into a variable (so it dosn't change mid loop)
     Front_IR_Emit=1			'output 38khz wave to left emittor
     DELAY_MS 1				'give it time to start the pulses
     IR_IN_LEFT = Front_IR_Detect	'read result into variable
     Front_IR_Emit_L=0			'force the led off (other pin is ground)
     Front_IR_Emit=2			'output 38khz wave to right emittor
     DELAY_MS 1
     IR_IN_RIGHT = Front_IR_Detect 
     Front_IR_Emit_R=0			'force the led off (other pin is ground)
     Front_IR_Emit=0			'turn emittor off
     IR_IN_CENTER = IR_IN_LEFT + IR_IN_RIGHT      'sensor is 0v for detect so 0+0=0= yes
    

     IF IR_IN_CENTER = yes THEN 	' yes is 0, 0+0 = 0 = yes ...nice eh?
	IR_IN_LEFT = no			'its a center, clear for below
	IR_IN_Right= no			'i could do an if else, but blah
	' oh no, both detectors see something, run away!
	Front_IR_Detect_Center_Status = 1          
        IF auto_enable = yes THEN
        	Left_Motor_R_Status  = 1
        	Right_Motor_R_Status  = 1
		Left_Motor_R = 1
		Right_Motor_R = 1
		DELAY_MS Motor_Delay
		Left_Motor_R = 0    
		Right_Motor_R = 0
	ENDIF
     ENDIF		

     IF ir_in_left = yes THEN
	Front_IR_Detect_Left_Status = 1          
        IF auto_enable = yes THEN
        	Left_Motor_F_Status  = 1
        	Left_Motor_R_Status  = 0
        	Left_Motor_F = 1
		Left_Motor_R = 0
		DELAY_MS Motor_Delay
		Left_Motor_F = 0    
        	Left_Motor_R = 0
	ENDIF
     ENDIF

     IF ir_in_right = yes THEN
	Front_IR_Detect_right_Status = 1          
        IF auto_enable = yes THEN
        	right_Motor_F_Status  = 1
        	right_Motor_R_Status  = 0
        	right_Motor_F = 1
		right_Motor_R = 0
		DELAY_MS Motor_Delay
		right_Motor_F = 0    
        	right_Motor_R = 0
	ENDIF
     ENDIF
  
  Loop
GOTO Main


' -------------------------------------------------------------------------
' Subroutine Code
' -------------------------------------------------------------------------
' Use: DELAY_MS msecs

DELAY_MS:
  IF __PARAMCNT = 1 THEN
    tmpW1 = __PARAM1                            ' save byte value
  ELSE
    tmpW1 = __WPARAM12                          ' save word value
  ENDIF
  PAUSE tmpW1 
RETURN