'******************************************************
'****************** Gripper Mouse *******************
'Mouse with a Robodyssey Gripper.

'@ Platform: NetMedia BasicX 24
'@ Version: 1.0
'@ Written by Brian Patton (brianp@robodyssey.com)
'@ Base code modified from:
'@ Author: Chris D. Odom (chris_odom@georgeschool.org)
' Robodyssey Systems, LLC. - www.robodyssey.com
'@ Last Updated: Jan 26, 2004

'May be freely distributed and modified, but must contain above information
'**********************Materials********************
' Robodyssey Mouse
' Motherboard with a BX-24
' Robodyssey gripper
' 3 Sharp Sensors
'***************************************************
' This fun little program makes a mouse heck bent on
' going strait. If it senses anything in it's way, it
' grabs it, backs up, turns, chews it and starts off again.

' Nice demo showing how to intergrate motion with a retained
'grip
'****************************************************
Option Explicit

Public Const Left_Forward As Single = 0.002
Public Const Right_Forward As Single = 0.001

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

Public Const Grip_Open As Single = 0.0013
Public Const Grip_Mid As Single = 0.0015
Public Const Grip_Close As Single = 0.0020

Public Const LeftServo As Byte = 5 ' (Pin #0 on RAMB)
Public Const RightServo As Byte = 6 ' (Pin #1 on RAMB)
Public Const GripServo As Byte = 12 ' (Pin #7 on RAMB)


Const LeftIRPin As Byte = 14 ' Left Sensor
Const RightIRPin As Byte = 15 ' Right Sensor
Const GripIRPin As Byte = 13 ' Sensor in the grip

Dim LSensorValue as Integer
Dim RSensorValue as Integer
Dim GSensorValue as Integer



'******************************************************
'********************* Main Loop **********************

Sub Main()
Sleep(3.0)
Call OpenGrip()
Call GetGoing()
End Sub
'******************************************************
Sub GetGoing()
Call CheckSensors()
End Sub

'******************************************************
' Check IR Sensor
'******************************************************

Sub CheckSensors()
Debug.Print "Checking Sensors"
Do
LSensorValue = GetADC(LeftIRPin)
RSensorValue = GetADC(RightIRPin)
GSensorValue = GetADC(GripIRPin)
Debug.Print CStr(LSensorValue) ; CStr(RSensorValue)

If LSensorValue < 100 Then
Sleep(1.0)
Call TurnRight()
End If

If RSensorValue < 100 Then
Sleep(1.0)
Call TurnLeft()
End If

If LSensorValue > 500 Then
Sleep(1.0)
Call TurnRight()
End If

If RSensorValue > 500 Then
Sleep(1.0)
Call TurnLeft()
End If

If GSensorValue > 450 Then
Sleep(1.0)
Call CheckFront()
End If

Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Delay(0.05)
Loop
End Sub
'******************************************************
'****************** Turn Right ************************
Sub TurnRight()
Dim i As Integer
For i = 1 to 8
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Sleep(0.08)
Next
End Sub

'******************************************************
'****************** Turn Left *************************
Sub TurnLeft()
Dim i As Integer
For i = 1 to 8
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Sleep(0.08)
Next
End Sub

'******************************************************
'****************** Back up *************************
Sub Backup()
Dim i As Integer
For i = 1 to 8
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Sleep(0.08)
Next
End Sub


'*********************************************************
'********** Check the Front ****************************
Sub CheckFront()
Dim i As Integer
For i = 1 to 8
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Sleep(0.08)
Next

Call CloseGrip()
Call CloseGripRight()
Call ChewIt
Call OpenGrip()
Call Backup()
Call TurnLeft()
Call CheckSensors()
End Sub
'*********************************************************
'******************* Open Gripper ************************
Sub OpenGrip()
Dim i As Integer
For i = 1 to 8
Call PulseOut(GripServo, Grip_Open, 1)
Sleep(0.02)
Next
End Sub
'*********************************************************
'******************* Close Grip ************************
Sub CloseGrip()
Dim i As Integer
For i = 1 to 8
Call PulseOut(GripServo, Grip_Close, 1)
Sleep(0.02)
Next
End Sub
'*********************************************************
'************* Close Grip and move Forward **************
Sub CloseGripForward()
Dim i As Integer
For i = 1 to 20
Call PulseOut(GripServo, Grip_Close, 1)
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Forward, 1)
Sleep(0.02)
Next
End Sub
'******************************************************
'****************** Close Grip and Turn Right **********
Sub CloseGripRight()
Dim i As Integer
For i = 1 to 8
Call PulseOut(GripServo, Grip_Close, 1)
Call PulseOut(LeftServo, Left_Forward, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Sleep(0.08)
Next
End Sub
'*********************************************************
'************* Close Grip and move Reverse **************
Sub CloseGripReverse()
Dim i As Integer
For i = 1 to 15
Call PulseOut(GripServo, Grip_Close, 1)
Call PulseOut(LeftServo, Left_Reverse, 1)
Call PulseOut(RightServo, Right_Reverse, 1)
Sleep(0.02)
Next
End Sub
'*********************************************************
'*************** Chew it up**************** **************
Sub ChewIt()
Dim i As Integer
For i = 1 to 15
Call PulseOut(GripServo, Grip_Close, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Open, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Close, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Open, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Close, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Open, 1)
Sleep(0.02)
Next
For i = 1 to 15
Call PulseOut(GripServo, Grip_Close, 1)
Sleep(0.02)
Next
End Sub