'******************************************************
'******************TABLETOPMOUSE*******************************
'TableTopMouse - FOUR Sharp Infrared Detector (GP2D12 and 2YOA21)
'@ Platform: NetMedia BasicX 24
'@ Version: 2.0
'@ Author: Chris D. Odom (chris_odom@georgeschool.org)
' Robodyssey Systems, LLC. - www.robodyssey.com
'@ Last Updated: July 25, 2003

'******************************************************
' The Robodyssey Mouse uses TWO front mounted, forward looking IR Sensors (GP2D12)
' and TWO side mounted, angled down IR sensors (2YOA21) to
' stay on a tabletop and avoid anything in front of it.
'******************************************************
'May be freely distributed and modified, but must contain above information

Option Explicit

'Declare Servo Constants - min 0.001 max 0.002
Public Const Left_Forward As Single = 0.001
Public Const Right_Forward As Single = 0.002

Public Const Left_Reverse As Single = 0.002
Public Const Right_Reverse As Single = 0.001

'Servo Pins
Public Const LeftServo As Byte = 5 ' (Pin #1 on RAMB)
Public Const RightServo As Byte = 6 ' (Pin #0 on RAMB)

'IR Pins
Const IRRForward As Byte = 13 ' Sensor looks forward and right (Pin #8 on RAMB)
Const IRLForward As Byte = 14 ' Sensor looks forward and left (Pin #9 on RAMB)
Const IRRDown as Byte = 15 ' Sensor looks down on the Mouse's right side (Pin #10 on RAMB)
Const IRLDown as Byte = 16 ' Sensor looks down on the Mouse's Left side (Pin #11 on RAMB)

' Servo constants and variables
Dim Speed as Single ' Delay between servo pulses (in seconds)
Const TopSpeed as Single = 0.020 ' it is possible to go faster than this, but the inertia is to great!
Const MidSpeed as Single = 0.200
Const SlowSpeed as Single = 0.500

' Sensor constants and variables
Dim RDSensor as Integer ' Value Right/Down sensor receives
Dim LDSensor as Integer ' Value Left/Down sensor receives
Dim RFSensor as Integer ' Value Right/Forward sensor receives
Dim LFSensor as Integer ' Value Left/Forward sensor receives
Dim LDVal as Integer ' Value of Left/Down Sensor while on table
Dim RDVal as Integer ' Nominal value of Right/Down Sensor while on table
Dim RDrop as Integer ' Value of Right Sensor when it reads off the table
Dim LDrop as Integer ' Value of Left Sensor when it reads off the table
Dim RFNear as Integer ' Value of R Forward Sensor when it is Near an object ( i.e., within 25 cm)
Dim LFNear as Integer ' Value of L Forward Sensor when it is Near an object ( i.e., within 25 cm)
Dim RFVeryNear as Integer ' Value of R Forward Sensor when it is Very Near an object ( i.e., within 10 cm)
Dim LFVeryNear as Integer ' Value of L Forward Sensor when it is Very Near an object ( i.e., within 10 cm)
Const DeltaR as Integer = 100 ' RDrop = RVal - DeltaR 100 is found empirically and varies with sensor
Const DeltaL as Integer = 100 ' LDrop = LVal - DeltaL 100 is found empirically and varies with sensor

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

Sub Main()

' Before the demonstration, perform the following steps.
' If Call LocateObject() is commented below, ignore steps 1-8, but Mouse must be flat in middle
' of table when it is powered!

' 1. Place a tall object with a flat side on the table.
' 2. Mark on the table distances of 10cm and 25cm from the object.
' 3. Place the Mouse on the table top facing the object. The Forward Sensor should be exactly 10cm away
' from the object. (Use the mark on the table to assist with this.)
' 4. Cycle the power, or hit the reset button on the motherboard
' 5. The program will record the sensor values to the tabletop and to the Object (10cm away or "VeryNear")
' 6. Once these are recorded, notice the Green LED will turn on. You have 10 seconds to move the
' Mouse so the Forward Sensor is now 25cm from the Object. The Red LED will light in the last
' 3 seconds.
' 7. The program will record the sensor value to the Object (25cm away or "Near")
' 8. When the Red LED is extinguished, the Mouse is programmed and the TableTop demonstration may begin.

' MAKE SURE THE PROGRAM IS INITIALIZED WHILE THE MOUSE IS IN THE READY POSITION ON THE TABLETOP!
Call LocateTable()
'Call LocateObject()

' When debugging, you may not wish to call LocateTable and LocateObject to save time. If so, run both
' following above procedure (steps 1-8). Then simply comment-out the above two lines and define the
' appropriate values below. BE CAREFUL, as the batteries weaken, the sensor values will float. Eventually
' the RDrop and LDrop values will not be realistic!
' RDVal = 512
' LDVal = 233
' RDrop = RVal - DeltaR
' LDrop = LVal - DeltaL
RFVeryNear = 530
LFVeryNear = 530
RFNear = 330
LFNear = 330

Speed = TopSpeed

Do
Call CheckSensors()
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
' debug.print "IRRForward = "; CStr(GetADC(IRRForward))
' debug.print "IRLForward = "; CStr(GetADC(IRLForward))
' debug.print "IRRDown = "; CStr(GetADC(IRRDown))
' debug.print "IRLDown = "; CStr(GetADC(IRLDown))
' debug.print " "
Delay(Speed)
loop
End Sub

'******************************************************
'****************** Locate the Table *********************

Sub LocateTable()
Dim i as Integer

' Have Mouse determine nominal values from Sensors to table top
RDVal = 0
LDVal = 0
' make 10 nominal readings, then find average
for i = 1 to 10
RDVal = RDVal + GetADC(IRRDown)
LDVal = LDVal + GetADC(IRLDown)
'debug.print CStr(RDVal) ; " " ; CStr(LDVal)
next

' RDVal and LDVal are nominal values that tell the Mouse the table is under the sensor
RDVal = RDVal \ 10
LDVal = LDVal \ 10
debug.print "Nominal Right Down Value (RDVal): " ; CStr(RDVal)
debug.print "Nominal Left Down Value (LDVal): " ; CStr(LDVal)

' Determine the down sensor values (L and R) when sensor is over the table's EDGE
RDrop = RDVal - DeltaR
LDrop = LDVal - DeltaL
End Sub


'*************************************************************
'****************** Locate the Reference Object **************

Sub LocateObject()
Dim i as Integer

' Have Mouse determine nominal value from Forward Sensor to Object when Very Near (10cm away)
' Make sure the Mouse is looking at an object exactly 10cm away when program initializes

RFVeryNear = 0
LFVeryNear = 0
' make 10 readings, then find average
for i = 1 to 10
RFVeryNear = RFVeryNear + GetADC(IRRForward)
LFVeryNear = LFVeryNear + GetADC(IRLForward)
next

' RFVeryNear is the R Forward Sensor value when the Object is Near (25cm away)

' LFVeryNear is the R Forward Sensor value when the Object is Near (25cm away)
RFVeryNear = RFVeryNear \ 10
LFVeryNear = LFVeryNear \ 10


' Turn on the Green LED to indicate it is time to move the Mouse to the 25cm mark. Give user 10s.
Call PutPin(26,0) ' turn on Green LED
Delay(7.0)
Call PutPin(26,1) ' turn off Green LED
Call PutPin(25,0) ' turn on Red LED as warning
Delay(3.0) ' Warning signal for 3 seconds

' Hopefully, by now the Mouse has been moved to the 25cm mark
RFNear = 0
LFNear = 0
' make 10 readings, then find average
for i = 1 to 10
RFNear = RFNear + GetADC(IRRForward)
LFNear = LFNear + GetADC(IRLForward)
next

' RFNear is the R Forward Sensor value when the Object is Near (25cm away)
' LFNear is the L Forward Sensor value when the Object is Near (25cm away)
RFNear = RFNear \ 10
LFNear = LFNear \ 10

' * Note: to print long strings, Click on Options >> Environment and change the Maximum String Length value
debug.print "R Forward Sensor Value for VeryNear Object (RFVeryNear): " ; CStr(RFVeryNear)
debug.print "L Forward Sensor Value for Near Object (RFNear): " ; CStr(RFNear)

Call PutPin(25,1) ' Turn off Red LED

End Sub

'******************************************************
'****************** Check Sensors *********************
Sub CheckSensors()

RFSensor = GetADC(IRRForward)
LFSensor = GetADC(IRLForward)
RDSensor = GetADC(IRRDown)
LDSensor = GetADC(IRLDown)

Call PutPin(25,1) ' turn OFF led's
Call PutPin(26,1)

' Search for obstacles in front of Mouse **************************
If RFSensor > RFVeryNear Then ' Object is VeryNear on the Right
Speed = SlowSpeed ' Slow WAY down
Call Left ' Turn Left
ElseIf LFSensor > LFVeryNear Then ' Object is VeryNear on the Left
Speed = SlowSpeed ' Slow WAY down
Call Right ' Turn Right
ElseIf RFSensor > RFNear Then ' Object is Near on Right
Speed = MidSpeed ' Slow down
ElseIf LFSensor > LFNear Then ' Object is Near on Left
Speed = MidSpeed ' Slow down
Else
Speed = TopSpeed ' Normal speed (fast)
End If
' *******************************************************************

' Search for the table's edge **************************************
If RDSensor < RDrop Then ' Right wheel near the edge
debug.print "No table on right!"
Call PutPin(25,0) ' Turn on Red Led
Call Reverse() ' Reverse the Mouse
Call QuarterLeft() ' Turn QuarterLeft instead of Left so it won't fall off near edge
End If

If LDSensor < LDrop Then ' Left wheel near the edge
debug.print "No table on left!"
Call PutPin(26,0) ' Turn on Green Led
Call Reverse()
Call QuarterRight() ' Turn QuarterRight instead of Right so it won't fall off near edge
End If
' *******************************************************************

End Sub

'******************************************************
'****************** Turn Right ************************
Sub Right()
Dim i As Integer
For i = 1 to 8
' check that there is a table below you!
RDSensor = GetADC(IRRDown)
LDSensor = GetADC(IRLDown)
if RDSensor < RDrop then
Exit For ' Stop turning if table drops away

end if
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Delay(SlowSpeed)
Next
End Sub

'******************************************************
'****************** Turn Left *************************
Sub Left()
Dim i As Integer
For i = 1 to 8
' check that there is a table below you!
RDSensor = GetADC(IRRDown)
LDSensor = GetADC(IRLDown)
if LDSensor < LDrop then
Exit For ' Stop turning if table drops away
end if
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Delay(SlowSpeed)
Next
End Sub
'*********************************************************
'****************** Turn Quarter Right ************************
Sub QuarterRight()
Dim i As Integer
For i = 1 to 5
' check that there is a table below you!
RDSensor = GetADC(IRRDown)
LDSensor = GetADC(IRLDown)
if RDSensor < RDrop then
Exit For ' Stop turning if table drops away
end if
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Delay(SlowSpeed)
Next
End Sub

'******************************************************
'****************** Turn Quarter Left *************************
Sub QuarterLeft()
Dim i As Integer
For i = 1 to 5
' check that there is a table below you!
RDSensor = GetADC(IRRDown)
LDSensor = GetADC(IRLDown)
if LDSensor < LDrop then
Exit For ' Stop turning if table drops away
end if
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Delay(SlowSpeed)
Next
End Sub
'*********************************************************
'****************** Reverse ******************************
Sub Reverse()
Dim i as Integer
For i = 1 to 3
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Delay(SlowSpeed)
Next
End Sub
'*********************************************************