'********************************************************
'************************Ant**************************

' Ant - Hexapod w/ Forward Sharp Sensor
' Platform: BasicX
' Version 1.1
' Author: Brian Patton (brianp@robodyssey.com)
' Robodyssey Systems, LLC. - www.robodyssey.com
' Last Updated: Jan 2, 2004

'*********Materials*****************************

' 1 Robodyssey Ant
' 1 Robodyssey Advanced MotherBoard
' 1 BasicX-24
' 1 Sharp GP2D12 sensor

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

Public Const LeftServo As Byte = 5 ' BlueBird Servos
Public Const RightServo As Byte = 6 ' BlueBird Servos
Public Const CenterServo As Byte = 7 ' BlueBird Servos
Public Const IRPin As Byte = 13

Public Const LeftServoMin As Single = 0.002 ' Amount Back
Public Const LeftServoMid As Single = 0.0015
Public Const LeftServoMax As Single = 0.001 ' Amount Forward

Public Const RightServoMin As Single = 0.001 ' Amount forward
Public Const RightServoMid As Single = 0.0015
Public Const RightServoMax As Single = 0.002 ' Amount Back

Public Const CenterServoLeft As Single = 0.0013 ' Rise in Right Leg
Public Const CenterServoMid As Single = 0.0015
Public Const CenterServoRight As Single = 0.0018 ' Rise in Left Leg

Public Const ServoCommandSleep As Integer = 10 ' 10 x 1/512 second =0.0195sec
Const ServoPulses As Byte = 16

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

Public LeftServoPos As Single
Public RightServoPos As Single
Public CenterServoPos As Single
Public SensorValue As Integer
Public SensorAve As Integer

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

Sub Main() 'Start of Main Program
Sleep(3.0)

Do 'Start of forward gait
LeftServoPos = LeftServoMin 'half step forward
RightServoPos = RightServoMax
CenterServoPos = CenterServoRight

Call CheckSensors() 'Check sensors
Call SendServoCommands() 'Make 'em walk

LeftServoPos = LeftServoMax 'second half of step
RightServoPos = RightServoMin
CenterServoPos = CenterServoLeft

Call CheckSensors() 'check sensors again
Call SendServoCommands()
Loop

End Sub 'Main Program

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

Sub CheckSensors()

SensorValue = GetADC(IRPin)+GetADC(IRPin)+GetADC(IRPin)
' Get three IR readings
SensorAve = Sensorvalue \ 3 ' Devide them by 3 to get the average
' Helps eliminate some error
Debug.Print CStr(SensorAve)

If Sensorave > 500 Then ' See if something is too close
Call TurnRight ' Call right
Call TurnRight
End If ' End of too close senario

If Sensorave < 150 Then ' See if something is too far like an edge
Call Backup
Call TurnLeft ' Call right
Call TurnLeft
End If ' End of too far senario

End Sub

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

Sub TurnLeft()
Dim i As Byte
For i=1 To 5
CenterServoPos = CenterServoRight
RightServoPos = RightServoMin 'notice Left and right are minimum
LeftServoPos = LeftServoMin 'this causes a turn
Call SendServoCommands()

CenterServoPos = CenterServoLeft
RightServoPos = RightServoMax 'now maximum
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next

End Sub

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

Sub TurnRight()
Dim i As Byte
For i=1 To 5
CenterServoPos = CenterServoRight
RightServoPos = RightServoMax 'Ah Ha now they are max
LeftServoPos = LeftServoMax
Call SendServoCommands()

CenterServoPos = CenterServoLeft
RightServoPos = RightServoMin 'and min
LeftServoPos = LeftServoMin
Call SendServoCommands()
Next

End Sub

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

Sub Backup()
Dim i As Byte
For i=1 To 3
CenterServoPos = CenterServoLeft 'hmm, this is just reverse of
RightServoPos = RightServoMax 'the forward code
LeftServoPos = LeftServoMin
Call SendServoCommands()
CenterServoPos = CenterServoRight
RightServoPos = RightServoMin
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next

End Sub
'********************************************************
'******** Send Commands to Servos ***********************

Sub SendServoCommands()
Dim j As Byte
For j = 1 to 8 'give center legs a head start
Call PulseOut(CenterServo, CenterServoPos, 1)
Sleep(ServoCommandSleep) 'servo pause time
Next

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

End Sub