YMotorDriverServo.cpp@1:d1ca02f9536c, 2015-08-30 (annotated)
- Committer:
- inst
- Date:
- Sun Aug 30 23:26:12 2015 +0000
- Revision:
- 1:d1ca02f9536c
- Parent:
- 0:9931e2ddc451
- Child:
- 2:16e29a3a8f58
; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
inst | 0:9931e2ddc451 | 1 | #include "mbed.h" |
inst | 0:9931e2ddc451 | 2 | #include "YMotorDriverServo.h" |
inst | 0:9931e2ddc451 | 3 | #include "YMotorDriverBase.h" |
inst | 0:9931e2ddc451 | 4 | |
inst | 0:9931e2ddc451 | 5 | const float YMotorDriverServo::mAllowableError = 0.035f; |
inst | 0:9931e2ddc451 | 6 | const PinName YMotorDriverServo::mAnalogInPinName = dp10; |
inst | 0:9931e2ddc451 | 7 | const float YMotorDriverServo::mLowPassFilterCoeff = 0.55f; |
inst | 0:9931e2ddc451 | 8 | const float YMotorDriverServo::mPCoeff[] = { |
inst | 0:9931e2ddc451 | 9 | 3.0f, |
inst | 0:9931e2ddc451 | 10 | 3.0f, |
inst | 0:9931e2ddc451 | 11 | 3.0f, |
inst | 1:d1ca02f9536c | 12 | 6.0f, |
inst | 1:d1ca02f9536c | 13 | 3.0f, |
inst | 1:d1ca02f9536c | 14 | 3.0f |
inst | 0:9931e2ddc451 | 15 | }; |
inst | 0:9931e2ddc451 | 16 | const float YMotorDriverServo::mICoeff[] = { |
inst | 0:9931e2ddc451 | 17 | 0.020f, |
inst | 0:9931e2ddc451 | 18 | 0.020f, |
inst | 0:9931e2ddc451 | 19 | 0.020f, |
inst | 1:d1ca02f9536c | 20 | 0.010f, |
inst | 1:d1ca02f9536c | 21 | 0.000f, |
inst | 1:d1ca02f9536c | 22 | 0.000f |
inst | 0:9931e2ddc451 | 23 | }; |
inst | 0:9931e2ddc451 | 24 | const float YMotorDriverServo::mDCoeff[] = { |
inst | 1:d1ca02f9536c | 25 | 0.020f, |
inst | 1:d1ca02f9536c | 26 | 0.020f, |
inst | 1:d1ca02f9536c | 27 | 0.020f, |
inst | 1:d1ca02f9536c | 28 | 0.000f, |
inst | 1:d1ca02f9536c | 29 | 0.000f, |
inst | 1:d1ca02f9536c | 30 | 0.020f |
inst | 0:9931e2ddc451 | 31 | }; |
inst | 0:9931e2ddc451 | 32 | |
inst | 1:d1ca02f9536c | 33 | const float YMotorDriverServo::mServoMaxDuty = 0.80f; |
inst | 1:d1ca02f9536c | 34 | const float YMotorDriverServo::mServoMinDuty = 0.35f; |
inst | 1:d1ca02f9536c | 35 | |
inst | 1:d1ca02f9536c | 36 | YMotorDriverServo::YMotorDriverServo( char address, int id ) : YMotorDriverBase( address, mServoMaxDuty, mServoMinDuty ), |
inst | 0:9931e2ddc451 | 37 | PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){ |
inst | 1:d1ca02f9536c | 38 | mAnalogIn = new AnalogIn( mAnalogInPinName ); |
inst | 1:d1ca02f9536c | 39 | mPosition = 0.5f; |
inst | 0:9931e2ddc451 | 40 | mTargetPosition = 0.5f; |
inst | 1:d1ca02f9536c | 41 | mHasWorked = false; |
inst | 1:d1ca02f9536c | 42 | mID = id; |
inst | 0:9931e2ddc451 | 43 | } |
inst | 0:9931e2ddc451 | 44 | |
inst | 0:9931e2ddc451 | 45 | YMotorDriverServo::~YMotorDriverServo(){ |
inst | 0:9931e2ddc451 | 46 | delete mAnalogIn; |
inst | 0:9931e2ddc451 | 47 | } |
inst | 0:9931e2ddc451 | 48 | |
inst | 1:d1ca02f9536c | 49 | void YMotorDriverServo::updateSpecial(){ |
inst | 0:9931e2ddc451 | 50 | // low pass filter |
inst | 0:9931e2ddc451 | 51 | //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff; |
inst | 0:9931e2ddc451 | 52 | mPosition = mAnalogIn->read(); |
inst | 0:9931e2ddc451 | 53 | updatePID( mTargetPosition - mPosition ); |
inst | 0:9931e2ddc451 | 54 | |
inst | 1:d1ca02f9536c | 55 | // 目標角度に達したらLEDを点灯する |
inst | 1:d1ca02f9536c | 56 | if ( abs( mTargetPosition - mPosition ) < mAllowableError ){ |
inst | 1:d1ca02f9536c | 57 | mLED->write( 1 ); |
inst | 1:d1ca02f9536c | 58 | } else { |
inst | 1:d1ca02f9536c | 59 | mLED->write( 0 ); |
inst | 1:d1ca02f9536c | 60 | } |
inst | 1:d1ca02f9536c | 61 | } |
inst | 1:d1ca02f9536c | 62 | |
inst | 1:d1ca02f9536c | 63 | void YMotorDriverServo::updateI2CSlave(){ |
inst | 1:d1ca02f9536c | 64 | switch ( mI2C->receive() ){ |
inst | 0:9931e2ddc451 | 65 | case I2CSlave::ReadAddressed:{ |
inst | 0:9931e2ddc451 | 66 | char buf[] = { |
inst | 1:d1ca02f9536c | 67 | // 0Byte目は、もう目的の角度に達したなら1、違うなら0 |
inst | 1:d1ca02f9536c | 68 | static_cast< char >( ( abs( mTargetPosition - mPosition ) < mAllowableError ) ), |
inst | 1:d1ca02f9536c | 69 | // 1Byte目は現在の位置0-255 |
inst | 1:d1ca02f9536c | 70 | static_cast< char >( mPosition * 255.0f ) |
inst | 0:9931e2ddc451 | 71 | }; |
inst | 0:9931e2ddc451 | 72 | mI2C->write( buf, 2 ); |
inst | 0:9931e2ddc451 | 73 | break; |
inst | 0:9931e2ddc451 | 74 | } |
inst | 0:9931e2ddc451 | 75 | case I2CSlave::WriteGeneral: |
inst | 0:9931e2ddc451 | 76 | break; |
inst | 1:d1ca02f9536c | 77 | |
inst | 0:9931e2ddc451 | 78 | case I2CSlave::WriteAddressed:{ |
inst | 0:9931e2ddc451 | 79 | char buf; |
inst | 0:9931e2ddc451 | 80 | mI2C->read( &buf, 1 ); |
inst | 0:9931e2ddc451 | 81 | // 0~255で移動させる位置を指定する |
inst | 0:9931e2ddc451 | 82 | mTargetPosition = static_cast< float >( buf ) / 255.0f; |
inst | 0:9931e2ddc451 | 83 | break; |
inst | 0:9931e2ddc451 | 84 | } |
inst | 1:d1ca02f9536c | 85 | |
inst | 0:9931e2ddc451 | 86 | case I2CSlave::NoData: |
inst | 0:9931e2ddc451 | 87 | break; |
inst | 0:9931e2ddc451 | 88 | } |
inst | 0:9931e2ddc451 | 89 | } |
inst | 0:9931e2ddc451 | 90 | |
inst | 0:9931e2ddc451 | 91 | void YMotorDriverServo::control( float c ){ |
inst | 0:9931e2ddc451 | 92 | if ( abs( mTargetPosition - mPosition ) < mAllowableError ){ |
inst | 0:9931e2ddc451 | 93 | setMotorAction( BRAKE ); |
inst | 0:9931e2ddc451 | 94 | return; |
inst | 0:9931e2ddc451 | 95 | } |
inst | 0:9931e2ddc451 | 96 | |
inst | 0:9931e2ddc451 | 97 | if ( c > 0.0f ){ |
inst | 0:9931e2ddc451 | 98 | setMotorAction( FORWARD ); |
inst | 0:9931e2ddc451 | 99 | } else if ( c < 0.0f ){ |
inst | 0:9931e2ddc451 | 100 | c *= -1.0f; |
inst | 0:9931e2ddc451 | 101 | setMotorAction( REVERSE ); |
inst | 0:9931e2ddc451 | 102 | } |
inst | 0:9931e2ddc451 | 103 | |
inst | 0:9931e2ddc451 | 104 | setPercent( c ); |
inst | 0:9931e2ddc451 | 105 | } |