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