Diff: I2CServo.cpp

Revision:

2:0f03a5729afc

Parent:

1:e06cf312e9f0

--- a/I2CServo.cpp	Fri Aug 21 04:51:40 2015 +0000
+++ b/I2CServo.cpp	Wed Oct 14 03:50:40 2015 +0000
@@ -3,48 +3,40 @@
 #include "I2CDevice.h"
 #include "Math.h"
 
-const float I2CServo::mRangeMin[] = {
-    // Steering
-    0.18f,
-    0.21f,
-    0.15f,
-    0.16f,
-    // AngleManager
-    0.20f,
-    // PositionManager
-    0.20f
-};
-const float I2CServo::mRangeMax[] = {
-    // Steering
-    0.81f,
-    0.82f,
-    0.80f,
-    0.82f,
-    // AngleManager
-    0.80f,
-    // PositionManager
-    0.80f
-};
+const float I2CServo::mAllowableError = 0.002f;
 
-I2CServo::I2CServo( char address, int id ) : I2CDevice( address ){
+I2CServo::I2CServo( char address ) : I2CDevice( address ){
     mTargetPosition = 0.5f;
-    mPosition = 0.5f;
-    mIsStop = false;
-    mID = id;
+    mPosition       = 0.5f;
+    mHasStopped     = false;
 }
 
-void I2CServo::write(){
-    float pos = convertRange( mTargetPosition, 0.0f, 1.0f, mRangeMin[ mID ], mRangeMax[ mID ] );
-    char buf = static_cast< char >( pos * 255.0f );
-    I2CDevice::write( &buf, 1 );
+void I2CServo::setTargetPosition( float p ){
+    if ( p < 0.0f ){
+        p = 0.0f;
+    } else if ( p > 1.0f ){
+        p = 1.0f;
+    }
+    
+    if ( ( p > mTargetPosition - mAllowableError) &&
+         ( p < mTargetPosition + mAllowableError ) ){
+        mHasStopped = true;
+    }
+    
+    mTargetPosition = p;
 }
 
-void I2CServo::read(){
+int I2CServo::write(){
+    char buf = static_cast< char >( mTargetPosition * 255.0f );
+    return I2CDevice::write( &buf, 1 );
+}
+
+int I2CServo::read(){
     char buf[ 2 ];
-    mI2C->read( mAddress, buf, 2 );
+    int val = I2CDevice::read( buf, 2 );
     
-    mIsStop = buf[ 0 ];
-    mPosition = static_cast< float >( buf[ 1 ] ) / 255.0f;
-    //mPosition = convertRange( mPosition, mRangeMin[ mID ], mRangeMax[ mID ], 0.0f, 1.0f );
-    mPosition = convertRange( mPosition, mRangeMin[ mID ], mRangeMax[ mID ], 0.0f, 1.0f );
+    mHasStopped = buf[ 0 ];
+    mPosition   = static_cast< float >( buf[ 1 ] ) / 255.0f;
+    
+    return val;
 }