File content as of revision 0:b1e3fa917367:

#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;
            }
        }
    }
    

}