Diff: I2CServo.cpp

Revision:

1:e06cf312e9f0

Parent:

0:d4e07340fb0e

Child:

2:0f03a5729afc

--- a/I2CServo.cpp	Thu Jul 02 00:58:28 2015 +0000
+++ b/I2CServo.cpp	Fri Aug 21 04:51:40 2015 +0000
@@ -1,14 +1,50 @@
 #include "mbed.h"
 #include "I2CServo.h"
+#include "I2CDevice.h"
+#include "Math.h"
 
-I2CServo::I2CServo( I2C* i2c, char address ) : I2CDevice( i2c, address ){
-    mPosition = 512;
+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
+};
+
+I2CServo::I2CServo( char address, int id ) : I2CDevice( address ){
+    mTargetPosition = 0.5f;
+    mPosition = 0.5f;
+    mIsStop = false;
+    mID = id;
 }
 
 void I2CServo::write(){
-    char trans[] = {
-        static_cast< char >( ( mPosition >> 8 ) & 0x03 ),
-        static_cast< char >( mPosition & 0xFF )
-    };
-    mI2C->write( mAddress, trans, 2 );
-}
\ No newline at end of file
+    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::read(){
+    char buf[ 2 ];
+    mI2C->read( mAddress, 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 );
+}