|
|
|
![]() |
|
|||||||
|
||||||||
![]() |
|
|
Thread Tools | Rate Thread | Display Modes |
|
|
|
#1
|
|||||
|
|||||
|
trentonDrive.c: our joystick/wheel drive code
I thought I'd share this bit of code. This is a single-joystick control routine for a robot chassis that uses two-wheel, two-motor drive. This is the version we are using in competition.
Its features include: Squaring the motor values (not the joystick axes). This desensitizes the joystick and compensates for the non-linear speed vs. voltage function of the drill motors. Turning radius independent of speed. The X Axis sets the ratio of the wheel speeds, not the difference. Y axis dead band independent of Victor deadband. Victor deadband bypassed in software. No x axis deadband. Dynamic rounding for an extra 2 bits of resolution for better slow-speed control. No floating point math. I have been very pleased with how this code performs. It probably won't work on your machine without some modification (e.g. my motors are on pwm10 and pwm11) -- your mileage may vary. But since so many robots use two-wheel drive, it might be useful to many people. Zipped code is attached. Code:
#include "trentonDrive.h"
int clipInt( int in, int lowLimit, int highLimit );
#define RUN_MOTORS 1
// global joystick calibration values.
//
unsigned char xMax = 254;
unsigned char xMin = 0;
unsigned char xMid = 127, yMid = 127;
unsigned char xHalfRange = 128;
#define ROUNDING_BITS 2
#define ROUNDING_CYCLES ( 1 << ROUNDING_BITS )
#define HALF_DEAD_BAND 3
#define CENTER_VALUE 127
#define LEFT_MOTOR pwm11
#define RIGHT_MOTOR pwm10
#define SIGN( x ) ( ((x) > 0 ) ? +1 : -1 )
#define ABS( x ) ( ((x) > 0 ) ? (x) : (-x) )
/*******************************************************************************
* FUNCTION NAME: trentonDrive
* PURPOSE: Interfaces one joystick to the motors for manual driving.
*
* X & Y axes are linear here; the *wheel* values are squared.
*
* I think the motors' speed vs. voltage curve flattens out at high speed,
* so squaring the motors tends to make it more linear. Part of the problem
* of control at high speeds is that making a small change to the motor
* voltage doesn't make much difference when the curve has a low slope. Squaring
* the motors helps this.
*
*
* CALLED FROM: this file, Default_routine
* ARGUMENTS: xAxis, yAxis: joystick values in.
* leftWheel, rightWheel: PWM values out
* RETURNS: nothing
*******************************************************************************/
void
trentonDrive( unsigned char xAxis, unsigned char yAxis )
{
int tmp;
static unsigned char fracCycle = 0; // for dynamic rounding
unsigned char xx, yy;
char signX, signY;
long innerWheel, outerWheel;
long left, right; // wheels
unsigned char xMid = 127;
unsigned char yMid = 127;
// break the x & y axes into absolute value and sign.
//
tmp = ( (int) xAxis ) - xMid;
signX = SIGN( tmp );
xx = ABS ( tmp );
tmp = ( (int) yAxis ) - yMid;
signY = SIGN( tmp );
yy = ABS ( tmp );
// give the y axis a deadband so the motors
// will stop when you let go!
//
if ( yy < HALF_DEAD_BAND )
signY = 0;
// The main feature of this routine is that
// for a given x axis position, the ratio of the
// wheel speeds is independent of the y axis. This
// means that the turning radius won't change as you
// change the speed of a turn.
//
// The outer wheel goes faster in a turn.
//
// When the X axis is all the way left, the left
// wheel will stop and the robot will turn on the
// left wheel at a speed determined by the y axis.
// The same goes for the right.
// these have a range as big as 2^7
//
outerWheel = (xHalfRange + (long)(xx)) * yy / xHalfRange;
innerWheel = (xHalfRange - (long)(xx)) * yy / xHalfRange;
// square the wheel values
// This compensates for the non-linear speed/voltage
// relationship of the motors.
//
// The range will be (2^7 * 2^7) = 2^14
//
outerWheel *= outerWheel;
innerWheel *= innerWheel;
// re-range it, 2^14 to 2^9
// the "+ 16" is like adding 0.5 before rounding.
//
outerWheel = ( outerWheel + 16 ) >> 5;
innerWheel = ( innerWheel + 16 ) >> 5;
// apply sign of Y axis to both wheels
//
outerWheel *= signY;
innerWheel *= signY;
// Use sign of x axis to determine if left
// or right wheel is the outer wheel of the turn.
//
if ( signX > 0 )
{
left = (int) outerWheel;
right = (int) innerWheel;
}
else
{
right = (int) outerWheel;
left = (int) innerWheel;
}
// The Victor speed controllers have a deadband of about +/- 7 that
// we will now bypass. We are still at the *4 range, so we use 4*7=28.
//
if ( left < 0 )
left -= 28;
else if ( left > 0 )
left += 28;
if ( right < 0 )
right -= 28;
else if ( right > 0 )
right += 28;
// dynamic rounding dither
//
fracCycle++;
if ( fracCycle >= ROUNDING_CYCLES )
fracCycle = 0;
// add 0,1,2,3,0,1,2,3,...
// this dynamic rounding hopes to achieve an extra 2 bits of
// control granularity
//
left = (left + fracCycle ) / 4; // must use division, not right shift,
right = (right + fracCycle ) / 4; // because these are signed values.
// limit the range and send it out
//
RIGHT_MOTOR = (unsigned char) clipInt( left, -127, 127 ) + CENTER_VALUE;
LEFT_MOTOR = (unsigned char) clipInt( right, -127, 127 ) + CENTER_VALUE;
#if RUN_MOTORS == 0
LEFT_MOTOR = CENTER_VALUE;
RIGHT_MOTOR = CENTER_VALUE;
#endif
}
int clipInt( int in, int lowLimit, int highLimit )
{
if ( in > highLimit )
return highLimit;
else if ( in < lowLimit )
return lowLimit;
else
return in;
}
|
|
#2
|
||||
|
||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Just want to say very clever.
![]() |
|
#3
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Very nice routine, think I'll take it for a spin |
|
#4
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
So, my actual declarations are Code:
// global calibration values. These values // are initalized from EEPROM. // unsigned char xMax, xMin, yMin, yMax; unsigned char xMid = 127, yMid = 127; // but get set to (xMax + xMin) >> 1 unsigned char xHalfRange = 128; And, in User_Initialization(), there is Code:
// restore joystick calibration values from EEPROM // xMin = readEE( XMINp ); // the arguments are address pointers xMax = readEE( XMAXp ); // they are #defined somewhere else to yMin = readEE( YMINp ); // be 0,1,2,3. The valid range is 0..1023. yMax = readEE( YMAXp ); // and derive some values... // xMid = ((int)xMax + (int)xMin + 1 ) >> 1; yMid = ((int)yMax + (int)yMin + 1 ) >> 1; xHalfRange = xMax - xMid; Code:
// process the EEPROM data writing queue. // Joystick calibration values are written there. // processEEQueue(); Quote:
The trentonDrive() function is set up to limit the turning radius to spinning around one wheel or the other. If you hold the stick all the way left and run the stick forward and back, the left wheel motor will not turn, and the robot spins around the left wheel. At one time I had trentonDrive() set up to spin on its axis at the left and right joystick extremes, but it was too hard to control larger-radius turns. But spinning on a dime is sometimes very useful. So we added a button (MANUAL_SPIN_BUTTON) that changed it to "spin on a dime" mode. This is great for maneuvering in tight spots. The function is called squaredSpin because it includes the same squaring of the motor values and dead-band removal that makes trentonDrive work well. So trentonDrive() is actually called like this (in Default_Routine()): Code:
if ( ! calibratingJoystickRange() )
{
if ( MANUAL_SPIN_BUTTON )
squaredSpin ( XAXIS ); // x-axis controls spin (only) speed
else
trentonDrive( XAXIS, YAXIS ); // outer wheel/inner wheel ratio-based drive.
}
Here is squaredSpin(): Code:
void
squaredSpin( unsigned char xAxis )
{
int sign, magnitude;
int temp;
int victorJumpValue;
temp = ((int) xAxis) - ((int) xMid);
if ( ABS( temp ) > HALF_DEAD_BAND )
victorJumpValue = 7;
else
victorJumpValue = 0;
sign = SIGN( temp );
temp *= temp ; // range 2^14
temp = (temp + 64) >> 7;
temp = clipInt( temp, -127, 127) ;
if ( sign > 0 )
{
RIGHT_MOTOR = (unsigned char) (+temp + 127 + victorJumpValue);
LEFT_MOTOR = (unsigned char) (-temp + 127 - victorJumpValue);
}
else
{
LEFT_MOTOR = (unsigned char) (+temp + 127 + victorJumpValue);
RIGHT_MOTOR = (unsigned char) (-temp + 127 - victorJumpValue);
}
}
BTW, our robot does not have wheel encoders. If your robot has wheel encoders and can actually control the speed of the wheels, not just the voltage to the motors, then squaring the motor PWM values in trentonDrive() is probably a bad idea, because it tends to mess up the idea that the ratio of the wheel speeds should not change when you change the Y axis. (To reduce joystick sensitivity, try squaring the joystick x/y axis values instead.) In my case (no encoders), squaring the wheel values was simply an heuristic that worked well, perhaps because it made the motors' voltage/speed function more nearly linear. And here is the calibration routine: Code:
#define LOW_TO_HIGH( a, b ) ( (a)==0 ) && ( (b) == 1 )
unsigned char
calibratingJoystickRange( void )
{
static unsigned char prevCalibrateFlag=0, calibrateFlag;
calibrateFlag = (unsigned char) CALIBRATE_SWITCHES_BOTH_PRESSED;
// detect button press:
//
if ( LOW_TO_HIGH( prevCalibrateFlag, calibrateFlag ) )
{
printf( "CALIBRATION VALUES RESET!!\n" );
// reset calibration values
//
xMax = 0;
yMax = 0;
xMin = 255;
yMin = 255;
}
// detect button *release*:
// (arguments are reversed!)
if ( LOW_TO_HIGH( calibrateFlag, prevCalibrateFlag ) )
{
printf( "\n SAVING CALIB VALUES \n" );
writeEE( XMINp, xMin );
writeEE( XMAXp, xMax );
writeEE( YMINp, yMin );
writeEE( YMAXp, yMax );
// Remember for next time...
//
prevCalibrateFlag = calibrateFlag;
return 0; // done!
}
// Remember for next time...
//
prevCalibrateFlag = calibrateFlag;
// CALIBRATION of X & Y mins and maxs
//
if ( calibrateFlag )
{
// stop motors!
//
LEFT_MOTOR = CENTER_VALUE;
RIGHT_MOTOR = CENTER_VALUE;
// find value that is most extreme
// and save it!
if ( XAXIS < 30 )
{
if ( xMin > XAXIS )
xMin = XAXIS;
}
else if ( YAXIS < 30 )
{
if ( yMin > YAXIS )
yMin = YAXIS;
}
else if ( XAXIS > 190 )
{
if ( xMax < XAXIS )
xMax = XAXIS;
}
else if ( YAXIS > 190 )
{
if ( yMax < YAXIS )
yMax = YAXIS;
}
xMid = ((int)xMax + (int)xMin + 1 ) >> 1;
yMid = ((int)yMax + (int)yMin + 1 ) >> 1;
xHalfRange = xMax - xMid;
return 1; // don't do anything else while calibrating!
}
return 0; // done!
}
|
|
#5
|
||||
|
||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
|
|
#6
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Isn't "Linear Curve" an oxymoron?
Also: LOOKUP TABLE. |
|
#7
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
The main feature of trentonDrive() is that the turning radius is independent of overall speed. The x-axis sets the turning radius, the y axis sets the speed. For a turning radius to be independent of speed, the ratio of the inner and outer wheel speeds must be constant: outer = faster * yAxis; inner = slower * yAxis; where 'faster' is, say, (1+x) and 'slower' is (1-x), where x runs from -1 to +1. But that doesn't matter, because what we care about is that the Yaxis term drops out of the ratio of outer to inner: outer / inner = (faster*yAxis) / (slower*yAxis) = faster / slower; So the ratio is independent of the yAxis. Now, intuitively, I suspected that squaring outer and inner would bring the yAxis into the ratio, but I was wrong: outer^2 = (faster*yAxis)^2 ; = faster^2 * yAxis^2; inner^2 = (slower*yAxis)^2 ; = slower^2 * yAxis^2; So the ratio would be: (faster^2 * yAxis^2) / ( slower^2 * yAxis^2 ) and the yAxis^2 terms once again cancel. So it's still okay to square the wheel values even if you have dead accurate speed control of the motors. trentonDrive() should work very well with such a system. Last edited by gnormhurst : 07-04-2004 at 14:58. |
|
#8
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Yes. I've been thinking about similar things.
Basically, you can determine the center (and angle) of rotation from the width of the bot and the distance the left and right sides travel in the same time. The only problem is that I don't know the formulas in that context. |
|
#9
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
If you think about it, the front wheels of a car are not always parallel -- during a turn the inside front wheel should be turned at a sharper angle. So this note applies only to two-wheeled bots. If the two wheels are 30" apart, and the outer one runs at three times the RPMs of the inner one, the outer one will trace a circle with three times the circumference, and thus three times the radius. The difference between the outer and inner radii is always 30", so in this case the inner radius must be 15" and the outer, 15 + 30 = 45". radiusInner = radiusOuter - 30; wheelSpeedRatio = radiusInner / radiusOuter; substituting ( radiusOuter - 30 ) for radiusInner: wheelSpeedRatio = ( radiusOuter - 30 ) / radiusOuter ; So if you want a certain turning radius, that is a formula for computing the ratio of wheel speeds that you need. |
|
#10
|
|||||
|
|||||
|
Quote:
Quote:
Quote:
sI*rO = (rO - W)sO sI*rO = rO*sO - W*sO sO*rO - sI*rO = W*sO (sO-sI)*rO = W*sO rO = (W*sO)/(sO-sI) rI=W-rO Thank you! |
|
#11
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Enough on drive philosophy. Quote:
trentonDrive() sets the inner wheel speed as sI = yAxis * ( 1 - abs( xAxis ) ) where abs( x ) goes from 0 to 1. This means that at the extreme of the xAxis, the inner wheel stops. That makes the bot turn around the inner wheel -- the turning radius of the inner wheel is zero. But if you double the xAxis in that equation: sI = yAxis * ( 1 - 2 * abs( xAxis ) ) now the inner wheel goes full speed reverse at the extreme of the xAxis, and the robot can spin on a dime. You can think of that as the inner radius going negative! I didn't like that on our machine, but it might work better on yours. trentonDrive would have to be reworked a bit to support negative values where it currently expects only positive. When I first worked out the "radius ratio" idea, I only thought to slow down the inner wheel -- I just sent the yAxis directly to the outer wheel: sI = yAxis * ( 1 - abs( xAxis ) ) sO = yAxis But that made it steer funny, like dragging your foot on one side or the other to make it turn. I didn't like how the robot slowed down when it turned. So I sped up the outer as much as I slowed down the inner: sI = yAxis * ( 1 - abs( xAxis ) ) sO = yAxis * ( 1 + abs( xAxis ) ) Now it maintains speed as it turns. Like a car. If you're going to nationals look me up at Team 381! |
|
#12
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Sorry, no. I worked out the behavior in something called Geometer's Sketchpad. Unfortunately, I don't think what I had was actually acurate, just described it.
I was hoping to do it for LVelocity and RVelocity and be able to describe the location as a signed value representing the distance towards the left side (negetive means towards the right). I mean just the algorithm; I didn't plan on putting it in the controller! but this is a whole lot closer. |
|
#13
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Good catch. |
|
#14
|
||||
|
||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Each time I compile the function it says
C:\FrcCode2005v2.2\trentonDrive.c:164:Error [1105] symbol 'pwm10' has not been defined C:\FrcCode2005v2.2\trentonDrive.c:164:Error [1101] lvalue required do you know what could be the problem? |
|
#15
|
|||||
|
|||||
|
Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Project -> Build Options... -> Project MPLAB C18 tab By "Macro Definitions" click on "Add..." type _FRC_BOARD in the popup, then OK Build everything again. |
![]() |
| Thread Tools | |
| Display Modes | Rate This Thread |
|
|
Similar Threads
|
||||
| Thread | Thread Starter | Forum | Replies | Last Post |
| heres the code. y this not working | omega | Programming | 16 | 31-03-2004 15:18 |
| What is wrong with this code???? It won't Compile and I don't know why? Please Help | CrashZero | Programming | 23 | 26-03-2004 09:44 |
| Changing 1 joystick code to 2 (rookie team) | Brawler006 | Programming | 5 | 20-02-2004 17:00 |
| TechnoKats Automated Test Drive Code | Greg McCoy | Programming | 1 | 16-01-2003 17:45 |
| "Motors and Drive train edition" of Fresh From the Forum | Ken Leung | CD Forum Support | 6 | 29-01-2002 12:32 |