File content as of revision 1:d1ca02f9536c:

#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,
    3.0f,
    3.0f
};
const float YMotorDriverServo::mICoeff[] = {
    0.020f,
    0.020f,
    0.020f,
    0.010f,
    0.000f,
    0.000f
};
const float YMotorDriverServo::mDCoeff[] = {
    0.020f,
    0.020f,
    0.020f,
    0.000f,
    0.000f,
    0.020f
};

const float YMotorDriverServo::mServoMaxDuty = 0.80f;
const float YMotorDriverServo::mServoMinDuty = 0.35f;

YMotorDriverServo::YMotorDriverServo( char address, int id ) : YMotorDriverBase( address, mServoMaxDuty, mServoMinDuty ),
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 = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
     mPosition = mAnalogIn->read();
     updatePID( mTargetPosition - mPosition );
     
     // 目標角度に達したらLEDを点灯する
     if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
        mLED->write( 1 );
    } else {
        mLED->write( 0 );
    }
}

void YMotorDriverServo::updateI2CSlave(){
    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;
    }
}

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 );
}