I2CServo.cpp
- Committer:
- inst
- Date:
- 2015-08-21
- Revision:
- 1:e06cf312e9f0
- Parent:
- 0:d4e07340fb0e
- Child:
- 2:0f03a5729afc
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 ); }