'********************************************************
'****************** Dan's Doom **************************
'Winner of TableTop on 7/28/04
'Smaller Than Normal Mouse w/ turret and two downward detectors
'And two IR edge Detectors
'@ Version 1.0
'@ Author: Brian Patton
'@ Last Updated: July  28, 2004 
'********************************************************
'************* Declare Constants ************************

'Each Servo may be slightly different but these two are:
'Left center is 0.00183, Slow Forward is .00175  Fast Forward = 0.00165 
'Right Center = 0.0018, Slow Forward is .0019  Fast Forward = 0.002

Dim Left_Forward As Single 
Dim Right_Forward As Single    

Public Const Left_Reverse As Single = 0.00195 'Bigger Faster Reverse
Public Const Right_Reverse As Single = 0.0017 'Smaller Faster Revers

Public Const TurretServoLeft As Single = 0.001
Public Const TurretServoMid As Single = 0.0015
Public Const TurretServoRight As Single = 0.0021 

'Servo Pins
Public Const LeftServo As Byte = 5
Public Const RightServo As Byte = 6
Public Const TurretServo As Byte = 20

'IR Pin
Const LeftLinePin As Byte = 13' (Pin8 on RAMB)
Const RightLinePin As Byte = 17' (Pin12 on RAMB)
Const LeftIRPin As Byte = 15' (Pin10 on RAMB)
Const RightIRPin As Byte = 16' (Pin11 on RAMB)
Const TurretSensorPin As Byte = 14' (Pin9 on RAMB)
Public Const Speed As Single = 0.02

Dim RotateTurretTaskStack (1 To 32) As Byte
Dim CheckTurretSensorTaskStack (1 To 64) As Byte
Dim ContinueForward As Boolean
Const Trigger As Integer = 300 ' If turret IR greater than this avoid
Const FallTrigger As Integer = 80 ' If turret IR less than this avoid

Dim TurretSensorReading as integer
Dim TurretModifier as Single
Dim TurretPosition as Single
Dim Line as Integer

'******************************************************
'****************** Start of Main Loop ****************

Sub Main()
'I made the forwards variable so that I can change the rate of turning
Left_Forward = 0.00166 ' bigger makes it go slower
Right_Forward = 0.002 ' bigger makes it go faster
Sleep (3.0)
TurretModifier = 0.00002
TurretPosition = TurretServoMid


Do
Call CheckTurret
Call RotateTurret
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Call CheckSensors()
Call CheckLine()
Left_Forward = Left_Forward - 0.0002
Right_Forward = Right_Forward + 0.0002
If Left_Forward < 0.00166 Then
Left_Forward = 0.00166
End If
If Right_Forward > 0.002 Then
Right_Forward = 0.002
End If
Delay(Speed)
Loop
End Sub

'******************************************************
'****************** Check Sensors *********************

Sub CheckSensors()

If GetADC(LeftIRPin) < 300 Then 'found an edge
  Call TurnRight          ' Call right 
End If  

If GetADC(RightIRPin) < 300 Then 'found an edge
  Call TurnLeft          ' Call Left
End If   

End Sub
'******************************************************
'****************** Check Line *********************

Sub CheckLine()
' These are IR LEDS and voltage dividers
If GetADC(LeftLinePin) > 300 Then 'found a Line 
Call TurnLeft          ' Call right  
End If
If GetADC(RightLinePin) > 300 Then 'found a Line
Call TurnRight          ' Call Left

End If   

End Sub
'********************************************************
'************* Rotate Turret Task ***********************

Sub RotateTurret()

TurretPosition = TurretPosition + TurretModifier
If TurretPosition < TurretServoLeft Then
TurretModifier = 0.0001
ElseIf TurretPosition > TurretServoRight Then
TurretModifier = -0.0001
End If
Call PulseOut(TurretServo, TurretPosition, 1)

End Sub

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

Sub CheckTurret()
TurretSensorReading = GetADC(TurretSensorPin)
If TurretSensorReading > Trigger Then
If TurretPosition < 0.00125 Then
Call TurnRight
ElseIf TurretPosition < 0.00175 Then
Call TurnLeft
Call TurnLeft
Else 
Call TurnLeft
End If
End If
If TurretSensorReading < FallTrigger Then ' Woa, See an Edge
Call TurnLeft
End If
End Sub

'******************************************************
'****************** Turn Right ************************

Sub TurnRight()
Right_Forward = Right_Forward - 0.0002 'Turn More every time it gets a hit
Dim i As Integer
  For i = 1 to 3
      Call PulseOut(LeftServo, Left_Forward, 1)
      Call PulseOut(RightServo, Right_Forward, 1)
      Delay(Speed)
  Next
If Right_Forward = 0.0017 then ' But don't let it go to far
Right_Forward = 0.00175
End If
  
End Sub

'******************************************************
'****************** Turn Left *************************

Sub TurnLeft()
Dim i As Integer
Left_Forward = Left_Forward + 0.0002 'Turn More every time it gets a hit
  For i = 1 to 3
      Call PulseOut(LeftServo, Left_Forward, 1)
      Call PulseOut(RightServo, Right_Forward, 1)
      Delay(Speed)
  Next
If Left_Forward = 0.00195 then ' But don't let it go to far 
Left_Forward = 0.0019
End If
End Sub
'******************************************************
'****************** Hard Left *************************

Sub HardLeft()
Dim i As Integer
Left_Forward = 0.00166
Right_Forward = 0.002
  For i = 1 to 10
      Call PulseOut(LeftServo, Left_Reverse, 1)
      Call PulseOut(RightServo, Right_Forward, 1)
      Delay(Speed)
  Next
  
End Sub
'******************************************************
'****************** Hard Right *************************

Sub HardRight()
Dim i As Integer
Left_Forward = 0.00166
Right_Forward = 0.002
  For i = 1 to 10
      Call PulseOut(LeftServo, Left_Forward, 1)
      Call PulseOut(RightServo, Right_Reverse, 1)
      Delay(Speed)
  Next
  
End Sub
'******************************************************
'****************** Spin *************************

Sub Spin()
Dim i As Integer
Left_Forward = 0.00166
Right_Forward = 0.002
  For i = 1 to 20
      Call PulseOut(LeftServo, Left_Reverse, 1)
      Call PulseOut(RightServo, Right_Forward, 1)
      Delay(Speed)
  Next
  
End Sub
'******************************************************
'****************** Backup *************************

Sub Backup()
Dim i As Integer

  For i = 1 to 10
      Call PulseOut(LeftServo, Left_Reverse, 1)
      Call PulseOut(RightServo, Right_Reverse, 1)
      Delay(Speed)
  Next 
End Sub
'*********************************************************