My boe-bot is moving not smooth at all!
I have just bought a Boe-Bot kit (BS2) a while ago. I'm interested in controlling the Boe-Bot using a Joystick via RF Transceiver. I have successfully sent serial data from the joystick to the Boe-Bot, but the movements of the Boe-Bot just gave me hard time. The Boe-Bot does not move as smooth as I expected. This is the code that I have been edited based on TiltControlledBoeBot.bs2. Please, give me some comments based on this code.
===============================================
Tansmitter Side:
'{$STAMP BS2}
'{$PBASIC 2.5}
'
PIN I/O Definitions
TRLine PIN 14
TxData PIN 15
'{Joystick}
Xaxispin PIN 6 'Pin of X axis pot on joystick
Yaxispin PIN 7 'pin of Y axis pot on joystick
'
CONSTANTS
T9600 CON 84
Inverted CON $4000 'Value for inverted serial format
Baud CON T9600+Inverted '9600 baud, 8,N,1 inverted
'
[ Variables ]
rcXtime VAR Word 'Time of cap decay on X axis pot
rcYtime VAR Word 'Time of cap decay on Y axis pot
'
Main Routine
HIGH TRLine ' T/R Line
DO
HIGH Xaxispin
PAUSE 20
RCTIME Xaxispin,1,rcXtime
HIGH Yaxispin
PAUSE 20
RCTIME Yaxispin,1,rcYtime
PULSOUT TxData, 1200
SEROUT TxData, Baud, [ "U!",rcXtime.HIGHBYTE, rcXtime.LOWBYTE, rcYtime.HIGHBYTE, rcYtime.LOWBYTE]
PAUSE 10
DEBUG HOME, "RCxtime = ", DEC5 rcXtime, CLREOL, CR,
"RCytime = ", DEC5 rcYtime, CLREOL
LOOP
Receiver Side:
'{$STAMP BS2}
'{$PBASIC 2.5}
'
PIN I/O Definitions
TRLine PIN 14
TxData PIN 15
'
CONSTANTS
T9600 CON 84
Inverted CON $4000 'Value for inverted serial format
Baud CON T9600+Inverted '9600 baud, 8,N,1 inverted
Negative CON 1 ' Left/right tilt
'
[ Variables ]
rcXtime VAR Word 'Time of cap decay on X axis pot
rcYtime VAR Word 'Time of cap decay on Y axis pot
rcYtimeSign VAR rcYtime.BIT15
pulseLeft VAR Word ' Current left pulse value
pulseLeftOld VAR Word ' Previous left pulse value
pulseRight VAR Word ' Current right pulse value
pulseRightOld VAR Word ' Previous right pulse value
d
'
[ Initialization ]
pulseLeft = 750 ' Start with all pulses centered
pulseRight = 750
pulseLeftOld = 750
pulseRightOld = 750
'
[ Main Routine ]
DO ' Begin main routine.
LOW TRLine
LOW TxData
' This SERIN command gets the rcXtime and rcYtime values from the
SERIN TxData, Baud, [WAIT("U!"), rcXtime.HIGHBYTE, rcXtime.LOWBYTE, rcYtime.HIGHBYTE, rcYtime.LOWBYTE]
HIGH TxData
DEBUG HOME, "RCxtime = ", DEC5 rcXtime, CLREOL, CR,
"RCytime = ", DEC5 rcYtime, CLREOL
rcXtime = (30 - ((rcXtime MIN 1 MAX 57) - 1))*5
rcYtime = (30 - ((rcYtime MIN 1 MAX 57) - 1))*5
' Navigation decisions
IF (ABS(rcXtime) > 40) OR (ABS(rcYtime) > 40) THEN
pulseLeft = 750 - rcYtime ' Forward/backward pulses
pulseRight = 750 + rcYtime
IF (rcYtimeSign = Negative) OR (ABS(rcYtime) < 40) THEN 'Turning for forward.
pulseLeft = pulseLeft - rcXtime
pulseRight = pulseRight - rcXtime
ELSE ' Turning for backward.
pulseLeft = pulseLeft + rcXtime
pulseRight = pulseRight + rcXtime
ENDIF
ELSE ' Stay still
pulseLeft = 750
pulseRight = 750
ENDIF
' Increment/decrement routine only adjusts pulse durations by 8 at a time.
IF pulseLeft > (pulseLeftOld + 6) THEN pulseleft = pulseLeftOld + 6
IF pulseLeft < (pulseLeftOld - 6) THEN pulseLeft = pulseLeftOld - 6
IF pulseRight > (pulseRightOld + 6) THEN pulseRight = pulseRightOld + 6
IF pulseRight < (pulseRightOld - 6) THEN pulseRight = pulseRightOld - 6
pulseLeft = pulseLeft MIN 650 MAX 850 ' Keep 650 <= durations <= 850
pulseRight = pulseRight MIN 650 MAX 850
pulseLeftOld = pulseLeft ' Remember old values
pulseRightOld = pulseRight
'DEBUG HOME, "Pin13 = ", DEC5 pulseLeft, CLREOL, CR,
' "Pin12 = ", DEC5 pulseRight, CLREOL
PULSOUT 13, pulseLeft ' Send control pulses to servos
PULSOUT 12, pulseRight
LOOP '
===============================================
Tansmitter Side:
'{$STAMP BS2}
'{$PBASIC 2.5}
'
PIN I/O Definitions
TRLine PIN 14
TxData PIN 15
'{Joystick}
Xaxispin PIN 6 'Pin of X axis pot on joystick
Yaxispin PIN 7 'pin of Y axis pot on joystick
'
CONSTANTS
T9600 CON 84
Inverted CON $4000 'Value for inverted serial format
Baud CON T9600+Inverted '9600 baud, 8,N,1 inverted
'
[ Variables ]
rcXtime VAR Word 'Time of cap decay on X axis pot
rcYtime VAR Word 'Time of cap decay on Y axis pot
'
Main Routine
HIGH TRLine ' T/R Line
DO
HIGH Xaxispin
PAUSE 20
RCTIME Xaxispin,1,rcXtime
HIGH Yaxispin
PAUSE 20
RCTIME Yaxispin,1,rcYtime
PULSOUT TxData, 1200
SEROUT TxData, Baud, [ "U!",rcXtime.HIGHBYTE, rcXtime.LOWBYTE, rcYtime.HIGHBYTE, rcYtime.LOWBYTE]
PAUSE 10
DEBUG HOME, "RCxtime = ", DEC5 rcXtime, CLREOL, CR,
"RCytime = ", DEC5 rcYtime, CLREOL
LOOP
Receiver Side:
'{$STAMP BS2}
'{$PBASIC 2.5}
'
PIN I/O Definitions
TRLine PIN 14
TxData PIN 15
'
CONSTANTS
T9600 CON 84
Inverted CON $4000 'Value for inverted serial format
Baud CON T9600+Inverted '9600 baud, 8,N,1 inverted
Negative CON 1 ' Left/right tilt
'
[ Variables ]
rcXtime VAR Word 'Time of cap decay on X axis pot
rcYtime VAR Word 'Time of cap decay on Y axis pot
rcYtimeSign VAR rcYtime.BIT15
pulseLeft VAR Word ' Current left pulse value
pulseLeftOld VAR Word ' Previous left pulse value
pulseRight VAR Word ' Current right pulse value
pulseRightOld VAR Word ' Previous right pulse value
d
'
[ Initialization ]
pulseLeft = 750 ' Start with all pulses centered
pulseRight = 750
pulseLeftOld = 750
pulseRightOld = 750
'
[ Main Routine ]
DO ' Begin main routine.
LOW TRLine
LOW TxData
' This SERIN command gets the rcXtime and rcYtime values from the
SERIN TxData, Baud, [WAIT("U!"), rcXtime.HIGHBYTE, rcXtime.LOWBYTE, rcYtime.HIGHBYTE, rcYtime.LOWBYTE]
HIGH TxData
DEBUG HOME, "RCxtime = ", DEC5 rcXtime, CLREOL, CR,
"RCytime = ", DEC5 rcYtime, CLREOL
rcXtime = (30 - ((rcXtime MIN 1 MAX 57) - 1))*5
rcYtime = (30 - ((rcYtime MIN 1 MAX 57) - 1))*5
' Navigation decisions
IF (ABS(rcXtime) > 40) OR (ABS(rcYtime) > 40) THEN
pulseLeft = 750 - rcYtime ' Forward/backward pulses
pulseRight = 750 + rcYtime
IF (rcYtimeSign = Negative) OR (ABS(rcYtime) < 40) THEN 'Turning for forward.
pulseLeft = pulseLeft - rcXtime
pulseRight = pulseRight - rcXtime
ELSE ' Turning for backward.
pulseLeft = pulseLeft + rcXtime
pulseRight = pulseRight + rcXtime
ENDIF
ELSE ' Stay still
pulseLeft = 750
pulseRight = 750
ENDIF
' Increment/decrement routine only adjusts pulse durations by 8 at a time.
IF pulseLeft > (pulseLeftOld + 6) THEN pulseleft = pulseLeftOld + 6
IF pulseLeft < (pulseLeftOld - 6) THEN pulseLeft = pulseLeftOld - 6
IF pulseRight > (pulseRightOld + 6) THEN pulseRight = pulseRightOld + 6
IF pulseRight < (pulseRightOld - 6) THEN pulseRight = pulseRightOld - 6
pulseLeft = pulseLeft MIN 650 MAX 850 ' Keep 650 <= durations <= 850
pulseRight = pulseRight MIN 650 MAX 850
pulseLeftOld = pulseLeft ' Remember old values
pulseRightOld = pulseRight
'DEBUG HOME, "Pin13 = ", DEC5 pulseLeft, CLREOL, CR,
' "Pin12 = ", DEC5 pulseRight, CLREOL
PULSOUT 13, pulseLeft ' Send control pulses to servos
PULSOUT 12, pulseRight
LOOP '
Comments
You also have two "PAUSE 20" and one "PAUSE 10" in your TX loop. The PAUSE 20 should be much smaller, it's purpose is only to allow time to charge the capacitor for the RCTIME function. Try Pause 1 and increase only if necessary.
I'm not sure that you need the PAUSE 10 after the SEROUT command. Try commenting it out.
Rich H
Thanks agian!
The key thing to remember is that the servos are each expecting a pulse once every 20 milliseconds. Too long between pulses and you will get jerky movements. A very short loop may need a PAUSE 20 to keep the pulses from coming too fast but a longer loop like you have probably takes more than 20 milliseconds to complete. SERIN and DEBUG statements can each take several milliseconds. The two PULSOUTs will take just over three milliseconds to execute. SERIN may wait quite a long while for the right data, unless you utilize it's timeout feature.
On the Transmitter side you want it to be sending the data continuously as fast as possible so that the receiver does not have to spend any extra time waiting for the transmission.
Rich H