Revision 3:ecae3d286a99, committed 2013-06-30

Comitter:

RichardE

Date:

Sun Jun 30 16:37:49 2013 +0000

Parent:

2:52c4a0b3a509

Child:

4:79d04d737f02

Commit message:

Calibration now works.

Changed in this revision

--- a/WiiClassicController.cpp	Sun Jun 30 12:05:27 2013 +0000
+++ b/WiiClassicController.cpp	Sun Jun 30 16:37:49 2013 +0000
@@ -8,8 +8,8 @@
 
 #include "WiiClassicController.h"
 
-// REMOVE THIS!
-extern Serial pc;
+// Delay to slow things down. Doesn;t work without this.
+#define WAIT_TIME ((float)0.01 )    // in seconds
 
 /** Constructor.
  * @param sda pin to use for SDA.
@@ -45,7 +45,7 @@
 bool WiiClassicController::ControllerInit( void ) {
     const UInt8 cmd[] = { CONTROLLER_REGADDR, 0x00 };
     bool ok = ( controllerPort.write( CONTROLLER_ADDR, (const char*)cmd, sizeof(cmd) ) == 0 );
-    pc.printf( "ControllerInit returned %d\r\n", (int)ok );
+    wait( WAIT_TIME );
     return ok;
 }
 
@@ -53,24 +53,21 @@
  * @returns true on success, false on failure.
  */
 bool WiiClassicController::ControllerRead() {
+    bool ok = false;
     // write the address we want to read from
     const UInt8 cmd[] = { 0x00 };
     if( controllerPort.write( CONTROLLER_ADDR, (const char*)cmd, sizeof(cmd) ) == 0 ) {
         // The Wii Classic Controller is non-standard I2C
         // and can't manage setting the read address and immediately supplying the data
         // so wait a bit.
-        wait( 0.01 );
+        wait( WAIT_TIME );
         if( controllerPort.read( CONTROLLER_ADDR, (char*)readBuf, sizeof(readBuf) ) == 0 ) {
             for( int i = 0; i < CONTROLLER_READLEN; ++i ) {
                 readBuf[ i ] = Decode( readBuf[ i ] );
             }
-            return true;
-        }
-        else {
-            return false;
+            ok = true;
         }
     }
-    else {
-        return false;
-    }
+    // wait( WAIT_TIME );
+    return ok;
 }

--- a/WiiClassicController.h	Sun Jun 30 12:05:27 2013 +0000
+++ b/WiiClassicController.h	Sun Jun 30 16:37:49 2013 +0000
@@ -35,7 +35,7 @@
     /** Read from the controller.
      * @returns true on success, false on failure.
      */
-     bool Read( void );
+    virtual bool Read( void );
     
     /** Read left joystick X axis.
      * @returns joystick reading as number between 0 and 63.

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

--- a/WiiClassicControllerWithCalibration.h	Sun Jun 30 12:05:27 2013 +0000
+++ b/WiiClassicControllerWithCalibration.h	Sun Jun 30 16:37:49 2013 +0000
@@ -37,12 +37,26 @@
          */
         virtual ~WiiClassicControllerWithCalibration();
 
+        /** Read from the controller.
+         * This override will also update calibration information
+         * when calibration is in progress.
+         * @returns true on success, false on failure.
+         */
+        virtual bool Read( void );
+
         /** Set scaling for a particular analogue input.
          * @param m scale (multiplier) for this analogue input.
          * @param c offset for this analogue input.
          */
          void SetScaling( AnaIn input, float m, float c );
          
+        /** 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 GetScaling( AnaIn input, float *m, float *c ) const;
+
         /** Get calibrated left joystick X reading.
          * @returns a reading between -1 and +1.
          */
@@ -73,13 +87,50 @@
          */
         float GetCalRightTrigger( void ) const;
 
+        /** Get indication that calibration is in progress.
+         * @returns true if calibration is in progress.
+         */
+        bool IsCalibrating( void ) const {
+            return calibrating;
+        }
+        
+        /** Start calibrating.
+         * Every call to Read method updates calibration until StopCalibrating is called.
+         */
+        void StartCalibrating( void );
+        
+        /** Stop calibrating.
+         */
+        void StopCalibrating( void );
+        
     private :
 
+        // Indicates calibration is in progress.
+        bool calibrating;
+        
         // Record for each analogue input.
         class AnaInRec {
+        
         public :
+        
             float Scale;
             float Offset;
+            UInt8 MaxCount;
+            UInt8 MinCount;
+            UInt8 ZeroCount;
+            
+            /** Calculate scale and offset for a bipolar reading.
+             * @param minValue minimum value to read.
+             * @param maxValue maximum value to read.
+             */
+             void CalculateScaleAndOffsetBiPolar( float minValue, float maxValue );
+             
+            /** Calculate scale and offset for a unipolar reading.
+             * Minimum value is assumed to be zero.
+             * @param maxValue maximum value to read.
+             */
+             void CalculateScaleAndOffsetUniPolar( float maxValue );
+             
         };
             
         // Records for all analogue inputs.
@@ -88,10 +139,22 @@
         /** 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 GetScaled( AnaIn input, UInt8 raw ) const;
+        float GetScaled( AnaIn input, UInt8 raw, float min, float max ) const;
+        
+        /** Update calibration information.
+         */
+        void UpdateCalibration( void );
         
+        /** Get the raw counts for one of the analogue inputs.
+         * @param input analogue input to read.
+         * @returns raw counts for input.
+         */
+        UInt8 GetAnaIn( AnaIn input ) const;
+
     };
 
 #endif