This is version 1.1.0 of my software for the Octabot and other similar BOE-Bot type robots. I’d love to hear how it works with a 4WD1 after modifying it to work with the Scorpion motor controller.
[code]
'*************************************************************************************
’ Program: diff-drive+irpd.bas
’
’ Author: Dale Weber [email protected]
’ Web: The Dynaplex Network - http://www.thedynaplex.org
’ Date: 26-Oct-2006
’ Version: 1.1.0
’ Atom IDE: 2.2.1.1
’
’ New: The robot now has control of motor speed and ramps up or down as required to
’ better navigate through its environment.
’
’ Turns are approximately 22 or 45 degrees.
’
’ If the robot detects an obstacle, it will reduce its speed slightly (down to
’ minspeed), so it will move slower in tighter spaces or places where there are a
’ lot of obstacles.
’
’ Each time it moves forward successfully, the robot will increase its speed,
’ slightly, up to maxspeed.
’
’ Purpose: To allow a two wheeled differential drive robot, such as
’ the BOE-Bot or similar robots, to avoid obstacles using the
’ IRPD (Infra Red Proximity Detector) sensor.
’
’ This program is very tunable to a specific robot without
’ modifying any of the actual code. At this time, no other sensors are
’ in use.
’
’ Hardware: Octabot II Robot Kit from Budget Robotics (http://www.budgetrobotics.com)
’ Pair of Large Moon Buggy Wheels from Budget Robotics
’ Lynxmotion Atom Bot Board (http://www.lynxmotion.com)
’ Lynxmotion SES Electronics Carrier
’ Basic Atom 28 Pin Microcontroller (NOT the Pro)
’ Lynxmotion IRPD sensor
’ 7.2V 2800 mAH NiMH Battery Pack
’
'**************************************************************************************
’ For sound output
speaker con p9
dur con 75 ’ Note Duration
note1 con 250
note2 con 2000
note3 con 4500
note4 con 2000
’ Motor Stuff
leftm con p9 ’ Left Motor I/O pin assignment
lefta con 0 ’ Left Motor Speed Adustment
leftd con -1 ’ Left Motor Direction
rightm con p10 ’ Right Motor I/O pin assignment
righta con 0 ’ Right Motor Speed Adustment
rightd con 1 ’ Right Motor Direction
revdir con -1 ’ To easily reverse direction
’ Servo Control (SERVO command)
forwardreps con 1 ’ Forward movement repetitions
backupreps con 10 ’ Backup Repetitions
turnreps con 3 ’ Turn Repetitions
normreps con 1 ’ Normal Repetitions
maxspeed con 1100 ’ Maximum Speed limit on currspeed
minspeed con 0 ’ Minimum Speed limit on currspeed
backspeed con 100 ’ Backup (Reverse) speed
normspeed con 700 ’ Normal Speed (currspeed)
turnspeed con 100 ’ Turning speed
’ IRPD Stuff
rights con p12 ’ Change pin number to pin you are using for your right led Voilet wire
lefts con p13 ’ Change pin number to pin you are using for your left led Blue wire
sensor var in14 ’ Change pin number to pin you are using for your sensor Yellow wire
templ var bit ’ Left sensor status
tempr var bit ’ Right sensor status
detected var word ’ 0 = Nothing detected, 1 = Left Side Hit, 2 = Right Side Hit, 3 = Center Hit
turnrate con 100 ’ Current turn speed
currspeed var word ’ Current Speed
currmove var byte ’ Current number of forward movement repetitions
currturn var byte ’ Current number of turn repetitions
reps var byte ’ How many repitions to do now
turncnt var byte ’ Counts how many times the robot has turned after detecting an obstacle
rampup con 10 ’ Speed increase increment
rampdown con 75 ’ Speed decrease increment
loop var word ’ General looping counters
loop2 var word
'========================================================================
'========================================================================
pause 2000
’
’ Initialize
’
turncnt = 0
currspeed = minspeed
currmove = normreps
currturn = normreps
reps = 1
’
’ Main Loop
’
while 1 ’ Loop forever or until reset/powered off
gosub Move_Forward
gosub Check_IRPD
Detect_Change:
branch detected, [New_Cycle, Hit_Left, Hit_Right, Hit_Center]
goto New_Cycle
Hit_Left:
while detected = 1
gosub Rotate_Right_22
gosub Check_Turns
gosub Check_IRPD
wend
if detected and detected <> 1 then
goto Detect_Change
else
goto New_Cycle
endif
Hit_Right:
while detected = 2
gosub Rotate_Left_22
gosub Check_Turns
gosub Check_IRPD
wend
if detected and detected <> 2 then
goto Detect_Change
else
goto New_Cycle
endif
Hit_Center:
while detected <> 0
’ Backup some
gosub Backup
gosub Check_IRPD
if detected = 1 then
gosub Rotate_Right_22
elseif detected >= 2
gosub Rotate_Left_22
endif
wend
New_Cycle:
wend
’
’ End Main Loop
’
Rotate_Right:
for loop = 1 to turnreps
servo leftm, (turnrate + lefta) * leftd, normreps
servo rightm, (turnrate + righta) * rightd * revdir, normreps
next
return
Rotate_Left:
for loop = 1 to turnreps
servo leftm, (turnrate + lefta) * leftd * revdir, normreps
servo rightm, (turnrate + righta) * rightd, normreps
next
return
Move_Forward:
for loop = 1 to forwardreps
servo leftm, (currspeed + lefta) * leftd, currmove
servo rightm, (currspeed + righta) * rightd, currmove
next
' Speed up a little each time we successfully move forward
currspeed = (currspeed + rampup) max maxspeed
return
Backup:
for loop = 1 to backupreps
servo leftm, (backspeed + lefta) * leftd * revdir, currmove
servo rightm, (backspeed + righta) * rightd * revdir, currmove
' Slow down a bit
currspeed = (currspeed - rampdown) min minspeed
next
return
Check_IRPD
’ Check the Left IR Emitter
for loop = 1 to 4
high lefts
pause 1
low lefts
pause 1
next
high lefts
pause 1
templ = sensor
low lefts
pause 1
' Check the Right IR Emitter
for loop = 1 to 4
high rights
pause 1
low rights
pause 1
next
high rights
pause 1
tempr = sensor
low rights
pause 1
' Set Obstacle Detection Flag
detected = 0
if templ = 0 then
detected = detected + 1
sound speaker,[dur\note1,dur\note2,dur\note3]
endif
if tempr = 0 then
detected = detected + 2
sound speaker,[dur\note3,dur\note2,dur\note1]
endif
if detected then ' Slow down a bit each time we detect an obstacle
currspeed = (currspeed - rampdown) min minspeed
endif
return
Check_Turns:
turncnt = turncnt + 1
if turncnt > 1 then
gosub Backup
turncnt = 0
endif
if detected = 1 then ' Detectøon on left side
gosub RotateÞRight_22
elseif detected = 2
I’m sure there is interest, heck I’m interested in seeing it.
My cat seems to have decided Octabot is her newest toy, so counter measures are needed.