YMotorDriverBase.cpp@0:a2bbf76ca734, 2015-08-23 (annotated)
- Committer:
- inst
- Date:
- Sun Aug 23 15:18:04 2015 +0000
- Revision:
- 0:a2bbf76ca734
- Child:
- 1:c8ed08beefb9
first
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
inst | 0:a2bbf76ca734 | 1 | #include "mbed.h" |
inst | 0:a2bbf76ca734 | 2 | #include "PWMOut.h" |
inst | 0:a2bbf76ca734 | 3 | #include "YMotorDriverBase.h" |
inst | 0:a2bbf76ca734 | 4 | |
inst | 0:a2bbf76ca734 | 5 | const PinName YMotorDriverBase::mMotorDriveDoutPinName[] = { |
inst | 0:a2bbf76ca734 | 6 | dp6, dp9, dp13, dp14 |
inst | 0:a2bbf76ca734 | 7 | }; |
inst | 0:a2bbf76ca734 | 8 | const PinName YMotorDriverBase::mMotorDrivePwmPinName = dp1; |
inst | 0:a2bbf76ca734 | 9 | const PinName YMotorDriverBase::mLEDPinName = dp28; |
inst | 0:a2bbf76ca734 | 10 | const PinName YMotorDriverBase::mSerialPinName[] = { |
inst | 0:a2bbf76ca734 | 11 | // tx rx |
inst | 0:a2bbf76ca734 | 12 | dp16, dp15 |
inst | 0:a2bbf76ca734 | 13 | }; |
inst | 0:a2bbf76ca734 | 14 | const PinName YMotorDriverBase::mI2CPinName[] = { |
inst | 0:a2bbf76ca734 | 15 | // sda scl |
inst | 0:a2bbf76ca734 | 16 | dp5, dp27 |
inst | 0:a2bbf76ca734 | 17 | }; |
inst | 0:a2bbf76ca734 | 18 | const int YMotorDriverBase::mPwmCycle_us = 300; |
inst | 0:a2bbf76ca734 | 19 | const float YMotorDriverBase::mMaxDuty = 0.90f; |
inst | 0:a2bbf76ca734 | 20 | const float YMotorDriverBase::mMinDuty = 0.30f; |
inst | 0:a2bbf76ca734 | 21 | |
inst | 0:a2bbf76ca734 | 22 | YMotorDriverBase::YMotorDriverBase( char address ) : mAddress( address ){ |
inst | 0:a2bbf76ca734 | 23 | mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] ); |
inst | 0:a2bbf76ca734 | 24 | mI2C->address( mAddress ); |
inst | 0:a2bbf76ca734 | 25 | |
inst | 0:a2bbf76ca734 | 26 | for ( int i = 0; i < 4; ++i ){ |
inst | 0:a2bbf76ca734 | 27 | mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] ); |
inst | 0:a2bbf76ca734 | 28 | } |
inst | 0:a2bbf76ca734 | 29 | |
inst | 0:a2bbf76ca734 | 30 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 31 | |
inst | 0:a2bbf76ca734 | 32 | mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName ); |
inst | 0:a2bbf76ca734 | 33 | mMotorDrivePwm->setPeriod_us( mPwmCycle_us ); |
inst | 0:a2bbf76ca734 | 34 | mMotorDrivePwm->setDuty( mMinDuty ); |
inst | 0:a2bbf76ca734 | 35 | |
inst | 0:a2bbf76ca734 | 36 | mLED = new DigitalOut( mLEDPinName, 0 ); |
inst | 0:a2bbf76ca734 | 37 | |
inst | 0:a2bbf76ca734 | 38 | write(); |
inst | 0:a2bbf76ca734 | 39 | } |
inst | 0:a2bbf76ca734 | 40 | |
inst | 0:a2bbf76ca734 | 41 | YMotorDriverBase::~YMotorDriverBase(){ |
inst | 0:a2bbf76ca734 | 42 | delete mI2C; |
inst | 0:a2bbf76ca734 | 43 | } |
inst | 0:a2bbf76ca734 | 44 | |
inst | 0:a2bbf76ca734 | 45 | void YMotorDriverBase::update(){ |
inst | 0:a2bbf76ca734 | 46 | switch ( mI2C->receive() ){ |
inst | 0:a2bbf76ca734 | 47 | case I2CSlave::ReadAddressed:{ |
inst | 0:a2bbf76ca734 | 48 | char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f }; |
inst | 0:a2bbf76ca734 | 49 | mI2C->write( buf, 2 ); |
inst | 0:a2bbf76ca734 | 50 | break; |
inst | 0:a2bbf76ca734 | 51 | } |
inst | 0:a2bbf76ca734 | 52 | case I2CSlave::WriteGeneral: |
inst | 0:a2bbf76ca734 | 53 | break; |
inst | 0:a2bbf76ca734 | 54 | |
inst | 0:a2bbf76ca734 | 55 | case I2CSlave::WriteAddressed:{ |
inst | 0:a2bbf76ca734 | 56 | char buf[ 2 ]; |
inst | 0:a2bbf76ca734 | 57 | mI2C->read( buf, 2 ); |
inst | 0:a2bbf76ca734 | 58 | // 初めの1Byteはモータの動作を指定する |
inst | 0:a2bbf76ca734 | 59 | mAction = static_cast< MotorAction >( buf[ 0 ] ); |
inst | 0:a2bbf76ca734 | 60 | // 次の1Byteは0~255でDutyを指定する |
inst | 0:a2bbf76ca734 | 61 | mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f; |
inst | 0:a2bbf76ca734 | 62 | break; |
inst | 0:a2bbf76ca734 | 63 | } |
inst | 0:a2bbf76ca734 | 64 | |
inst | 0:a2bbf76ca734 | 65 | case I2CSlave::NoData: |
inst | 0:a2bbf76ca734 | 66 | break; |
inst | 0:a2bbf76ca734 | 67 | } |
inst | 0:a2bbf76ca734 | 68 | |
inst | 0:a2bbf76ca734 | 69 | write(); |
inst | 0:a2bbf76ca734 | 70 | } |
inst | 0:a2bbf76ca734 | 71 | void YMotorDriverBase::write(){ |
inst | 0:a2bbf76ca734 | 72 | updatePwmDuty(); |
inst | 0:a2bbf76ca734 | 73 | |
inst | 0:a2bbf76ca734 | 74 | switch ( mAction ){ |
inst | 0:a2bbf76ca734 | 75 | case FORWARD: |
inst | 0:a2bbf76ca734 | 76 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 77 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 78 | mMotorDriveDout[ 2 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 79 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 80 | break; |
inst | 0:a2bbf76ca734 | 81 | |
inst | 0:a2bbf76ca734 | 82 | case REVERSE: |
inst | 0:a2bbf76ca734 | 83 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 84 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 85 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 86 | mMotorDriveDout[ 3 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 87 | break; |
inst | 0:a2bbf76ca734 | 88 | |
inst | 0:a2bbf76ca734 | 89 | case BRAKE: |
inst | 0:a2bbf76ca734 | 90 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 91 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 92 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 93 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 94 | break; |
inst | 0:a2bbf76ca734 | 95 | |
inst | 0:a2bbf76ca734 | 96 | case RELEASE: |
inst | 0:a2bbf76ca734 | 97 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 98 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 99 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 100 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 101 | break; |
inst | 0:a2bbf76ca734 | 102 | |
inst | 0:a2bbf76ca734 | 103 | default: |
inst | 0:a2bbf76ca734 | 104 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 105 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 106 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 107 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 108 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 109 | break; |
inst | 0:a2bbf76ca734 | 110 | } |
inst | 0:a2bbf76ca734 | 111 | } |
inst | 0:a2bbf76ca734 | 112 | |
inst | 0:a2bbf76ca734 | 113 | void YMotorDriverBase::updatePwmDuty(){ |
inst | 0:a2bbf76ca734 | 114 | static float prevDuty = mMinDuty; |
inst | 0:a2bbf76ca734 | 115 | float d = middle( mMinDuty, mDuty, mMaxDuty ); |
inst | 0:a2bbf76ca734 | 116 | |
inst | 0:a2bbf76ca734 | 117 | if ( d != prevDuty ){ |
inst | 0:a2bbf76ca734 | 118 | mMotorDrivePwm->setDuty( d ); |
inst | 0:a2bbf76ca734 | 119 | } |
inst | 0:a2bbf76ca734 | 120 | prevDuty = d; |
inst | 0:a2bbf76ca734 | 121 | } |
inst | 0:a2bbf76ca734 | 122 | |
inst | 0:a2bbf76ca734 | 123 | void YMotorDriverBase::setPercent( float p ){ |
inst | 0:a2bbf76ca734 | 124 | p = middle( 0.0f, p, 1.0f ); |
inst | 0:a2bbf76ca734 | 125 | mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty; |
inst | 0:a2bbf76ca734 | 126 | } |