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 MotorStack(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 


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, "MOTOR TEST CODE") 
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


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

ServoLT = 0.0015 
ServoRT= 0.0015 



	call delay(2.0)  

call forward

'DONT FORGET TO CHECK THE 'P' COLUMN FOR PINS 10 AND 15 TO ACTIVATE THE TOUCH SENSORS

DO 

        'When the right bump switch is closed,  the GetPin(10)
        'will return a 0

           If GetPin(10) = 0 Then
                'Turn Left for 1.5 seconds then go forward again

                Call left
                Call Delay(1.5)
        		  Call forward

        'When the left bump switch is closed,  the GetPin(15)
        'will return a 0

           ElseIf GetPin(15) = 0 Then
				  'Turn Right for 1.5 seconds then go forward again
                Call Right                
                Call Delay(1.5)
		       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