trentonDrive.c: our joystick/wheel drive code

[gnormhurst]

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.

#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;
}

```<br><br><a class='attachment' href='/uploads/default/original/3X/1/b/1b2dbe520c1792dbc717ef79c877e3da8085b758.zip'>trentonDrive.zip</a> (2.27 KB)<br><br><br><a class='attachment' href='/uploads/default/original/3X/1/b/1b2dbe520c1792dbc717ef79c877e3da8085b758.zip'>trentonDrive.zip</a> (2.27 KB)<br>

[Ryan_M]

Just want to say very clever.

[The_Lucas]

// global joystick calibration values.
//
unsigned char xMax = 254;
unsigned char xMin = 0;
unsigned char xMid = 127, yMid = 127;
unsigned char xHalfRange = 128;

Just curious, Why did you make these global variables instead of constants? yMid and xMid are also declared locally in trentonDrive(). Do you run a calibration routine and set these vars in another section of the code?

Very nice routine, think I’ll take it for a spin

[gnormhurst]

@The Lucas

Just curious, Why did you make these global variables instead of constants? yMid and xMid are also declared locally in trentonDrive(). Do you run a calibration routine and set these vars in another section of the code?

Very perceptive of you! Yes, this code works best when the actual extremes of the joystick axes are known. I wrote a separate calibration routine to determine these values and store them in EEPROM (using the code from Wizard of AZ). So each time the code initializes, the EEPROM values are recovered and used to restore xMax, Xmin, yMin and yMax.

So, my actual declarations are

// 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

  // 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;

In the Default_Routine() (but it’s no longer default because I added stuff to it), there is the all-important call to service the EEPROM write FIFO:

  // process the EEPROM data writing queue.
  // Joystick calibration values are written there.
  //
  processEEQueue();

Very nice routine, think I’ll take it for a spin

Thanks, and speaking of “spin”…

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()):

  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.
  }

The function calibratingJoystickRange() is shown at the end of this post.

Here is squaredSpin():

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:

#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!!
" );
    // reset calibration values
    //
    xMax = 0;
    yMax = 0;
    xMin = 255;
    yMin = 255;
  }

  // detect button *release*:
  // (arguments are reversed!)
  if ( LOW_TO_HIGH( calibrateFlag, prevCalibrateFlag ) )
  {
    printf( "
    SAVING CALIB VALUES 
" );
    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!

}

Well, as they say, that’s the rest of the story! And that’s probably more than you wanted to know!

[Max_Lobovsky]

@gnormhurst

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.

I’m writing code for a drivetrain that absolutely needs an accurate velocity control (with a linear curve for inputs to outputs) for each wheel so i was planning on using a PID loop and encoders to get the velocity correct. I think the PID feedback loop would be more effective if the voltage was nearer to linear to speed. This way, the system could be made very responsive by having PID coefficients that work for the whole range, not a set of coefficients designed to work over a wide range of non linear values (probably a large proportional coefficient would do that). I would do the squaring right before the PID loop (and after a bunch of trigonometry, limiting, deadbanding, etc…) So basically, im asking why exactly the squaring would be bad with a PID loop?

[Astronouth7303]

Isn’t “Linear Curve” an oxymoron?

Also: LOOKUP TABLE.

[gnormhurst]

@maxlobovsky

I’m writing code for a drivetrain that absolutely needs an accurate velocity control (with a linear curve for inputs to outputs) for each wheel so i was planning on using a PID loop and encoders to get the velocity correct. I think the PID feedback loop would be more effective if the voltage was nearer to linear to speed. This way, the system could be made very responsive by having PID coefficients that work for the whole range, not a set of coefficients designed to work over a wide range of non linear values (probably a large proportional coefficient would do that). I would do the squaring right before the PID loop (and after a bunch of trigonometry, limiting, deadbanding, etc…) So basically, im asking why exactly the squaring would be bad with a PID loop?

After I posted my long note last night I asked myself the same question, and then did a little math and decided I was wrong, and that squaring the motor values would still be legit. Here’s my reasoning.

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 = (fasteryAxis) / (sloweryAxis)
= 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.

[Astronouth7303]

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.

[gnormhurst]

@Astronouth7303

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.

I’m not sure what formula you wish you had, but think about this: while the bot is turning the wheels will trace out two concentric circles. In general, when any wheeled vehicle is turning, you want the wheels to all travel in concentric circles, with each wheel tangent to the circle it traces out. If this is not the case, something will have to give, and that’s why you see four-wheeled bots jumping around as they try to turn on the carpet.

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.

[gnormhurst]

@The Lucas

yMid and xMid are also declared locally in trentonDrive().

That was simply a mistake I made while trying to isolate the function and make it compile. The x & y Mid, Max, and Min values are declared globally in my code.

Good catch.

[Astronouth7303]

@gnormhurst

I’m not sure what formula you wish you had, but think about this: while the bot is turning the wheels will trace out two concentric circles. In general, when any wheeled vehicle is turning, you want the wheels to all travel in concentric circles, with each wheel tangent to the circle it traces out. If this is not the case, something will have to give, and that’s why you see four-wheeled bots jumping around as they try to turn on the carpet.

I managed to figure that much out, fortumately. I just couldn’t figure out how to solve for Radius.

@gnormhurst

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.

And Skid Steer/Tamk Dirve, right? just put the ‘Left’ and the ‘right’ at the average point of where the wheels touch the ground

@gnormhurst

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.

sI/sO = 1-W/rO? You’re good.

sIrO = (rO - W)sO
sIrO = rOsO - WsO
sOrO - sIrO = WsO
(sO-sI)rO = WsO
**rO = (WsO)/(sO-sI)
rI=W-rO**

Thank you!

[gnormhurst]

@Astronouth7303

I managed to figure that much out, fortumately. I just couldn’t figure out how to solve for Radius.

And Skid Steer/Tamk Dirve, right? just put the ‘Left’ and the ‘right’ at the average point of where the wheels touch the ground

Yeah, that’s right – any bot with just two independent rolling points on the ground. Track drives work best on slippery or sandy surfaces, but carpet is tougher. Some people have the drive wheels in the center of each track and a little bit lower than the rest of the track, so that it can turn without dragging the entire track sideways across the carpet. Tracks certainly help when climbing. We opted to stay off the platform – our bot has less than 1" clearance to the floor. It’s never flipped over.

Enough on drive philosophy.

sI/sO = 1-W/rO? You’re good.

sIrO = (rO - W)sO
sIrO = rOsO - WsO
sOrO - sIrO = WsO
(sO-sI)rO = WsO
**rO = (WsO)/(sO-sI)
rI=W-rO**

Thank you!

You’re welcome! I think your last equation is reversed: the inner radius is less than the outer radius, so rI = rO - W. But I’m sure you knew that.

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!

[Astronouth7303]

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.

[willie837]

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?

[Anton]

Each time I compile the function it says
C:\FrcCode2005v2.2 rentonDrive.c:164:Error [1105] symbol ‘pwm10’ has not been defined
C:\FrcCode2005v2.2 rentonDrive.c:164:Error [1101] lvalue required

do you know what could be the problem?

[Mark_McLeod]

@Anton

Each time I compile the function it says
C:\FrcCode2005v2.2 rentonDrive.c:164:Error [1105] symbol ‘pwm10’ has not been defined
C:\FrcCode2005v2.2 rentonDrive.c:164:Error [1101] lvalue required

do you know what could be the problem?

Probably missing a required compiler flag in MPLAB for the FRC. The code isn’t setup for the EDU controller.

Project -> Build Options… -> Project
MPLAB C18 tab
By “Macro Definitions” click on “Add…”
type _FRC_BOARD in the popup, then OK
Build everything again.

[Anton]

@Mark McLeod

Probably missing a required compiler flag in MPLAB for the FRC. The code isn’t setup for the EDU controller.

Project -> Build Options… -> Project
MPLAB C18 tab
By “Macro Definitions” click on “Add…”
type _FRC_BOARD in the popup, then OK
Build everything again.

I already fixed the problem by including everyhing included in user_routines.c to your file. It probably is just a waste of resources, but I think that i’ll find by trial&error the neccesary files to be included.
thanks anyway.