- Don't think anything was changed or updated.... simply viewed the class
Dependents: Component_Test_Interface
Fork of BridgeDriver by
BridgeDriver.cpp@6:d4afd3284a04, 2014-07-24 (annotated)
- Committer:
- jason701802
- Date:
- Thu Jul 24 21:30:41 2014 +0000
- Revision:
- 6:d4afd3284a04
- Parent:
- 4:87768972db3f
- Child:
- 7:7dabd934ebe4
Formatting;
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 | 0:b1e3fa917367 | 78 | enablePwm(BRIDGE_A, enPwmA); |
jason701802 | 0:b1e3fa917367 | 79 | enablePwm(BRIDGE_B, enPwmB); |
jason701802 | 0:b1e3fa917367 | 80 | enablePwm(BRIDGE_C, enPwmC); |
jason701802 | 0:b1e3fa917367 | 81 | enablePwm(BRIDGE_D, enPwmD); |
jason701802 | 0:b1e3fa917367 | 82 | } |
jason701802 | 6:d4afd3284a04 | 83 | |
jason701802 | 0:b1e3fa917367 | 84 | void BridgeDriver::enablePwm(Bridges bridge, uint8_t enPwm){ |
jason701802 | 0:b1e3fa917367 | 85 | int bNum = static_cast<int>(bridge); //numeric bridge |
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 | 0:b1e3fa917367 | 101 | enableBraking(BRIDGE_A, enBrakeA); |
jason701802 | 0:b1e3fa917367 | 102 | enableBraking(BRIDGE_B, enBrakeB); |
jason701802 | 0:b1e3fa917367 | 103 | enableBraking(BRIDGE_C, enBrakeC); |
jason701802 | 0:b1e3fa917367 | 104 | enableBraking(BRIDGE_D, enBrakeD); |
jason701802 | 0:b1e3fa917367 | 105 | } |
jason701802 | 6:d4afd3284a04 | 106 | |
jason701802 | 0:b1e3fa917367 | 107 | void BridgeDriver::enableBraking(Bridges bridge, uint8_t enBrake) { |
jason701802 | 0:b1e3fa917367 | 108 | _braking[static_cast<int>(bridge)] = 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 | 0:b1e3fa917367 | 122 | int BridgeDriver::forceBrake(Bridges bridge){ //force a specific motor to GND without changing braking default |
jason701802 | 0:b1e3fa917367 | 123 | return static_cast<int>(drive(bridge, 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 | 0:b1e3fa917367 | 127 | if( _pwm[(ch-1)/2]){ |
jason701802 | 0:b1e3fa917367 | 128 | return -1; |
jason701802 | 0:b1e3fa917367 | 129 | }else{ |
jason701802 | 0:b1e3fa917367 | 130 | enableCh(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 131 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 132 | return *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 133 | } |
jason701802 | 0:b1e3fa917367 | 134 | } |
jason701802 | 6:d4afd3284a04 | 135 | |
jason701802 | 0:b1e3fa917367 | 136 | int BridgeDriver::forceFloat(Bridges bridge){ //force a specific motor to float without changing braking default |
jason701802 | 0:b1e3fa917367 | 137 | int bNum = static_cast<int>(bridge); //numeric bridge |
jason701802 | 0:b1e3fa917367 | 138 | |
jason701802 | 0:b1e3fa917367 | 139 | if(_pwm[bNum] == 0){ |
jason701802 | 0:b1e3fa917367 | 140 | return -1; |
jason701802 | 0:b1e3fa917367 | 141 | }else{ |
jason701802 | 4:87768972db3f | 142 | setPwm(2*bNum, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 143 | setPwm(2*bNum + 1, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 144 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 145 | setLed(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 146 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 147 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 148 | return *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 149 | } |
jason701802 | 0:b1e3fa917367 | 150 | } |
jason701802 | 0:b1e3fa917367 | 151 | |
jason701802 | 0:b1e3fa917367 | 152 | int BridgeDriver::drive(uint8_t state){ |
jason701802 | 0:b1e3fa917367 | 153 | if(_pwm[0] || _pwm[1] || _pwm[2] || _pwm[3]){ |
jason701802 | 0:b1e3fa917367 | 154 | return -1; |
jason701802 | 0:b1e3fa917367 | 155 | }else{ |
jason701802 | 0:b1e3fa917367 | 156 | setLed(state); |
jason701802 | 0:b1e3fa917367 | 157 | for(int i=0; i<8; i++){ |
jason701802 | 0:b1e3fa917367 | 158 | if(state & (1 << i)){ //channel i should be on |
jason701802 | 0:b1e3fa917367 | 159 | *_d[i] = 1; |
jason701802 | 0:b1e3fa917367 | 160 | enableCh(i, 1); |
jason701802 | 0:b1e3fa917367 | 161 | }else{ //channel i should be off |
jason701802 | 3:247939286d2c | 162 | if(_braking[i/2]){ |
jason701802 | 0:b1e3fa917367 | 163 | *_d[i] = 0; //brake output |
jason701802 | 0:b1e3fa917367 | 164 | enableCh(i, 1); |
jason701802 | 0:b1e3fa917367 | 165 | }else{ |
jason701802 | 0:b1e3fa917367 | 166 | enableCh(i, 0); |
jason701802 | 4:87768972db3f | 167 | *_d[i] = 0; //float/coast output |
jason701802 | 0:b1e3fa917367 | 168 | } |
jason701802 | 0:b1e3fa917367 | 169 | } |
jason701802 | 0:b1e3fa917367 | 170 | } |
jason701802 | 0:b1e3fa917367 | 171 | return !!state; |
jason701802 | 0:b1e3fa917367 | 172 | } |
jason701802 | 0:b1e3fa917367 | 173 | } |
jason701802 | 6:d4afd3284a04 | 174 | |
jason701802 | 0:b1e3fa917367 | 175 | int BridgeDriver::drive(uint8_t ch, uint8_t on){ |
jason701802 | 0:b1e3fa917367 | 176 | if( _pwm[(ch-1)/2]){ |
jason701802 | 0:b1e3fa917367 | 177 | return -1; |
jason701802 | 0:b1e3fa917367 | 178 | }else if(on){ //on |
jason701802 | 4:87768972db3f | 179 | *_d[ch-1] = 1; |
jason701802 | 0:b1e3fa917367 | 180 | enableCh(ch-1, 1); |
jason701802 | 0:b1e3fa917367 | 181 | setLed(ch-1, 1); |
jason701802 | 4:87768972db3f | 182 | return 1; |
jason701802 | 4:87768972db3f | 183 | }else if(_braking[(ch-1)/2]){ //off, brake |
jason701802 | 0:b1e3fa917367 | 184 | *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 185 | enableCh(ch-1, 1); |
jason701802 | 0:b1e3fa917367 | 186 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 187 | return 0; |
jason701802 | 0:b1e3fa917367 | 188 | }else{ //off, float |
jason701802 | 0:b1e3fa917367 | 189 | enableCh(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 190 | setLed(ch-1, 0); |
jason701802 | 0:b1e3fa917367 | 191 | return *_d[ch-1] = 0; |
jason701802 | 0:b1e3fa917367 | 192 | } |
jason701802 | 0:b1e3fa917367 | 193 | } |
jason701802 | 6:d4afd3284a04 | 194 | |
jason701802 | 0:b1e3fa917367 | 195 | float BridgeDriver::drive(Bridges bridge, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking |
jason701802 | 0:b1e3fa917367 | 196 | int bNum = static_cast<int>(bridge); //numeric bridge |
jason701802 | 0:b1e3fa917367 | 197 | |
jason701802 | 0:b1e3fa917367 | 198 | if(_pwm[bNum] == 0){ |
jason701802 | 1:c8e328389a98 | 199 | return -20 - bNum; |
jason701802 | 0:b1e3fa917367 | 200 | } |
jason701802 | 0:b1e3fa917367 | 201 | |
jason701802 | 0:b1e3fa917367 | 202 | if(speed == 0){ |
jason701802 | 0:b1e3fa917367 | 203 | return drive(bridge, 1, 0); |
jason701802 | 0:b1e3fa917367 | 204 | }else if(speed > 0){ |
jason701802 | 0:b1e3fa917367 | 205 | return drive(bridge, 1, speed); |
jason701802 | 0:b1e3fa917367 | 206 | }else{ |
jason701802 | 0:b1e3fa917367 | 207 | return drive(bridge, -1, -1*speed); |
jason701802 | 0:b1e3fa917367 | 208 | } |
jason701802 | 0:b1e3fa917367 | 209 | } |
jason701802 | 6:d4afd3284a04 | 210 | |
jason701802 | 0:b1e3fa917367 | 211 | float BridgeDriver::drive(Bridges bridge, int8_t dir, float speed){ |
jason701802 | 0:b1e3fa917367 | 212 | //dir: 1=fwd, -1=rev, 0=brake (regardless of setting of _braking |
jason701802 | 0:b1e3fa917367 | 213 | //speed from 0 to 1, speed of 0 will coast or brake depending on setting of _braking |
jason701802 | 0:b1e3fa917367 | 214 | |
jason701802 | 0:b1e3fa917367 | 215 | int bNum = static_cast<uint8_t>(bridge); //numeric bridge |
jason701802 | 0:b1e3fa917367 | 216 | |
jason701802 | 0:b1e3fa917367 | 217 | if(_pwm[bNum] == 0){ |
jason701802 | 1:c8e328389a98 | 218 | return -10 - bNum; |
jason701802 | 0:b1e3fa917367 | 219 | } |
jason701802 | 0:b1e3fa917367 | 220 | if(speed < -1 || speed > 1){ //maybe this should be bounding instead? |
jason701802 | 0:b1e3fa917367 | 221 | return -1; |
jason701802 | 0:b1e3fa917367 | 222 | } |
jason701802 | 0:b1e3fa917367 | 223 | |
jason701802 | 0:b1e3fa917367 | 224 | if(dir == 0 || (speed == 0 && _braking[bNum])){ //brake |
jason701802 | 0:b1e3fa917367 | 225 | if(_dir[bNum] != 0){ |
jason701802 | 0:b1e3fa917367 | 226 | setPwm(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 227 | setPwm(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 228 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 229 | } |
jason701802 | 0:b1e3fa917367 | 230 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 231 | *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 232 | enableCh(2*bNum, 1); //enable channels for braking |
jason701802 | 0:b1e3fa917367 | 233 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 234 | setLed(2*bNum, 0); |
jason701802 | 4:87768972db3f | 235 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 236 | |
jason701802 | 0:b1e3fa917367 | 237 | return 0; |
jason701802 | 0:b1e3fa917367 | 238 | |
jason701802 | 0:b1e3fa917367 | 239 | }else if(speed == 0){ //coast |
jason701802 | 4:87768972db3f | 240 | setPwm(2*bNum, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 241 | setPwm(2*bNum + 1, 0); //channel is disabled in setPwm() |
jason701802 | 4:87768972db3f | 242 | _dir[bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 243 | setLed(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 244 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 245 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 246 | return *_d[2*bNum + 1] = 0; |
jason701802 | 4:87768972db3f | 247 | }else if(dir == 1){ //forward |
jason701802 | 0:b1e3fa917367 | 248 | if(_dir[bNum] != 1){ |
jason701802 | 0:b1e3fa917367 | 249 | setPwm(2*bNum, 1); |
jason701802 | 0:b1e3fa917367 | 250 | setPwm(2*bNum + 1, 0); |
jason701802 | 1:c8e328389a98 | 251 | _dir[bNum] = 1; |
jason701802 | 0:b1e3fa917367 | 252 | } |
jason701802 | 0:b1e3fa917367 | 253 | *_p[2*bNum] = speed; |
jason701802 | 0:b1e3fa917367 | 254 | *_d[2*bNum + 1] = 0; |
jason701802 | 0:b1e3fa917367 | 255 | enableCh(2*bNum, 1); //enable channels |
jason701802 | 0:b1e3fa917367 | 256 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 257 | setLed(2*bNum, 1); |
jason701802 | 4:87768972db3f | 258 | setLed(2*bNum + 1, 0); |
jason701802 | 0:b1e3fa917367 | 259 | return speed; |
jason701802 | 4:87768972db3f | 260 | }else if(dir == -1){ //reverse |
jason701802 | 1:c8e328389a98 | 261 | if(_dir[bNum] != -1){ |
jason701802 | 0:b1e3fa917367 | 262 | setPwm(2*bNum, 0); |
jason701802 | 0:b1e3fa917367 | 263 | setPwm(2*bNum + 1, 1); |
jason701802 | 1:c8e328389a98 | 264 | _dir[bNum] = -1; |
jason701802 | 0:b1e3fa917367 | 265 | } |
jason701802 | 0:b1e3fa917367 | 266 | *_d[2*bNum] = 0; |
jason701802 | 0:b1e3fa917367 | 267 | *_p[2*bNum + 1] = speed; |
jason701802 | 0:b1e3fa917367 | 268 | enableCh(2*bNum, 1); //enable channels |
jason701802 | 0:b1e3fa917367 | 269 | enableCh(2*bNum + 1, 1); |
jason701802 | 4:87768972db3f | 270 | setLed(2*bNum, 0); |
jason701802 | 4:87768972db3f | 271 | setLed(2*bNum + 1, 1); |
jason701802 | 0:b1e3fa917367 | 272 | return speed; |
jason701802 | 0:b1e3fa917367 | 273 | } |
jason701802 | 1:c8e328389a98 | 274 | return -2; |
jason701802 | 0:b1e3fa917367 | 275 | } |
jason701802 | 0:b1e3fa917367 | 276 | |
jason701802 | 0:b1e3fa917367 | 277 | void BridgeDriver::enableCh(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 278 | static uint8_t state = 0x00; |
jason701802 | 0:b1e3fa917367 | 279 | static uint8_t oldState = 0x00; |
jason701802 | 0:b1e3fa917367 | 280 | |
jason701802 | 0:b1e3fa917367 | 281 | if(en){ |
jason701802 | 0:b1e3fa917367 | 282 | state = state | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 283 | }else{ |
jason701802 | 0:b1e3fa917367 | 284 | state = state & ~(1 << ch); |
jason701802 | 0:b1e3fa917367 | 285 | } |
jason701802 | 0:b1e3fa917367 | 286 | |
jason701802 | 0:b1e3fa917367 | 287 | if(state != oldState){ |
jason701802 | 0:b1e3fa917367 | 288 | _EnCtl->write(PORT_A, state); |
jason701802 | 0:b1e3fa917367 | 289 | oldState = state; |
jason701802 | 0:b1e3fa917367 | 290 | } |
jason701802 | 0:b1e3fa917367 | 291 | } |
jason701802 | 0:b1e3fa917367 | 292 | |
jason701802 | 0:b1e3fa917367 | 293 | void BridgeDriver::setLed(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 294 | uint8_t state; |
jason701802 | 0:b1e3fa917367 | 295 | if(en){ |
jason701802 | 0:b1e3fa917367 | 296 | state = _oldLedState | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 297 | }else{ |
jason701802 | 0:b1e3fa917367 | 298 | state = _oldLedState & ~(1 << ch); |
jason701802 | 0:b1e3fa917367 | 299 | } |
jason701802 | 0:b1e3fa917367 | 300 | |
jason701802 | 0:b1e3fa917367 | 301 | if(state != _oldLedState){ |
jason701802 | 0:b1e3fa917367 | 302 | _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low |
jason701802 | 0:b1e3fa917367 | 303 | _oldLedState = state; |
jason701802 | 0:b1e3fa917367 | 304 | } |
jason701802 | 0:b1e3fa917367 | 305 | } |
jason701802 | 0:b1e3fa917367 | 306 | |
jason701802 | 0:b1e3fa917367 | 307 | void BridgeDriver::setLed(uint8_t state){ |
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::setPwm(uint8_t ch, uint8_t en){ |
jason701802 | 0:b1e3fa917367 | 315 | enableCh(ch, 0); |
jason701802 | 0:b1e3fa917367 | 316 | |
jason701802 | 1:c8e328389a98 | 317 | |
jason701802 | 1:c8e328389a98 | 318 | if(_pwmCh & (1 << ch)){ //pwm is currently enabled on this channel |
jason701802 | 0:b1e3fa917367 | 319 | if(en == 0){ |
jason701802 | 0:b1e3fa917367 | 320 | delete _p[ch]; |
jason701802 | 0:b1e3fa917367 | 321 | switch(ch){ |
jason701802 | 1:c8e328389a98 | 322 | case 0: |
jason701802 | 1:c8e328389a98 | 323 | _d[ch] = new DigitalOut(PIN_CH1); |
jason701802 | 1:c8e328389a98 | 324 | break; |
jason701802 | 1:c8e328389a98 | 325 | case 1: |
jason701802 | 1:c8e328389a98 | 326 | _d[ch] = new DigitalOut(PIN_CH2); |
jason701802 | 1:c8e328389a98 | 327 | break; |
jason701802 | 1:c8e328389a98 | 328 | case 2: |
jason701802 | 1:c8e328389a98 | 329 | _d[ch] = new DigitalOut(PIN_CH3); |
jason701802 | 1:c8e328389a98 | 330 | break; |
jason701802 | 1:c8e328389a98 | 331 | case 3: |
jason701802 | 1:c8e328389a98 | 332 | _d[ch] = new DigitalOut(PIN_CH4); |
jason701802 | 1:c8e328389a98 | 333 | break; |
jason701802 | 1:c8e328389a98 | 334 | case 4: |
jason701802 | 1:c8e328389a98 | 335 | _d[ch] = new DigitalOut(PIN_CH5); |
jason701802 | 1:c8e328389a98 | 336 | break; |
jason701802 | 1:c8e328389a98 | 337 | case 5: |
jason701802 | 1:c8e328389a98 | 338 | _d[ch] = new DigitalOut(PIN_CH6); |
jason701802 | 1:c8e328389a98 | 339 | break; |
jason701802 | 1:c8e328389a98 | 340 | case 6: |
jason701802 | 1:c8e328389a98 | 341 | _d[ch] = new DigitalOut(PIN_CH7); |
jason701802 | 1:c8e328389a98 | 342 | break; |
jason701802 | 1:c8e328389a98 | 343 | case 7: |
jason701802 | 1:c8e328389a98 | 344 | _d[ch] = new DigitalOut(PIN_CH8); |
jason701802 | 1:c8e328389a98 | 345 | break; |
jason701802 | 0:b1e3fa917367 | 346 | } |
jason701802 | 1:c8e328389a98 | 347 | _pwmCh = _pwmCh & ~(1 << ch); |
jason701802 | 1:c8e328389a98 | 348 | |
jason701802 | 0:b1e3fa917367 | 349 | } |
jason701802 | 0:b1e3fa917367 | 350 | }else{ //pwm is currently disabled on this channel |
jason701802 | 0:b1e3fa917367 | 351 | if(en == 1){ |
jason701802 | 0:b1e3fa917367 | 352 | delete _d[ch]; |
jason701802 | 0:b1e3fa917367 | 353 | switch(ch){ |
jason701802 | 1:c8e328389a98 | 354 | case 0: |
jason701802 | 1:c8e328389a98 | 355 | _p[ch] = new PwmOut(PIN_CH1); |
jason701802 | 1:c8e328389a98 | 356 | break; |
jason701802 | 1:c8e328389a98 | 357 | case 1: |
jason701802 | 1:c8e328389a98 | 358 | _p[ch] = new PwmOut(PIN_CH2); |
jason701802 | 1:c8e328389a98 | 359 | break; |
jason701802 | 1:c8e328389a98 | 360 | case 2: |
jason701802 | 1:c8e328389a98 | 361 | _p[ch] = new PwmOut(PIN_CH3); |
jason701802 | 1:c8e328389a98 | 362 | break; |
jason701802 | 1:c8e328389a98 | 363 | case 3: |
jason701802 | 1:c8e328389a98 | 364 | _p[ch] = new PwmOut(PIN_CH4); |
jason701802 | 1:c8e328389a98 | 365 | break; |
jason701802 | 1:c8e328389a98 | 366 | case 4: |
jason701802 | 1:c8e328389a98 | 367 | _p[ch] = new PwmOut(PIN_CH5); |
jason701802 | 1:c8e328389a98 | 368 | break; |
jason701802 | 1:c8e328389a98 | 369 | case 5: |
jason701802 | 1:c8e328389a98 | 370 | _p[ch] = new PwmOut(PIN_CH6); |
jason701802 | 1:c8e328389a98 | 371 | break; |
jason701802 | 1:c8e328389a98 | 372 | case 6: |
jason701802 | 1:c8e328389a98 | 373 | _p[ch] = new PwmOut(PIN_CH7); |
jason701802 | 1:c8e328389a98 | 374 | break; |
jason701802 | 1:c8e328389a98 | 375 | case 7: |
jason701802 | 1:c8e328389a98 | 376 | _p[ch] = new PwmOut(PIN_CH8); |
jason701802 | 1:c8e328389a98 | 377 | break; |
jason701802 | 0:b1e3fa917367 | 378 | } |
jason701802 | 4:87768972db3f | 379 | _p[ch]->period(PWM_PERIOD); |
jason701802 | 1:c8e328389a98 | 380 | _pwmCh = _pwmCh | (1 << ch); |
jason701802 | 0:b1e3fa917367 | 381 | } |
jason701802 | 0:b1e3fa917367 | 382 | } |
jason701802 | 0:b1e3fa917367 | 383 | |
jason701802 | 0:b1e3fa917367 | 384 | |
jason701802 | 0:b1e3fa917367 | 385 | } |
jason701802 | 0:b1e3fa917367 | 386 | |
jason701802 | 1:c8e328389a98 | 387 | void BridgeDriver::diagnostic(TextLCD_I2C *lcd){ |
jason701802 | 1:c8e328389a98 | 388 | lcd->setAddress(0,1); |
jason701802 | 1:c8e328389a98 | 389 | lcd->printf("PWM:%d,%d,%d,%d ", _pwm[0], _pwm[1], _pwm[2], _pwm[3]); |
jason701802 | 1:c8e328389a98 | 390 | lcd->setAddress(0,2); |
jason701802 | 1:c8e328389a98 | 391 | lcd->printf("pCH:%i%i%i%i%i%i%i%i", !!(_pwmCh & (1 << 0)), |
jason701802 | 1:c8e328389a98 | 392 | !!(_pwmCh & (1 << 1)), |
jason701802 | 1:c8e328389a98 | 393 | !!(_pwmCh & (1 << 2)), |
jason701802 | 1:c8e328389a98 | 394 | !!(_pwmCh & (1 << 3)), |
jason701802 | 1:c8e328389a98 | 395 | !!(_pwmCh & (1 << 4)), |
jason701802 | 1:c8e328389a98 | 396 | !!(_pwmCh & (1 << 5)), |
jason701802 | 1:c8e328389a98 | 397 | !!(_pwmCh & (1 << 6)), |
jason701802 | 1:c8e328389a98 | 398 | !!(_pwmCh & (1 << 7)) |
jason701802 | 1:c8e328389a98 | 399 | ); |
jason701802 | 1:c8e328389a98 | 400 | lcd->setAddress(0,3); |
jason701802 | 1:c8e328389a98 | 401 | lcd->printf("dir:%+d,%+d,%+d,%+d ", _dir[0], _dir[1], _dir[2], _dir[3]); |
jason701802 | 1:c8e328389a98 | 402 | } |