YMotorDriverBase.cpp@3:22f19e076931, 2015-11-13 (annotated)
- Committer:
- inst
- Date:
- Fri Nov 13 07:48:56 2015 +0000
- Revision:
- 3:22f19e076931
- Parent:
- 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.22f // supplier |
inst | 1:c8ed08beefb9 | 27 | }; |
inst | 1:c8ed08beefb9 | 28 | const float YMotorDriverBase::mMaxDutyList[] = { |
inst | 1:c8ed08beefb9 | 29 | 0.85f, // steering |
inst | 1:c8ed08beefb9 | 30 | 0.85f, // |
inst | 1:c8ed08beefb9 | 31 | 0.85f, // |
inst | 1:c8ed08beefb9 | 32 | 1.0f, // shooter |
inst | 1:c8ed08beefb9 | 33 | 0.9f, // general |
inst | 1:c8ed08beefb9 | 34 | 0.50f // supplier |
inst | 1:c8ed08beefb9 | 35 | }; |
inst | 0:a2bbf76ca734 | 36 | |
inst | 1:c8ed08beefb9 | 37 | YMotorDriverBase::YMotorDriverBase( char address ) : |
inst | 1:c8ed08beefb9 | 38 | mAddress( address ), |
inst | 1:c8ed08beefb9 | 39 | mMaxDuty( mMaxDutyList[ YMotorDriver::GENERAL_ID ] ), mMinDuty( mMinDutyList[ YMotorDriver::GENERAL_ID ] ){ |
inst | 1:c8ed08beefb9 | 40 | init(); |
inst | 1:c8ed08beefb9 | 41 | } |
inst | 1:c8ed08beefb9 | 42 | |
inst | 1:c8ed08beefb9 | 43 | YMotorDriverBase::YMotorDriverBase( char address, int id ) : |
inst | 1:c8ed08beefb9 | 44 | mAddress( address ), mMaxDuty( mMaxDutyList[ id ] ), mMinDuty( mMinDutyList[ id ] ){ |
inst | 1:c8ed08beefb9 | 45 | init(); |
inst | 1:c8ed08beefb9 | 46 | } |
inst | 1:c8ed08beefb9 | 47 | |
inst | 1:c8ed08beefb9 | 48 | YMotorDriverBase::YMotorDriverBase( char address, float maxDuty, float minDuty ) : |
inst | 1:c8ed08beefb9 | 49 | mAddress( address ), mMaxDuty( maxDuty ), mMinDuty( minDuty ){ |
inst | 1:c8ed08beefb9 | 50 | init(); |
inst | 1:c8ed08beefb9 | 51 | } |
inst | 1:c8ed08beefb9 | 52 | |
inst | 1:c8ed08beefb9 | 53 | void YMotorDriverBase::init(){ |
inst | 0:a2bbf76ca734 | 54 | mI2C = new I2CSlave( mI2CPinName[ 0 ], mI2CPinName[ 1 ] ); |
inst | 0:a2bbf76ca734 | 55 | mI2C->address( mAddress ); |
inst | 0:a2bbf76ca734 | 56 | |
inst | 0:a2bbf76ca734 | 57 | for ( int i = 0; i < 4; ++i ){ |
inst | 0:a2bbf76ca734 | 58 | mMotorDriveDout[ i ] = new DigitalOut( mMotorDriveDoutPinName[ i ] ); |
inst | 0:a2bbf76ca734 | 59 | } |
inst | 0:a2bbf76ca734 | 60 | |
inst | 0:a2bbf76ca734 | 61 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 62 | |
inst | 0:a2bbf76ca734 | 63 | mMotorDrivePwm = new PWMOut( mMotorDrivePwmPinName ); |
inst | 0:a2bbf76ca734 | 64 | mMotorDrivePwm->setPeriod_us( mPwmCycle_us ); |
inst | 0:a2bbf76ca734 | 65 | mMotorDrivePwm->setDuty( mMinDuty ); |
inst | 0:a2bbf76ca734 | 66 | |
inst | 0:a2bbf76ca734 | 67 | mLED = new DigitalOut( mLEDPinName, 0 ); |
inst | 0:a2bbf76ca734 | 68 | |
inst | 0:a2bbf76ca734 | 69 | write(); |
inst | 0:a2bbf76ca734 | 70 | } |
inst | 0:a2bbf76ca734 | 71 | |
inst | 0:a2bbf76ca734 | 72 | YMotorDriverBase::~YMotorDriverBase(){ |
inst | 0:a2bbf76ca734 | 73 | delete mI2C; |
inst | 0:a2bbf76ca734 | 74 | } |
inst | 0:a2bbf76ca734 | 75 | |
inst | 0:a2bbf76ca734 | 76 | void YMotorDriverBase::update(){ |
inst | 1:c8ed08beefb9 | 77 | updateI2CSlave(); |
inst | 1:c8ed08beefb9 | 78 | updateSpecial(); |
inst | 1:c8ed08beefb9 | 79 | write(); |
inst | 1:c8ed08beefb9 | 80 | } |
inst | 1:c8ed08beefb9 | 81 | |
inst | 1:c8ed08beefb9 | 82 | void YMotorDriverBase::updateI2CSlave(){ |
inst | 0:a2bbf76ca734 | 83 | switch ( mI2C->receive() ){ |
inst | 0:a2bbf76ca734 | 84 | case I2CSlave::ReadAddressed:{ |
inst | 0:a2bbf76ca734 | 85 | char buf[] = { mMinDuty * 255.0f, mMaxDuty * 255.0f }; |
inst | 0:a2bbf76ca734 | 86 | mI2C->write( buf, 2 ); |
inst | 0:a2bbf76ca734 | 87 | break; |
inst | 0:a2bbf76ca734 | 88 | } |
inst | 0:a2bbf76ca734 | 89 | case I2CSlave::WriteGeneral: |
inst | 0:a2bbf76ca734 | 90 | break; |
inst | 0:a2bbf76ca734 | 91 | |
inst | 0:a2bbf76ca734 | 92 | case I2CSlave::WriteAddressed:{ |
inst | 0:a2bbf76ca734 | 93 | char buf[ 2 ]; |
inst | 0:a2bbf76ca734 | 94 | mI2C->read( buf, 2 ); |
inst | 0:a2bbf76ca734 | 95 | // 初めの1Byteはモータの動作を指定する |
inst | 0:a2bbf76ca734 | 96 | mAction = static_cast< MotorAction >( buf[ 0 ] ); |
inst | 0:a2bbf76ca734 | 97 | // 次の1Byteは0~255でDutyを指定する |
inst | 0:a2bbf76ca734 | 98 | mDuty = static_cast< float >( buf[ 1 ] ) / 255.0f; |
inst | 0:a2bbf76ca734 | 99 | break; |
inst | 0:a2bbf76ca734 | 100 | } |
inst | 0:a2bbf76ca734 | 101 | |
inst | 0:a2bbf76ca734 | 102 | case I2CSlave::NoData: |
inst | 0:a2bbf76ca734 | 103 | break; |
inst | 0:a2bbf76ca734 | 104 | } |
inst | 0:a2bbf76ca734 | 105 | } |
inst | 1:c8ed08beefb9 | 106 | |
inst | 0:a2bbf76ca734 | 107 | void YMotorDriverBase::write(){ |
inst | 2:871d1f6d311e | 108 | mMotorDrivePwm->setDuty( middle( mMinDuty, mDuty, mMaxDuty ) ); |
inst | 0:a2bbf76ca734 | 109 | |
inst | 0:a2bbf76ca734 | 110 | switch ( mAction ){ |
inst | 0:a2bbf76ca734 | 111 | case FORWARD: |
inst | 0:a2bbf76ca734 | 112 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 113 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 114 | mMotorDriveDout[ 2 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 115 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 116 | break; |
inst | 0:a2bbf76ca734 | 117 | |
inst | 0:a2bbf76ca734 | 118 | case REVERSE: |
inst | 0:a2bbf76ca734 | 119 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 120 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 121 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 122 | mMotorDriveDout[ 3 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 123 | break; |
inst | 0:a2bbf76ca734 | 124 | |
inst | 0:a2bbf76ca734 | 125 | case BRAKE: |
inst | 0:a2bbf76ca734 | 126 | mMotorDriveDout[ 0 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 127 | mMotorDriveDout[ 1 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 128 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 129 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 130 | break; |
inst | 0:a2bbf76ca734 | 131 | |
inst | 0:a2bbf76ca734 | 132 | case RELEASE: |
inst | 0:a2bbf76ca734 | 133 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 134 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 135 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 136 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 137 | break; |
inst | 0:a2bbf76ca734 | 138 | |
inst | 0:a2bbf76ca734 | 139 | default: |
inst | 0:a2bbf76ca734 | 140 | mAction = RELEASE; |
inst | 0:a2bbf76ca734 | 141 | mMotorDriveDout[ 0 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 142 | mMotorDriveDout[ 1 ]->write( 1 ); |
inst | 0:a2bbf76ca734 | 143 | mMotorDriveDout[ 2 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 144 | mMotorDriveDout[ 3 ]->write( 0 ); |
inst | 0:a2bbf76ca734 | 145 | break; |
inst | 0:a2bbf76ca734 | 146 | } |
inst | 0:a2bbf76ca734 | 147 | } |
inst | 0:a2bbf76ca734 | 148 | |
inst | 0:a2bbf76ca734 | 149 | void YMotorDriverBase::setPercent( float p ){ |
inst | 0:a2bbf76ca734 | 150 | p = middle( 0.0f, p, 1.0f ); |
inst | 0:a2bbf76ca734 | 151 | mDuty = p * mMaxDuty + ( 1.0f - p ) * mMinDuty; |
inst | 0:a2bbf76ca734 | 152 | } |