

YO DAWGS. I GOT THIS.  LiquidMagik [more] 



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 singlejoystick control routine for a robot chassis that uses twowheel, twomotor 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 nonlinear 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 slowspeed 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 twowheel 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 nonlinear speed/voltage // relationship of the motors. // // The range will be (2^7 * 2^7) = 2^14 // outerWheel *= outerWheel; innerWheel *= innerWheel; // rerange 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 largerradius 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 deadband 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 ); // xaxis controls spin (only) speed else trentonDrive( XAXIS, YAXIS ); // outer wheel/inner wheel ratiobased 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 xaxis 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 (1x), 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 : 04072004 at 01:58 PM. 
#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 twowheeled 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




Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Good catch. 
#11




Re: trentonDrive.c: our joystick/wheel drive code
Quote:
Quote:
Quote:
sI*rO = (rO  W)sO sI*rO = rO*sO  W*sO sO*rO  sI*rO = W*sO (sOsI)*rO = W*sO rO = (W*sO)/(sOsI) rI=WrO Thank you! 
#12




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! 
#13




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. 
#14




Re: trentonDrive.c: our joystick/wheel drive code
Can I keep the foward and backward motions on the joystick the same and delete the motions caused by moving along the x axis? If so, how?

#15




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? 
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  03312004 02:18 PM 
What is wrong with this code???? It won't Compile and I don't know why? Please Help  CrashZero  Programming  23  03262004 08:44 AM 
Changing 1 joystick code to 2 (rookie team)  Brawler006  Programming  5  02202004 04:00 PM 
TechnoKats Automated Test Drive Code  Greg McCoy  Programming  1  01162003 04:45 PM 
"Motors and Drive train edition" of Fresh From the Forum  Ken Leung  CD Forum Support  6  01292002 11:32 AM 