File content as of revision 2:0f03a5729afc:

#include "mbed.h"
#include "I2CServo.h"
#include "I2CDevice.h"
#include "Math.h"

const float I2CServo::mAllowableError = 0.002f;

I2CServo::I2CServo( char address ) : I2CDevice( address ){
    mTargetPosition = 0.5f;
    mPosition       = 0.5f;
    mHasStopped     = false;
}

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

int I2CServo::write(){
    char buf = static_cast< char >( mTargetPosition * 255.0f );
    return I2CDevice::write( &buf, 1 );
}

int I2CServo::read(){
    char buf[ 2 ];
    int val = I2CDevice::read( buf, 2 );
    
    mHasStopped = buf[ 0 ];
    mPosition   = static_cast< float >( buf[ 1 ] ) / 255.0f;
    
    return val;
}