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