Revision 2:16e29a3a8f58, committed 2015-10-14

Comitter:

inst

Date:

Wed Oct 14 06:04:45 2015 +0000

Parent:

1:d1ca02f9536c

Commit message:

Changed in this revision

--- 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;
+// １ループあたりだいたい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 ) );
 }

--- a/YMotorDriverServo.h	Sun Aug 30 23:26:12 2015 +0000
+++ b/YMotorDriverServo.h	Wed Oct 14 06:04:45 2015 +0000
@@ -10,19 +10,41 @@
     YMotorDriverServo( char address, int id );
     ~YMotorDriverServo();
     
+protected:
+    void stop(){
+        mTargetPosition = mPosition;
+    }
+    void setTargetPosition( float p ){
+        if ( p < 0.0f ){
+            p = 0.0f;
+        } else if ( p > 1.0f ){
+            p = 1.0f;
+        }
+        mTargetPosition = ( mServoRangeMax[ mID ] - mServoRangeMin[ mID ] ) * p + mServoRangeMin[ mID ];
+    }
+    float getPosition(){
+        return mPosition;
+    }
+    float getTargetPosition(){
+        return mTargetPosition;
+    }
+    
 private:
     virtual void updateSpecial();
     virtual void updateI2CSlave();
     virtual void control( float c );
+    virtual void updateServoSpecial(){}
 
-    static const float mAllowableError;
+    static const float mAllowableError[];
     static const PinName mAnalogInPinName;
-    static const float mLowPassFilterCoeff;
+    static const float mLowPassFilterCoeff[];
     static const float mPCoeff[];
     static const float mICoeff[];
     static const float mDCoeff[];
-    static const float mServoMaxDuty;
-    static const float mServoMinDuty;
+    static const float mServoMaxDuty[];
+    static const float mServoMinDuty[];
+    static const float mServoRangeMin[];
+    static const float mServoRangeMax[];
     
     AnalogIn* mAnalogIn;
     float mPosition;