Diff: BridgeDriver.cpp

Revision:

0:b1e3fa917367

Child:

1:c8e328389a98

diff -r 000000000000 -r b1e3fa917367 BridgeDriver.cpp
--- /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;
+            }
+        }
+    }
+    
+
+}
+
+
+
+