Diff: WiiClassicControllerWithCalibration.cpp

Revision:

3:ecae3d286a99

Parent:

2:52c4a0b3a509

Child:

4:79d04d737f02

--- a/WiiClassicControllerWithCalibration.cpp	Sun Jun 30 12:05:27 2013 +0000
+++ b/WiiClassicControllerWithCalibration.cpp	Sun Jun 30 16:37:49 2013 +0000
@@ -12,7 +12,8 @@
  * @param scl pin to use for SCL.
  */
 WiiClassicControllerWithCalibration::WiiClassicControllerWithCalibration( PinName sda, PinName scl ) :
-    WiiClassicController( sda, scl )
+    WiiClassicController( sda, scl ),
+    calibrating( false )
 {
     // Set default scaling factors.
     // Left joystick is 6 bit reading. Raw reading of 0 gives -1. Raw reading of 63 gives +1.
@@ -33,6 +34,7 @@
 }
 
 /** Set scaling for a particular analogue input.
+ * @param input channel to read.
  * @param m scale (multiplier) for this analogue input.
  * @param c offset for this analogue input.
  */
@@ -44,59 +46,224 @@
     }
  }
 
+/** Get scaling for a particular analogue input.
+ * @param input channel to read.
+ * @param m scale (multiplier) for this analogue input return here.
+ * @param c offset for this analogue input returned here.
+ */
+ void WiiClassicControllerWithCalibration::GetScaling( AnaIn input, float *m, float *c ) const {
+    if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
+        const AnaInRec *ptr = records + (int)input;
+        *m = ptr->Scale;
+        *c = ptr->Offset;
+    }
+ }
+
+/** Read from the controller.
+ * This override will also update calibration information
+ * when calibration is in progress.
+ * @returns true on success, false on failure.
+ */
+bool WiiClassicControllerWithCalibration::Read( void ) {
+    // Call base class method to do the actual reading.
+    if( WiiClassicController::Read() ) {
+        // That worked so update calibration information if
+        // calibration is in progress.
+        if( calibrating ) {
+            UpdateCalibration();
+        }
+        return true;
+    }
+    else {
+        return false;
+    }
+}
+
 /** Get calibrated left joystick X reading.
  * @returns a reading between -1 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalLJoyX( void ) const {
-    return GetScaled( LeftJoyX, GetLJoyX() );
+    return GetScaled( LeftJoyX, GetLJoyX(), -1, 1 );
 }
 
 /** Get calibrated left joystick Y reading.
  * @returns a reading between -1 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalLJoyY( void ) const {
-    return GetScaled( LeftJoyY, GetLJoyY() );
+    return GetScaled( LeftJoyY, GetLJoyY(), -1, 1 );
 }
 
 /** Get calibrated right joystick X reading.
  * @returns a reading between -1 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalRJoyX( void ) const {
-    return GetScaled( RightJoyX, GetRJoyX() );
+    return GetScaled( RightJoyX, GetRJoyX(), -1, 1 );
 }
 
 /** Get calibrated right joystick Y reading.
  * @returns a reading between -1 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalRJoyY( void ) const {
-    return GetScaled( RightJoyY, GetRJoyY() );
+    return GetScaled( RightJoyY, GetRJoyY(), -1, 1 );
 }
 
 /** Get calibrated left trigger reading.
  * @returns a reading between 0 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalLeftTrigger( void ) const {
-    return GetScaled( LeftTrigger, GetLeftTrigger() );
+    return GetScaled( LeftTrigger, GetLeftTrigger(), 0, 1 );
 }
 
 /** Get calibrated right trigger reading.
  * @returns a reading between 0 and +1.
  */
 float WiiClassicControllerWithCalibration::GetCalRightTrigger( void ) const {
-    return GetScaled( RightTrigger, GetRightTrigger() );
+    return GetScaled( RightTrigger, GetRightTrigger(), 0, 1 );
 }
 
 /** Get scaled reading.
  * @param input analogue input to scale.
  * @param raw raw readings in counts.
+ * @param min minimum permitted value.
+ * @param max maximum permited value.
  * @returns scaled reading.
  */
-float WiiClassicControllerWithCalibration::GetScaled( AnaIn input, UInt8 raw ) const {
+float WiiClassicControllerWithCalibration::GetScaled( AnaIn input, UInt8 raw, float min, float max ) const {
     if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
         const AnaInRec *ptr = records + (int)input;
-        return (float)raw * ptr->Scale + ptr->Offset;
+        float y = (float)raw * ptr->Scale + ptr->Offset;
+        if( y < min ) {
+            y = min;
+        }
+        else if( y > max ) {
+            y = max;
+        }
+        return y;
     }
     else {
         return (float)0;
     }
 }
+
+/** Start calibrating.
+ * Every call to Read method updates calibration until StopCalibrating is called.
+ */
+void WiiClassicControllerWithCalibration::StartCalibrating( void ) {
+    if( ! calibrating ) {
+        // Initialise all records so that the minimum count is very large,
+        // the maximum count is very small and the zero count is whatever
+        // the joystick is reading right now.
+        AnaInRec *rec;
+        for( int i = 0; i < (int)AnaInCount; ++i ) {
+            rec = records + i;
+            rec->MinCount = 255;
+            rec->MaxCount = 0;
+            rec->ZeroCount = GetAnaIn( (AnaIn)i );
+        }
+        calibrating = true;
+    }
+}
+
+/** Get the raw counts for one of the analogue inputs.
+ * @param input analogue input to read.
+ * @returns raw counts for input.
+ */
+UInt8 WiiClassicControllerWithCalibration::GetAnaIn( AnaIn input ) const {
+    switch( input ) {
+    case LeftJoyX :
+        return GetLJoyX();
+    case LeftJoyY :
+        return GetLJoyY();
+    case RightJoyX :
+        return GetRJoyX();
+    case RightJoyY :
+        return GetRJoyY();
+    case LeftTrigger :
+        return GetLeftTrigger();
+    case RightTrigger :
+        return GetRightTrigger();
+    default :
+        return 0;
+    }
+}
+
+/** Stop calibrating.
+ */
+void WiiClassicControllerWithCalibration::StopCalibrating( void ) {
+    if( calibrating ) {
+        AnaInRec *rec;
+        for( int i = 0; i < (int)AnaInCount; ++i ) {
+            rec = records + i;
+            switch( (AnaIn)i ) {
+            case LeftJoyX :
+            case LeftJoyY :
+            case RightJoyX :
+            case RightJoyY :
+                rec->CalculateScaleAndOffsetBiPolar( -1, 1 );
+                break;
+            case LeftTrigger :
+            case RightTrigger :
+                rec->CalculateScaleAndOffsetUniPolar( 1 );
+                break;
+            }
+        }
+        calibrating = false;
+    }
+}
+        
+
+/** Update calibration information.
+ */
+void WiiClassicControllerWithCalibration::UpdateCalibration( void ) {
+    AnaInRec *rec;
+    UInt8 count;
+    for( int i = 0; i < (int)AnaInCount; ++i ) {
+        count = GetAnaIn( (AnaIn)i );
+        rec = records + i;
+        if( count < rec->MinCount ) {
+            rec->MinCount = count;
+        }
+        if( count > rec->MaxCount ) {
+            rec->MaxCount = count;
+        }
+    }
+}
+
+/** Calculate scale and offset for a bipolar reading.
+ * @param minValue minimum value to read.
+ * @param maxValue maximum value to read.
+ */
+ void WiiClassicControllerWithCalibration::AnaInRec::CalculateScaleAndOffsetBiPolar( float minValue, float maxValue ) {
+    UInt8 a;
+    float b;
+    if( ( ZeroCount - MinCount ) < ( MaxCount - ZeroCount ) ) {
+        a = MinCount;
+        b = minValue;
+    }
+    else {
+        a = MaxCount;
+        b = maxValue;
+    }
+    // Prevent a division by zero, but still a nonsense result.
+    if( a == ZeroCount ) {
+        a++;
+    }
+    Scale = ( b - (float)0 ) / (float)( a - ZeroCount );
+    Offset = -Scale * (float)ZeroCount;
+ }
+
+ /** Calculate scale and offset for a unipolar reading.
+ * Minimum value is assumed to be zero.
+ * @param maxValue maximum value to read.
+ */
+ void WiiClassicControllerWithCalibration::AnaInRec::CalculateScaleAndOffsetUniPolar( float maxValue ) {
+    UInt8 a = MaxCount;
+    float b = maxValue;
+    // Prevent a division by zero, but still a nonsense result.
+    if( a == MinCount ) {
+        a++;
+    }
+    Scale = ( b - (float)0 ) / (float)( a - MinCount );
+    Offset = -Scale * (float)MinCount;
+ }
+ 
\ No newline at end of file