Attribute VB_Name = "Module1" 

Option Explicit 

'Below will create space called Buffers so the processor can print data to the Download Window in BasicX 

Public OutBuffer(1 To 50) As Byte 

Public InBuffer(1 To 50) As Byte 

'BLUE TEXT MEANS ADDED SUMO SENSOR CODE.

Dim VoltageReading As Single
Public RANGE as Single
Public VRFive as Single  

Const DIRPIN As Byte = 14 '(you can use 13-20, this is the pin where the white wire is Plugged)


Const LT_DOWN_LED as Byte = 7 
Const LT_DOWN_SENSOR as Byte = 8 
Const RT_DOWN_LED as Byte = 16
Const RT_DOWN_SENSOR as Byte = 17

Const BLACK as Byte = 1 
Const WHITE as Byte = 0



Dim MotorStack(1 To 40) As Byte 
Dim SensorStack(1 To 40) As Byte

'Created below are 2 Global Variables to store motor speed info for the servo motors. 

	Public ServoLT As Single 

	Public ServoRT As Single 

	Public StatusRight as byte 

	Public StatusLeft as byte 


Sub Main() 'All programs have ONLY one Sub Main. This is where the IF statement that controls behavior of 'your RawBot is placed. 

' The next three commands established communication with the computer. 
' First, it establishes buffers, and then it opens communications 
' to transmit the contents to Com 1 at 19200 baud rate. 

Call OpenQueue(OutBuffer, 50) 
Call OpenQueue(InBuffer, 50) 
Call OpenCom(1, 19200, InBuffer, OutBuffer) 

'Print out the words "RAWBOTS RULE" and then a carriage return and line feed 

Call PutQueueStr(OutBuffer, "RAWBOTS RULE") 
Call PutQueueStr(OutBuffer, Chr(13)) 
Call PutQueueStr(OutBuffer, Chr(10)) 

' Below starts a background task that continually pulses electronic 
' pulses to the servo motors. The Servo 
' Motors have an internal 
' circuit that monitors these pulses to determine which direction 
' to rotate and at what 'speed. This background task runs on its 
' own while the main code runs at the same time. 

CallTask "Motor", MotorStack
Calltask "sensor", Sensorstack 'starts the back ground task for sonar reading

'Below sets the Servo Variables in an initial state of DO NOT MOVE for 2 seconds. 

ServoLT = 0.0015 
ServoRT= 0.0015 

statusright = 0
statusleft = 0



	call delay(5.0)   'THIS USE TO BE 2.0

' Below calls the subroutine to start your robot spinning to look for other robot.

	call LEFT  'THIS USE TO BE FORWARD

' We now enter the Main DO LOOP with an IF statement inside it. 
' The program starts at the DO of DO LOOP and repeats any code inside 
' the Loop over and over until the chip is switched off. This means the 
' IF statement is repeated over and over. The IF statement is the BEHAVOIR 
' of our RawBot. If the line of the IF statement is true, it enters that portion 
' of the IF statement and executes the code inside. 

DO 

'NOTE  BE SURE TO COMMENT OUT THE DEBUG PRINT LINES BELOW WHEN IN ACTAUL SUMO 'COMPETITION SO AS NOT TO SLOW DOWN THE RUNNING OF THE CODE WHEN IN BATTLE.

Debug.print " DIR = "; cstr(RANGE)  'prints the value of the Distance Sensor to the screen
 
	delay(0.01) 

debug.print " status right = "; cstr(statusright) 

	call delay(0.01) 

debug.print " status left = "; cstr(statusleft) 

	call delay(0.1)


if (statusright = 1) then  'white line is found, stop, backup and spin again to find other robot

	call halt
	call delay(0.2)
	call reverse
	call delay(0.5)
	call left 

elseif (statusleft = 1) then  ''white line is found, stop, backup and spin again to find other robot

	call halt
	call delay(0.2)
	call reverse
	call delay(0.5)
	call right


elseif range > 10.0 Then 'Turns Left looking for other robots. 

	Call left 
 

ElseIf range < 10.0 Then ' the other robot is found, go forward and push it! 
	
	Call forward

End If 

Loop 

End Sub 

'Main program is now closed ... below are the Subroutines 

Sub Forward() 'Remember the pulse rate to say STOP for the Motors is 0.0015.

' If you add 0.0002 to this rate it moves in one 
' direction at full 
' speed and if you subtract 0.0002 from this rate, you move in the 
' other direction at full speed 

	ServoLT = 0.0015 + 0.0002 
	ServoRT = 0.0015 - 0.0002 

End Sub 

Sub Reverse() 

	ServoLT = 0.0015 - 0.0002 
	ServoRT = 0.0015 + 0.0002 

End Sub 

Sub Right() 

	ServoLT = 0.0015 - 0.0002 
	ServoRT = 0.0015 - 0.0002 

End Sub 

Sub Left() 

	ServoLT = 0.0015 + 0.0002 
	ServoRT = 0.0015 + 0.0002 

End Sub 

Sub halt() 

	ServoLT = 0.0015 
	ServoRT = 0.0015 

End Sub 




Sub Motor()
 'This task runs in the background sending pulses to the left and right servos. The global variable ServoLT 'controls the left servo, and the global variable ServoRT controls the right servo. When ServoLT and ServoRT are 'set to 1.5e-3, then the wheels are stopped. By changing the pulse width greater than or less , the motors turn. 

Do ' Send a pulse on pin 12 for the left servo.


	Call PulseOut(12, ServoLT, 1) ' This is to produce a pulse rate of about 50 Hz. 

	Call Delay(0.01) ' Send a pulse on pin 13 for the right servo. 

	Call PulseOut(13, ServoRT, 1) ' This is to produce a pulse rate of about 50 Hz. 

	Call Delay(0.01) 

Loop 

End Sub



Sub sensor() 

Do 

	     Call GetADC(DIRPIN, VoltageReading)
		  
     VRFive = voltagereading*5.0      'multiplies voltage reading by 5
	 RANGE = ((34.633/VRFive) - 5.162)/2.54      'gives distance in inches
		   
	 Call Delay(0.1)


'START INFRARED ******************************* 

IF GetPin(RT_DOWN_SENSOR) = 0 THEN 

	Statusright = black 

	call delay(0.01) 

Elseif getpin(RT_DOWN_SENSOR) = 1 THEN 

	Statusright = white 
	call delay(0.01) 

end if

if getpin(LT_DOWN_SENSOR) = 0 THEN 

	Statusleft = black 
	call delay(0.01) 

Elseif getpin(LT_DOWN_SENSOR) = 1 THEN 

	Statusleft = white
	call delay(0.01)

end if

loop

end sub