Diff: YMotorDriverServo.cpp

Revision:

0:9931e2ddc451

Child:

1:d1ca02f9536c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YMotorDriverServo.cpp	Sun Aug 23 15:18:10 2015 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "YMotorDriverServo.h"
+#include "YMotorDriverBase.h"
+
+const float YMotorDriverServo::mAllowableError = 0.035f;
+const PinName YMotorDriverServo::mAnalogInPinName = dp10;
+const float YMotorDriverServo::mLowPassFilterCoeff = 0.55f;
+const float YMotorDriverServo::mPCoeff[] = {
+    3.0f,
+    3.0f,
+    3.0f,
+    6.0f
+};
+const float YMotorDriverServo::mICoeff[] = {
+    0.020f,
+    0.020f,
+    0.020f,
+    0.010f
+};
+const float YMotorDriverServo::mDCoeff[] = {
+    0.02f,
+    0.02f,
+    0.02f,
+    0.02f
+};
+
+YMotorDriverServo::YMotorDriverServo( char address, int id ) :
+YMotorDriverBase( address ),
+PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){
+    mAnalogIn = new AnalogIn( mAnalogInPinName );
+    mPosition = 0.5f;
+    mTargetPosition = 0.5f;
+    mID = id;
+}
+
+YMotorDriverServo::~YMotorDriverServo(){
+    delete mAnalogIn;
+}
+
+void YMotorDriverServo::update(){
+     // low pass filter
+     //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
+     mPosition = mAnalogIn->read();
+     
+     updatePID( mTargetPosition - mPosition );
+     
+     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 )
+            };
+            mI2C->write( buf, 2 );
+            break;
+        }
+        case I2CSlave::WriteGeneral:
+            break;
+             
+        case I2CSlave::WriteAddressed:{
+            char buf;
+            mI2C->read( &buf, 1 );
+            // 0~255で移動させる位置を指定する
+            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 );
+    }
+}
+
+void YMotorDriverServo::control( float c ){
+     if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
+        setMotorAction( BRAKE );
+        return;
+    }
+     
+     if ( c > 0.0f ){
+        setMotorAction( FORWARD );
+    } else if ( c < 0.0f ){
+        c *= -1.0f;
+        setMotorAction( REVERSE );
+    }
+    
+    setPercent( c );
+}