File content as of revision 2:16e29a3a8f58:

#include "mbed.h"
#include "YMotorDriverServo.h"
#include "YMotorDriverBase.h"

// １ループあたりだいたい80us程度

const float YMotorDriverServo::mAllowableError[] = {
    0.030,  // Steering
    0.030,  // Steering
    0.030,  // Steering
    0.010,  // Position Manager
    0.012   // Angle Manager
};
const PinName YMotorDriverServo::mAnalogInPinName   = dp10;
const float YMotorDriverServo::mLowPassFilterCoeff[] = {
    0.10f,   // Steering
    0.10f,   // Steering
    0.10f,   // Steering
    0.865f,  // PositionManager
    0.80f    // Angle Manager
};

const float YMotorDriverServo::mPCoeff[] = {
    3.0f,   // Steering
    3.0f,   // Steering
    3.0f,   // Steering
    3.0f,   // Position Manager
    3.0f    // Angle Manager
};
const float YMotorDriverServo::mICoeff[] = {
    0.020f,     // Steering
    0.020f,     // Steering
    0.020f,     // Steering
    0.000f,     // Position Manager
    0.000f      // Angle Manager
};
const float YMotorDriverServo::mDCoeff[] = {
    0.020f,     // Steering
    0.020f,     // Steering
    0.020f,     // Steering
    0.000f,     // Position Manager
    0.000f      // Angle Manager
};
const float YMotorDriverServo::mServoMaxDuty[] = {
    0.80f,     // Steering
    0.80f,     // Steering
    0.80f,     // Steering
    0.80f,     // Position Manager
    0.90f      // Angle Manager
};
const float YMotorDriverServo::mServoMinDuty[] = {
    0.43f,     // Steering
    0.43f,     // Steering
    0.43f,     // Steering
    0.40f,     // Position Manager
    0.85f      // Angle Manager
};

const float YMotorDriverServo::mServoRangeMin[] = {
    // Steering
    0.145098f,
    0.078431f,
    0.168627f,
    // Position Manager
    //0.0f,
    0.262745f,
    //0.196078f,
    // AngleManager
    0.00f
};

const float YMotorDriverServo::mServoRangeMax[] = {
    // Steering
    0.800000,
    0.768627f,
    0.850980f,
    // Position Manager
    //0.901961f,
    0.937255f,
    // AngleManager
    1.00f
};

YMotorDriverServo::YMotorDriverServo( char address, int id ) :
        YMotorDriverBase( address, mServoMaxDuty[ id ], mServoMinDuty[ id ] ),
        PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){
            
    mAnalogIn   = new AnalogIn( mAnalogInPinName );
    mPosition   = 0.5f;
    mTargetPosition = 0.5f;
    mHasWorked  = false;
    mID         = id;
}

YMotorDriverServo::~YMotorDriverServo(){
    delete mAnalogIn;
}

void YMotorDriverServo::updateSpecial(){
     // low pass filter
     mPosition = mAnalogIn->read() * ( 1.0f - mLowPassFilterCoeff[ mID ] ) + mPosition * mLowPassFilterCoeff[ mID ];
     
     updateServoSpecial();
     
     float diff = mTargetPosition - mPosition;
     mHasWorked = ( abs( diff ) < mAllowableError[ mID ] );
     
     updatePID( diff );
     
     // 目標角度に達したらLEDを点灯する
    mLED->write( mHasWorked );
}

float map( float val, float oldMin, float oldMax, float newMin, float newMax ){
    return ( newMax - newMin ) * ( val - oldMin ) / ( oldMax - oldMin ) + newMin;
}

void YMotorDriverServo::updateI2CSlave(){
    switch ( mI2C->receive() ){
        case I2CSlave::ReadAddressed:{
            char buf[] = {
                // 0Byte目は、もう目的の角度に達したなら1、違うなら0
                mHasWorked,
                // 1Byte目は現在の位置0-255
                static_cast< char >( map( mPosition, mServoRangeMin[ mID ], mServoRangeMax[ mID ], 0.0f, 1.0f ) * 255.0f )
            };
            mI2C->write( buf, 2 );
            break;
        }
        case I2CSlave::WriteGeneral:
            break;
        
        case I2CSlave::WriteAddressed:{
            char buf;
            mI2C->read( &buf, 1 );
            // 0~255で移動させる位置を指定する
            /*
            float p = static_cast< float >( buf ) / 255.0f;
            setTargetPosition( map( p, 0.0f, 1.0f, mServoRangeMin[ mID ], mServoRangeMax[ mID ] ) );
            */
            setTargetPosition( static_cast< float >( buf ) / 255.0f );
            break;
        }
        
        case I2CSlave::NoData:
            break;
    }
}

void YMotorDriverServo::control( float c ){
     if ( mHasWorked ){
        setMotorAction( BRAKE );
        return;
    }
     
     if ( c > 0.0f ){
        setMotorAction( FORWARD );
    } else if ( c < 0.0f ){
        setMotorAction( REVERSE );
    }
    
    setPercent( abs( c ) );
}