Option Explicit

Public Sub Main()
' The Sharp Sensor GP2D12 is used in this application.
' The sensor is connected to Pin #13 on the BX-24, #8 on the RAMB.
' With our work on the IRRadad.bas application, we are now able to determine if the
' robot's dance partner is moving toward or away from the robot.
' The speed of motion is also measured and accounted for.
' The end result is a robot that will mirror-image mimic the motion of any object
' in front of the IR sensor.
' You should play around with the 't' and 'xTolerance' Constants to get reasonable values
' for your system. (Minimum velocity that can be read is xTolerance / t.)
' For good accuracy and good velocity range but slow response time, choose t=1.0, xTolerance=2.0
' For fast response and so-so accuracy, choose t=0.25 and xTolerance=1.0
' Also play around with the NumSteps. Take into account the velocity of the object whose motion
' you are tring to mimic!
' Below, I do not set NumSteps as a Const. Rather, I have the robot take more steps if the object
' is far away and fewer steps if the object is near.

Dim IRSTotal as Integer ' Total IR received per loop
Dim IRSAvg as Integer ' Average IR reading per loop
Dim IRS as Single ' Will convert from Int to Sng
Dim IRSTotalOld as Integer ' Total IR received in previous loop
Dim IRSAvgOld as Integer ' Average IR reading in previous loop
Dim IRSOld as Single ' Will convert from Int to Sng
Dim x as Single ' Distance of object from Sensor
Dim xOld as Single ' Previously read Sensor value
Dim xAvg as Single ' Average distance -- point of velocity reading
Const t as Single = 1.0 ' Time bewteen distance readings (in seconds)
Const xTolerance as Single = 2.0 ' Tolerance in change in position
Dim v as Single ' Calculated velocity (velocity = distance/time, or v = x/t)
Dim Direction as String ' Direction of target's motion
Const Coef as Single = 4187.8 ' Value from Excel
Const Expon as Single = -0.9042 ' Value from Excel
Dim i as Integer ' A counter
Const NumLoops as Integer = 10 ' Number of readings to average
Dim NumSteps as Integer ' Take this many steps when direction of motion is determined

'Servo Pin Constants
Const LeftServo As Byte = 5 ' Pin #0 on the RAMB
Const RightServo As Byte = 6 ' Pin #1 on the RAMB

'Servo Direction Constants
Const Left_Forward As Single = 0.0020
Const Right_Forward As Single = 0.0010

Const Left_Reverse As Single = 0.0010
Const Right_Reverse As Single = 0.0020


' Read sensor in continuous loop
' Must stop execution with Stop Button


Do
' Grab a few sensor readings from the IR sensor plugged into pin 13:
' Take an initial (old) distance reading

IRSTotalOld = 0 ' Reset to zero
For i = 1 to NumLoops
IRSTotalOld = IRSTotalOld + GetADC(13)
Next

Call Delay(t) ' Delay some time

' Take an final distance reading
IRSTotal = 0 ' Reset to zero
For i = 1 to NumLoops
IRSTotal = IRSTotal + GetADC(13)
Next

' Average the readings and convert to a distance:
IRSAvgOld = IRSTotalOld\NumLoops ' Initial data
IRSOld = CSng(IRSAvgOld) ' Convert to Single before using
xOld = (IRSOld/Coef)^(1.0/Expon) ' From Excel curve fit
If (xOld > 100.0) Then
xOld = 100.0
End If
' Debug.Print "IRSAvgOld = "; CStr(IRSAvgOld)
' Debug.Print "xOld = "; CStr(xOld); "cm"

IRSAvg = IRSTotal\NumLoops ' Final data
IRS = CSng(IRSAvg) ' Convert to Single before using
x = (IRS/Coef)^(1.0/Expon) ' From Excel curve fit
If (x > 100.0) Then
x = 100.0
End If
' Debug.Print "IRSAvg = "; CStr(IRSAvg)
' Debug.Print "x = "; CStr(x); "cm"

' Calculate the target's velocity
' Prevent small (i.e., floating zero velocity readings)
if (Abs(x - xOld)) < xTolerance Then
v = 0.0
Else
v = (x - xOld)/t
End If

' Calculate average distance to target
xAvg = (x + xOld)/2.0

' Determine the number of steps the robot will take (if xAvg is large, then many steps)
' If target is 10cm away, take 1 step. If 100cm away, take 10 steps.
NumSteps = CInt(xAvg / 8.0)


' Determine if target is moving toward, away, or is still
If (v > 0.0) Then
Direction = "AWAY"
' Move forward
For i = 1 to NumSteps
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Call Delay(0.02)
Next
ElseIf (v < 0.0) Then
Direction = "TOWARD"
' Move backward
For i = 1 to NumSteps
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Call Delay(0.02)
Next
Else
Direction = "STILL"
End If


Debug.Print CStr(v); " cm/s, "; Direction ; ", @ "; CStr(xAvg); " cm"
Debug.Print
Loop


End Sub