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