Diff: YMotorDriverServo.cpp
- Revision:
- 2:16e29a3a8f58
- Parent:
- 1:d1ca02f9536c
diff -r d1ca02f9536c -r 16e29a3a8f58 YMotorDriverServo.cpp --- a/YMotorDriverServo.cpp Sun Aug 30 23:26:12 2015 +0000 +++ b/YMotorDriverServo.cpp Wed Oct 14 06:04:45 2015 +0000 @@ -2,39 +2,89 @@ #include "YMotorDriverServo.h" #include "YMotorDriverBase.h" -const float YMotorDriverServo::mAllowableError = 0.035f; -const PinName YMotorDriverServo::mAnalogInPinName = dp10; -const float YMotorDriverServo::mLowPassFilterCoeff = 0.55f; +// 1ループあたりだいたい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, - 3.0f, - 3.0f, - 6.0f, - 3.0f, - 3.0f + 3.0f, // Steering + 3.0f, // Steering + 3.0f, // Steering + 3.0f, // Position Manager + 3.0f // Angle Manager }; const float YMotorDriverServo::mICoeff[] = { - 0.020f, - 0.020f, - 0.020f, - 0.010f, - 0.000f, - 0.000f + 0.020f, // Steering + 0.020f, // Steering + 0.020f, // Steering + 0.000f, // Position Manager + 0.000f // Angle Manager }; const float YMotorDriverServo::mDCoeff[] = { - 0.020f, - 0.020f, - 0.020f, - 0.000f, - 0.000f, - 0.020f + 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::mServoMaxDuty = 0.80f; -const float YMotorDriverServo::mServoMinDuty = 0.35f; +const float YMotorDriverServo::mServoRangeMin[] = { + // Steering + 0.145098f, + 0.078431f, + 0.168627f, + // Position Manager + //0.0f, + 0.262745f, + //0.196078f, + // AngleManager + 0.00f +}; -YMotorDriverServo::YMotorDriverServo( char address, int id ) : YMotorDriverBase( address, mServoMaxDuty, mServoMinDuty ), -PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){ +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; @@ -48,26 +98,31 @@ void YMotorDriverServo::updateSpecial(){ // low pass filter - //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff; - mPosition = mAnalogIn->read(); - updatePID( mTargetPosition - mPosition ); + mPosition = mAnalogIn->read() * ( 1.0f - mLowPassFilterCoeff[ mID ] ) + mPosition * mLowPassFilterCoeff[ mID ]; + + updateServoSpecial(); + + float diff = mTargetPosition - mPosition; + mHasWorked = ( abs( diff ) < mAllowableError[ mID ] ); + + updatePID( diff ); // 目標角度に達したらLEDを点灯する - if ( abs( mTargetPosition - mPosition ) < mAllowableError ){ - mLED->write( 1 ); - } else { - mLED->write( 0 ); - } + 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 - static_cast< char >( ( abs( mTargetPosition - mPosition ) < mAllowableError ) ), - // 1Byte目は現在の位置0-255 - static_cast< char >( mPosition * 255.0f ) + // 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; @@ -79,7 +134,11 @@ char buf; mI2C->read( &buf, 1 ); // 0~255で移動させる位置を指定する - mTargetPosition = static_cast< float >( buf ) / 255.0f; + /* + 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; } @@ -89,7 +148,7 @@ } void YMotorDriverServo::control( float c ){ - if ( abs( mTargetPosition - mPosition ) < mAllowableError ){ + if ( mHasWorked ){ setMotorAction( BRAKE ); return; } @@ -97,9 +156,8 @@ if ( c > 0.0f ){ setMotorAction( FORWARD ); } else if ( c < 0.0f ){ - c *= -1.0f; setMotorAction( REVERSE ); } - setPercent( c ); + setPercent( abs( c ) ); }