|
| Proto Code | UserPreferences |
| MoinMoin Wiki | FrontPage | RecentChanges | TitleIndex | WordIndex | Help |
The code is based around a threshold detector. Each sensed pulse of the emitter LED is counted and multiplied by an arbitrary number. That is, the counts are not pure counts; they are weighted. One of my sensors seems to be more sensitive than the other so the weights are different. We are also more likely to see a no-detect than a detect so I back off the count at a slower rate for no-detects. If I don't do these things the count will back off faster than my detects and I will never reach my threshold (another arbitrary value, for the moment).
' Rotate to LED
' In PBASIC
'
' { $STAMP BS2 }
' Variables
r_Eye var Bit '** i = Left Eye
l_Eye var Bit '** j = Right Eye
threshold var Byte '** threshold
l_count var Byte '** left eye pulse detect count
r_count var Byte '** right eye pulse 1detect count
threshold = 50 '** set threshold count. This is currently arbitrary.
l_count = 0
r_count = 0
r_Eye = 1
l_Eye = 1 '** eyes are active low on detect
' Mainline
HIGH SC ' Set the I/O Bits As O/P
HIGH SD ' and High
HIGH LED ' Turn OFF LED
PAUSE 100 ' Wait for Robot to Get Setup
RobotData = RobotStop ' Stop Random Movement
GOSUB RobotSend
PAUSE 100 ' Wait for Robot to Get Setup
LOW LED ' Flash LED Twice
PAUSE 200
HIGH LED
PAUSE 200
LOW LED
PAUSE 200
HIGH LED
' Mobile Coaster Control Code Follows
' rev. P-01
'
' Mark Hinkle
'
' Copyright (C) 2003 Mark Hinkle
' May not be reprinted without permission.
'
MainLoop: ' Begin the main program loop
GOSUB S_CheckSensors
GOSUB S_RotateLeft
GOSUB S_RotateRight
DEBUG "l_Count=", DEC l_count," r_Count=", DEC r_count,CR
GOTO MainLoop
END ' We should never get here. If we do, something is broken.
S_RotateLeft: ' Left Eye and seek LED
IF (l_count > threshold) then RotateLeft
RETURN
RotateLeft:
DEBUG "RotateLeft",CR
RobotData = RobotLeft '** Robot(TurnLeft, 80ms)
GOSUB RobotSend '** Robot(TurnLeft, 80ms)
PAUSE 100 '** Robot(TurnLeft, 10ms)
l_count = 0 '** reset count
RETURN
GOTO MainLoop
S_RotateRight: ' Right Eye and seek LED
IF (r_count > threshold) THEN RotateRight
RETURN
RotateRight:
DEBUG "RotateRight",CR
RobotData = RobotRight '** Robot(TurnRight, 80ms)
GOSUB RobotSend '** Robot(TurnRight, 80ms)
PAUSE 100 '** Robot(TurnRight, 10ms)
r_count = 0 '** reset count
RETURN
GOTO MainLoop
S_CheckSensors: ' eye exam
r_Eye = IN3 '** read Right Eye
l_Eye = IN0 '** read Left Eye
' PAUSE 100
' DEBUG "EYE=",DEC l_Eye," ",DEC r_Eye,CR
IF (r_Eye = 0) THEN Update_R_Count
S_Check1:
IF (l_Eye = 0) THEN Update_L_Count
S_Check2: ' Check if no LED was detected
IF ((r_Eye = 1) & (r_count > 0)) THEN Decrement_R_Count '** don't decrement zero or you get nonzero values
S_Check3: ' Check if no LED was detected
IF ((l_Eye = 1) & (l_count > 0)) THEN Decrement_L_Count '** don't decrement zero or you get nonzero values
RETURN
Update_R_Count: ' LED was detected so count it
r_count = r_count + 10 '** less detects are found than no-detects, add a tweaked value
GOTO S_Check1
Update_L_Count: ' LED was detected so count it
l_count = l_count + 20 '** less detects are found than no-detects, add a tweaked value
GOTO S_Check2
Decrement_R_Count: ' no LED detect so back off count
r_count = r_count - 1 '** mcu is fast enough that a no-detect is more likely, so dec slowly
GOTO S_Check3
Decrement_L_Count: ' no LED detect so back off count
l_count = l_count - 1 '** mcu is fast enough that a no-detect is more likely, so dec slowly
RETURN
GOTO MainLoop
END ' we should never get here, if we do then halt all operation.
' SumoBot Interface Code Follows
'
' Myke Predko
'
' Copyright (C) 2001 & 2002 McGraw-Hill
'
' Robot Commands
RobotStop con 0 ' Stop the Robot
Behavior1 con 1 ' Random Movement
Behavior2 con 2 ' Photovore
Behavior3 con 3 ' Photophobe
Behavior4 con 4 ' Wall Hugger/Maze Solver
RobotForward con 5 ' Move Forward for 200 msecs
RobotReverse con 6 ' Move Reverse for 200 msecs
RobotLeft con 7 ' Turn Left for 200 msecs
RobotRight con 8 ' Turn Right for 200 msecs
RobotLEDOn con 9 ' Turn on the Robot's LED
RobotLEDOff con 10 ' Turn off the Robot's LED
RobotPWM0 con 11 ' PWM = 0% Duty Cycle
RobotPWM1 con 12 ' PWM = 1st "Notch"
RobotPWM2 con 13 ' PWM = 2nd "Notch"
RobotPWM3 con 14 ' PWM = 3rd "Notch"
RobotPWM4 con 15 ' PWM = 100% Duty Cycle
RobotPWM con 16 ' Return the Current PWM Value
RobotState con 17 ' Return the Executing State
RobotWhiskers con 18 ' Return State of the "Whiskers"
' Bit 0 - Left "Whisker"
' Bit 1 - Right "Whisker"
RobotCDSL con 19 ' Return Value of Left CDS Cell
RobotCDSR con 20 ' Return Value of Right CDS Cell
RobotButton con 21 ' Return the Last Remote Button Press
' 0 - No Buttons Pressed
' 1 - Leftmost Button Pressed
' 2 - Middle Button Pressed
' 3 - Rightmost Button Pressed
' After "RobotButton" Operation,
' Button Save is Cleared
' Robot Interface Pins
LED con 11 ' LED, Negative Active On
RIR con 12 ' Right Infra-Red Detector
LIR con 13 ' Left Infra-Red Detector
SC con 14 ' Define the I/O Pins
SD con 15
' Robot Interface Variables
RobotData var byte ' Data Byte to Send to/Receive
' from Robot
' Robot Operation Subroutines
RobotSend ' Send the Byte in "RobotData"
low SC ' Hold Low for 1 msec before
pause 1 ' ShIFting in Data
shiftout SD, SC, LSBFIRST, [RobotData]
high SC
return
RobotSendReceive ' Send the Byte in "RobotData"
low SC ' Hold Low for 1 msec before
pause 1 ' ShIFting in Data
shiftout SD, SC, LSBFIRST, [RobotData]
pause 1 ' Wait for Operation to Complete
shiftin SD, SC, LSBPOST, [RobotData]
high SC
return
Just a prototype. Breathe. Inhale... Exhale... Good. Just prototype code...
Since I am primarily a hardware guy, I'd appreciate any CONSTRUCTIVE comments on how to improve my coding.
previous: Mobile Coaster
EditText
of this page
(last modified 2003-10-26 21:40:37)