'Simple-Sumo V1.0 
'written by Chris H.
'
' you need to have the sensors wired as in the CERc V1.0 schematics
' the motors are using the recycled RC-servo control boards
' the RC-servo boards look like a servo tot the CERc board
' the motors are wired to P6 and P6 of the BX chip
'
' Ram for our servo motor task to run with
Dim Task_Ram_Space(1 to 40) As Byte 

' Pin constants for the sensors
Const R_IR_Pin As Byte = 15
Const L_IR_Pin As Byte = 16

Const R_LineS_Pin As Byte = 13
Const L_LineS_Pin As Byte = 14

' Constant for pin high, something was detected
Const Detection As Byte = 0

' Constants for servo motor directions
Const R_Wheel_Halt As Integer = 1380 ' About 1.5 ms
Const L_Wheel_Halt As Integer = 1380 ' About 1.5 ms
Const R_Wheel_Forward As Integer = 2000 '
Const L_Wheel_Forward As Integer = 760 '
Const R_Wheel_Backward As Integer = 760 '
Const L_Wheel_Backward As Integer = 2000 '

' Values to Servo motors
Dim R_Wheel As Integer
Dim L_Wheel As Integer

' Value for combined IR sensor data
Dim Action As Byte



' Main program loop. Reads the sensors and updates the
' Servo postions maintained by the servo task
' *******************************************************
sub main()


Call Sleep(5.0)	' On power up or reset wait for 5 seconds


' Start the servo motor task
CallTask "Drive_System_Task", Task_Ram_Space



Do

Action = 0		' Set IR sensor data to 0

Action = GetPin(R_IR_Pin)	' Set Actions bit0 to 1 if right sensor sees something
Action = Action Or (GetPin(L_IR_Pin) * 2) ' Set Actions bit1 to 1 if right sensor sees something

' Read the combined IR sensor data to determine what actions to take, action can only be 0,1,2 or 3
Select Case Action

Case 0	' Both IR sensors see nothing
' Enemy robot not detected turn right and look for him
R_Wheel = R_Wheel_Backward
L_Wheel = L_Wheel_Forward

Case 1	' Right IR sensor sees something
' Enemy robot detected on right, vear right
R_Wheel = R_Wheel_Halt
L_Wheel = L_Wheel_Forward

Case 2	' Left IR sensor sees something
' Enemy robot detected on left, vear left
R_Wheel = R_Wheel_Forward
L_Wheel = L_Wheel_Halt

Case 3	' Both IR sensors sees something
' Enemy robot detected strait ahead, attack!
R_Wheel = R_Wheel_Forward
L_Wheel = L_Wheel_Forward

End Select



' Before the new servo motor values take affect read the 
' line sensors and if need be override the motor values that
' were set above

If GetPin(R_LineS_Pin) = Detection Then
L_Wheel = L_Wheel_Backward
R_Wheel = R_Wheel_Backward
Call Sleep(2.0)
End If

If GetPin(L_LineS_Pin) = Detection Then
L_Wheel = L_Wheel_Backward
R_Wheel = R_Wheel_Backward
Call Sleep(2.0)
End If

' Let the new servo motor values have time to take affect
Call Sleep(50)


' Show me what the sensors read. Make sure to open the "monitor port"
' at 19,200 to veiw.

Debug.Print "Val= " & CStr(Action) ' Print "Val= " and the contents of "Action" in ASCII



Loop


End Sub




' *******************************************************
' This task creates the pulses needed to drive the servo motors
' Task is called once and will continue to run in background 

Sub Drive_System_Task()

Const Right_Wheel_Pin As Byte = 6
Const Left_Wheel_Pin As Byte = 5

Do

	Call PulseOut(Right_Wheel_Pin, R_Wheel, 1)
	Call PulseOut(Left_Wheel_Pin, L_Wheel, 1)

	Call Sleep(15)

Loop


End Sub