- Don't think anything was changed or updated.... simply viewed the class
Dependents: Component_Test_Interface
Fork of BridgeDriver by
BridgeDriver.cpp@7:7dabd934ebe4, 2014-07-24 (annotated)
- Committer:
- jason701802
- Date:
- Thu Jul 24 21:48:32 2014 +0000
- Revision:
- 7:7dabd934ebe4
- Parent:
- 6:d4afd3284a04
- Child:
- 8:36e2cd31ccf3
renamed bridge to motor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jason701802 | 0:b1e3fa917367 | 1 | |
jason701802 | 0:b1e3fa917367 | 2 | #include "BridgeDriver.h" |
jason701802 | 0:b1e3fa917367 | 3 | #include "mbed.h" |
jason701802 | 1:c8e328389a98 | 4 | #include "TextLCD.h" // if using diagnostic |
jason701802 | 0:b1e3fa917367 | 5 | |
jason701802 | 0:b1e3fa917367 | 6 | #include "MCP23017.h" |
jason701802 | 0:b1e3fa917367 | 7 | |
jason701802 | 0:b1e3fa917367 | 8 | |
jason701802 | 1:c8e328389a98 | 9 | BridgeDriver::BridgeDriver( I2C *i2c, uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD, uint8_t enAddr, uint8_t ledAddr) : |
jason701802 | 0:b1e3fa917367 | 10 | _i2c(i2c), |
jason701802 | 0:b1e3fa917367 | 11 | _enAddr(enAddr), |
jason701802 | 0:b1e3fa917367 | 12 | _ledAddr(ledAddr), |
jason701802 | 1:c8e328389a98 | 13 | _pwmCh(0x00), |
jason701802 | 0:b1e3fa917367 | 14 | _oldLedState(0x00) |
jason701802 | 0:b1e3fa917367 | 15 | { |
jason701802 | 0:b1e3fa917367 | 16 | |
jason701802 | 0:b1e3fa917367 | 17 | |
jason701802 | 0:b1e3fa917367 | 18 | _pwm[0] = enPwmA; |
jason701802 | 0:b1e3fa917367 | 19 | _pwm[1] = enPwmB; |
jason701802 | 0:b1e3fa917367 | 20 | _pwm[2] = enPwmC; |
jason701802 | 0:b1e3fa917367 | 21 | _pwm[3] = enPwmD; |
jason701802 | 0:b1e3fa917367 | 22 | |
jason701802 | 0:b1e3fa917367 | 23 | for(int i = 0; i<4; i++){ |
jason701802 | 0:b1e3fa917367 | 24 | _dir[i] = 0; |
jason701802 | 0:b1e3fa917367 | 25 | _braking[i] = 0; |
jason701802 | 0:b1e3fa917367 | 26 | } |
jason701802 | 0:b1e3fa917367 | 27 | |
jason701802 | 0:b1e3fa917367 | 28 | |
jason701802 | 0:b1e3fa917367 | 29 | _EnCtl = new MCP23017(*_i2c, _enAddr); //MCP23008 -Only use PORT_A |
jason701802 | 0:b1e3fa917367 | 30 | _IO = new MCP23017(*_i2c, _ledAddr); |
jason701802 | 0:b1e3fa917367 | 31 | |
jason701802 | 0:b1e3fa917367 | 32 | _IO->direction(PORT_B, 0x00); //LEDs are outputs |
jason701802 | 0:b1e3fa917367 | 33 | _IO->write(PORT_B, ~0x00); //bitwise not (~) b/c leds are active low |
jason701802 | 0:b1e3fa917367 | 34 | _EnCtl->configureBanked(BNK); |
jason701802 | 0:b1e3fa917367 | 35 | _EnCtl->direction(PORT_A, 0x00); //all outputs |
jason701802 | 0:b1e3fa917367 | 36 | _EnCtl->write(PORT_A, 0x00); //leave all channels disabled |
jason701802 | 0:b1e3fa917367 | 37 | |
jason701802 | 0:b1e3fa917367 | 38 | |
jason701802 | 0:b1e3fa917367 | 39 | |
jason701802 | 0:b1e3fa917367 | 40 | _d[0] = new DigitalOut(PIN_CH1); |
jason701802 | 0:b1e3fa917367 | 41 | _d[1] = new DigitalOut(PIN_CH2); |
jason701802 | 0:b1e3fa917367 | 42 | _d[2] = new DigitalOut(PIN_CH3); |
jason701802 | 0:b1e3fa917367 | 43 | _d[3] = new DigitalOut(PIN_CH4); |
jason701802 | 0:b1e3fa917367 | 44 | _d[4] = new DigitalOut(PIN_CH5); |
jason701802 | 0:b1e3fa917367 | 45 | _d[5] = new DigitalOut(PIN_CH6); |
jason701802 | 0:b1e3fa917367 | 46 | _d[6] = new DigitalOut(PIN_CH7); |
jason701802 | 0:b1e3fa917367 | 47 | _d[7] = new DigitalOut(PIN_CH8); |
jason701802 | 0:b1e3fa917367 | 48 | |
jason701802 | 0:b1e3fa917367 | 49 | |
jason701802 | 0:b1e3fa917367 | 50 | |
jason701802 | 0:b1e3fa917367 | 51 | } |
jason701802 | 6:d4afd3284a04 | 52 | |
jason701802 | 0:b1e3fa917367 | 53 | BridgeDriver::~BridgeDriver(){ |
jason701802 | 0:b1e3fa917367 | 54 | for(int i=0; i<4; i++){ |
jason701802 | 0:b1e3fa917367 | 55 | if(_pwm[i]){ |
jason701802 | 0:b1e3fa917367 | 56 | switch(_dir[i]){ |
jason701802 | 0:b1e3fa917367 | 57 | case 1: |
jason701802 | 0:b1e3fa917367 | 58 | delete _p[2*i]; |
jason701802 | 0:b1e3fa917367 | 59 | delete _d[2*i + 1]; |
jason701802 | 0:b1e3fa917367 | 60 | break; |
jason701802 | 0:b1e3fa917367 | 61 | case -1: |
jason701802 | 0:b1e3fa917367 | 62 | delete _d[2*i]; |
jason701802 | 0:b1e3fa917367 | 63 | delete _p[2*i + 1]; |
jason701802 | 0:b1e3fa917367 | 64 | break; |
jason701802 | 0:b1e3fa917367 | 65 | case 0: |
jason701802 | 0:b1e3fa917367 | 66 | delete _d[2*i]; |
jason701802 | 0:b1e3fa917367 | 67 | delete _d[2*i + 1]; |
jason701802 | 0:b1e3fa917367 | 68 | break; |
jason701802 | 0:b1e3fa917367 | 69 | } |
jason701802 | 0:b1e3fa917367 | 70 | }else{ |
jason701802 | 0:b1e3fa917367 | 71 | delete _d[2*i]; |
jason701802 | 0:b1e3fa917367 | 72 | delete _d[2*i + 1]; |
jason701802 | 0:b1e3fa917367 | 73 | } |
jason701802 | 0:b1e3fa917367 | 74 | } |
jason701802 | 0:b1e3fa917367 | 75 | } |
jason701802 | 0:b1e3fa917367 | 76 | |
jason701802 | 0:b1e3fa917367 | 77 | void BridgeDriver::enablePwm(uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD){ |
jason701802 | 7:7dabd934ebe4 | 78 | enablePwm(MOTOR_A, enPwmA); |
jason701802 | 7:7dabd934ebe4 | 79 | enablePwm(MOTOR_B, enPwmB); |
jason701802 | 7:7dabd934ebe4 | 80 | enablePwm(MOTOR_C, enPwmC); |
jason701802 | 7:7dabd934ebe4 | 81 | enablePwm(MOTOR_D, enPwmD); |
jason701802 | 0:b1e3fa917367 | 82 | } |
jason701802 | 6:d4afd3284a04 | 83 | |
jason701802 | 7:7dabd934ebe4 | 84 | void BridgeDriver::enablePwm(Motors motor, uint8_t enPwm){ |
jason701802 | 7:7dabd934ebe4 | 85 | int bNum = static_cast<int>(motor); //numeric motor |
jason701802 | 0:b1e3fa917367 | 86 | if(enPwm == _pwm[bNum]){ |
jason701802 | 0:b1e3fa917367 | 87 | return; |
jason701802 | 0:b1e3fa917367 | 88 | }else if(enPwm == 0){ //disable pwm |
jason701802 | 0:b1e3fa917367 | 89 | setPwm(2*bNum, 0); //channels are disabled in setPwm() |
jason701802 | 0:b1e3fa917367 | 90 | setPwm(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 91 | _pwm[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 92 | }else{ //enable pwm |
jason701802 | 0:b1e3fa917367 | 93 | enableCh(2*bNum, 0); //disable channels |
jason701802 | 0:b1e3fa917367 | 94 | enableCh(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 95 | _pwm[bNum] = 1; //pwm for individual channels is turned on as necessary when running motor |
jason701802 | 0:b1e3fa917367 | 96 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 97 | } |
jason701802 | 0:b1e3fa917367 | 98 | } |
jason701802 | 0:b1e3fa917367 | 99 | |
jason701802 | 0:b1e3fa917367 | 100 | void BridgeDriver::enableBraking(uint8_t enBrakeA, uint8_t enBrakeB, uint8_t enBrakeC, uint8_t enBrakeD){ //1 - drives output to GND when off; 0 - floats output when off |
jason701802 | 7:7dabd934ebe4 | 101 | enableBraking(MOTOR_A, enBrakeA); |
jason701802 | 7:7dabd934ebe4 | 102 | enableBraking(MOTOR_B, enBrakeB); |
jason701802 | 7:7dabd934ebe4 | 103 | enableBraking(MOTOR_C, enBrakeC); |
jason701802 | 7:7dabd934ebe4 | 104 | enableBraking(MOTOR_D, enBrakeD); |
jason701802 | 0:b1e3fa917367 | 105 | } |
jason701802 | 6:d4afd3284a04 | 106 | |
jason701802 | 7:7dabd934ebe4 | 107 | void BridgeDriver::enableBraking(Motors motor, uint8_t enBrake) { |
jason701802 | 7:7dabd934ebe4 | 108 | _braking[static_cast<int>(motor)] = enBrake; |
jason701802 | 0:b1e3fa917367 | 109 | } |
jason701802 | 0:b1e3fa917367 | 110 | |
jason701802 | 0:b1e3fa917367 | 111 | int BridgeDriver::forceBrake(uint8_t ch){ //force a specific channel to GND without changing braking default |
jason701802 | 0:b1e3fa917367 | 112 | if( _pwm[(ch-1)/2]){ |
jason701802 | 0:b1e3fa917367 | 113 | return -1; |
jason701802 | 0:b1e3fa917367 | 114 | }else{ |
jason701802 | 0:b1e3fa917367 | 115 | *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 116 | enableCh(ch-1, 1); |
jason701802 | 0:b1e3fa917367 | 117 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 118 | return 0; |
jason701802 | 0:b1e3fa917367 | 119 | } |
jason701802 | 0:b1e3fa917367 | 120 | } |
jason701802 | 6:d4afd3284a04 | 121 | |
jason701802 | 7:7dabd934ebe4 | 122 | int BridgeDriver::forceBrake(Motors motor){ //force a specific motor to GND without changing braking default |
jason701802 | 7:7dabd934ebe4 | 123 | return static_cast<int>(drive(motor, 0, 0)); |
jason701802 | 0:b1e3fa917367 | 124 | } |
jason701802 | 6:d4afd3284a04 | 125 | |
jason701802 | 0:b1e3fa917367 | 126 | int BridgeDriver::forceFloat(uint8_t ch){ //force a specific channel to float without changing braking default |
jason701802 | 7:7dabd934ebe4 | 127 | int bNum = (ch-1)/2; |
jason701802 | 7:7dabd934ebe4 | 128 | if( _pwm[bNum] ){ |
jason701802 | 7:7dabd934ebe4 | 129 | setPwm(2*bNum, 0); //channel is disabled in setPwm() |
jason701802 | 7:7dabd934ebe4 | 130 | setPwm(2*bNum + 1, 0); //channel is disabled in setPwm() |
jason701802 | 7:7dabd934ebe4 | 131 | _dir[bNum] = 0; |
jason701802 | 7:7dabd934ebe4 | 132 | setLed(2*bNum, 0); |
jason701802 | 7:7dabd934ebe4 | 133 | setLed(2*bNum + 1, 0); |
jason701802 | 7:7dabd934ebe4 | 134 | *_d[2*bNum] = 0; |
jason701802 | 7:7dabd934ebe4 | 135 | return *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 136 | }else{ |
jason701802 | 0:b1e3fa917367 | 137 | enableCh(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 138 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 139 | return *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 140 | } |
jason701802 | 0:b1e3fa917367 | 141 | } |
jason701802 | 6:d4afd3284a04 | 142 | |
jason701802 | 7:7dabd934ebe4 | 143 | int BridgeDriver::forceFloat(Motors motor){ //force a specific motor to float without changing braking default |
jason701802 | 7:7dabd934ebe4 | 144 | int bNum = static_cast<int>(motor); //numeric motor |
jason701802 | 0:b1e3fa917367 | 145 | |
jason701802 | 0:b1e3fa917367 | 146 | if(_pwm[bNum] == 0){ |
jason701802 | 0:b1e3fa917367 | 147 | return -1; |
jason701802 | 0:b1e3fa917367 | 148 | }else{ |
jason701802 | 4:87768972db3f | 149 | setPwm(2*bNum, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 150 | setPwm(2*bNum + 1, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 151 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 152 | setLed(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 153 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 154 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 155 | return *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 156 | } |
jason701802 | 0:b1e3fa917367 | 157 | } |
jason701802 | 0:b1e3fa917367 | 158 | |
jason701802 | 0:b1e3fa917367 | 159 | int BridgeDriver::drive(uint8_t state){ |
jason701802 | 0:b1e3fa917367 | 160 | if(_pwm[0] || _pwm[1] || _pwm[2] || _pwm[3]){ |
jason701802 | 0:b1e3fa917367 | 161 | return -1; |
jason701802 | 0:b1e3fa917367 | 162 | }else{ |
jason701802 | 0:b1e3fa917367 | 163 | setLed(state); |
jason701802 | 0:b1e3fa917367 | 164 | for(int i=0; i<8; i++){ |
jason701802 | 0:b1e3fa917367 | 165 | if(state & (1 << i)){ //channel i should be on |
jason701802 | 0:b1e3fa917367 | 166 | *_d[i] = 1; |
jason701802 | 0:b1e3fa917367 | 167 | enableCh(i, 1); |
jason701802 | 0:b1e3fa917367 | 168 | }else{ //channel i should be off |
jason701802 | 3:247939286d2c | 169 | if(_braking[i/2]){ |
jason701802 | 0:b1e3fa917367 | 170 | *_d[i] = 0; //brake output |
jason701802 | 0:b1e3fa917367 | 171 | enableCh(i, 1); |
jason701802 | 0:b1e3fa917367 | 172 | }else{ |
jason701802 | 0:b1e3fa917367 | 173 | enableCh(i, 0); |
jason701802 | 4:87768972db3f | 174 | *_d[i] = 0; //float/coast output |
jason701802 | 0:b1e3fa917367 | 175 | } |
jason701802 | 0:b1e3fa917367 | 176 | } |
jason701802 | 0:b1e3fa917367 | 177 | } |
jason701802 | 0:b1e3fa917367 | 178 | return !!state; |
jason701802 | 0:b1e3fa917367 | 179 | } |
jason701802 | 0:b1e3fa917367 | 180 | } |
jason701802 | 6:d4afd3284a04 | 181 | |
jason701802 | 0:b1e3fa917367 | 182 | int BridgeDriver::drive(uint8_t ch, uint8_t on){ |
jason701802 | 0:b1e3fa917367 | 183 | if( _pwm[(ch-1)/2]){ |
jason701802 | 0:b1e3fa917367 | 184 | return -1; |
jason701802 | 0:b1e3fa917367 | 185 | }else if(on){ //on |
jason701802 | 4:87768972db3f | 186 | *_d[ch-1] = 1; |
jason701802 | 0:b1e3fa917367 | 187 | enableCh(ch-1, 1); |
jason701802 | 0:b1e3fa917367 | 188 | setLed(ch-1, 1); |
jason701802 | 4:87768972db3f | 189 | return 1; |
jason701802 | 4:87768972db3f | 190 | }else if(_braking[(ch-1)/2]){ //off, brake |
jason701802 | 0:b1e3fa917367 | 191 | *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 192 | enableCh(ch-1, 1); |
jason701802 | 0:b1e3fa917367 | 193 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 194 | return 0; |
jason701802 | 0:b1e3fa917367 | 195 | }else{ //off, float |
jason701802 | 0:b1e3fa917367 | 196 | enableCh(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 197 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 198 | return *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 199 | } |
jason701802 | 0:b1e3fa917367 | 200 | } |
jason701802 | 6:d4afd3284a04 | 201 | |
jason701802 | 7:7dabd934ebe4 | 202 | float BridgeDriver::drive(Motors motor, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking |
jason701802 | 7:7dabd934ebe4 | 203 | int bNum = static_cast<int>(motor); //numeric motor |
jason701802 | 0:b1e3fa917367 | 204 | |
jason701802 | 0:b1e3fa917367 | 205 | if(_pwm[bNum] == 0){ |
jason701802 | 1:c8e328389a98 | 206 | return -20 - bNum; |
jason701802 | 0:b1e3fa917367 | 207 | } |
jason701802 | 0:b1e3fa917367 | 208 | |
jason701802 | 0:b1e3fa917367 | 209 | if(speed == 0){ |
jason701802 | 7:7dabd934ebe4 | 210 | return drive(motor, 1, 0); |
jason701802 | 0:b1e3fa917367 | 211 | }else if(speed > 0){ |
jason701802 | 7:7dabd934ebe4 | 212 | return drive(motor, 1, speed); |
jason701802 | 0:b1e3fa917367 | 213 | }else{ |
jason701802 | 7:7dabd934ebe4 | 214 | return drive(motor, -1, -1*speed); |
jason701802 | 0:b1e3fa917367 | 215 | } |
jason701802 | 0:b1e3fa917367 | 216 | } |
jason701802 | 6:d4afd3284a04 | 217 | |
jason701802 | 7:7dabd934ebe4 | 218 | float BridgeDriver::drive(Motors motor, int8_t dir, float speed){ |
jason701802 | 0:b1e3fa917367 | 219 | //dir: 1=fwd, -1=rev, 0=brake (regardless of setting of _braking |
jason701802 | 0:b1e3fa917367 | 220 | //speed from 0 to 1, speed of 0 will coast or brake depending on setting of _braking |
jason701802 | 0:b1e3fa917367 | 221 | |
jason701802 | 7:7dabd934ebe4 | 222 | int bNum = static_cast<uint8_t>(motor); //numeric motor |
jason701802 | 0:b1e3fa917367 | 223 | |
jason701802 | 0:b1e3fa917367 | 224 | if(_pwm[bNum] == 0){ |
jason701802 | 1:c8e328389a98 | 225 | return -10 - bNum; |
jason701802 | 0:b1e3fa917367 | 226 | } |
jason701802 | 0:b1e3fa917367 | 227 | if(speed < -1 || speed > 1){ //maybe this should be bounding instead? |
jason701802 | 0:b1e3fa917367 | 228 | return -1; |
jason701802 | 0:b1e3fa917367 | 229 | } |
jason701802 | 0:b1e3fa917367 | 230 | |
jason701802 | 0:b1e3fa917367 | 231 | if(dir == 0 || (speed == 0 && _braking[bNum])){ //brake |
jason701802 | 0:b1e3fa917367 | 232 | if(_dir[bNum] != 0){ |
jason701802 | 0:b1e3fa917367 | 233 | setPwm(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 234 | setPwm(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 235 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 236 | } |
jason701802 | 0:b1e3fa917367 | 237 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 238 | *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 239 | enableCh(2*bNum, 1); //enable channels for braking |
jason701802 | 0:b1e3fa917367 | 240 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 241 | setLed(2*bNum, 0); |
jason701802 | 4:87768972db3f | 242 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 243 | |
jason701802 | 0:b1e3fa917367 | 244 | return 0; |
jason701802 | 0:b1e3fa917367 | 245 | |
jason701802 | 0:b1e3fa917367 | 246 | }else if(speed == 0){ //coast |
jason701802 | 4:87768972db3f | 247 | setPwm(2*bNum, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 248 | setPwm(2*bNum + 1, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 249 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 250 | setLed(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 251 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 252 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 253 | return *_d[2*bNum + 1] = 0; |
jason701802 | 4:87768972db3f | 254 | }else if(dir == 1){ //forward |
jason701802 | 0:b1e3fa917367 | 255 | if(_dir[bNum] != 1){ |
jason701802 | 0:b1e3fa917367 | 256 | setPwm(2*bNum, 1); |
jason701802 | 0:b1e3fa917367 | 257 | setPwm(2*bNum + 1, 0); |
jason701802 | 1:c8e328389a98 | 258 | _dir[bNum] = 1; |
jason701802 | 0:b1e3fa917367 | 259 | } |
jason701802 | 0:b1e3fa917367 | 260 | *_p[2*bNum] = speed; |
jason701802 | 0:b1e3fa917367 | 261 | *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 262 | enableCh(2*bNum, 1); //enable channels |
jason701802 | 0:b1e3fa917367 | 263 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 264 | setLed(2*bNum, 1); |
jason701802 | 4:87768972db3f | 265 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 266 | return speed; |
jason701802 | 4:87768972db3f | 267 | }else if(dir == -1){ //reverse |
jason701802 | 1:c8e328389a98 | 268 | if(_dir[bNum] != -1){ |
jason701802 | 0:b1e3fa917367 | 269 | setPwm(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 270 | setPwm(2*bNum + 1, 1); |
jason701802 | 1:c8e328389a98 | 271 | _dir[bNum] = -1; |
jason701802 | 0:b1e3fa917367 | 272 | } |
jason701802 | 0:b1e3fa917367 | 273 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 274 | *_p[2*bNum + 1] = speed; |
jason701802 | 0:b1e3fa917367 | 275 | enableCh(2*bNum, 1); //enable channels |
jason701802 | 0:b1e3fa917367 | 276 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 277 | setLed(2*bNum, 0); |
jason701802 | 4:87768972db3f | 278 | setLed(2*bNum + 1, 1); |
jason701802 | 0:b1e3fa917367 | 279 | return speed; |
jason701802 | 0:b1e3fa917367 | 280 | } |
jason701802 | 1:c8e328389a98 | 281 | return -2; |
jason701802 | 0:b1e3fa917367 | 282 | } |
jason701802 | 0:b1e3fa917367 | 283 | |
jason701802 | 0:b1e3fa917367 | 284 | void BridgeDriver::enableCh(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 285 | static uint8_t state = 0x00; |
jason701802 | 0:b1e3fa917367 | 286 | static uint8_t oldState = 0x00; |
jason701802 | 0:b1e3fa917367 | 287 | |
jason701802 | 0:b1e3fa917367 | 288 | if(en){ |
jason701802 | 0:b1e3fa917367 | 289 | state = state | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 290 | }else{ |
jason701802 | 0:b1e3fa917367 | 291 | state = state & ~(1 << ch); |
jason701802 | 0:b1e3fa917367 | 292 | } |
jason701802 | 0:b1e3fa917367 | 293 | |
jason701802 | 0:b1e3fa917367 | 294 | if(state != oldState){ |
jason701802 | 0:b1e3fa917367 | 295 | _EnCtl->write(PORT_A, state); |
jason701802 | 0:b1e3fa917367 | 296 | oldState = state; |
jason701802 | 0:b1e3fa917367 | 297 | } |
jason701802 | 0:b1e3fa917367 | 298 | } |
jason701802 | 0:b1e3fa917367 | 299 | |
jason701802 | 0:b1e3fa917367 | 300 | void BridgeDriver::setLed(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 301 | uint8_t state; |
jason701802 | 0:b1e3fa917367 | 302 | if(en){ |
jason701802 | 0:b1e3fa917367 | 303 | state = _oldLedState | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 304 | }else{ |
jason701802 | 0:b1e3fa917367 | 305 | state = _oldLedState & ~(1 << ch); |
jason701802 | 0:b1e3fa917367 | 306 | } |
jason701802 | 0:b1e3fa917367 | 307 | |
jason701802 | 0:b1e3fa917367 | 308 | if(state != _oldLedState){ |
jason701802 | 0:b1e3fa917367 | 309 | _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low |
jason701802 | 0:b1e3fa917367 | 310 | _oldLedState = state; |
jason701802 | 0:b1e3fa917367 | 311 | } |
jason701802 | 0:b1e3fa917367 | 312 | } |
jason701802 | 0:b1e3fa917367 | 313 | |
jason701802 | 0:b1e3fa917367 | 314 | void BridgeDriver::setLed(uint8_t state){ |
jason701802 | 0:b1e3fa917367 | 315 | if(state != _oldLedState){ |
jason701802 | 0:b1e3fa917367 | 316 | _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low |
jason701802 | 0:b1e3fa917367 | 317 | _oldLedState = state; |
jason701802 | 0:b1e3fa917367 | 318 | } |
jason701802 | 0:b1e3fa917367 | 319 | } |
jason701802 | 0:b1e3fa917367 | 320 | |
jason701802 | 0:b1e3fa917367 | 321 | void BridgeDriver::setPwm(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 322 | enableCh(ch, 0); |
jason701802 | 0:b1e3fa917367 | 323 | |
jason701802 | 1:c8e328389a98 | 324 | |
jason701802 | 1:c8e328389a98 | 325 | if(_pwmCh & (1 << ch)){ //pwm is currently enabled on this channel |
jason701802 | 0:b1e3fa917367 | 326 | if(en == 0){ |
jason701802 | 0:b1e3fa917367 | 327 | delete _p[ch]; |
jason701802 | 0:b1e3fa917367 | 328 | switch(ch){ |
jason701802 | 1:c8e328389a98 | 329 | case 0: |
jason701802 | 1:c8e328389a98 | 330 | _d[ch] = new DigitalOut(PIN_CH1); |
jason701802 | 1:c8e328389a98 | 331 | break; |
jason701802 | 1:c8e328389a98 | 332 | case 1: |
jason701802 | 1:c8e328389a98 | 333 | _d[ch] = new DigitalOut(PIN_CH2); |
jason701802 | 1:c8e328389a98 | 334 | break; |
jason701802 | 1:c8e328389a98 | 335 | case 2: |
jason701802 | 1:c8e328389a98 | 336 | _d[ch] = new DigitalOut(PIN_CH3); |
jason701802 | 1:c8e328389a98 | 337 | break; |
jason701802 | 1:c8e328389a98 | 338 | case 3: |
jason701802 | 1:c8e328389a98 | 339 | _d[ch] = new DigitalOut(PIN_CH4); |
jason701802 | 1:c8e328389a98 | 340 | break; |
jason701802 | 1:c8e328389a98 | 341 | case 4: |
jason701802 | 1:c8e328389a98 | 342 | _d[ch] = new DigitalOut(PIN_CH5); |
jason701802 | 1:c8e328389a98 | 343 | break; |
jason701802 | 1:c8e328389a98 | 344 | case 5: |
jason701802 | 1:c8e328389a98 | 345 | _d[ch] = new DigitalOut(PIN_CH6); |
jason701802 | 1:c8e328389a98 | 346 | break; |
jason701802 | 1:c8e328389a98 | 347 | case 6: |
jason701802 | 1:c8e328389a98 | 348 | _d[ch] = new DigitalOut(PIN_CH7); |
jason701802 | 1:c8e328389a98 | 349 | break; |
jason701802 | 1:c8e328389a98 | 350 | case 7: |
jason701802 | 1:c8e328389a98 | 351 | _d[ch] = new DigitalOut(PIN_CH8); |
jason701802 | 1:c8e328389a98 | 352 | break; |
jason701802 | 0:b1e3fa917367 | 353 | } |
jason701802 | 1:c8e328389a98 | 354 | _pwmCh = _pwmCh & ~(1 << ch); |
jason701802 | 1:c8e328389a98 | 355 | |
jason701802 | 0:b1e3fa917367 | 356 | } |
jason701802 | 0:b1e3fa917367 | 357 | }else{ //pwm is currently disabled on this channel |
jason701802 | 0:b1e3fa917367 | 358 | if(en == 1){ |
jason701802 | 0:b1e3fa917367 | 359 | delete _d[ch]; |
jason701802 | 0:b1e3fa917367 | 360 | switch(ch){ |
jason701802 | 1:c8e328389a98 | 361 | case 0: |
jason701802 | 1:c8e328389a98 | 362 | _p[ch] = new PwmOut(PIN_CH1); |
jason701802 | 1:c8e328389a98 | 363 | break; |
jason701802 | 1:c8e328389a98 | 364 | case 1: |
jason701802 | 1:c8e328389a98 | 365 | _p[ch] = new PwmOut(PIN_CH2); |
jason701802 | 1:c8e328389a98 | 366 | break; |
jason701802 | 1:c8e328389a98 | 367 | case 2: |
jason701802 | 1:c8e328389a98 | 368 | _p[ch] = new PwmOut(PIN_CH3); |
jason701802 | 1:c8e328389a98 | 369 | break; |
jason701802 | 1:c8e328389a98 | 370 | case 3: |
jason701802 | 1:c8e328389a98 | 371 | _p[ch] = new PwmOut(PIN_CH4); |
jason701802 | 1:c8e328389a98 | 372 | break; |
jason701802 | 1:c8e328389a98 | 373 | case 4: |
jason701802 | 1:c8e328389a98 | 374 | _p[ch] = new PwmOut(PIN_CH5); |
jason701802 | 1:c8e328389a98 | 375 | break; |
jason701802 | 1:c8e328389a98 | 376 | case 5: |
jason701802 | 1:c8e328389a98 | 377 | _p[ch] = new PwmOut(PIN_CH6); |
jason701802 | 1:c8e328389a98 | 378 | break; |
jason701802 | 1:c8e328389a98 | 379 | case 6: |
jason701802 | 1:c8e328389a98 | 380 | _p[ch] = new PwmOut(PIN_CH7); |
jason701802 | 1:c8e328389a98 | 381 | break; |
jason701802 | 1:c8e328389a98 | 382 | case 7: |
jason701802 | 1:c8e328389a98 | 383 | _p[ch] = new PwmOut(PIN_CH8); |
jason701802 | 1:c8e328389a98 | 384 | break; |
jason701802 | 0:b1e3fa917367 | 385 | } |
jason701802 | 4:87768972db3f | 386 | _p[ch]->period(PWM_PERIOD); |
jason701802 | 1:c8e328389a98 | 387 | _pwmCh = _pwmCh | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 388 | } |
jason701802 | 0:b1e3fa917367 | 389 | } |
jason701802 | 0:b1e3fa917367 | 390 | |
jason701802 | 0:b1e3fa917367 | 391 | |
jason701802 | 0:b1e3fa917367 | 392 | } |
jason701802 | 0:b1e3fa917367 | 393 | |
jason701802 | 1:c8e328389a98 | 394 | void BridgeDriver::diagnostic(TextLCD_I2C *lcd){ |
jason701802 | 1:c8e328389a98 | 395 | lcd->setAddress(0,1); |
jason701802 | 1:c8e328389a98 | 396 | lcd->printf("PWM:%d,%d,%d,%d ", _pwm[0], _pwm[1], _pwm[2], _pwm[3]); |
jason701802 | 1:c8e328389a98 | 397 | lcd->setAddress(0,2); |
jason701802 | 1:c8e328389a98 | 398 | lcd->printf("pCH:%i%i%i%i%i%i%i%i", !!(_pwmCh & (1 << 0)), |
jason701802 | 1:c8e328389a98 | 399 | !!(_pwmCh & (1 << 1)), |
jason701802 | 1:c8e328389a98 | 400 | !!(_pwmCh & (1 << 2)), |
jason701802 | 1:c8e328389a98 | 401 | !!(_pwmCh & (1 << 3)), |
jason701802 | 1:c8e328389a98 | 402 | !!(_pwmCh & (1 << 4)), |
jason701802 | 1:c8e328389a98 | 403 | !!(_pwmCh & (1 << 5)), |
jason701802 | 1:c8e328389a98 | 404 | !!(_pwmCh & (1 << 6)), |
jason701802 | 1:c8e328389a98 | 405 | !!(_pwmCh & (1 << 7)) |
jason701802 | 1:c8e328389a98 | 406 | ); |
jason701802 | 1:c8e328389a98 | 407 | lcd->setAddress(0,3); |
jason701802 | 1:c8e328389a98 | 408 | lcd->printf("dir:%+d,%+d,%+d,%+d ", _dir[0], _dir[1], _dir[2], _dir[3]); |
jason701802 | 1:c8e328389a98 | 409 | } |