View Single Post
  #4   Spotlight this post!  
Unread 06-04-2004, 23:06
gnormhurst's Avatar
gnormhurst gnormhurst is offline
Norm Hurst
AKA: gnorm
#0381 (The Tornadoes)
Team Role: Programmer
 
Join Date: Jan 2004
Location: Trenton, NJ
Posts: 138
gnormhurst will become famous soon enoughgnormhurst will become famous soon enough
Re: trentonDrive.c: our joystick/wheel drive code

Quote:
Originally Posted by 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
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;
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:
Code:
  // process the EEPROM data writing queue.
  // Joystick calibration values are written there.
  //
  processEEQueue();
Quote:
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()):
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.
  }
The function calibratingJoystickRange() is shown at the end of this post.

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!

}
Well, as they say, that's the rest of the story! And that's probably more than you wanted to know!
__________________
Trenton Tornadoes 381
2004 Philadelphia Regional Winners
2006 Xerox Creativity Award
---
My corner of the USPTO.
My favorite error message from gcc: main is usually a function
My favorite error message from Windows: There is not enough disk space available to delete this file.