YMotorDriverServo.cpp@2:16e29a3a8f58, 2015-10-14 (annotated)
- Committer:
- inst
- Date:
- Wed Oct 14 06:04:45 2015 +0000
- Revision:
- 2:16e29a3a8f58
- Parent:
- 1:d1ca02f9536c
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 | 2:16e29a3a8f58 | 5 | // 1ループあたりだいたい80us程度 |
inst | 2:16e29a3a8f58 | 6 | |
inst | 2:16e29a3a8f58 | 7 | const float YMotorDriverServo::mAllowableError[] = { |
inst | 2:16e29a3a8f58 | 8 | 0.030, // Steering |
inst | 2:16e29a3a8f58 | 9 | 0.030, // Steering |
inst | 2:16e29a3a8f58 | 10 | 0.030, // Steering |
inst | 2:16e29a3a8f58 | 11 | 0.010, // Position Manager |
inst | 2:16e29a3a8f58 | 12 | 0.012 // Angle Manager |
inst | 2:16e29a3a8f58 | 13 | }; |
inst | 2:16e29a3a8f58 | 14 | const PinName YMotorDriverServo::mAnalogInPinName = dp10; |
inst | 2:16e29a3a8f58 | 15 | const float YMotorDriverServo::mLowPassFilterCoeff[] = { |
inst | 2:16e29a3a8f58 | 16 | 0.10f, // Steering |
inst | 2:16e29a3a8f58 | 17 | 0.10f, // Steering |
inst | 2:16e29a3a8f58 | 18 | 0.10f, // Steering |
inst | 2:16e29a3a8f58 | 19 | 0.865f, // PositionManager |
inst | 2:16e29a3a8f58 | 20 | 0.80f // Angle Manager |
inst | 2:16e29a3a8f58 | 21 | }; |
inst | 2:16e29a3a8f58 | 22 | |
inst | 0:9931e2ddc451 | 23 | const float YMotorDriverServo::mPCoeff[] = { |
inst | 2:16e29a3a8f58 | 24 | 3.0f, // Steering |
inst | 2:16e29a3a8f58 | 25 | 3.0f, // Steering |
inst | 2:16e29a3a8f58 | 26 | 3.0f, // Steering |
inst | 2:16e29a3a8f58 | 27 | 3.0f, // Position Manager |
inst | 2:16e29a3a8f58 | 28 | 3.0f // Angle Manager |
inst | 0:9931e2ddc451 | 29 | }; |
inst | 0:9931e2ddc451 | 30 | const float YMotorDriverServo::mICoeff[] = { |
inst | 2:16e29a3a8f58 | 31 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 32 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 33 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 34 | 0.000f, // Position Manager |
inst | 2:16e29a3a8f58 | 35 | 0.000f // Angle Manager |
inst | 0:9931e2ddc451 | 36 | }; |
inst | 0:9931e2ddc451 | 37 | const float YMotorDriverServo::mDCoeff[] = { |
inst | 2:16e29a3a8f58 | 38 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 39 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 40 | 0.020f, // Steering |
inst | 2:16e29a3a8f58 | 41 | 0.000f, // Position Manager |
inst | 2:16e29a3a8f58 | 42 | 0.000f // Angle Manager |
inst | 2:16e29a3a8f58 | 43 | }; |
inst | 2:16e29a3a8f58 | 44 | const float YMotorDriverServo::mServoMaxDuty[] = { |
inst | 2:16e29a3a8f58 | 45 | 0.80f, // Steering |
inst | 2:16e29a3a8f58 | 46 | 0.80f, // Steering |
inst | 2:16e29a3a8f58 | 47 | 0.80f, // Steering |
inst | 2:16e29a3a8f58 | 48 | 0.80f, // Position Manager |
inst | 2:16e29a3a8f58 | 49 | 0.90f // Angle Manager |
inst | 2:16e29a3a8f58 | 50 | }; |
inst | 2:16e29a3a8f58 | 51 | const float YMotorDriverServo::mServoMinDuty[] = { |
inst | 2:16e29a3a8f58 | 52 | 0.43f, // Steering |
inst | 2:16e29a3a8f58 | 53 | 0.43f, // Steering |
inst | 2:16e29a3a8f58 | 54 | 0.43f, // Steering |
inst | 2:16e29a3a8f58 | 55 | 0.40f, // Position Manager |
inst | 2:16e29a3a8f58 | 56 | 0.85f // Angle Manager |
inst | 0:9931e2ddc451 | 57 | }; |
inst | 0:9931e2ddc451 | 58 | |
inst | 2:16e29a3a8f58 | 59 | const float YMotorDriverServo::mServoRangeMin[] = { |
inst | 2:16e29a3a8f58 | 60 | // Steering |
inst | 2:16e29a3a8f58 | 61 | 0.145098f, |
inst | 2:16e29a3a8f58 | 62 | 0.078431f, |
inst | 2:16e29a3a8f58 | 63 | 0.168627f, |
inst | 2:16e29a3a8f58 | 64 | // Position Manager |
inst | 2:16e29a3a8f58 | 65 | //0.0f, |
inst | 2:16e29a3a8f58 | 66 | 0.262745f, |
inst | 2:16e29a3a8f58 | 67 | //0.196078f, |
inst | 2:16e29a3a8f58 | 68 | // AngleManager |
inst | 2:16e29a3a8f58 | 69 | 0.00f |
inst | 2:16e29a3a8f58 | 70 | }; |
inst | 1:d1ca02f9536c | 71 | |
inst | 2:16e29a3a8f58 | 72 | const float YMotorDriverServo::mServoRangeMax[] = { |
inst | 2:16e29a3a8f58 | 73 | // Steering |
inst | 2:16e29a3a8f58 | 74 | 0.800000, |
inst | 2:16e29a3a8f58 | 75 | 0.768627f, |
inst | 2:16e29a3a8f58 | 76 | 0.850980f, |
inst | 2:16e29a3a8f58 | 77 | // Position Manager |
inst | 2:16e29a3a8f58 | 78 | //0.901961f, |
inst | 2:16e29a3a8f58 | 79 | 0.937255f, |
inst | 2:16e29a3a8f58 | 80 | // AngleManager |
inst | 2:16e29a3a8f58 | 81 | 1.00f |
inst | 2:16e29a3a8f58 | 82 | }; |
inst | 2:16e29a3a8f58 | 83 | |
inst | 2:16e29a3a8f58 | 84 | YMotorDriverServo::YMotorDriverServo( char address, int id ) : |
inst | 2:16e29a3a8f58 | 85 | YMotorDriverBase( address, mServoMaxDuty[ id ], mServoMinDuty[ id ] ), |
inst | 2:16e29a3a8f58 | 86 | PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){ |
inst | 2:16e29a3a8f58 | 87 | |
inst | 1:d1ca02f9536c | 88 | mAnalogIn = new AnalogIn( mAnalogInPinName ); |
inst | 1:d1ca02f9536c | 89 | mPosition = 0.5f; |
inst | 0:9931e2ddc451 | 90 | mTargetPosition = 0.5f; |
inst | 1:d1ca02f9536c | 91 | mHasWorked = false; |
inst | 1:d1ca02f9536c | 92 | mID = id; |
inst | 0:9931e2ddc451 | 93 | } |
inst | 0:9931e2ddc451 | 94 | |
inst | 0:9931e2ddc451 | 95 | YMotorDriverServo::~YMotorDriverServo(){ |
inst | 0:9931e2ddc451 | 96 | delete mAnalogIn; |
inst | 0:9931e2ddc451 | 97 | } |
inst | 0:9931e2ddc451 | 98 | |
inst | 1:d1ca02f9536c | 99 | void YMotorDriverServo::updateSpecial(){ |
inst | 0:9931e2ddc451 | 100 | // low pass filter |
inst | 2:16e29a3a8f58 | 101 | mPosition = mAnalogIn->read() * ( 1.0f - mLowPassFilterCoeff[ mID ] ) + mPosition * mLowPassFilterCoeff[ mID ]; |
inst | 2:16e29a3a8f58 | 102 | |
inst | 2:16e29a3a8f58 | 103 | updateServoSpecial(); |
inst | 2:16e29a3a8f58 | 104 | |
inst | 2:16e29a3a8f58 | 105 | float diff = mTargetPosition - mPosition; |
inst | 2:16e29a3a8f58 | 106 | mHasWorked = ( abs( diff ) < mAllowableError[ mID ] ); |
inst | 2:16e29a3a8f58 | 107 | |
inst | 2:16e29a3a8f58 | 108 | updatePID( diff ); |
inst | 0:9931e2ddc451 | 109 | |
inst | 1:d1ca02f9536c | 110 | // 目標角度に達したらLEDを点灯する |
inst | 2:16e29a3a8f58 | 111 | mLED->write( mHasWorked ); |
inst | 2:16e29a3a8f58 | 112 | } |
inst | 2:16e29a3a8f58 | 113 | |
inst | 2:16e29a3a8f58 | 114 | float map( float val, float oldMin, float oldMax, float newMin, float newMax ){ |
inst | 2:16e29a3a8f58 | 115 | return ( newMax - newMin ) * ( val - oldMin ) / ( oldMax - oldMin ) + newMin; |
inst | 1:d1ca02f9536c | 116 | } |
inst | 1:d1ca02f9536c | 117 | |
inst | 1:d1ca02f9536c | 118 | void YMotorDriverServo::updateI2CSlave(){ |
inst | 1:d1ca02f9536c | 119 | switch ( mI2C->receive() ){ |
inst | 0:9931e2ddc451 | 120 | case I2CSlave::ReadAddressed:{ |
inst | 0:9931e2ddc451 | 121 | char buf[] = { |
inst | 2:16e29a3a8f58 | 122 | // 0Byte目は、もう目的の角度に達したなら1、違うなら0 |
inst | 2:16e29a3a8f58 | 123 | mHasWorked, |
inst | 2:16e29a3a8f58 | 124 | // 1Byte目は現在の位置0-255 |
inst | 2:16e29a3a8f58 | 125 | static_cast< char >( map( mPosition, mServoRangeMin[ mID ], mServoRangeMax[ mID ], 0.0f, 1.0f ) * 255.0f ) |
inst | 0:9931e2ddc451 | 126 | }; |
inst | 0:9931e2ddc451 | 127 | mI2C->write( buf, 2 ); |
inst | 0:9931e2ddc451 | 128 | break; |
inst | 0:9931e2ddc451 | 129 | } |
inst | 0:9931e2ddc451 | 130 | case I2CSlave::WriteGeneral: |
inst | 0:9931e2ddc451 | 131 | break; |
inst | 1:d1ca02f9536c | 132 | |
inst | 0:9931e2ddc451 | 133 | case I2CSlave::WriteAddressed:{ |
inst | 0:9931e2ddc451 | 134 | char buf; |
inst | 0:9931e2ddc451 | 135 | mI2C->read( &buf, 1 ); |
inst | 0:9931e2ddc451 | 136 | // 0~255で移動させる位置を指定する |
inst | 2:16e29a3a8f58 | 137 | /* |
inst | 2:16e29a3a8f58 | 138 | float p = static_cast< float >( buf ) / 255.0f; |
inst | 2:16e29a3a8f58 | 139 | setTargetPosition( map( p, 0.0f, 1.0f, mServoRangeMin[ mID ], mServoRangeMax[ mID ] ) ); |
inst | 2:16e29a3a8f58 | 140 | */ |
inst | 2:16e29a3a8f58 | 141 | setTargetPosition( static_cast< float >( buf ) / 255.0f ); |
inst | 0:9931e2ddc451 | 142 | break; |
inst | 0:9931e2ddc451 | 143 | } |
inst | 1:d1ca02f9536c | 144 | |
inst | 0:9931e2ddc451 | 145 | case I2CSlave::NoData: |
inst | 0:9931e2ddc451 | 146 | break; |
inst | 0:9931e2ddc451 | 147 | } |
inst | 0:9931e2ddc451 | 148 | } |
inst | 0:9931e2ddc451 | 149 | |
inst | 0:9931e2ddc451 | 150 | void YMotorDriverServo::control( float c ){ |
inst | 2:16e29a3a8f58 | 151 | if ( mHasWorked ){ |
inst | 0:9931e2ddc451 | 152 | setMotorAction( BRAKE ); |
inst | 0:9931e2ddc451 | 153 | return; |
inst | 0:9931e2ddc451 | 154 | } |
inst | 0:9931e2ddc451 | 155 | |
inst | 0:9931e2ddc451 | 156 | if ( c > 0.0f ){ |
inst | 0:9931e2ddc451 | 157 | setMotorAction( FORWARD ); |
inst | 0:9931e2ddc451 | 158 | } else if ( c < 0.0f ){ |
inst | 0:9931e2ddc451 | 159 | setMotorAction( REVERSE ); |
inst | 0:9931e2ddc451 | 160 | } |
inst | 0:9931e2ddc451 | 161 | |
inst | 2:16e29a3a8f58 | 162 | setPercent( abs( c ) ); |
inst | 0:9931e2ddc451 | 163 | } |