Weird servo behavior, can't find it in my code
flowstate
Posts: 3
I've got a modified version of the SumoWrestler code from the Applied Robotics pack. It's been modified to use 2 Ping sensors. They are working just fine, but for some reason, the right servo seems to be freaking out randomly (most typically in a CurveRight or RotateRight maneuver). Below is the code in its entirety. Any help would be appreciated:
' -----[ Title ]--------------------------------------------------------------' Applied Robotics with the SumoBot - SumoWrestler.bs2
' SumoWrestler.bs2 modified so that each state is contained by a
' subroutine.
' {$STAMP BS2} ' Target = BASIC Stamp 2
' {$PBASIC 2.5} ' Language = PBASIC 2.5
' -----[ I/O Definitions ]---------------------------------------------------
ServoLeft PIN 13 ' Left servo connected to P13
ServoRight PIN 12 ' Right servo connected to P12
qtiPwrLeft PIN 10 ' Left QTI on/off pin P10
qtiSigLeft PIN 9 ' Left QTI signal pin P9
qtiPwrRight PIN 7 ' Right QTI on/off pin P7
qtiSigRight PIN 8 ' Right QTI signal pin P8
DummyPin PIN 6 ' I/O pin for pulse-decay P6
PingPin PIN 15 ' Left Ping Sensor SIG Pin
PingPin2 PIN 14 ' Right Ping Sensor SIG Pin
PingLedLeft PIN 1 ' Debug LED for Left Ping Status
PingLedRight PIN 0 ' Debug LED for Right Ping Status
QTILeftLed PIN 3 ' Debug LED for Left QTI Status
QTIRightLed PIN 2 ' Debug LED for Right QTI Status
' -----[ Constants ]----------------------------------------------------------
InConstant CON 890 ' Constant for converting Ping echo time to distance
inDistance VAR Word ' Distance for left Ping
inDistance2 VAR Word ' Distance for right Ping
time VAR Word ' Stores ping echo time
' SumoBot maneuvers
Forward CON 0 ' Forward
Backward CON 1 ' Backward
RotateLeft CON 2 ' RotateLeft
RotateRight CON 3 ' RotateRight
PivotLeft CON 4 ' Pivot to the left
PivotRight CON 5 ' Pivot to the right
CurveLeft CON 6 ' Curve to the left
CurveRight CON 7 ' Curve to the right
' Servo pulse width rotations
FS_CCW CON 850 ' Full speed counterclockwise
FS_CW CON 650 ' Full speed clockwise
NO_ROT CON 750 ' No rotation
LS_CCW CON 770 ' Low speed counterclockwise
LS_CW CON 730 ' Low speed clockwise
' IR object detectors
IrFreq CON 38500 ' IR LED frequency
' -----[ Variables ]----------------------------------------------------------
temp VAR Word ' Temporary variable
multi VAR Word ' Multipurpose variable
counter VAR Byte ' Loop counting variable.
maneuver VAR Nib ' SumoBot travel maneuver
sensors VAR Byte ' Sensor flags byte
qtiLF VAR sensors.BIT5 ' Stores snapshot of QtiSigLeft
qtiRF VAR sensors.BIT4 ' Stores snapshot of QtiSigRight
irLS VAR sensors.BIT3 ' State of Left Side IR
irLF VAR sensors.BIT2 ' State of Left Front IR
irRF VAR sensors.BIT1 ' State of Right Front IR
irRS VAR sensors.BIT0 ' State of Right Side IR
' -----[ EEPROM Data ]--------------------------------------------------------
RunStatus DATA 0 ' Run status EEPROM byte
QtiThresh DATA Word 0 ' Word for QTI threshold time
' -----[ Initialization ]-----------------------------------------------------
GOSUB Reset ' 5 Second delay
GOSUB Calibrate_Qtis ' Determine b/w threshold
GOSUB Look_About ' Was Goto Look_About
' -----[ Main Routine ]-------------------------------------------------------
DO
IF qtiLF = 1 THEN ' Left qti sees line?
GOSUB Avoid_Tawara_Left ' State = avoid left tawara
ELSEIF qtiRF = 1 THEN ' Right qti sees line?
GOSUB Avoid_Tawara_Right ' State = avoid right tawara
ELSEIF irLF = 1 AND irRF = 1 THEN ' Both? Lunge forward
GOSUB Go_Forward ' State = Go forward
ELSEIF irLF = 1 THEN ' Just left?
GOSUB Track_Front_Left_Object ' State = Track front left obj.
ELSEIF irRF = 1 THEN ' Just right?
GOSUB Track_Front_Right_Object ' State = Track front right obj.
ELSE ' Nothing sensed?
GOSUB Search_Pattern ' State = Search pattern
ENDIF
LOOP
' -----[ Subroutine - Reset ]-------------------------------------------------
Reset:
READ RunStatus, temp ' Byte @RunStatus -> temp
temp = temp + 1 ' Increment temp
WRITE RunStatus, temp ' Store new value for next time
IF (temp.BIT0 = 1) THEN ' Examine temp.BIT0
DEBUG CLS, "Press/release Reset", CR, ' 1 -> end, 0 -> keep going
"button..."
END
ELSE
DEBUG CR, "Program running..."
ENDIF
RETURN
' -----[ Subroutine - Calibrate_Qtis ]----------------------------------------
Calibrate_Qtis:
HIGH qtiPwrLeft ' Turn left QTI on
HIGH qtiSigLeft ' Discharge capacitor
PAUSE 1
RCTIME qtiSigLeft, 1, temp ' Measure charge time
LOW qtiPwrLeft ' Turn left QTI off
multi = temp ' Free temp for another RCTIME
HIGH qtiPwrRight ' Turn right QTI on
HIGH qtiSigRight ' Discharge capacitor
PAUSE 1
RCTIME qtiSigRight, 1, temp ' Measure charge time
multi = (multi + temp) / 2 ' Calculate average
multi = multi / 4 ' Take 1/4 average
IF multi > 220 THEN ' Account for code overhead
multi = multi - 220
ELSE
multi = 0
ENDIF
WRITE QtiThresh, Word multi ' Threshold to EEPROM
RETURN
' -----[ Subroutine - Servos_And_Sensors ]------------------------------------
Servos_And_Sensors:
GOSUB Pulse_Servos ' Call Pulse_Servos subroutine
' Call sensor subroutine(s).
sensors = 0 ' Clear previous sensor values
GOSUB Read_Object_Detectors ' Call Read_Object_Detectors
GOSUB Read_Line_Sensors ' Look for lines
RETURN
' -----[ Subroutine - Pulse_Servos ]------------------------------------------
Pulse_Servos:
' Pulse to left servo
LOOKUP maneuver, [ FS_CCW, FS_CW, FS_CW, FS_CCW,
NO_ROT, FS_CCW, LS_CCW, FS_CCW ], temp
PULSOUT ServoLeft, temp
' Pulse to right servo
LOOKUP maneuver, [ FS_CW, FS_CCW, FS_CW, FS_CCW,
FS_CW, NO_ROT, FS_CW, LS_CW ], temp
PULSOUT ServoRight, temp
RETURN
' -----[ Subroutine - Read_Object_Detectors ]---------------------------------
Read_Object_Detectors:
PULSOUT PingPin, 5 ' Send short pulse to Ping
PULSIN PingPin, 1, time ' Wait for echo
inDistance = inConstant ** time
GOSUB Pulse_Servos ' Called to avoid pausing/stuttering on servos
IF inDistance < 12 THEN
irLF = 1
HIGH PingLedLeft
ELSE
irLF = 0
LOW PingLedLeft
ENDIF
PULSOUT PingPin2, 5 ' Send short pulse to Ping
PULSIN PingPin2, 1, time ' Wait for echo
inDistance2 = inConstant ** time
GOSUB Pulse_Servos
IF inDistance2 < 12 THEN
irRF = 1
HIGH PingLedRight
ELSE
irRF = 0
LOW PingLedRight
ENDIF
RETURN
' -----[ Subroutine - Read_Line_Sensors ]-------------------------------------
Read_Line_Sensors:
HIGH qtiPwrLeft ' Turn on QTIs
HIGH qtiPwrRight
HIGH qtiSigLeft ' Push signal voltages to 5 V
HIGH qtiSigRight
PAUSE 1 ' Wait 1 ms for capacitors
READ QtiThresh, Word temp ' Get threshold time
INPUT qtiSigLeft ' Start the decays
INPUT qtiSigRight
PULSOUT DummyPin, temp ' Wait threshold time
qtiLF = ~qtiSigLeft ' Snapshot of QTI signal states
qtiRF = ~qtiSigRight
IF qtiLF = 1 THEN
HIGH QTILeftLed
ELSE
LOW QTILeftLed
ENDIF
IF qtiRF = 1 THEN
HIGH QTIRightLed
ELSE
LOW QTIRightLed
ENDIF
LOW qtiPwrLeft ' Turn off QTIS
LOW qtiPwrRight
RETURN
' -----[ Subroutine - Avoid_Tawara_Left ]-------------------------------------
Avoid_Tawara_Left:
FOR counter = 1 TO 7 ' Back up
maneuver = Backward
GOSUB Servos_And_Sensors
NEXT
FOR counter = 1 TO 7 ' Turn right
maneuver = RotateRight
GOSUB Servos_And_Sensors
NEXT
RETURN
' -----[ Subroutine - Avoid_Tawara_Right ]------------------------------------
Avoid_Tawara_Right:
FOR counter = 1 TO 7 ' Back up
maneuver = Backward
GOSUB Servos_And_Sensors
NEXT
FOR counter = 1 TO 7 ' Turn left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
NEXT
RETURN
' -----[ Subroutine - Go_Forward ]--------------------------------------------
Go_Forward:
maneuver = Forward ' 1 forward pulse
GOSUB Servos_And_Sensors
RETURN
' -----[ Subroutine - Track_Front_Left_Object ]-------------------------------
Track_Front_Left_Object:
counter = 0
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 7
maneuver = CurveLeft ' Curve left 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = RotateLeft ' Rotate left 30
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
RETURN
' -----[ Subroutine - Track_Front_Right_Object ]------------------------------
Track_Front_Right_Object:
counter = 0
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 7
maneuver = CurveRight ' Curve right 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = RotateRight ' Rotate right 30
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
RETURN
' -----[ Subroutine - Track_Side_Left_Object ]--------------------------------
Track_Side_Left_Object:
DO UNTIL irRF = 1 OR irLF = 1 ' Rotate left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
LOOP
RETURN
' -----[ Subroutine - Track_Side_Right_Object ]-------------------------------
Track_Side_Right_Object:
DO UNTIL irRF = 1 OR irLF = 1 ' Rotate right
maneuver = RotateRight
GOSUB Servos_And_Sensors
LOOP
RETURN
' -----[ Subroutine - Search_Pattern ]----------------------------------------
Search_Pattern:
FOR counter = 1 TO 20 ' and watch all sensors
maneuver = Forward ' Forward
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
Look_About:
FOR counter = 1 TO 6 ' Look right
maneuver = RotateRight
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
FOR counter = 1 TO 12 ' Look left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
FOR counter = 1 TO 6 ' Re-align to forward
maneuver = RotateRight
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
Next_State: ' Exit point of search pattern
RETURN
Comments
Try swapping sensors to see if the problem is with one of the sensors.