Attribute VB_Name = "PatrolBot"
'-------------------------------------------------------------------------------
Option Explicit

' Pin Definitions
Public Const RightMotorMinus As Byte = 6
Public Const LeftMotorMinus As Byte = 8
Public Const IRBackRight As Byte = 13
Public Const IRFrontRight As Byte = 14
Public Const IRFront As Byte = 15
Public Const IRFrontLeft As Byte = 16
Public Const IRBackLeft As Byte = 17

Public Const EncoderOutPin As Byte = 13
Public Const EncoderInPin As Byte = 17

Public Const IRForkRight As Byte = 18
Public Const IRForkLeft As Byte = 19

Public Const PiezoPin As Byte = 20

Public Const AttachmentIDPin1 As Byte = 9
Public Const AttachmentIDPin2 As Byte = 10

Public Const ALED As Byte = 11
Public Const BLED As Byte = 12
'Public Const RedLED As Byte = 25
'Public Const GreenLED As Byte = 26

Const SinFrontRight	As Single	=  0.342020	' 20 degrees
Const CosFrontRight	As Single	=  0.939693
Const SinFront 	As Single 	=  1.0		' 90 degrees
Const CosFront 	As Single	=  0.0
Const SinFrontLeft	As Single	=  0.342020	' 160 degrees
Const CosFrontLeft	As Single	= -0.939693
Const SinBackLeft	As Single 	= -0.707107		
Const CosBackLeft	As Single	= -0.707107
Const SinBackRight	As Single 	= -0.707107	
Const CosBackRight	As Single	=  0.707107

Public Const LineFollower As Byte = 0
Public Const CanCage As Byte = 1
Public Const NoAttachment As Byte = 3

Public Attachment As Byte
Public LineColor As Byte

Private CanDetected As Boolean
Private CanDistance As Single

'-------------------------------------------------------------------------------
Public Sub Main()

	Dim Tx As String

	Call PutPin(RightMotorPWM, 0)
	Call PutPin(LeftMotorPWM, 0)

	Call OpenSerialPort(1, 19200)

	Call DefineCom3(EncoderInPin, EncoderOutPin, bx0000_1000)
	Call OpenSerialPort_3(9600)

	Call InitNotes
	Call InitGP2D12CalibrationTable
	Call InitSpeedControl

	Tx = "PatrolBot ready for action!"
	Call PutLine(Tx)
	Call BlinkLEDs
'	Call TestNotes(PiezoPin)
'	Call TestPiezo
	Call InitializePWM(1)

'	Call SpinLeft(4.0)
'	Call SpinRight(4.0)
'	Call Wander
'	Call TestIR
'	Call BeAloof

'	Call TestGP2D12

'	Call TestCom3

'	Call TestEncoder

'	Call DetermineAttachment

'	Call WaitForIRStart

'	If Attachment = LineFollower Then
'		Call LineFollow
'	ElseIF Attachment = CanCage Then
'		Call CanCan
'	Else
'		Call BeAloof
'	End If

	Call SpeedControl

	Debug.Print "End of Main"
	Do
	Loop
End Sub
'-------------------------------------------------------------------------------
Sub TestCom3()
	Dim LeftEncoderCount As Byte
	Dim RightEncoderCount As Byte
	Dim Success As Boolean

Do
	Call GetByte_3(RightEncoderCount, Success)
	If Success Then
		Call PutByte(RightEncoderCount)
     End If
Loop

End Sub
'-------------------------------------------------------------------------------
Sub TestEncoder()
	
	Dim C As Byte
	Dim LeftEncoderCount As Integer
	Dim RightEncoderCount As Integer
	Dim Success As Boolean

	Call PutPinPWM(RightMotorPWM, 0.5)
	Call PutPinPWM(LeftMotorPWM, 0.5)

Do
	Do
	    Call GetByte_3(C, Success)
	Loop Until (Success And (C = 13))

	Call GetHexWord_3(LeftEncoderCount, Success)
	If Success then
		Call GetByte_3(C, Success)
         If Success And (C = 32) Then
			Call GetHexWord_3(RightEncoderCount, Success)
			If Success Then
				Debug.Print "L"; Cstr(LeftEncoderCount); " R"; Cstr(RightEncoderCount)
			End If
		End If
	End If
Loop

End Sub
'-------------------------------------------------------------------------------
Sub GetHexWord_3(ByRef Value As Integer, ByRef Success As Boolean)

    Dim D As Byte
    Dim i As Integer
    Dim MySuccess As Boolean
    Const Base As Integer = 16
    Const ASCIIhexBias As Byte = 55

	Value = 0

	For i = 1 to 4
		Do
	    		Call GetByte_3(D, MySuccess)
		Loop Until MySuccess

    		If (D >= Asc("0")) And (D <= Asc("9")) Then
			Value = Value * Base + CInt(D - Asc("0"))
         ElseIf (D >= Asc("A")) And (D <= Asc("F")) Then
			Value = Value * Base + CInt(D - ASCIIHexBias)
		Else
			Success = False
			Exit Sub
		End If
	Next

	Success = True

End Sub
'-------------------------------------------------------------------------------
Sub DetermineAttachment()

	Call PutPin(AttachmentIDPin1, bxInputPullup)
	Call PutPin(AttachmentIDPin2, bxInputPullup)

	Attachment = GetPin(AttachmentIDPin1) * 2 + GetPin(AttachmentIDPin2)

	Debug.Print "Attachment = "; CStr(Attachment)	

End Sub
'-------------------------------------------------------------------------------

Sub WaitForIRStart()

	Debug.Print "Awaiting Start (Left Front IR)"

	LineColor = 1

	Call PutPin(ALED, LineColor)
	
	Do
		Call PlayNote(PiezoPin, 1, 3, 200)
		If (GetADC(IRBackLeft) > 140) Then
			LineColor = 1 - LineColor
			Call PutPin(ALED, LineColor)
		End If
	Loop Until GetADC(IRFrontLeft) > 140

End Sub
'-------------------------------------------------------------------------------

Sub CanCan()

	Dim CansCollected As Integer

	CansCollected = 0

	Do
		Call GoToNextSearchPoint
		Call DetectACan

		If CanDetected Then
			Call ApproachCan
			Call RetrieveCan
			Call ReleaseCan
			CansCollected = CansCollected + 1
			Call PutPin(ALED, 0)
			Call PutPin(BLED, 0)
		End If

	Loop Until CansCollected = 6

End Sub
'-------------------------------------------------------------------------------
Sub BlinkLEDs()

    Const LEDon As Byte = 0
    Const LEDoff As Byte = 1

    ' A pulse.
    Call PutPin(ALED, 1)
    Call Delay(0.5)
    Call PutPin(ALED, 0)

    ' B pulse.
    Call PutPin(BLED, 1)
    Call Delay(0.5)
    Call PutPin(BLED, 0)

    ' Red pulse.
    Call PutPin(RedLED, 0)
    Call Delay(0.5)
    Call PutPin(RedLED, 1)

    ' Green pulse.
'    Call PutPin(GreenLED, 0)
'    Call Delay(0.5)
'    Call PutPin(GreenLED, 1)


End Sub
'-------------------------------------------------------------------------------
Sub TestIR()

    Dim PotPosition As Single  ' Ranges from 0.0 to 1.0.

    Dim PotPin As Byte
Do
    For PotPin = 13 to 17
        ' Read nondimensional pot position.
        Call GetAdc(PotPin, PotPosition)
        Call PutS(PotPosition)
        Call PutByte(32)
    Next
    Call Delay(0.5)
    Call NewLine
Loop
End Sub
'-------------------------------------------------------------------------------
Sub TestPiezo()
	Dim Frequencies(1 to 9) as Integer
	Dim IRReading as Single
	Dim Index as Integer

	Frequencies(1) = 2093
	Frequencies(2) = 2350
	Frequencies(3) = 2638
	Frequencies(4) = 2795
	Frequencies(5) = 3137
	Frequencies(6) = 3520
	Frequencies(7) = 3951
	Frequencies(8) = 4187
	Frequencies(9) = 4700

Do
	Call GetAdc(IRFront, IRReading)
	Index = CInt(IRReading * 25.0) - 2
	If Index > 8 Then
		Index = 8
	End If
	Call PutI(Index)
	Call NewLine
	If Index > 0 Then
		Call FreqOut(PiezoPin, Frequencies(Index), Frequencies(Index)+4, 20)
	End If
Loop

End Sub
'-------------------------------------------------------------------------------
Sub GoToNextSearchPoint()

	Dim I as Integer, J As Integer

	Dim IRReading As Integer

	Debug.Print "Going To Next Search Point"

	I = 0

	Call PutPin(RightMotorMinus, 0)
	Call PutPin(LeftMotorMinus, 1)

	Do
		Call PutPinPWM(RightMotorPWM, 1.0)
		Call PutPinPWM(LeftMotorPWM,0.0)

		Call PlayNote(PiezoPin, 3, 1, 20)

		I = I + 1

	Loop Until ((GetADC(IRBackRight) > 120) And (GetADC(IRBackLeft) < 100)) Or (I >= 20)

	For J = 1 to 10
		Call PutPinPWM(RightMotorPWM, 1.0 - (CSng(J) / 10.0))
		Call PutPinPWM(LeftMotorPWM,0.0 + (CSng(J) / 10.0))
		Call Delay(0.01)
	Next

	Call PutPin(LeftMotorMinus, 0)

	For J = 1 To 60
		IRReading = GetADC(IRBackRight)

		If IRReading > 140 Then
			Call PutPinPWM(RightMotorPWM, 0.6)
			Call PutPinPWM(LeftMotorPWM, 1.0)
			Call PlayNote(PiezoPin, 3, 4, 50)
		ElseIf IRReading < 80 Then
			Call PutPinPWM(RightMotorPWM, 1.0)
			Call PutPinPWM(LeftMotorPWM, 0.6)
			Call PlayNote(PiezoPin, 1, 4, 50)
		Else
			Call PutPinPWM(RightMotorPWM, 0.95)
			Call PutPinPWM(LeftMotorPWM, 1.0)
			Call PlayNote(PiezoPin, 2, 4, 50)
		End If
	
	Next

End Sub
'-------------------------------------------------------------------------------
Sub DetectACan()

	Dim LeftDuty As Single, RightDuty as Single
	Dim IRFrontDist As Single
	Dim PrevFrontDist As Single
	Dim I As Byte

	Const DetectionThreshold As Single = 0.07
	Const CoarseRotateSpeed As Single = 0.5
	Const FineRotateSpeed As Single = 0.45

	PrevFrontDist = 1.0
	Call PutPin(ALED, 0)
	CanDetected = False
	Debug.Print "Detecting"

	Do
		Call GetAdc(IRFront, IRFrontDist)
'		Call GetAdc(IRForkLeft, IRForkLeftDist)
'		Call GetAdc(IRForkRight, IRForkRightDist)

'		Call PutS(IRForkLeftDist)
'		Call PutByte(Asc(" "))
'		Call PutS(IRFrontDist)
'		Call PutByte(Asc(" "))
'		Call PutS(IRForkRightDist)
'		Call NewLine

		If (IRFrontDist > (PrevFrontDist + DetectionThreshold)) Then
			Call Delay (0.02)
			Call PutPin(LeftMotorMinus, 0)
			Call PutPin(RightMotorMinus, 0)
			Call PutPinPWM(RightMotorPWM, 0.0)
			Call PutPinPWM(LeftMotorPWM, 0.0)
			Call PutPin(ALED, 1)
			CanDetected = True
			CanDistance = IRFrontDist
			Exit Sub
		Else
			' rotate left quickly
			Call PutPin(LeftMotorMinus, 1)
			Call PutPin(RightMotorMinus, 0)
			LeftDuty = 1.0 - CoarseRotateSpeed
			RightDuty = CoarseRotateSpeed
		End If

		Call PutPinPWM(RightMotorPWM, RightDuty)
		Call PutPinPWM(LeftMotorPWM, LeftDuty)
		PrevFrontDist = IRFrontDist
		Call Delay(0.01)
    Loop

End Sub
'-------------------------------------------------------------------------------
Sub ApproachCan()
	Dim i As Integer, LeftDuty As Single, RightDuty as Single
	Dim IRFrontDist As Single, IRLeftDist As Single, IRRightDist As Single
	Dim IRForkLeftDist As Single, IRForkRightDist As Single

	Const DetectionThreshold As Single = 0.07
	Const AcquireThreshold As Single = 0.5
	Const ApproachSpeed As Single = 0.6
	Const CoarseRotateSpeed As Single = 0.55
	Const FineRotateSpeed As Single = 0.6
	Const SlowSpeed As Single = 0.5

	Debug.Print "Approaching"

	Call PutPin(BLED, 0)
	Do
		Call GetAdc(IRFront, IRFrontDist)
		Call GetAdc(IRForkLeft, IRForkLeftDist)
		Call GetAdc(IRForkRight, IRForkRightDist)

'		Call PutS(IRForkLeftDist)
'		Call PutByte(Asc(" "))
'		Call PutS(IRFrontDist)
'		Call PutByte(Asc(" "))
'		Call PutS(IRForkRightDist)
'		Call NewLine

		Call PutPin(LeftMotorMinus, 0)
		Call PutPin(RightMotorMinus, 0)

		If IRFrontDist > AcquireThreshold Then
			' can acquired, stop
			Call PutPinPWM(RightMotorPWM, 0.0)
			Call PutPinPWM(LeftMotorPWM, 0.0)
			Call PutPin(BLED, 1)
			Call PutPin(RedLED, 1)
			CanDetected = True
			Exit Sub
		ElseIf (IRFrontDist >= (CanDistance - 0.02)) And _
		((IRForkLeftDist < CanDistance) OR (IRForkRightDist < CanDistance)) Then
			' Go forward
			Call PutPin(RedLED, 1)
			If IRForkLeftDist < IRFrontDist Then
				LeftDuty = ApproachSpeed
			Else
				LeftDuty = SlowSpeed
			End If
			If IRForkRightDist < IRFrontDist Then
				RightDuty = ApproachSpeed
			Else
				RightDuty = SlowSpeed
			End If
		ElseIf IRFrontDist < (CanDistance - 0.02) Then
			' Uh oh, we lost track of the can with the center sensor...
			Call PutPin(RedLED, 0)
			If IRForkLeftDist >= CanDistance Then
				' Left sensor still sees it, go sharp left
				Call PlayNote(PiezoPin, 2, 1, 100)
				LeftDuty = 0.0
				RightDuty = SlowSpeed
			ElseIf IRForkRightDist >= CanDistance Then
				' Right sensor still sees it, go sharp right
				Call PlayNote(PiezoPin, 3, 1, 100)
				RightDuty = 0.0
				LeftDuty = SlowSpeed
			Else
				' Lost track completely! relocate.
				Call PlayNote(PiezoPin, 4, 1, 100)
				Call PlayNote(PiezoPin, 3, 5, 100)
				Call DetectACan
			End If
		Else
			'We were sensing a wall! go back to detection routine.
			Call DetectACan
		End If

		Call PutPinPWM(RightMotorPWM, RightDuty)
		Call PutPinPWM(LeftMotorPWM, LeftDuty)
		Call Delay (0.01)

    Loop


End Sub
'-------------------------------------------------------------------------------
Sub RetrieveCan()

	Dim I as Integer, J as Integer
	Dim LeftDuty As Single, RightDuty As Single

	Debug.Print "Retrieving"

	' ramp up to full speed
	For I = 6 To 9
		Call PutPinPWM(RightMotorPWM, CSng(I) * 0.1)
		Call PutPinPWM(LeftMotorPWM, CSng(I) * 0.1)
		Call Delay(0.1)
	Next 

	' come around to the left
	For I = 9 to 6 Step -1
		Call PutPinPWM(LeftMotorPWM, CSng(I) * 0.1)
		Call Delay(0.2)
	Next 

	Call Delay(0.2)

	' straighten out while slowing down
	For I = 6 to 9
		Call PutPinPWM(LeftMotorPWM, CSng(I) * 0.1)
		Call Delay(0.2)
	Next 

End Sub

Sub ReleaseCan()

	Dim I As Byte

	Debug.Print "Releasing"

	' slow to a stop
	For I = 5 To 0 Step -1
		Call PutPinPWM(RightMotorPWM, CSng(I) * 0.1)
		Call PutPinPWM(LeftMotorPWM, CSng(I) * 0.1)
		Call Delay(0.2)
	Next 

	' back up
	Call PutPin(RightMotorMinus, 1)
	Call PutPin(LeftMotorMinus, 1)
	For I = 3 To 8
		Call PutPinPWM(RightMotorPWM, 1.0 - (CSng(I) * 0.1))
		Call PutPinPWM(LeftMotorPWM, 1.0 - (CSng(I) * 0.1))
		Call Delay(0.2)
	Next 

	' slow to a stop

	For I = 7 To 0 Step -1
		Call PutPinPWM(RightMotorPWM, 1.0 - (CSng(I) * 0.1))
		Call PutPinPWM(LeftMotorPWM, 1.0 - (CSng(I) * 0.1))
		Call Delay(0.2)
	Next 
	Call PutPin(RightMotorMinus, 0)
	Call PutPinPWM(RightMotorPWM, 0.0)
	Call PutPin(LeftMotorMinus, 0)
	Call PutPinPWM(LeftMotorPWM, 0.0)

End Sub
'-------------------------------------------------------------------------------
Sub Wander()

	Dim i As Integer, LeftDuty As Single, RightDuty as Single
	Dim IRFrontDist As Single, IRLeftDist As Single, IRRightDist As Single
	Dim IRBackLeftDist As Single, IRBackRightDist As Single
	Dim ClosestDist As Single
	Dim NoteIndex As Integer, Octave As Integer, Note As Integer


    Do
		Call GetAdc(IRFront, IRFrontDist)
		ClosestDist = IRFrontDist

		Call GetAdc(IRFrontRight, IRRightDist)
		If IRRightDist > ClosestDist Then
			ClosestDist = IRRightDist
		End If

		Call GetAdc(IRFrontLeft, IRLeftDist)
		If IRLeftDist > ClosestDist Then
			ClosestDist = IRLeftDist
		End If

		Call GetAdc(IRBackRight, IRBackRightDist)
		If IRBackRightDist > ClosestDist Then
			ClosestDist = IRBackRightDist
		End If

		Call GetAdc(IRBackLeft, IRBackLeftDist)
		If IRBackLeftDist > ClosestDist Then
			ClosestDist = IRBackLeftDist
		End If

		If IRFrontDist > 0.25 Then
			Call PutPin(RightMotorMinus, 1)
			LeftDuty = 1.0
			RightDuty = 0.0
		Else
			Call PutPin(RightMotorMinus, 0)
			LeftDuty = 1.0 - (IRRightDist * 1.8)
			RightDuty = 1.0 - (IRLeftDist * 1.8)
		End If

		Call PutPinPWM(RightMotorPWM, RightDuty)
		Call PutPinPWM(LeftMotorPWM, LeftDuty)

		If ClosestDist > 0.04 Then
			NoteIndex = CInt(ClosestDist * 80.0)
			Octave = NoteIndex \ 8
			If Octave < 1 Then
				Octave = 1
			ElseIf Octave > 4 Then
				Octave = 4
			End If
			Note = NoteIndex Mod 8
			If Note < 1 Then
				Note = 1
			ElseIf Note > 8 Then
				Note = 8
			End If
			PutI Octave
			PutByte(32)
			PutI Note
			NewLine
         		Call PlayNote(PiezoPin, Octave, Note, 100)
		Else
			Call Delay(0.1)
		End If

    Loop

End Sub
'-------------------------------------------------------------------------------
Sub BeAloof()

	Dim i As Integer, LeftDuty As Single, RightDuty as Single
	Dim NoteIndex As Integer, Octave As Integer, Note As Integer
	Dim XForce As Single, YForce As Single
	Dim OldLeftDuty As Single, OldRightDuty As Single

	OldLeftDuty = 0.0
	OldRightDuty = 0.0

	Const MaxDeltaSpeed As Single = 0.3
	Const Gain As Single = 20.0

 	Do
		Call CalculateObstacleForce(XForce, YForce)

		If YForce < 0.1 Then
			If XForce < 0.0 Then
				LeftDuty = 1.0
				RightDuty = 1.0 - (XForce * Gain)
			Else
				LeftDuty = 1.0 - (XForce * Gain)
				RightDuty = 1.0
			End If
		Else
			LeftDuty = (-YForce * Gain) - (XForce * Gain)
			RightDuty = (-YForce * Gain) + (XForce * Gain)
		End If

'		Call PutS(XForce)
'		Call PutByte(Asc(" "))
'		Call PutS(YForce)
'		Call PutByte(Asc(" "))
'		Call PutS(LeftDuty)
'		Call PutByte(Asc(" "))
'		Call PutS(RightDuty)
'		Call NewLine

		If LeftDuty > OldLeftDuty + MaxDeltaSpeed Then
			LeftDuty = OldLeftDuty + MaxDeltaSpeed
		ElseIf LeftDuty < OldLeftDuty - MaxDeltaSpeed Then
			LeftDuty = OldLeftDuty - MaxDeltaSpeed
		End If
		If RightDuty > OldRightDuty + MaxDeltaSpeed Then
			RightDuty = OldRightDuty + MaxDeltaSpeed
		ElseIf RightDuty < OldRightDuty - MaxDeltaSpeed Then
			RightDuty = OldRightDuty - MaxDeltaSpeed
		End If

		If LeftDuty > 1.0 Then 
			LeftDuty = 1.0
		ElseIf LeftDuty < -1.0 Then 
			LeftDuty = -1.0
		End If
		If RightDuty > 0.95 Then 
			RightDuty = 0.95
		ElseIf RightDuty < -0.95 Then 
			RightDuty = -0.95
		End If


		If RightDuty < 0.0 Then
			Call PutPin(RightMotorMinus, 1)
			Call PutPinPWM(RightMotorPWM, 1.0 - Abs(RightDuty))
		Else
			Call PutPin(RightMotorMinus, 0)
			Call PutPinPWM(RightMotorPWM, RightDuty)
		End If

		If LeftDuty < 0.0 Then
			Call PutPin(LeftMotorMinus, 1)
			Call PutPinPWM(LeftMotorPWM, 1.0 - Abs(LeftDuty))
		Else
			Call PutPin(LeftMotorMinus, 0)
			Call PutPinPWM(LeftMotorPWM, LeftDuty)
		End If

		OldRightDuty = RightDuty
		OldLeftDuty = LeftDuty
		Call PlayNoteForForce(Sqr((XForce * XForce) + (YForce * YForce)))
    Loop

End Sub
'-------------------------------------------------------------------------------
Sub CalculateObstacleForce( _
	ByRef XForce As Single, _
	ByRef YForce As Single)

	XForce = 0.0
	YForce = 0.0
	Call AddSensorForce(IRFrontRight, CosFrontRight, SinFrontRight, 0.666667, XForce, YForce)
	Call AddSensorForce(IRFront, CosFront, SinFront, 0.666667, XForce, YForce)
	Call AddSensorForce(IRFrontLeft, CosFrontLeft, SinFrontLeft, 0.666667, XForce, YForce)
	Call AddSensorForce(IRBackLeft, CosBackLeft, SinBackLeft, 1.0, XForce, YForce)
	Call AddSensorForce(IRBackRight, CosBackRight, SinBackRight, 1.0, XForce, YForce)

End Sub
'-------------------------------------------------------------------------------
Sub AddSensorForce( _
	ByVal Pin As Byte, _
	ByVal CosTheta As Single, _
	ByVal SinTheta As Single, _
	ByVal ScaleFactor As Single, _
	ByRef TotalXForce As Single, _
	ByRef TotalYForce As Single)

	Dim XForce As Single, YForce As Single

	Call CalculateSensorForce(Pin, CosTheta, SinTheta, XForce, YForce)
	TotalXForce = TotalXForce + (XForce * ScaleFactor)
	TotalYForce = TotalYForce + (YForce * ScaleFactor)
End Sub
'-------------------------------------------------------------------------------
Sub CalculateSensorForce( _
	ByVal Pin As Byte, _
	ByVal CosTheta As Single, _
	ByVal SinTheta As Single, _
	ByRef XForce As Single, _
	ByRef YForce As Single)

	Const ScaleFactor As Single = 1.8
	Dim SensorValue As Single, R as Single

	Call GetADC(Pin, SensorValue)
	R = SensorValue * ScaleFactor
	XForce = R * CosTheta
	YForce = R * SinTheta

End Sub
'-------------------------------------------------------------------------------
Sub SpinRight(ByVal duration As Single)
	Dim LeftDuty As Single, RightDuty as Single

	Call PutPin(RightMotorMinus, 1)
	Call PutPin(LeftMotorMinus, 0)
	LeftDuty = 1.0
	RightDuty = 0.0
	Call PutPinPWM(RightMotorPWM, RightDuty)
	Call PutPinPWM(LeftMotorPWM, LeftDuty)

	Call Delay(Duration)

	Call PutPin(RightMotorMinus, 0)
	LeftDuty = 0.0
	RightDuty = 0.0
	Call PutPinPWM(RightMotorPWM, RightDuty)
	Call PutPinPWM(LeftMotorPWM, LeftDuty)
End Sub
'-------------------------------------------------------------------------------
Sub SpinLeft(ByVal duration As Single)
	Dim LeftDuty As Single, RightDuty as Single

	Call PutPin(RightMotorMinus, 0)
	Call PutPin(LeftMotorMinus, 1)
	LeftDuty = 0.0
	RightDuty = 1.0
	Call PutPinPWM(RightMotorPWM, RightDuty)
	Call PutPinPWM(LeftMotorPWM, LeftDuty)

	Call Delay(Duration)

	Call PutPin(LeftMotorMinus, 0)
	LeftDuty = 0.0
	RightDuty = 0.0
	Call PutPinPWM(RightMotorPWM, RightDuty)
	Call PutPinPWM(LeftMotorPWM, LeftDuty)
End Sub
'-------------------------------------------------------------------------------
Sub PlayNoteForForce(ByVal Force As Single)

	Dim NoteIndex As Integer, Octave As Integer, Note As Integer

'	Call PutS(Force)
'	Call NewLine

		If Force > 0.05 Then
			NoteIndex = CInt(Force * 80.0)
			Octave = NoteIndex \ 8
			If Octave < 1 Then
				Octave = 1
			ElseIf Octave > 4 Then
				Octave = 4
			End If
			Note = NoteIndex Mod 8
			If Note < 1 Then
				Note = 1
			ElseIf Note > 8 Then
				Note = 8
			End If
'			PutI Octave
'			PutByte(32)
'			PutI Note
'			NewLine
         		Call PlayNote(PiezoPin, Octave, Note, 50)
		Else
			Call Delay(0.05)
		End If
End Sub