'********************************************************
'************* Declare Constants ************************

Public Const LeftServo As Byte = 20
Public Const RightServo As Byte = 18
Public Const CenterServo As Byte = 19
Public Const TurretSensorServo As Byte = 17
Public Const TurretSensorPin As Byte = 13

Public Const LeftServoMin As Single = 0.00195
Public Const LeftServoMid As Single = 0.0015
Public Const LeftServoMax As Single = 0.00105

Public Const RightServoMin As Single = 0.00105
Public Const RightServoMid As Single = 0.0015
Public Const RightServoMax As Single = 0.00195

Public Const CenterServoLeft As Single = 0.00125
Public Const CenterServoMid As Single = 0.0015
Public Const CenterServoRight As Single = 0.00165

Public Const TurretSensorServoRight As Single = 0.0005
Public Const TurretSensorServoMid As Single = 0.0015
Public Const TurretSensorServoLeft As Single = 0.00245

Public Const ServoCommandSleep As Integer = 5
Const ServoPulses As Byte = 17

Const ClearThreshold As Integer = 200

'********************************************************
'************* Declare Variables ************************

Public LeftServoPos As Single
Public RightServoPos As Single
Public CenterServoPos As Single

Dim i As Byte
Dim j As Byte
Dim TurretSensorReading As Integer
Dim TurretSensorPosition As Single
Dim TurretSensorModifier As Single
Dim RotateTurretTaskStack (1 To 32) As Byte
Dim CheckTurretSensorTaskStack (1 To 64) As Byte
Dim ContinueForward As Boolean

'********************************************************
'************* Main *************************************

Sub Main() 'Start of Main Program

'Call CenterLegs()
TurretSensorModifier = 0.00001
TurretSensorPosition = TurretSensorServoMid

ContinueForward = True

CallTask "RotateTurret", RotateTurretTaskStack
CallTask "CheckTurret", CheckTurretSensorTaskStack

Do
If ContinueForward = True Then
LeftServoPos = LeftServoMin
RightServoPos = RightServoMax
CenterServoPos = CenterServoRight

Call SendServoCommands()

LeftServoPos = LeftServoMax
RightServoPos = RightServoMin
CenterServoPos = CenterServoLeft

Call SendServoCommands()
Else
Sleep(0.0)
End If
Loop

End Sub 'Main Program

'********************************************************
'************* Rotate Turret Task ***********************

Sub RotateTurret()
Do
TurretSensorPosition = TurretSensorPosition + TurretSensorModifier
If TurretSensorPosition > TurretSensorServoLeft Then
TurretSensorModifier = -0.00001
ElseIf TurretSensorPosition < TurretSensorServoRight Then
TurretSensorModifier = 0.00001
End If
Call PulseOut(TurretSensorServo, TurretSensorPosition, 1)
Sleep(ServoCommandSleep)
Loop
End Sub

'********************************************************
'************* Check Turret Sensor **********************

Sub CheckTurret()
Do
TurretSensorReading = GetADC(TurretSensorPin)
If TurretSensorReading > ClearThreshold Then
If TurretSensorPosition < 0.00125 Then
ContinueForward = False
TurnLeft
ElseIf TurretSensorPosition < 0.00175 Then
ContinueForward = False
Backup
TurnRight
TurnRight
Else
ContinueForward = False
TurnRight
End If
End If
ContinueForward = True
Sleep(25)
Loop
End Sub

'********************************************************
'***** Turn left ****************************************

Sub TurnLeft()
For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMin
LeftServoPos = LeftServoMin
Call SendServoCommands()

CenterServoPos = CenterServoRight
RightServoPos = RightServoMax
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next

End Sub

'********************************************************
'***** Turn right ***************************************

Sub TurnRight()

For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMax
LeftServoPos = LeftServoMax
Call SendServoCommands()

CenterServoPos = CenterServoRight
RightServoPos = RightServoMin
LeftServoPos = LeftServoMin
Call SendServoCommands()
Next

End Sub

'********************************************************
'******** Backup ****************************************

Sub Backup()

For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMax
LeftServoPos = LeftServoMin
Call SendServoCommands()

CenterServoPos = CenterServoRight
RightServoPos = RightServoMin
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next

End Sub

'********************************************************
'******** Center Legs ***********************************

Sub CenterLegs()

CenterServoPos = CenterServoLeft
RightServoPos = RightServoMid

Call SendServoCommands()

CenterServoPos = CenterServoRight
LeftServoPos = LeftServoMid

Call SendServoCommands()

CenterServoPos = CenterServoMid

Call SendServoCommands()

Sleep(2)

End Sub

'********************************************************
'******** Send Commands to Servos ***********************

Sub SendServoCommands()

For j = 1 to ServoPulses
Call PulseOut(CenterServo, CenterServoPos, 1)
Sleep(ServoCommandSleep)
Next

For j = 1 to ServoPulses
Call PulseOut(LeftServo, LeftServoPos, 1)
Call PulseOut(RightServo, RightServoPos, 1)
Call PulseOut(CenterServo, CenterServoPos, 1)
Sleep(ServoCommandSleep)
Next

End Sub