tarou yamada / YMotorDriverServo

YMotorDriverServo.cpp@1:d1ca02f9536c, 2015-08-30 (annotated)

Committer:
inst
Date:
Sun Aug 30 23:26:12 2015 +0000
Revision:
1:d1ca02f9536c
Parent:
0:9931e2ddc451
Child:
2:16e29a3a8f58
; ;

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 1:d1ca02f9536c 12 6.0f,
inst 1:d1ca02f9536c 13 3.0f,
inst 1:d1ca02f9536c 14 3.0f
inst 0:9931e2ddc451 15 };
inst 0:9931e2ddc451 16 const float YMotorDriverServo::mICoeff[] = {
inst 0:9931e2ddc451 17 0.020f,
inst 0:9931e2ddc451 18 0.020f,
inst 0:9931e2ddc451 19 0.020f,
inst 1:d1ca02f9536c 20 0.010f,
inst 1:d1ca02f9536c 21 0.000f,
inst 1:d1ca02f9536c 22 0.000f
inst 0:9931e2ddc451 23 };
inst 0:9931e2ddc451 24 const float YMotorDriverServo::mDCoeff[] = {
inst 1:d1ca02f9536c 25 0.020f,
inst 1:d1ca02f9536c 26 0.020f,
inst 1:d1ca02f9536c 27 0.020f,
inst 1:d1ca02f9536c 28 0.000f,
inst 1:d1ca02f9536c 29 0.000f,
inst 1:d1ca02f9536c 30 0.020f
inst 0:9931e2ddc451 31 };
inst 0:9931e2ddc451 32
inst 1:d1ca02f9536c 33 const float YMotorDriverServo::mServoMaxDuty = 0.80f;
inst 1:d1ca02f9536c 34 const float YMotorDriverServo::mServoMinDuty = 0.35f;
inst 1:d1ca02f9536c 35
inst 1:d1ca02f9536c 36 YMotorDriverServo::YMotorDriverServo( char address, int id ) : YMotorDriverBase( address, mServoMaxDuty, mServoMinDuty ),
inst 0:9931e2ddc451 37 PID( mPCoeff[ id ], mICoeff[ id ], mDCoeff[ id ] ){
inst 1:d1ca02f9536c 38 mAnalogIn = new AnalogIn( mAnalogInPinName );
inst 1:d1ca02f9536c 39 mPosition = 0.5f;
inst 0:9931e2ddc451 40 mTargetPosition = 0.5f;
inst 1:d1ca02f9536c 41 mHasWorked = false;
inst 1:d1ca02f9536c 42 mID = id;
inst 0:9931e2ddc451 43 }
inst 0:9931e2ddc451 44
inst 0:9931e2ddc451 45 YMotorDriverServo::~YMotorDriverServo(){
inst 0:9931e2ddc451 46 delete mAnalogIn;
inst 0:9931e2ddc451 47 }
inst 0:9931e2ddc451 48
inst 1:d1ca02f9536c 49 void YMotorDriverServo::updateSpecial(){
inst 0:9931e2ddc451 50 // low pass filter
inst 0:9931e2ddc451 51 //mPosition = mPosition * ( 1.0f - mLowPassFilterCoeff ) + mAnalogIn->read() * mLowPassFilterCoeff;
inst 0:9931e2ddc451 52 mPosition = mAnalogIn->read();
inst 0:9931e2ddc451 53 updatePID( mTargetPosition - mPosition );
inst 0:9931e2ddc451 54
inst 1:d1ca02f9536c 55 // 目標角度に達したらLEDを点灯する
inst 1:d1ca02f9536c 56 if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
inst 1:d1ca02f9536c 57 mLED->write( 1 );
inst 1:d1ca02f9536c 58 } else {
inst 1:d1ca02f9536c 59 mLED->write( 0 );
inst 1:d1ca02f9536c 60 }
inst 1:d1ca02f9536c 61 }
inst 1:d1ca02f9536c 62
inst 1:d1ca02f9536c 63 void YMotorDriverServo::updateI2CSlave(){
inst 1:d1ca02f9536c 64 switch ( mI2C->receive() ){
inst 0:9931e2ddc451 65 case I2CSlave::ReadAddressed:{
inst 0:9931e2ddc451 66 char buf[] = {
inst 1:d1ca02f9536c 67 // 0Byte目は、もう目的の角度に達したなら1、違うなら0
inst 1:d1ca02f9536c 68 static_cast< char >( ( abs( mTargetPosition - mPosition ) < mAllowableError ) ),
inst 1:d1ca02f9536c 69 // 1Byte目は現在の位置0-255
inst 1:d1ca02f9536c 70 static_cast< char >( mPosition * 255.0f )
inst 0:9931e2ddc451 71 };
inst 0:9931e2ddc451 72 mI2C->write( buf, 2 );
inst 0:9931e2ddc451 73 break;
inst 0:9931e2ddc451 74 }
inst 0:9931e2ddc451 75 case I2CSlave::WriteGeneral:
inst 0:9931e2ddc451 76 break;
inst 1:d1ca02f9536c 77
inst 0:9931e2ddc451 78 case I2CSlave::WriteAddressed:{
inst 0:9931e2ddc451 79 char buf;
inst 0:9931e2ddc451 80 mI2C->read( &buf, 1 );
inst 0:9931e2ddc451 81 // 0~255で移動させる位置を指定する
inst 0:9931e2ddc451 82 mTargetPosition = static_cast< float >( buf ) / 255.0f;
inst 0:9931e2ddc451 83 break;
inst 0:9931e2ddc451 84 }
inst 1:d1ca02f9536c 85
inst 0:9931e2ddc451 86 case I2CSlave::NoData:
inst 0:9931e2ddc451 87 break;
inst 0:9931e2ddc451 88 }
inst 0:9931e2ddc451 89 }
inst 0:9931e2ddc451 90
inst 0:9931e2ddc451 91 void YMotorDriverServo::control( float c ){
inst 0:9931e2ddc451 92 if ( abs( mTargetPosition - mPosition ) < mAllowableError ){
inst 0:9931e2ddc451 93 setMotorAction( BRAKE );
inst 0:9931e2ddc451 94 return;
inst 0:9931e2ddc451 95 }
inst 0:9931e2ddc451 96
inst 0:9931e2ddc451 97 if ( c > 0.0f ){
inst 0:9931e2ddc451 98 setMotorAction( FORWARD );
inst 0:9931e2ddc451 99 } else if ( c < 0.0f ){
inst 0:9931e2ddc451 100 c *= -1.0f;
inst 0:9931e2ddc451 101 setMotorAction( REVERSE );
inst 0:9931e2ddc451 102 }
inst 0:9931e2ddc451 103
inst 0:9931e2ddc451 104 setPercent( c );
inst 0:9931e2ddc451 105 }