WiiClassicControllerWithCalibration.cpp

00001 /*
00002  * SOURCE FILE : WiiClassicControllerWithCalibration.cpp
00003  *
00004  * Definition of class WiiClassicControllerWithCalibration.
00005  *
00006  */
00007 
00008 #include "WiiClassicControllerWithCalibration.h"
00009 
00010 /** Constructor
00011  * @param sda pin to use for SDA.
00012  * @param scl pin to use for SCL.
00013  */
00014 WiiClassicControllerWithCalibration::WiiClassicControllerWithCalibration( PinName sda, PinName scl ) :
00015     WiiClassicController( sda, scl ),
00016     calibrating( false )
00017 {
00018     // Set default scaling factors.
00019     // Left joystick is 6 bit reading. Raw reading of 0 gives -1. Raw reading of 63 gives +1.
00020     SetScaling( LeftJoyX, (float)( 2.0 / 63.0 ), -1 );
00021     SetScaling( LeftJoyY, (float)( 2.0 / 63.0 ), -1 );
00022     // Right joystick is 5 bit reading. Raw reading of 0 gives -1. Raw reading of 31 gives +1.
00023     SetScaling( RightJoyX, (float)( 2.0 / 31.0 ), -1 );
00024     SetScaling( RightJoyY, (float)( 2.0 / 31.0 ), -1 );
00025     // Left trigger is 5 bit reading. Raw reading of 0 gives 0. Raw reading of 31 gives +1.
00026     SetScaling( LeftTrigger, (float)( 1.0 / 31.0 ), 0 );
00027     // Right trigger is 5 bit reading. Raw reading of 0 gives 0. Raw reading of 31 gives +1.
00028     SetScaling( RightTrigger, (float)( 1.0 / 31.0 ), 0 );
00029 }
00030 
00031 /** Destructor
00032  */
00033 WiiClassicControllerWithCalibration::~WiiClassicControllerWithCalibration() {
00034 }
00035 
00036 /** Set scaling for a particular analogue input.
00037  * @param input channel to change.
00038  * @param m scale (multiplier) for this analogue input.
00039  * @param c offset for this analogue input.
00040  */
00041  void WiiClassicControllerWithCalibration::SetScaling( AnaIn input, float m, float c ) {
00042     if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
00043         AnaInRec *ptr = records + (int)input;
00044         ptr->Scale = m;
00045         ptr->Offset = c;
00046     }
00047  }
00048 
00049 /** Get scaling for a particular analogue input.
00050  * @param input channel to read.
00051  * @param m scale (multiplier) for this analogue input return here.
00052  * @param c offset for this analogue input returned here.
00053  */
00054  void WiiClassicControllerWithCalibration::GetScaling( AnaIn input, float *m, float *c ) const {
00055     if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
00056         const AnaInRec *ptr = records + (int)input;
00057         *m = ptr->Scale;
00058         *c = ptr->Offset;
00059     }
00060  }
00061 
00062 /** Read from the controller.
00063  * This override will also update calibration information
00064  * when calibration is in progress.
00065  * @returns true on success, false on failure.
00066  */
00067 bool WiiClassicControllerWithCalibration::Read( void ) {
00068     // Call base class method to do the actual reading.
00069     if( WiiClassicController::Read() ) {
00070         // That worked so update calibration information if
00071         // calibration is in progress.
00072         if( calibrating ) {
00073             UpdateCalibration();
00074         }
00075         return true;
00076     }
00077     else {
00078         return false;
00079     }
00080 }
00081 
00082 /** Get calibrated left joystick X reading.
00083  * @returns a reading between -1 and +1.
00084  */
00085 float WiiClassicControllerWithCalibration::GetCalLJoyX( void ) const {
00086     return GetScaled( LeftJoyX, GetLJoyX(), -1, 1 );
00087 }
00088 
00089 /** Get calibrated left joystick Y reading.
00090  * @returns a reading between -1 and +1.
00091  */
00092 float WiiClassicControllerWithCalibration::GetCalLJoyY( void ) const {
00093     return GetScaled( LeftJoyY, GetLJoyY(), -1, 1 );
00094 }
00095 
00096 /** Get calibrated right joystick X reading.
00097  * @returns a reading between -1 and +1.
00098  */
00099 float WiiClassicControllerWithCalibration::GetCalRJoyX( void ) const {
00100     return GetScaled( RightJoyX, GetRJoyX(), -1, 1 );
00101 }
00102 
00103 /** Get calibrated right joystick Y reading.
00104  * @returns a reading between -1 and +1.
00105  */
00106 float WiiClassicControllerWithCalibration::GetCalRJoyY( void ) const {
00107     return GetScaled( RightJoyY, GetRJoyY(), -1, 1 );
00108 }
00109 
00110 /** Get calibrated left trigger reading.
00111  * @returns a reading between 0 and +1.
00112  */
00113 float WiiClassicControllerWithCalibration::GetCalLeftTrigger( void ) const {
00114     return GetScaled( LeftTrigger, GetLeftTrigger(), 0, 1 );
00115 }
00116 
00117 /** Get calibrated right trigger reading.
00118  * @returns a reading between 0 and +1.
00119  */
00120 float WiiClassicControllerWithCalibration::GetCalRightTrigger( void ) const {
00121     return GetScaled( RightTrigger, GetRightTrigger(), 0, 1 );
00122 }
00123 
00124 /** Get scaled reading.
00125  * @param input analogue input to scale.
00126  * @param raw raw readings in counts.
00127  * @param min minimum permitted value.
00128  * @param max maximum permited value.
00129  * @returns scaled reading.
00130  */
00131 float WiiClassicControllerWithCalibration::GetScaled( AnaIn input, UInt8 raw, float min, float max ) const {
00132     if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
00133         const AnaInRec *ptr = records + (int)input;
00134         float y = (float)raw * ptr->Scale + ptr->Offset;
00135         if( y < min ) {
00136             y = min;
00137         }
00138         else if( y > max ) {
00139             y = max;
00140         }
00141         return y;
00142     }
00143     else {
00144         return (float)0;
00145     }
00146 }
00147 
00148 /** Start calibrating.
00149  * Every call to Read method updates calibration until StopCalibrating is called.
00150  */
00151 void WiiClassicControllerWithCalibration::StartCalibrating( void ) {
00152     if( ! calibrating ) {
00153         // Initialise all records so that the minimum count is very large,
00154         // the maximum count is very small and the zero count is whatever
00155         // the joystick is reading right now.
00156         AnaInRec *rec;
00157         for( int i = 0; i < (int)AnaInCount; ++i ) {
00158             rec = records + i;
00159             rec->MinCount = 255;
00160             rec->MaxCount = 0;
00161             rec->ZeroCount = GetAnaIn( (AnaIn)i );
00162         }
00163         calibrating = true;
00164     }
00165 }
00166 
00167 /** Get the raw counts for one of the analogue inputs.
00168  * @param input analogue input to read.
00169  * @returns raw counts for input.
00170  */
00171 UInt8 WiiClassicControllerWithCalibration::GetAnaIn( AnaIn input ) const {
00172     switch( input ) {
00173     case LeftJoyX :
00174         return GetLJoyX();
00175     case LeftJoyY :
00176         return GetLJoyY();
00177     case RightJoyX :
00178         return GetRJoyX();
00179     case RightJoyY :
00180         return GetRJoyY();
00181     case LeftTrigger :
00182         return GetLeftTrigger();
00183     case RightTrigger :
00184         return GetRightTrigger();
00185     default :
00186         return 0;
00187     }
00188 }
00189 
00190 /** Stop calibrating.
00191  */
00192 void WiiClassicControllerWithCalibration::StopCalibrating( void ) {
00193     if( calibrating ) {
00194         AnaInRec *rec;
00195         for( int i = 0; i < (int)AnaInCount; ++i ) {
00196             rec = records + i;
00197             switch( (AnaIn)i ) {
00198             case LeftJoyX :
00199             case LeftJoyY :
00200             case RightJoyX :
00201             case RightJoyY :
00202                 rec->CalculateScaleAndOffsetBiPolar( -1, 1 );
00203                 break;
00204             case LeftTrigger :
00205             case RightTrigger :
00206                 rec->CalculateScaleAndOffsetUniPolar( 1 );
00207                 break;
00208             }
00209         }
00210         calibrating = false;
00211     }
00212 }
00213         
00214 
00215 /** Update calibration information.
00216  */
00217 void WiiClassicControllerWithCalibration::UpdateCalibration( void ) {
00218     AnaInRec *rec;
00219     UInt8 count;
00220     for( int i = 0; i < (int)AnaInCount; ++i ) {
00221         count = GetAnaIn( (AnaIn)i );
00222         rec = records + i;
00223         if( count < rec->MinCount ) {
00224             rec->MinCount = count;
00225         }
00226         if( count > rec->MaxCount ) {
00227             rec->MaxCount = count;
00228         }
00229     }
00230 }
00231 
00232 /** Calculate scale and offset for a bipolar reading.
00233  * @param minValue minimum value to read.
00234  * @param maxValue maximum value to read.
00235  */
00236  void WiiClassicControllerWithCalibration::AnaInRec::CalculateScaleAndOffsetBiPolar( float minValue, float maxValue ) {
00237     UInt8 a;
00238     float b;
00239     if( ( ZeroCount - MinCount ) < ( MaxCount - ZeroCount ) ) {
00240         a = MinCount;
00241         b = minValue;
00242     }
00243     else {
00244         a = MaxCount;
00245         b = maxValue;
00246     }
00247     // Prevent a division by zero, but still a nonsense result.
00248     if( a == ZeroCount ) {
00249         a++;
00250     }
00251     Scale = ( b - (float)0 ) / (float)( a - ZeroCount );
00252     Offset = -Scale * (float)ZeroCount;
00253  }
00254 
00255  /** Calculate scale and offset for a unipolar reading.
00256  * Minimum value is assumed to be zero.
00257  * @param maxValue maximum value to read.
00258  */
00259  void WiiClassicControllerWithCalibration::AnaInRec::CalculateScaleAndOffsetUniPolar( float maxValue ) {
00260     UInt8 a = MaxCount;
00261     float b = maxValue;
00262     // Prevent a division by zero, but still a nonsense result.
00263     if( a == MinCount ) {
00264         a++;
00265     }
00266     Scale = ( b - (float)0 ) / (float)( a - MinCount );
00267     Offset = -Scale * (float)MinCount;
00268  }
00269