File content as of revision 3:ecae3d286a99:

/*
 * SOURCE FILE : WiiClassicControllerWithCalibration.cpp
 *
 * Definition of class WiiClassicControllerWithCalibration.
 *
 */

#include "WiiClassicControllerWithCalibration.h"

/** Constructor
 * @param sda pin to use for SDA.
 * @param scl pin to use for SCL.
 */
WiiClassicControllerWithCalibration::WiiClassicControllerWithCalibration( PinName sda, PinName 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.
    SetScaling( LeftJoyX, (float)( 2.0 / 63.0 ), -1 );
    SetScaling( LeftJoyY, (float)( 2.0 / 63.0 ), -1 );
    // Right joystick is 5 bit reading. Raw reading of 0 gives -1. Raw reading of 31 gives +1.
    SetScaling( RightJoyX, (float)( 2.0 / 31.0 ), -1 );
    SetScaling( RightJoyY, (float)( 2.0 / 31.0 ), -1 );
    // Left trigger is 5 bit reading. Raw reading of 0 gives 0. Raw reading of 31 gives +1.
    SetScaling( LeftTrigger, (float)( 1.0 / 31.0 ), 0 );
    // Right trigger is 5 bit reading. Raw reading of 0 gives 0. Raw reading of 31 gives +1.
    SetScaling( RightTrigger, (float)( 1.0 / 31.0 ), 0 );
}

/** Destructor
 */
WiiClassicControllerWithCalibration::~WiiClassicControllerWithCalibration() {
}

/** 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.
 */
 void WiiClassicControllerWithCalibration::SetScaling( AnaIn input, float m, float c ) {
    if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
        AnaInRec *ptr = records + (int)input;
        ptr->Scale = m;
        ptr->Offset = 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 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(), -1, 1 );
}

/** Get calibrated left joystick Y reading.
 * @returns a reading between -1 and +1.
 */
float WiiClassicControllerWithCalibration::GetCalLJoyY( void ) const {
    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(), -1, 1 );
}

/** Get calibrated right joystick Y reading.
 * @returns a reading between -1 and +1.
 */
float WiiClassicControllerWithCalibration::GetCalRJoyY( void ) const {
    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(), 0, 1 );
}

/** Get calibrated right trigger reading.
 * @returns a reading between 0 and +1.
 */
float WiiClassicControllerWithCalibration::GetCalRightTrigger( void ) const {
    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, float min, float max ) const {
    if( ( (int)input >= 0 ) && ( (int)input < (int)AnaInCount ) ) {
        const AnaInRec *ptr = records + (int)input;
        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;
 }