tarou yamada / YMotorDriverServo

YMotorDriverServo.cpp@0:9931e2ddc451, 2015-08-23 (annotated)

Committer:
inst
Date:
Sun Aug 23 15:18:10 2015 +0000
Revision:
0:9931e2ddc451
Child:
1:d1ca02f9536c
first;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inst 0:9931e2ddc451 1 #include "mbed.h"
inst 0:9931e2ddc451 2 #include "YMotorDriverServo.h"
inst 0:9931e2ddc451 3 #include "YMotorDriverBase.h"
inst 0:9931e2ddc451 4
inst 0:9931e2ddc451 5 const float YMotorDriverServo::mAllowableError = 0.035f;
inst 0:9931e2ddc451 6 const PinName YMotorDriverServo::mAnalogInPinName = dp10;
inst 0:9931e2ddc451 7 const float YMotorDriverServo::mLowPassFilterCoeff = 0.55f;
inst 0:9931e2ddc451 8 const float YMotorDriverServo::mPCoeff[] = {
inst 0:9931e2ddc451 9 3.0f,
inst 0:9931e2ddc451 10 3.0f,
inst 0:9931e2ddc451 11 3.0f,
inst 0:9931e2ddc451 12 6.0f
inst 0:9931e2ddc451 13 };
inst 0:9931e2ddc451 14 const float YMotorDriverServo::mICoeff[] = {
inst 0:9931e2ddc451 15 0.020f,
inst 0:9931e2ddc451 16 0.020f,
inst 0:9931e2ddc451 17 0.020f,
inst 0:9931e2ddc451 18 0.010f
inst 0:9931e2ddc451 19 };
inst 0:9931e2ddc451 20 const float YMotorDriverServo::mDCoeff[] = {
inst 0:9931e2ddc451 21 0.02f,
inst 0:9931e2ddc451 22 0.02f,
inst 0:9931e2ddc451 23 0.02f,
inst 0:9931e2ddc451 24 0.02f
inst 0:9931e2ddc451 25 };
inst 0:9931e2ddc451 26
inst 0:9931e2ddc451 27 YMotorDriverServo::YMotorDriverServo( char address, int id ) :
inst 0:9931e2ddc451 28 YMotorDriverBase( address ),
inst 0:9931e2ddc451 29 PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){
inst 0:9931e2ddc451 30 mAnalogIn = new AnalogIn( mAnalogInPinName );
inst 0:9931e2ddc451 31 mPosition = 0.5f;
inst 0:9931e2ddc451 32 mTargetPosition = 0.5f;
inst 0:9931e2ddc451 33 mID = id;
inst 0:9931e2ddc451 34 }
inst 0:9931e2ddc451 35
inst 0:9931e2ddc451 36 YMotorDriverServo::~YMotorDriverServo(){
inst 0:9931e2ddc451 37 delete mAnalogIn;
inst 0:9931e2ddc451 38 }
inst 0:9931e2ddc451 39
inst 0:9931e2ddc451 40 void YMotorDriverServo::update(){
inst 0:9931e2ddc451 41 // low pass filter
inst 0:9931e2ddc451 42 //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
inst 0:9931e2ddc451 43 mPosition = mAnalogIn->read();
inst 0:9931e2ddc451 44
inst 0:9931e2ddc451 45 updatePID( mTargetPosition - mPosition );
inst 0:9931e2ddc451 46
inst 0:9931e2ddc451 47 switch ( mI2C->receive() ){
inst 0:9931e2ddc451 48 case I2CSlave::ReadAddressed:{
inst 0:9931e2ddc451 49 char buf[] = {
inst 0:9931e2ddc451 50 // 0Byte目は、もう目的の角度に達したなら1、違うなら0
inst 0:9931e2ddc451 51 static_cast< char >( ( abs( mTargetPosition - mPosition ) < mAllowableError ) ),
inst 0:9931e2ddc451 52 // 1Byte目は現在の位置0-255
inst 0:9931e2ddc451 53 static_cast< char >( mPosition * 255.0f )
inst 0:9931e2ddc451 54 };
inst 0:9931e2ddc451 55 mI2C->write( buf, 2 );
inst 0:9931e2ddc451 56 break;
inst 0:9931e2ddc451 57 }
inst 0:9931e2ddc451 58 case I2CSlave::WriteGeneral:
inst 0:9931e2ddc451 59 break;
inst 0:9931e2ddc451 60
inst 0:9931e2ddc451 61 case I2CSlave::WriteAddressed:{
inst 0:9931e2ddc451 62 char buf;
inst 0:9931e2ddc451 63 mI2C->read( &buf, 1 );
inst 0:9931e2ddc451 64 // 0~255で移動させる位置を指定する
inst 0:9931e2ddc451 65 mTargetPosition = static_cast< float >( buf ) / 255.0f;
inst 0:9931e2ddc451 66 break;
inst 0:9931e2ddc451 67 }
inst 0:9931e2ddc451 68
inst 0:9931e2ddc451 69 case I2CSlave::NoData:
inst 0:9931e2ddc451 70 break;
inst 0:9931e2ddc451 71 }
inst 0:9931e2ddc451 72
inst 0:9931e2ddc451 73 write();
inst 0:9931e2ddc451 74
inst 0:9931e2ddc451 75 if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
inst 0:9931e2ddc451 76 mLED->write( 1 );
inst 0:9931e2ddc451 77 } else {
inst 0:9931e2ddc451 78 mLED->write( 0 );
inst 0:9931e2ddc451 79 }
inst 0:9931e2ddc451 80 }
inst 0:9931e2ddc451 81
inst 0:9931e2ddc451 82 void YMotorDriverServo::control( float c ){
inst 0:9931e2ddc451 83 if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
inst 0:9931e2ddc451 84 setMotorAction( BRAKE );
inst 0:9931e2ddc451 85 return;
inst 0:9931e2ddc451 86 }
inst 0:9931e2ddc451 87
inst 0:9931e2ddc451 88 if ( c > 0.0f ){
inst 0:9931e2ddc451 89 setMotorAction( FORWARD );
inst 0:9931e2ddc451 90 } else if ( c < 0.0f ){
inst 0:9931e2ddc451 91 c *= -1.0f;
inst 0:9931e2ddc451 92 setMotorAction( REVERSE );
inst 0:9931e2ddc451 93 }
inst 0:9931e2ddc451 94
inst 0:9931e2ddc451 95 setPercent( c );
inst 0:9931e2ddc451 96 }