Option Explicit

'////// Globals ///////
' Servo Pin Constants
Public Const LeftServo as Byte = 5	' Pin #0 on RAMB
Public Const RightServo as Byte = 6	' Pin #1 on RAMB

' Left Servo Pulse Width Constants
Public Const Left_Reverse_Fast as Single = 0.00082	' Min Left	
Public Const Left_Reverse_Slow as Single = 0.00128
Public Const Left_Center as Single = 0.00132	' Left Center 
Public Const Left_Forward_Slow as Single = 0.001359
Public Const Left_Forward_Fast as Single = 0.00182	' Max Left 

' Right Servo Pulse Width Constants
Public Const Right_Forward_Fast as Single = 0.00084	' Min Right
Public Const Right_Forward_Slow as Single = 0.001233	
Public Const Right_Center as Single = 0.00127	' Right Center 
Public Const Right_Reverse_Slow as Single = 0.00131
Public Const Right_Reverse_Fast as Single = 0.00142	' Max Right

' Photoresistor constants
Public Const PhotoCell as Byte = 13	' Pin #8 on RAMB
Public Const MaxLight as Integer = 90	' Greater than this and the robot starts

' Forward IR sensor constants
Public Const ForwardIR as Byte = 14	' Pin #9 on RAMB
Public Const Num2Avg as Integer = 10	' How many IR readings to average
Public Const MaxFDistance as Single = 25.0	' If object is within this distance (in cm), move forward and grab it!

' Right IR sensor constants
Public Const RightIR as Byte = 15	' Pin #10 on RAMB
Public Const MaxRDistance as Single = 20.0	' If table is far away...

' Left IR sensor constants
Public Const LeftIR as Byte = 16	' Pin #11 on RAMB
Public Const MaxLDistance as Single = 20.0	' If table is far away ...

' Gripper constants
Public Const GripperServo as Byte = 20	' #15 on RAMB
Public Const GripDepth as Single = -15.0 	' Stop short this much (in cm) so gripper can grip
Public Const Opened as Single = 0.0014		' PW of open gripper
Public Const Closed as Single = 0.0016		' PW of closed gripper

' Piezo Constants
Public Const Piezo as Byte = 12	' #7 on RAMB


Public Sub Main()
	Dim FRange as Single	' Forward Range (in cm)
	Dim LRange as Single
	Dim RRange as Single

	Call StartSequence		' Check to see if light is dark
	Do
		FRange = GetRange(ForwardIR, Num2Avg)	' get forward range of the object in front of IR sensor
		' Check to see if object is in front of Mouse
		If (FRange <= MaxFDistance) Then 
'			Debug.Print "Forward Range = " & CStr(FRange) & " cm"
			Call GrabSequence(FRange)
			Call ReverseF(5.0)
		End If

		LRange = GetRange(LeftIR, Num2Avg)	' get left range of the object in front of IR sensor
		' Check to see if object is in front of Mouse
		If (LRange > MaxLDistance) Then 
'			Debug.Print "Left Range = " & CStr(LRange) & " cm"
			Call RightS(30.0)
		End If

		RRange = GetRange(RightIR, Num2Avg)	' get right range of the object in front of IR sensor
		' Check to see if object is in front of Mouse
		If (RRange > MaxRDistance) Then 
'			Debug.Print "Right Range = " & CStr(RRange) & " cm"
			Call LeftS(30.0)
		End If

		Call PulseOut(LeftServo, Left_Forward_Fast, 1)
		Call PulseOut(RightServo, Right_Forward_Fast, 1)	
		Call Delay(0.02)	' A necessary delay
	Loop
End Sub


'//////// StartSequence ///////////
Public Sub StartSequence()
	Call PutPin(25, 0)		' turn red light on
	Do Until GetADC(PhotoCell) > MaxLight
		' wait until photocell goes dark
	Loop
	Call PutPin(25, 1)		' turn red light off
	Call PutPin(26, 0)		' turn green light on
End Sub


' /////// GetRange ///////
Public Function GetRange(ByVal PN as Byte, ByVal NOR as Integer) as Single
	Dim i as Integer	' our loop counter
	Dim SensorTotal as Integer	' the running sum
	Dim AvgSensor as Single	' the average reading

	' Make IR readings, compute the running total, and find the average:
	SensorTotal = 0	' initialize the variable
	For i = 1 to NOR
		' add the current sensor reading to the previous total:
		SensorTotal = SensorTotal + GetADC(PN)
	Next

	' Find and return the average of all readings:
	AvgSensor = CSng(SensorTotal) / CSng(NOR)

	' Calculate Range
	GetRange = (4187.8 / CSng(AvgSensor))^1.106	' in centimeters
End Function


' /////// GrabSequence ////////
Public Sub GrabSequence(ByVal R as Single)
	Call Gripper(Opened)
	Call ForwardF(R - GripDepth)	' Move forward, but stop short
	Call Gripper(Closed)
	Call DropIt
	Call DropSound
End Sub


' /////// Gripper ///////
Public Sub Gripper(ByVal PW as Single)
	Dim i as Integer
	For i = 1 to 17
		Call PulseOut(GripperServo, PW, 1)
		Call Delay(0.02)	' A necessary delay
	Next
End Sub


'///////// DropIt /////////
Public Sub DropIt()
	Dim LeftRange as Single, RightRange as Single
	Do
		LeftRange = GetRange(LeftIR, Num2Avg)
		RightRange = GetRange(RightIR, Num2Avg)
		Call PulseOut(GripperServo, Closed, 1)	' keep gripper closed
		If ((LeftRange > MaxLDistance) And (RightRange > MaxRDistance)) Then
			Call Gripper(Opened)
			Exit Sub
		ElseIf (LeftRange > MaxLDistance) Then
			Call PulseOut(RightServo, Right_Forward_Slow, 1)	
			Call Delay(0.02)	' A necessary delay
		ElseIf (RightRange > MaxRDistance) Then
			Call PulseOut(LeftServo, Left_Forward_Slow, 1)	
			Call Delay(0.02)	' A necessary delay
		Else
			Call PulseOut(LeftServo, Left_Forward_Fast, 1)
			Call PulseOut(RightServo, Right_Forward_Fast, 1)	
			Call Delay(0.02)	' A necessary delay
		End If
	Loop
End Sub


' //////// DropSound ////////
Public Sub DropSound()
	Dim i as Integer
	Dim t as Single
		t = 0.0
		For i = 0 to 40
			t = t + CSng(i) * 0.00008
			Call PutPin(12, 1)	' Turn on piezo
			Call Delay(t)
			Call PutPin(12,0)	' Turn off piezo
			Call Delay(t)
		Next
End Sub


' /////// ForwardF ///////
Public Sub ForwardF(ByVal Distance as Single)
	Dim NumOfPulses as Integer, i as Integer
	Const FF_ppcm as Single = 1.664	' Pulses per cm conversion

	NumOfPulses = CInt(Distance * FF_ppcm)
	For i = 1 to NumOfPulses
		Call PulseOut(LeftServo, Left_Forward_Fast, 1)
		Call PulseOut(RightServo, Right_Forward_Fast, 1)	
		Call Delay(0.02)	' A necessary delay
	Next
End Sub


' /////// ReverseF ///////
Public Sub ReverseF(ByVal Distance as Single)
	Dim NumOfPulses as Integer, i as Integer
	Const FR_ppcm as Single = 1.664	' Pulses per cm conversion

	NumOfPulses = CInt(Distance * FR_ppcm)
	For i = 1 to NumOfPulses
		Call PulseOut(LeftServo, Left_Reverse_Fast, 1)
		Call PulseOut(RightServo, Right_Reverse_Fast, 1)	
		Call Delay(0.02)	' A necessary delay
	Next
End Sub


' /////// LeftS ///////
Public Sub LeftS(ByVal Angle as Single)
	Dim NumOfPulses as Integer, i as Integer
	Const LS_ppd as Single = 0.4944	' Pulses per degree conversion

	NumOfPulses = CInt(Angle * LS_ppd)
	For i = 1 to NumOfPulses
		Call PulseOut(LeftServo, Left_Reverse_Slow, 1)
		Call PulseOut(RightServo, Right_Forward_Slow, 1)
		Call Delay(0.02)	' A necessary delay
	Next
End Sub


' /////// RightS ///////
Public Sub RightS(ByVal Angle as Single)
	Dim NumOfPulses as Integer, i as Integer
	Const RS_ppd as Single = 0.5056	' Pulses per degree conversion

	NumOfPulses = CInt(Angle * RS_ppd)
	For i = 1 to NumOfPulses
		Call PulseOut(LeftServo, Left_Forward_Slow, 1)
		Call PulseOut(RightServo, Right_Reverse_Slow, 1)
		Call Delay(0.02)	' A necessary delay
	Next
End Sub