Diff: YMotorDriverServo.cpp

Revision:

1:d1ca02f9536c

Parent:

0:9931e2ddc451

Child:

2:16e29a3a8f58

--- a/YMotorDriverServo.cpp	Sun Aug 23 15:18:10 2015 +0000
+++ b/YMotorDriverServo.cpp	Sun Aug 30 23:26:12 2015 +0000
@@ -9,55 +9,72 @@
     3.0f,
     3.0f,
     3.0f,
-    6.0f
+    6.0f,
+    3.0f,
+    3.0f
 };
 const float YMotorDriverServo::mICoeff[] = {
     0.020f,
     0.020f,
     0.020f,
-    0.010f
+    0.010f,
+    0.000f,
+    0.000f
 };
 const float YMotorDriverServo::mDCoeff[] = {
-    0.02f,
-    0.02f,
-    0.02f,
-    0.02f
+    0.020f,
+    0.020f,
+    0.020f,
+    0.000f,
+    0.000f,
+    0.020f
 };
 
-YMotorDriverServo::YMotorDriverServo( char address, int id ) :
-YMotorDriverBase( address ),
+const float YMotorDriverServo::mServoMaxDuty = 0.80f;
+const float YMotorDriverServo::mServoMinDuty = 0.35f;
+
+YMotorDriverServo::YMotorDriverServo( char address, int id ) : YMotorDriverBase( address, mServoMaxDuty, mServoMinDuty ),
 PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){
-    mAnalogIn = new AnalogIn( mAnalogInPinName );
-    mPosition = 0.5f;
+    mAnalogIn   = new AnalogIn( mAnalogInPinName );
+    mPosition   = 0.5f;
     mTargetPosition = 0.5f;
-    mID = id;
+    mHasWorked  = false;
+    mID         = id;
 }
 
 YMotorDriverServo::~YMotorDriverServo(){
     delete mAnalogIn;
 }
 
-void YMotorDriverServo::update(){
+void YMotorDriverServo::updateSpecial(){
      // low pass filter
      //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
      mPosition = mAnalogIn->read();
-     
      updatePID( mTargetPosition - mPosition );
      
-     switch ( mI2C->receive() ){
+     // 目標角度に達したらLEDを点灯する
+     if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
+        mLED->write( 1 );
+    } else {
+        mLED->write( 0 );
+    }
+}
+
+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
+            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 );
@@ -65,17 +82,9 @@
             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 );
     }
 }