File content as of revision 1:e06cf312e9f0:

#include "mbed.h"
#include "I2CServo.h"
#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
};

I2CServo::I2CServo( char address, int id ) : I2CDevice( address ){
    mTargetPosition = 0.5f;
    mPosition = 0.5f;
    mIsStop = false;
    mID = id;
}

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::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 );
}