YMotorDriverBase.cpp@1:c8ed08beefb9, 2015-10-14 (annotated)
- Committer:
- inst
- Date:
- Wed Oct 14 06:04:34 2015 +0000
- Revision:
- 1:c8ed08beefb9
- Parent:
- 0:a2bbf76ca734
- Child:
- 2:871d1f6d311e
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 | 1:c8ed08beefb9 | 4 | #include "YMotorDriver.h" |
inst | 0:a2bbf76ca734 | 5 | |
inst | 0:a2bbf76ca734 | 6 | const PinName YMotorDriverBase::mMotorDriveDoutPinName[] = { |
inst | 0:a2bbf76ca734 | 7 | dp6, dp9, dp13, dp14 |
inst | 0:a2bbf76ca734 | 8 | }; |
inst | 0:a2bbf76ca734 | 9 | const PinName YMotorDriverBase::mMotorDrivePwmPinName = dp1; |
inst | 0:a2bbf76ca734 | 10 | const PinName YMotorDriverBase::mLEDPinName = dp28; |
inst | 0:a2bbf76ca734 | 11 | const PinName YMotorDriverBase::mSerialPinName[] = { |
inst | 0:a2bbf76ca734 | 12 | // tx rx |
inst | 0:a2bbf76ca734 | 13 | dp16, dp15 |
inst | 0:a2bbf76ca734 | 14 | }; |
inst | 0:a2bbf76ca734 | 15 | const PinName YMotorDriverBase::mI2CPinName[] = { |
inst | 0:a2bbf76ca734 | 16 | // sda scl |
inst | 0:a2bbf76ca734 | 17 | dp5, dp27 |
inst | 0:a2bbf76ca734 | 18 | }; |
inst | 0:a2bbf76ca734 | 19 | const int YMotorDriverBase::mPwmCycle_us = 300; |
inst | 1:c8ed08beefb9 | 20 | const float YMotorDriverBase::mMinDutyList[] = { |
inst | 1:c8ed08beefb9 | 21 | 0.285f, // steering |
inst | 1:c8ed08beefb9 | 22 | 0.285f, // |
inst | 1:c8ed08beefb9 | 23 | 0.285f, // |
inst | 1:c8ed08beefb9 | 24 | 0.0f, // shooter |
inst | 1:c8ed08beefb9 | 25 | 0.3f, // general |
inst | 1:c8ed08beefb9 | 26 | //0.25f // supplier |
inst | 1:c8ed08beefb9 | 27 | 0.22f // supplier |
inst | 1:c8ed08beefb9 | 28 | }; |
inst | 1:c8ed08beefb9 | 29 | const float YMotorDriverBase::mMaxDutyList[] = { |
inst | 1:c8ed08beefb9 | 30 | 0.85f, // steering |
inst | 1:c8ed08beefb9 | 31 | 0.85f, // |
inst | 1:c8ed08beefb9 | 32 | 0.85f, // |
inst | 1:c8ed08beefb9 | 33 | 1.0f, // shooter |
inst | 1:c8ed08beefb9 | 34 | 0.9f, // general |
inst | 1:c8ed08beefb9 | 35 | 0.50f // supplier |
inst | 1:c8ed08beefb9 | 36 | }; |
inst | 0:a2bbf76ca734 | 37 | |
inst | 1:c8ed08beefb9 | 38 | YMotorDriverBase::YMotorDriverBase( char address ) : |
inst | 1:c8ed08beefb9 | 39 | mAddress( address ), |
inst | 1:c8ed08beefb9 | 40 | mMaxDuty( mMaxDutyList[ YMotorDriver::GENERAL_ID ] ), mMinDuty( mMinDutyList[ YMotorDriver::GENERAL_ID ] ){ |
inst | 1:c8ed08beefb9 | 41 | |
inst | 1:c8ed08beefb9 | 42 | init(); |
inst | 1:c8ed08beefb9 | 43 | } |
inst | 1:c8ed08beefb9 | 44 | |
inst | 1:c8ed08beefb9 | 45 | YMotorDriverBase::YMotorDriverBase( char address, int id ) : |
inst | 1:c8ed08beefb9 | 46 | mAddress( address ), mMaxDuty( mMaxDutyList[ id ] ), mMinDuty( mMinDutyList[ id ] ){ |
inst | 1:c8ed08beefb9 | 47 | init(); |
inst | 1:c8ed08beefb9 | 48 | } |
inst | 1:c8ed08beefb9 | 49 | |
inst | 1:c8ed08beefb9 | 50 | YMotorDriverBase::YMotorDriverBase( char address, float maxDuty, float minDuty ) : |
inst | 1:c8ed08beefb9 | 51 | mAddress( address ), mMaxDuty( maxDuty ), mMinDuty( minDuty ){ |
inst | 1:c8ed08beefb9 | 52 | init(); |
inst | 1:c8ed08beefb9 | 53 | } |
inst | 1:c8ed08beefb9 | 54 | |
inst | 1:c8ed08beefb9 | 55 | void YMotorDriverBase::init(){ |
inst | 0:a2bbf76ca734 | 56 | mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] ); |
inst | 0:a2bbf76ca734 | 57 | mI2C->address( mAddress ); |
inst | 0:a2bbf76ca734 | 58 | |
inst | 0:a2bbf76ca734 | 59 | for ( int i = 0; i < 4; ++i ){ |
inst | 0:a2bbf76ca734 | 60 | mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] ); |
inst | 0:a2bbf76ca734 | 61 | } |
inst | 0:a2bbf76ca734 | 62 | |
inst | 0:a2bbf76ca734 | 63 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 64 | |
inst | 0:a2bbf76ca734 | 65 | mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName ); |
inst | 0:a2bbf76ca734 | 66 | mMotorDrivePwm->setPeriod_us( mPwmCycle_us ); |
inst | 0:a2bbf76ca734 | 67 | mMotorDrivePwm->setDuty( mMinDuty ); |
inst | 0:a2bbf76ca734 | 68 | |
inst | 0:a2bbf76ca734 | 69 | mLED = new DigitalOut( mLEDPinName, 0 ); |
inst | 0:a2bbf76ca734 | 70 | |
inst | 0:a2bbf76ca734 | 71 | write(); |
inst | 0:a2bbf76ca734 | 72 | } |
inst | 0:a2bbf76ca734 | 73 | |
inst | 0:a2bbf76ca734 | 74 | YMotorDriverBase::~YMotorDriverBase(){ |
inst | 0:a2bbf76ca734 | 75 | delete mI2C; |
inst | 0:a2bbf76ca734 | 76 | } |
inst | 0:a2bbf76ca734 | 77 | |
inst | 0:a2bbf76ca734 | 78 | void YMotorDriverBase::update(){ |
inst | 1:c8ed08beefb9 | 79 | updateI2CSlave(); |
inst | 1:c8ed08beefb9 | 80 | updateSpecial(); |
inst | 1:c8ed08beefb9 | 81 | write(); |
inst | 1:c8ed08beefb9 | 82 | } |
inst | 1:c8ed08beefb9 | 83 | |
inst | 1:c8ed08beefb9 | 84 | void YMotorDriverBase::updateI2CSlave(){ |
inst | 0:a2bbf76ca734 | 85 | switch ( mI2C->receive() ){ |
inst | 0:a2bbf76ca734 | 86 | case I2CSlave::ReadAddressed:{ |
inst | 0:a2bbf76ca734 | 87 | char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f }; |
inst | 0:a2bbf76ca734 | 88 | mI2C->write( buf, 2 ); |
inst | 0:a2bbf76ca734 | 89 | break; |
inst | 0:a2bbf76ca734 | 90 | } |
inst | 0:a2bbf76ca734 | 91 | case I2CSlave::WriteGeneral: |
inst | 0:a2bbf76ca734 | 92 | break; |
inst | 0:a2bbf76ca734 | 93 | |
inst | 0:a2bbf76ca734 | 94 | case I2CSlave::WriteAddressed:{ |
inst | 0:a2bbf76ca734 | 95 | char buf[ 2 ]; |
inst | 0:a2bbf76ca734 | 96 | mI2C->read( buf, 2 ); |
inst | 0:a2bbf76ca734 | 97 | // 初めの1Byteはモータの動作を指定する |
inst | 0:a2bbf76ca734 | 98 | mAction = static_cast< MotorAction >( buf[ 0 ] ); |
inst | 0:a2bbf76ca734 | 99 | // 次の1Byteは0~255でDutyを指定する |
inst | 0:a2bbf76ca734 | 100 | mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f; |
inst | 0:a2bbf76ca734 | 101 | break; |
inst | 0:a2bbf76ca734 | 102 | } |
inst | 0:a2bbf76ca734 | 103 | |
inst | 0:a2bbf76ca734 | 104 | case I2CSlave::NoData: |
inst | 0:a2bbf76ca734 | 105 | break; |
inst | 0:a2bbf76ca734 | 106 | } |
inst | 0:a2bbf76ca734 | 107 | } |
inst | 1:c8ed08beefb9 | 108 | |
inst | 0:a2bbf76ca734 | 109 | void YMotorDriverBase::write(){ |
inst | 0:a2bbf76ca734 | 110 | updatePwmDuty(); |
inst | 0:a2bbf76ca734 | 111 | |
inst | 0:a2bbf76ca734 | 112 | switch ( mAction ){ |
inst | 0:a2bbf76ca734 | 113 | case FORWARD: |
inst | 0:a2bbf76ca734 | 114 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 115 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 116 | mMotorDriveDout[ 2 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 117 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 118 | break; |
inst | 0:a2bbf76ca734 | 119 | |
inst | 0:a2bbf76ca734 | 120 | case REVERSE: |
inst | 0:a2bbf76ca734 | 121 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 122 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 123 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 124 | mMotorDriveDout[ 3 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 125 | break; |
inst | 0:a2bbf76ca734 | 126 | |
inst | 0:a2bbf76ca734 | 127 | case BRAKE: |
inst | 0:a2bbf76ca734 | 128 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 129 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 130 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 131 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 132 | break; |
inst | 0:a2bbf76ca734 | 133 | |
inst | 0:a2bbf76ca734 | 134 | case RELEASE: |
inst | 0:a2bbf76ca734 | 135 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 136 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 137 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 138 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 139 | break; |
inst | 0:a2bbf76ca734 | 140 | |
inst | 0:a2bbf76ca734 | 141 | default: |
inst | 0:a2bbf76ca734 | 142 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 143 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 144 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 145 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 146 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 147 | break; |
inst | 0:a2bbf76ca734 | 148 | } |
inst | 0:a2bbf76ca734 | 149 | } |
inst | 0:a2bbf76ca734 | 150 | |
inst | 0:a2bbf76ca734 | 151 | void YMotorDriverBase::updatePwmDuty(){ |
inst | 0:a2bbf76ca734 | 152 | static float prevDuty = mMinDuty; |
inst | 0:a2bbf76ca734 | 153 | float d = middle( mMinDuty, mDuty, mMaxDuty ); |
inst | 0:a2bbf76ca734 | 154 | |
inst | 0:a2bbf76ca734 | 155 | if ( d != prevDuty ){ |
inst | 0:a2bbf76ca734 | 156 | mMotorDrivePwm->setDuty( d ); |
inst | 0:a2bbf76ca734 | 157 | } |
inst | 0:a2bbf76ca734 | 158 | prevDuty = d; |
inst | 0:a2bbf76ca734 | 159 | } |
inst | 0:a2bbf76ca734 | 160 | |
inst | 0:a2bbf76ca734 | 161 | void YMotorDriverBase::setPercent( float p ){ |
inst | 0:a2bbf76ca734 | 162 | p = middle( 0.0f, p, 1.0f ); |
inst | 0:a2bbf76ca734 | 163 | mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty; |
inst | 0:a2bbf76ca734 | 164 | } |