YMotorDriverServo.cpp
- Committer:
- inst
- Date:
- 2015-08-23
- Revision:
- 0:9931e2ddc451
- Child:
- 1:d1ca02f9536c
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 ); }