Li Weiyi
/
Cube
Microduino的cube小车。
Microduino_Motor.cpp@4:0670023d3f36, 2016-05-28 (annotated)
- Committer:
- lixianyu
- Date:
- Sat May 28 05:09:18 2016 +0000
- Revision:
- 4:0670023d3f36
- Parent:
- 3:e4ac7c1a14de
??work????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lixianyu | 1:758ccab13947 | 1 | #include "Microduino_Motor.h" |
lixianyu | 1:758ccab13947 | 2 | |
lixianyu | 1:758ccab13947 | 3 | static motor_t motors[10]; // static array of key structures |
lixianyu | 1:758ccab13947 | 4 | |
lixianyu | 1:758ccab13947 | 5 | uint8_t MotorCount = 0; // the total number of attached keys |
lixianyu | 1:758ccab13947 | 6 | |
lixianyu | 2:70ca3e685cca | 7 | Motor::Motor(PinName _motor_pinA, PinName _motor_pinB) |
lixianyu | 1:758ccab13947 | 8 | { |
lixianyu | 1:758ccab13947 | 9 | if ( MotorCount < 10) { |
lixianyu | 1:758ccab13947 | 10 | this->motorIndex = MotorCount++; // assign a key index to this instance |
lixianyu | 2:70ca3e685cca | 11 | //if (_motor_pinA < NUM_DIGITAL_PINS && _motor_pinB < NUM_DIGITAL_PINS) { |
lixianyu | 2:70ca3e685cca | 12 | if (true) { |
lixianyu | 3:e4ac7c1a14de | 13 | #if 0 |
lixianyu | 3:e4ac7c1a14de | 14 | pinMode( _motor_pinA, OUTPUT) ; |
lixianyu | 3:e4ac7c1a14de | 15 | #else |
lixianyu | 4:0670023d3f36 | 16 | _period_us = 255; // 500Hz |
lixianyu | 3:e4ac7c1a14de | 17 | pwmout_init(&_pwmA, _motor_pinA); |
lixianyu | 3:e4ac7c1a14de | 18 | pwmout_period_us(&_pwmA, _period_us); // 500Hz |
lixianyu | 4:0670023d3f36 | 19 | pwmout_pulsewidth_us(&_pwmA, 1); |
lixianyu | 3:e4ac7c1a14de | 20 | #endif |
lixianyu | 1:758ccab13947 | 21 | motors[this->motorIndex].Pin.nbr_A = _motor_pinA; |
lixianyu | 1:758ccab13947 | 22 | |
lixianyu | 3:e4ac7c1a14de | 23 | #if 0 |
lixianyu | 3:e4ac7c1a14de | 24 | pinMode( _motor_pinB, OUTPUT) ; |
lixianyu | 3:e4ac7c1a14de | 25 | #else |
lixianyu | 3:e4ac7c1a14de | 26 | pwmout_init(&_pwmB, _motor_pinB); |
lixianyu | 3:e4ac7c1a14de | 27 | pwmout_period_us(&_pwmB, _period_us); // 500Hz |
lixianyu | 4:0670023d3f36 | 28 | pwmout_pulsewidth_us(&_pwmB, 1); |
lixianyu | 3:e4ac7c1a14de | 29 | #endif |
lixianyu | 1:758ccab13947 | 30 | motors[this->motorIndex].Pin.nbr_B = _motor_pinB; |
lixianyu | 1:758ccab13947 | 31 | |
lixianyu | 1:758ccab13947 | 32 | this->fix=1; |
lixianyu | 1:758ccab13947 | 33 | } |
lixianyu | 3:e4ac7c1a14de | 34 | } else { |
lixianyu | 1:758ccab13947 | 35 | this->motorIndex = 255 ; // too many keys |
lixianyu | 3:e4ac7c1a14de | 36 | } |
lixianyu | 1:758ccab13947 | 37 | } |
lixianyu | 1:758ccab13947 | 38 | |
lixianyu | 1:758ccab13947 | 39 | void Motor::Fix(float _fix) |
lixianyu | 1:758ccab13947 | 40 | { |
lixianyu | 1:758ccab13947 | 41 | this->fix=_fix; |
lixianyu | 1:758ccab13947 | 42 | } |
lixianyu | 1:758ccab13947 | 43 | |
lixianyu | 4:0670023d3f36 | 44 | int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir) |
lixianyu | 1:758ccab13947 | 45 | { |
lixianyu | 1:758ccab13947 | 46 | this->_motor_vol = _throttle; |
lixianyu | 1:758ccab13947 | 47 | |
lixianyu | 4:0670023d3f36 | 48 | if(_dir == 1) |
lixianyu | 4:0670023d3f36 | 49 | { |
lixianyu | 1:758ccab13947 | 50 | this->_motor_vol -= _steering / 2; |
lixianyu | 4:0670023d3f36 | 51 | } |
lixianyu | 1:758ccab13947 | 52 | else |
lixianyu | 4:0670023d3f36 | 53 | { |
lixianyu | 1:758ccab13947 | 54 | this->_motor_vol += _steering / 2; |
lixianyu | 4:0670023d3f36 | 55 | } |
lixianyu | 1:758ccab13947 | 56 | if (this->_motor_vol > 255) |
lixianyu | 1:758ccab13947 | 57 | this->_motor_vol = 255; |
lixianyu | 1:758ccab13947 | 58 | else if (this->_motor_vol < -255) |
lixianyu | 1:758ccab13947 | 59 | this->_motor_vol = -255; |
lixianyu | 1:758ccab13947 | 60 | |
lixianyu | 4:0670023d3f36 | 61 | //this->_motor_vol *= fix; |
lixianyu | 1:758ccab13947 | 62 | |
lixianyu | 1:758ccab13947 | 63 | return this->_motor_vol; |
lixianyu | 1:758ccab13947 | 64 | } |
lixianyu | 1:758ccab13947 | 65 | |
lixianyu | 3:e4ac7c1a14de | 66 | #if 0 |
lixianyu | 1:758ccab13947 | 67 | void Motor::Driver(int16_t _motor_driver) |
lixianyu | 1:758ccab13947 | 68 | { |
lixianyu | 1:758ccab13947 | 69 | int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; |
lixianyu | 1:758ccab13947 | 70 | int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; |
lixianyu | 1:758ccab13947 | 71 | if (_motor_driver == 0) { |
lixianyu | 1:758ccab13947 | 72 | digitalWrite(channel_A, LOW); |
lixianyu | 1:758ccab13947 | 73 | digitalWrite(channel_B, LOW); |
lixianyu | 1:758ccab13947 | 74 | } else if (_motor_driver > 0) { |
lixianyu | 1:758ccab13947 | 75 | analogWrite(channel_A, _motor_driver); |
lixianyu | 1:758ccab13947 | 76 | digitalWrite(channel_B, LOW); |
lixianyu | 1:758ccab13947 | 77 | } else { |
lixianyu | 1:758ccab13947 | 78 | analogWrite(channel_A, 255 + _motor_driver); |
lixianyu | 1:758ccab13947 | 79 | digitalWrite(channel_B, HIGH); |
lixianyu | 1:758ccab13947 | 80 | } |
lixianyu | 1:758ccab13947 | 81 | } |
lixianyu | 3:e4ac7c1a14de | 82 | #else |
lixianyu | 3:e4ac7c1a14de | 83 | void Motor::Driver(int16_t _motor_driver) |
lixianyu | 3:e4ac7c1a14de | 84 | { |
lixianyu | 4:0670023d3f36 | 85 | //static bool flag = true; |
lixianyu | 4:0670023d3f36 | 86 | uint32_t pulseWidth = 0; |
lixianyu | 4:0670023d3f36 | 87 | //PinName channel_A = motors[this->motorIndex].Pin.nbr_A; |
lixianyu | 4:0670023d3f36 | 88 | //PinName channel_B = motors[this->motorIndex].Pin.nbr_B; |
lixianyu | 4:0670023d3f36 | 89 | #if 0 |
lixianyu | 4:0670023d3f36 | 90 | pwmout_pulsewidth_us(&_pwmA, _period_us/2); |
lixianyu | 4:0670023d3f36 | 91 | pwmout_pulsewidth_us(&_pwmB, 0); |
lixianyu | 4:0670023d3f36 | 92 | return; |
lixianyu | 4:0670023d3f36 | 93 | #endif |
lixianyu | 4:0670023d3f36 | 94 | #if 0 |
lixianyu | 4:0670023d3f36 | 95 | pwmout_pulsewidth_us(&_pwmA, 0); |
lixianyu | 4:0670023d3f36 | 96 | pwmout_pulsewidth_us(&_pwmB, _period_us/2); |
lixianyu | 4:0670023d3f36 | 97 | return; |
lixianyu | 4:0670023d3f36 | 98 | #endif |
lixianyu | 3:e4ac7c1a14de | 99 | if (_motor_driver == 0) { |
lixianyu | 3:e4ac7c1a14de | 100 | pwmout_pulsewidth_us(&_pwmA, 0); |
lixianyu | 3:e4ac7c1a14de | 101 | pwmout_pulsewidth_us(&_pwmB, 0); |
lixianyu | 3:e4ac7c1a14de | 102 | } else if (_motor_driver > 0) { |
lixianyu | 4:0670023d3f36 | 103 | #if 1 |
lixianyu | 4:0670023d3f36 | 104 | pulseWidth = _period_us / 255 * _motor_driver; |
lixianyu | 3:e4ac7c1a14de | 105 | pwmout_pulsewidth_us(&_pwmA, pulseWidth); |
lixianyu | 4:0670023d3f36 | 106 | pwmout_pulsewidth_us(&_pwmB, 2); |
lixianyu | 4:0670023d3f36 | 107 | #else |
lixianyu | 4:0670023d3f36 | 108 | pwmout_pulsewidth_us(&_pwmA, _period_us/2); |
lixianyu | 3:e4ac7c1a14de | 109 | pwmout_pulsewidth_us(&_pwmB, 0); |
lixianyu | 4:0670023d3f36 | 110 | #endif |
lixianyu | 3:e4ac7c1a14de | 111 | } else { |
lixianyu | 4:0670023d3f36 | 112 | #if 0 |
lixianyu | 3:e4ac7c1a14de | 113 | _motor_driver = 255 + _motor_driver; |
lixianyu | 4:0670023d3f36 | 114 | pulseWidth = _period_us / 255 * _motor_driver; |
lixianyu | 4:0670023d3f36 | 115 | pwmout_pulsewidth_us(&_pwmA, 0); |
lixianyu | 4:0670023d3f36 | 116 | pwmout_pulsewidth_us(&_pwmB, pulseWidth); |
lixianyu | 4:0670023d3f36 | 117 | #elif 1 |
lixianyu | 4:0670023d3f36 | 118 | _motor_driver = abs(_motor_driver); |
lixianyu | 4:0670023d3f36 | 119 | pulseWidth = _period_us / 255 * _motor_driver; |
lixianyu | 4:0670023d3f36 | 120 | pwmout_pulsewidth_us(&_pwmA, 2); |
lixianyu | 4:0670023d3f36 | 121 | pwmout_pulsewidth_us(&_pwmB, pulseWidth); |
lixianyu | 4:0670023d3f36 | 122 | #elif 0 |
lixianyu | 4:0670023d3f36 | 123 | _motor_driver = 255 + _motor_driver; |
lixianyu | 4:0670023d3f36 | 124 | pulseWidth = _period_us / 255 * _motor_driver; |
lixianyu | 4:0670023d3f36 | 125 | pwmout_pulsewidth_us(&_pwmA, _period_us); |
lixianyu | 4:0670023d3f36 | 126 | pwmout_pulsewidth_us(&_pwmB, pulseWidth); |
lixianyu | 4:0670023d3f36 | 127 | #else |
lixianyu | 4:0670023d3f36 | 128 | pwmout_pulsewidth_us(&_pwmA, 0); |
lixianyu | 4:0670023d3f36 | 129 | pwmout_pulsewidth_us(&_pwmB, 241); |
lixianyu | 4:0670023d3f36 | 130 | #endif |
lixianyu | 3:e4ac7c1a14de | 131 | } |
lixianyu | 3:e4ac7c1a14de | 132 | } |
lixianyu | 3:e4ac7c1a14de | 133 | #endif |
lixianyu | 1:758ccab13947 | 134 | |
lixianyu | 1:758ccab13947 | 135 | void Motor::Free() |
lixianyu | 1:758ccab13947 | 136 | { |
lixianyu | 1:758ccab13947 | 137 | int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; |
lixianyu | 1:758ccab13947 | 138 | int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; |
lixianyu | 3:e4ac7c1a14de | 139 | #if 0 |
lixianyu | 1:758ccab13947 | 140 | digitalWrite(channel_A, LOW); |
lixianyu | 1:758ccab13947 | 141 | digitalWrite(channel_B, LOW); |
lixianyu | 3:e4ac7c1a14de | 142 | #else |
lixianyu | 3:e4ac7c1a14de | 143 | pwmout_pulsewidth_us(&_pwmA, 0); |
lixianyu | 3:e4ac7c1a14de | 144 | pwmout_pulsewidth_us(&_pwmB, 0); |
lixianyu | 3:e4ac7c1a14de | 145 | #endif |
lixianyu | 1:758ccab13947 | 146 | } |
lixianyu | 1:758ccab13947 | 147 | |
lixianyu | 1:758ccab13947 | 148 | void Motor::Brake() |
lixianyu | 1:758ccab13947 | 149 | { |
lixianyu | 1:758ccab13947 | 150 | int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; |
lixianyu | 1:758ccab13947 | 151 | int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; |
lixianyu | 3:e4ac7c1a14de | 152 | #if 0 |
lixianyu | 1:758ccab13947 | 153 | digitalWrite(channel_A, HIGH); |
lixianyu | 1:758ccab13947 | 154 | digitalWrite(channel_B, HIGH); |
lixianyu | 3:e4ac7c1a14de | 155 | #else |
lixianyu | 3:e4ac7c1a14de | 156 | pwmout_pulsewidth_us(&_pwmA, _period_us); |
lixianyu | 3:e4ac7c1a14de | 157 | pwmout_pulsewidth_us(&_pwmB, _period_us); |
lixianyu | 3:e4ac7c1a14de | 158 | #endif |
lixianyu | 1:758ccab13947 | 159 | } |