|
'******************* MiniRoach **************************
'Mini Roach - Hexapod w/ Forward Sharp Sensor
'@ Version 1.0
'@ Author: Brian Patton
'@ Last Updated: feb 20, 2003
'********************************************************
'************* Declare Constants ************************
Public Const LeftServo As Byte = 6 ' Left servo connected to RAMB pin 1
Public Const RightServo As Byte = 5 ' Right servo connected to RAMB pin 0
Public Const CenterServo As Byte = 7 ' Center Servo connected to RAMB pin 2
Public Const IRPin As Byte = 13
Public Const LeftServoMin As Single = 0.0019 ' Amount Back
Public Const LeftServoMid As Single = 0.0015
Public Const LeftServoMax As Single = 0.0011 ' Amount Forward
Public Const RightServoMin As Single = 0.0011 ' Amount forward
Public Const RightServoMid As Single = 0.0015
Public Const RightServoMax As Single = 0.0019 ' Amount Back
Public Const CenterServoLeft As Single = 0.0012 ' Rise in Right Leg
Public Const CenterServoMid As Single = 0.0015
Public Const CenterServoRight As Single = 0.0017 ' Rise in Left Leg
Public Const ServoCommandSleep As Integer = 10 ' 10 x 1/512 second =0.0195sec
Const ServoPulses As Byte = 15
'********************************************************
'************* Declare Variables ************************ **
Public LeftServoPos As Single
Public RightServoPos As Single
Public CenterServoPos As Single
Public SensorValue As Integer
Public SensorAve As Integer
Dim i As Byte
Dim j As Byte
Dim ContinueForward As Boolean
'********************************************************
'************* Main *************************************
Sub Main() 'Start of Main Program
Sleep(3.0)
ContinueForward = True
Do
If ContinueForward = True Then
LeftServoPos = LeftServoMin
RightServoPos = RightServoMax
CenterServoPos = CenterServoRight
Call CheckSensors()
Call SendServoCommands()
LeftServoPos = LeftServoMax
RightServoPos = RightServoMin
CenterServoPos = CenterServoLeft
Call CheckSensors()
Call SendServoCommands()
Else
Sleep(1.0)
End If
Loop
End Sub 'Main Program
'******************************************************
'****************** Check Sensors *********************
Sub CheckSensors()
SensorValue = GetADC(IRPin)+GetADC(IRPin)+GetADC(IRPin)
' Get three IR readings
SensorAve = Sensorvalue \ 3 ' Divide them by 3 to get the average
' Helps eliminate some error
Debug.Print CStr(SensorAve) ' Print value on the screen
If Sensorave > 500 Then ' See if something is too close
Call TurnRight ' Call right
End If
' End of too close scenario
If Sensorave < 250 Then ' See if something is too far like an edge
Call Backup
Call TurnLeft ' Call right
End If ' End of too far scenario
End Sub
'********************************************************
'***** Turn left ****************************************
Sub TurnLeft()
Dim i As Byte
For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMin
LeftServoPos = LeftServoMin
Call SendServoCommands()
CenterServoPos = CenterServoRight
RightServoPos = RightServoMax
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next
End Sub
'********************************************************
'***** Turn right ***************************************
Sub TurnRight()
Dim i As Byte
For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMax
LeftServoPos = LeftServoMax
Call SendServoCommands()
CenterServoPos = CenterServoRight
RightServoPos = RightServoMin
LeftServoPos = LeftServoMin
Call SendServoCommands()
Next
End Sub
'********************************************************
'******** Backup ****************************************
Sub Backup()
Dim i As Byte
For i=1 To 3
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMax
LeftServoPos = LeftServoMin
Call SendServoCommands()
CenterServoPos = CenterServoRight
RightServoPos = RightServoMin
LeftServoPos = LeftServoMax
Call SendServoCommands()
Next
End Sub
'********************************************************
'******** Center Legs ***********************************
Sub CenterLegs()
CenterServoPos = CenterServoLeft
RightServoPos = RightServoMid
Call SendServoCommands()
CenterServoPos = CenterServoRight
LeftServoPos = LeftServoMid
Call SendServoCommands()
CenterServoPos = CenterServoMid
Call SendServoCommands()
Sleep(2)
End Sub
'********************************************************
'******** Send Commands to Servos ***********************
Sub SendServoCommands()
Dim j As Byte
For j = 1 to ServoPulses
Call PulseOut(CenterServo, CenterServoPos, 1)
Sleep(ServoCommandSleep)
Next
For j = 1 to ServoPulses
Call PulseOut(LeftServo, LeftServoPos, 1)
Call PulseOut(RightServo, RightServoPos, 1)
Call PulseOut(CenterServo, CenterServoPos, 1)
Sleep(ServoCommandSleep)
Next
End Sub
|
Robodyssey Systems manufactures Robot Kits including Autonomous Mobile
Walking Robots, Wheeled Robots, Talking Robots, Expressive Robots, and
Social Robots. We also sell Robotics Accessories including Nexcell NiMH AA
Rechargeable Batteries, Nexcell NiMH AAA Rechargeable Batteries, Battery Holders,
Velcro Straps for Battery Packs, Battery Chargers, Robot Grippers, Hobby Servos,
Servos Modified for Continuous Rotation, Tail Wheels for all Rolling Robotic Platforms,
Polyurethane Skate Wheels with Servo Adapter Hubs, Sharp IR Sensors and Adjustable
Sensor Brackets, Sensor Cables, and Programming Cables. All of our Robot Kits
can be purchased as easy to assemble robot kits or as fully assembled stationary
or mobile robots. Robodyssey uses 1/8 inch aluminum and acetyl for most all of
our mobile robot components and robotics accessories. We provide classroom
training for teachers interested in integrating robotics, computer programming
and electronics into the classroom.
We hope you enjoy our web site. Feel free to use any robotic resources.
Robodyssey is your complete solution for Educational Robotics as well as Hobby Robotics.
|