File content as of revision 0:9931e2ddc451:

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

const float YMotorDriverServo::mAllowableError = 0.035f;
const PinName YMotorDriverServo::mAnalogInPinName = dp10;
const float YMotorDriverServo::mLowPassFilterCoeff = 0.55f;
const float YMotorDriverServo::mPCoeff[] = {
    3.0f,
    3.0f,
    3.0f,
    6.0f
};
const float YMotorDriverServo::mICoeff[] = {
    0.020f,
    0.020f,
    0.020f,
    0.010f
};
const float YMotorDriverServo::mDCoeff[] = {
    0.02f,
    0.02f,
    0.02f,
    0.02f
};

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

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

void YMotorDriverServo::update(){
     // low pass filter
     //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
     mPosition = mAnalogIn->read();
     
     updatePID( mTargetPosition - mPosition );
     
     switch ( mI2C->receive() ){
        case I2CSlave::ReadAddressed:{
            char buf[] = {
                // 0Byte目は、もう目的の角度に達したなら1、違うなら0
                static_cast< char >( ( abs( mTargetPosition - mPosition ) < mAllowableError ) ),
                // 1Byte目は現在の位置0-255
                static_cast< char >( mPosition * 255.0f )
            };
            mI2C->write( buf, 2 );
            break;
        }
        case I2CSlave::WriteGeneral:
            break;
             
        case I2CSlave::WriteAddressed:{
            char buf;
            mI2C->read( &buf, 1 );
            // 0~255で移動させる位置を指定する
            mTargetPosition = static_cast< float >( buf ) / 255.0f;
            break;
        }
            
        case I2CSlave::NoData:
            break;
     }
     
     write();
     
     if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
        mLED->write( 1 );
    } else {
        mLED->write( 0 );
    }
}

void YMotorDriverServo::control( float c ){
     if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
        setMotorAction( BRAKE );
        return;
    }
     
     if ( c > 0.0f ){
        setMotorAction( FORWARD );
    } else if ( c < 0.0f ){
        c *= -1.0f;
        setMotorAction( REVERSE );
    }
    
    setPercent( c );
}