tarou yamada
I2CServo.cpp@2:0f03a5729afc, 2015-10-14 (annotated)
- Committer:
- inst
- Date:
- Wed Oct 14 03:50:40 2015 +0000
- Revision:
- 2:0f03a5729afc
- Parent:
- 1:e06cf312e9f0
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| inst | 0:d4e07340fb0e | 1 | #include "mbed.h" |
| inst | 0:d4e07340fb0e | 2 | #include "I2CServo.h" |
| inst | 1:e06cf312e9f0 | 3 | #include "I2CDevice.h" |
| inst | 1:e06cf312e9f0 | 4 | #include "Math.h" |
| inst | 0:d4e07340fb0e | 5 | |
| inst | 2:0f03a5729afc | 6 | const float I2CServo::mAllowableError = 0.002f; |
| inst | 1:e06cf312e9f0 | 7 | |
| inst | 2:0f03a5729afc | 8 | I2CServo::I2CServo( char address ) : I2CDevice( address ){ |
| inst | 1:e06cf312e9f0 | 9 | mTargetPosition = 0.5f; |
| inst | 2:0f03a5729afc | 10 | mPosition = 0.5f; |
| inst | 2:0f03a5729afc | 11 | mHasStopped = false; |
| inst | 0:d4e07340fb0e | 12 | } |
| inst | 0:d4e07340fb0e | 13 | |
| inst | 2:0f03a5729afc | 14 | void I2CServo::setTargetPosition( float p ){ |
| inst | 2:0f03a5729afc | 15 | if ( p < 0.0f ){ |
| inst | 2:0f03a5729afc | 16 | p = 0.0f; |
| inst | 2:0f03a5729afc | 17 | } else if ( p > 1.0f ){ |
| inst | 2:0f03a5729afc | 18 | p = 1.0f; |
| inst | 2:0f03a5729afc | 19 | } |
| inst | 2:0f03a5729afc | 20 | |
| inst | 2:0f03a5729afc | 21 | if ( ( p > mTargetPosition - mAllowableError) && |
| inst | 2:0f03a5729afc | 22 | ( p < mTargetPosition + mAllowableError ) ){ |
| inst | 2:0f03a5729afc | 23 | mHasStopped = true; |
| inst | 2:0f03a5729afc | 24 | } |
| inst | 2:0f03a5729afc | 25 | |
| inst | 2:0f03a5729afc | 26 | mTargetPosition = p; |
| inst | 1:e06cf312e9f0 | 27 | } |
| inst | 1:e06cf312e9f0 | 28 | |
| inst | 2:0f03a5729afc | 29 | int I2CServo::write(){ |
| inst | 2:0f03a5729afc | 30 | char buf = static_cast< char >( mTargetPosition * 255.0f ); |
| inst | 2:0f03a5729afc | 31 | return I2CDevice::write( &buf, 1 ); |
| inst | 2:0f03a5729afc | 32 | } |
| inst | 2:0f03a5729afc | 33 | |
| inst | 2:0f03a5729afc | 34 | int I2CServo::read(){ |
| inst | 1:e06cf312e9f0 | 35 | char buf[ 2 ]; |
| inst | 2:0f03a5729afc | 36 | int val = I2CDevice::read( buf, 2 ); |
| inst | 1:e06cf312e9f0 | 37 | |
| inst | 2:0f03a5729afc | 38 | mHasStopped = buf[ 0 ]; |
| inst | 2:0f03a5729afc | 39 | mPosition = static_cast< float >( buf[ 1 ] ) / 255.0f; |
| inst | 2:0f03a5729afc | 40 | |
| inst | 2:0f03a5729afc | 41 | return val; |
| inst | 1:e06cf312e9f0 | 42 | } |