I2CServo.cpp
- Committer:
- inst
- Date:
- 2015-10-14
- Revision:
- 2:0f03a5729afc
- Parent:
- 1:e06cf312e9f0
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; }