Revision 2:0f03a5729afc, committed 2015-10-14

Comitter:

inst

Date:

Wed Oct 14 03:50:40 2015 +0000

Parent:

1:e06cf312e9f0

Commit message:

Changed in this revision

diff -r e06cf312e9f0 -r 0f03a5729afc I2CServo.cpp
--- 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;
 }

diff -r e06cf312e9f0 -r 0f03a5729afc I2CServo.h
--- a/I2CServo.h	Fri Aug 21 04:51:40 2015 +0000
+++ b/I2CServo.h	Wed Oct 14 03:50:40 2015 +0000
@@ -6,41 +6,30 @@
 
 class I2CServo : public I2CDevice{
 public:
-    I2CServo( char address, int id );
+    I2CServo( char address );
     
-    void setTargetPosition( float p ){
-        if ( p < 0.0f ){
-            p = 0.0f;
-        } else if ( p > 1.0f ){
-            p = 1.0f;
-        }
-        
-        if ( p != mTargetPosition ){
-            mIsStop = false;
-        }
-        
-        mTargetPosition = p;
-    }
+    void setTargetPosition( float p );
     
-    bool isStop(){
-        return mIsStop;
+    bool hasStopped(){
+        return mHasStopped;
     }
     float getPosition(){
         return mPosition;
     }
     
-    virtual void write();
-    virtual void read();
+    void stop(){
+        mTargetPosition = mPosition;
+        mHasStopped = true;
+    }
+    
+    virtual int write();
+    virtual int read();
     
 private:
+    static const float mAllowableError;
     float mTargetPosition;
-    bool mIsStop;
+    bool mHasStopped;
     float mPosition;
-    
-    int mID;
-    
-    static const float mRangeMax[];
-    static const float mRangeMin[];
 };
 
 #endif
\ No newline at end of file