frequency change?
Fork of BridgeDriver by
Diff: BridgeDriver.cpp
- Revision:
- 0:b1e3fa917367
- Child:
- 1:c8e328389a98
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BridgeDriver.cpp Thu Jul 03 17:09:29 2014 +0000 @@ -0,0 +1,389 @@ + +#include "BridgeDriver.h" +#include "mbed.h" + +#include "MCP23017.h" + +//TO-DO: check order of setting output and enabling/disabling channel + +BridgeDriver::BridgeDriver( I2C *i2c, const uint8_t enPwmA, const uint8_t enPwmB, const uint8_t enPwmC, const uint8_t enPwmD, const uint8_t enAddr, const uint8_t ledAddr) : + _i2c(i2c), + _enAddr(enAddr), + _ledAddr(ledAddr), + _oldLedState(0x00) + { + + + _pwm[0] = enPwmA; + _pwm[1] = enPwmB; + _pwm[2] = enPwmC; + _pwm[3] = enPwmD; + + for(int i = 0; i<4; i++){ + _pwm[i] = 0; + _dir[i] = 0; + _braking[i] = 0; + } + + + _EnCtl = new MCP23017(*_i2c, _enAddr); //MCP23008 -Only use PORT_A + _IO = new MCP23017(*_i2c, _ledAddr); + + _IO->direction(PORT_B, 0x00); //LEDs are outputs + _IO->write(PORT_B, ~0x00); //bitwise not (~) b/c leds are active low + _EnCtl->configureBanked(BNK); + _EnCtl->direction(PORT_A, 0x00); //all outputs + _EnCtl->write(PORT_A, 0x00); //leave all channels disabled + + + + _d[0] = new DigitalOut(PIN_CH1); + _d[1] = new DigitalOut(PIN_CH2); + _d[2] = new DigitalOut(PIN_CH3); + _d[3] = new DigitalOut(PIN_CH4); + _d[4] = new DigitalOut(PIN_CH5); + _d[5] = new DigitalOut(PIN_CH6); + _d[6] = new DigitalOut(PIN_CH7); + _d[7] = new DigitalOut(PIN_CH8); + + + +} + + +BridgeDriver::~BridgeDriver(){ + for(int i=0; i<4; i++){ + if(_pwm[i]){ + switch(_dir[i]){ + case 1: + delete _p[2*i]; + delete _d[2*i + 1]; + break; + case -1: + delete _d[2*i]; + delete _p[2*i + 1]; + break; + case 0: + delete _d[2*i]; + delete _d[2*i + 1]; + break; + } + }else{ + delete _d[2*i]; + delete _d[2*i + 1]; + } + } +} + + +void BridgeDriver::enablePwm(uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD){ + enablePwm(BRIDGE_A, enPwmA); + enablePwm(BRIDGE_B, enPwmB); + enablePwm(BRIDGE_C, enPwmC); + enablePwm(BRIDGE_D, enPwmD); +} +void BridgeDriver::enablePwm(Bridges bridge, uint8_t enPwm){ + int bNum = static_cast<int>(bridge); //numeric bridge + if(enPwm == _pwm[bNum]){ + return; + }else if(enPwm == 0){ //disable pwm + setPwm(2*bNum, 0); //channels are disabled in setPwm() + setPwm(2*bNum + 1, 0); + _pwm[bNum] = 0; + }else{ //enable pwm + enableCh(2*bNum, 0); //disable channels + enableCh(2*bNum + 1, 0); + _pwm[bNum] = 1; //pwm for individual channels is turned on as necessary when running motor + _dir[bNum] = 0; + } +} + +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 + enableBraking(BRIDGE_A, enBrakeA); + enableBraking(BRIDGE_B, enBrakeB); + enableBraking(BRIDGE_C, enBrakeC); + enableBraking(BRIDGE_D, enBrakeD); +} +void BridgeDriver::enableBraking(Bridges bridge, uint8_t enBrake) { + _braking[static_cast<int>(bridge)] = enBrake; +} + +int BridgeDriver::forceBrake(uint8_t ch){ //force a specific channel to GND without changing braking default + if( _pwm[(ch-1)/2]){ + return -1; + }else{ + *_d[ch-1] = 0; + enableCh(ch-1, 1); + setLed(ch-1, 0); + return 0; + } +} +int BridgeDriver::forceBrake(Bridges bridge){ //force a specific motor to GND without changing braking default + return static_cast<int>(drive(bridge, 0, 0)); +} +int BridgeDriver::forceFloat(uint8_t ch){ //force a specific channel to float without changing braking default + if( _pwm[(ch-1)/2]){ + return -1; + }else{ + enableCh(ch-1, 0); + setLed(ch-1, 0); + return *_d[ch-1] = 0; + } +} +int BridgeDriver::forceFloat(Bridges bridge){ //force a specific motor to float without changing braking default + int bNum = static_cast<int>(bridge); //numeric bridge + + if(_pwm[bNum] == 0){ + return -1; + }else{ + if(_dir[bNum] != 0){ + setPwm(2*bNum, 0); + setPwm(2*bNum + 1, 0); + _dir[bNum] = 0; + } + setLed(2*bNum, 0); + setLed(2*bNum + 1, 0); + enableCh(2*bNum, 0); //enable/disable channels as necessary for braking/coasting + enableCh(2*bNum + 1, 0); + *_d[2*bNum] = 0; + return *_d[2*bNum + 1] = 0; + } +} + +int BridgeDriver::drive(uint8_t state){ + if(_pwm[0] || _pwm[1] || _pwm[2] || _pwm[3]){ + return -1; + }else{ + setLed(state); + for(int i=0; i<8; i++){ + if(state & (1 << i)){ //channel i should be on + *_d[i] = 1; + enableCh(i, 1); + }else{ //channel i should be off + if(_braking[1/2]){ + *_d[i] = 0; //brake output + enableCh(i, 1); + }else{ + enableCh(i, 0); + *_d[i] = 0; //brake output + } + } + } + return !!state; + } +} +int BridgeDriver::drive(uint8_t ch, uint8_t on){ + if( _pwm[(ch-1)/2]){ + return -1; + }else if(on){ //on + enableCh(ch-1, 1); + setLed(ch-1, 1); + return *_d[ch-1] = 1; //off, brake + }else if(_braking[(ch-1)/2]){ + *_d[ch-1] = 0; + enableCh(ch-1, 1); + setLed(ch-1, 0); + return 0; + }else{ //off, float + enableCh(ch-1, 0); + setLed(ch-1, 0); + return *_d[ch-1] = 0; + } +} +float BridgeDriver::drive(Bridges bridge, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking + int bNum = static_cast<int>(bridge); //numeric bridge + + if(_pwm[bNum] == 0){ + return -1; + } + + if(speed == 0){ + return drive(bridge, 1, 0); + }else if(speed > 0){ + return drive(bridge, 1, speed); + }else{ + return drive(bridge, -1, -1*speed); + } +} +float BridgeDriver::drive(Bridges bridge, int8_t dir, float speed){ + //dir: 1=fwd, -1=rev, 0=brake (regardless of setting of _braking + //speed from 0 to 1, speed of 0 will coast or brake depending on setting of _braking + + int bNum = static_cast<uint8_t>(bridge); //numeric bridge + + if(_pwm[bNum] == 0){ + return -1; + } + if(speed < -1 || speed > 1){ //maybe this should be bounding instead? + return -1; + } + + if(dir == 0 || (speed == 0 && _braking[bNum])){ //brake + if(_dir[bNum] != 0){ + setPwm(2*bNum, 0); + setPwm(2*bNum + 1, 0); + _dir[bNum] = 0; + } + setLed(2*bNum, 0); + setLed(2*bNum + 1, 0); + *_d[2*bNum] = 0; + *_d[2*bNum + 1] = 0; + enableCh(2*bNum, 1); //enable channels for braking + enableCh(2*bNum + 1, 1); + + return 0; + + }else if(speed == 0){ //coast + if(_dir[bNum] != 0){ + setPwm(2*bNum, 0); + setPwm(2*bNum + 1, 0); + _dir[bNum] = 0; + } + setLed(2*bNum, 0); + setLed(2*bNum + 1, 0); + enableCh(2*bNum, 0); //coast channels + enableCh(2*bNum + 1, 0); + *_d[2*bNum] = 0; + return *_d[2*bNum + 1] = 0; + }else if(dir == 1){ + if(_dir[bNum] != 1){ + setPwm(2*bNum, 1); + setPwm(2*bNum + 1, 0); + _dir[bNum] = 0; + } + setLed(2*bNum, 1); + setLed(2*bNum + 1, 0); + *_p[2*bNum] = speed; + *_d[2*bNum + 1] = 0; + enableCh(2*bNum, 1); //enable channels + enableCh(2*bNum + 1, 1); + return speed; + }else if(dir == -1){ + if(_dir[bNum] != 1){ + setPwm(2*bNum, 0); + setPwm(2*bNum + 1, 1); + _dir[bNum] = 0; + } + setLed(2*bNum, 0); + setLed(2*bNum + 1, 1); + *_d[2*bNum] = 0; + *_p[2*bNum + 1] = speed; + enableCh(2*bNum, 1); //enable channels + enableCh(2*bNum + 1, 1); + return speed; + } + return -1; +} + +void BridgeDriver::enableCh(uint8_t ch, uint8_t en){ + static uint8_t state = 0x00; + static uint8_t oldState = 0x00; + + if(en){ + state = state | (1 << ch); + }else{ + state = state & ~(1 << ch); + } + + if(state != oldState){ + _EnCtl->write(PORT_A, state); + oldState = state; + } +} + +void BridgeDriver::setLed(uint8_t ch, uint8_t en){ + uint8_t state; + if(en){ + state = _oldLedState | (1 << ch); + }else{ + state = _oldLedState & ~(1 << ch); + } + + if(state != _oldLedState){ + _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low + _oldLedState = state; + } +} + +void BridgeDriver::setLed(uint8_t state){ + if(state != _oldLedState){ + _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low + _oldLedState = state; + } +} + +void BridgeDriver::setPwm(uint8_t ch, uint8_t en){ + enableCh(ch, 0); + + if(_pwm[ch/2] && ( //if pwm is currently enabled + ( ch%2 && _dir[ch/2] == -1) || //higher numbered channel (ch%2) and dir = -1 + ( !ch%2 && _dir[ch/2] == 1) //lower numbered channel (!ch%2) and dir = 1 + ) ){ + //pwn is currently enabled on this channel + if(en == 0){ + delete _p[ch]; + switch(ch){ + case 0: + _d[ch] = new DigitalOut(PIN_CH1); + break; + case 1: + _d[ch] = new DigitalOut(PIN_CH2); + break; + case 2: + _d[ch] = new DigitalOut(PIN_CH3); + break; + case 3: + _d[ch] = new DigitalOut(PIN_CH4); + break; + case 4: + _d[ch] = new DigitalOut(PIN_CH5); + break; + case 5: + _d[ch] = new DigitalOut(PIN_CH6); + break; + case 6: + _d[ch] = new DigitalOut(PIN_CH7); + break; + case 7: + _d[ch] = new DigitalOut(PIN_CH8); + break; + } + } + }else{ //pwm is currently disabled on this channel + if(en == 1){ + delete _d[ch]; + switch(ch){ + case 0: + _p[ch] = new PwmOut(PIN_CH1); + break; + case 1: + _p[ch] = new PwmOut(PIN_CH2); + break; + case 2: + _p[ch] = new PwmOut(PIN_CH3); + break; + case 3: + _p[ch] = new PwmOut(PIN_CH4); + break; + case 4: + _p[ch] = new PwmOut(PIN_CH5); + break; + case 5: + _p[ch] = new PwmOut(PIN_CH6); + break; + case 6: + _p[ch] = new PwmOut(PIN_CH7); + break; + case 7: + _p[ch] = new PwmOut(PIN_CH8); + break; + } + } + } + + +} + + + +