File content as of revision 13:6561ddb98d6e:

#include "BridgeDriver.h"
#include "mbed.h"
#include "TextLCD.h" // if using diagnostic

#include "MCP23017.h"


BridgeDriver::BridgeDriver( I2C *i2c, uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD, uint8_t enAddr, uint8_t ledAddr) :
                            _i2c(i2c),
                            _enAddr(enAddr),
                            _ledAddr(ledAddr),
                            _pwmCh(0x00),
                            _oldLedState(0x00),
                            _PWMperiod(DEFAULT_PWM_PERIOD)
                          {


    _pwm[0] = enPwmA;
    _pwm[1] = enPwmB;
    _pwm[2] = enPwmC;
    _pwm[3] = enPwmD;
    
    for(int i = 0; i<4; i++){
        _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(MOTOR_A, enPwmA);
    enablePwm(MOTOR_B, enPwmB);
    enablePwm(MOTOR_C, enPwmC);
    enablePwm(MOTOR_D, enPwmD);
}

void BridgeDriver::enablePwm(Motors motor, uint8_t enPwm){
    int bNum = static_cast<int>(motor); //numeric motor
    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(MOTOR_A, enBrakeA);
    enableBraking(MOTOR_B, enBrakeB);
    enableBraking(MOTOR_C, enBrakeC);
    enableBraking(MOTOR_D, enBrakeD);
}

void BridgeDriver::enableBraking(Motors motor, uint8_t enBrake) {
    _braking[static_cast<int>(motor)] = 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(Motors motor){     //force a specific motor to GND without changing braking default
    return static_cast<int>(drive(motor, 0, 0));
}

int BridgeDriver::forceFloat(uint8_t ch){             //force a specific channel to float without changing braking default
    int bNum = (ch-1)/2;
    if( _pwm[bNum] ){
        setPwm(2*bNum, 0);  //channel is disabled in setPwm()
        setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
        _dir[bNum] = 0;
        setLed(2*bNum, 0);
        setLed(2*bNum + 1, 0);
        *_d[2*bNum] = 0;
        return *_d[2*bNum + 1] = 0;
    }else{
        enableCh(ch-1, 0);
        setLed(ch-1, 0);
        return *_d[ch-1] = 0;
    }
}

int BridgeDriver::forceFloat(Motors motor){     //force a specific motor to float without changing braking default
    int bNum = static_cast<int>(motor); //numeric motor
    
    if(_pwm[bNum] == 0){
        return -1;
    }else{
        setPwm(2*bNum, 0);  //channel is disabled in setPwm()
        setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
        _dir[bNum] = 0;
        setLed(2*bNum, 0);
        setLed(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[i/2]){      
                   *_d[i] = 0;         //brake output
                    enableCh(i, 1);
                }else{
                    enableCh(i, 0);
                    *_d[i] = 0;         //float/coast output
                }
            }
        }
        return !!state;             
    }
}

int BridgeDriver::drive(uint8_t ch, uint8_t on){
    if( _pwm[(ch-1)/2]){
        return -1;
    }else if(on){               //on
        *_d[ch-1] = 1;
        enableCh(ch-1, 1);
        setLed(ch-1, 1);
        return 1;   
    }else if(_braking[(ch-1)/2]){   //off, brake
        *_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(Motors motor, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking
    int bNum = static_cast<int>(motor); //numeric motor
    
    if(_pwm[bNum] == 0){
        return -20 - bNum;
    }
    
    if(speed == 0){
        return drive(motor, 1, 0);
    }else if(speed > 0){
        return drive(motor, 1, speed);
    }else{  
        return drive(motor, -1, -1*speed);
    }
}

float BridgeDriver::drive(Motors motor, 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>(motor); //numeric motor
    
    if(_pwm[bNum] == 0){
        return -10 - bNum;
    }
    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;
        }
        *_d[2*bNum] = 0;
        *_d[2*bNum + 1] = 0;
        enableCh(2*bNum, 1);        //enable channels for braking
        enableCh(2*bNum + 1, 1);
        setLed(2*bNum, 0);
        setLed(2*bNum + 1, 0);
        
        return 0;
        
    }else if(speed == 0){       //coast
        setPwm(2*bNum, 0);  //channel is disabled in setPwm()
        setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
        _dir[bNum] = 0;
        setLed(2*bNum, 0);
        setLed(2*bNum + 1, 0);
        *_d[2*bNum] = 0;
        return *_d[2*bNum + 1] = 0;
    }else if(dir == 1){         //forward
        if(_dir[bNum] != 1){
            setPwm(2*bNum, 1);
            setPwm(2*bNum + 1, 0);
            _dir[bNum] = 1;
        }
        *_p[2*bNum] = speed;
        *_d[2*bNum + 1] = 0;
        enableCh(2*bNum, 1);        //enable channels
        enableCh(2*bNum + 1, 1);
        setLed(2*bNum, 1);
        setLed(2*bNum + 1, 0);
        return speed;
    }else if(dir == -1){        //reverse
        if(_dir[bNum] != -1){
            setPwm(2*bNum, 0);
            setPwm(2*bNum + 1, 1);
            _dir[bNum] = -1;
        }
        *_d[2*bNum] = 0;
        *_p[2*bNum + 1] = speed;
        enableCh(2*bNum, 1);        //enable channels
        enableCh(2*bNum + 1, 1);
        setLed(2*bNum, 0);
        setLed(2*bNum + 1, 1);
        return speed;
    } 
    return -2;
}

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(_pwmCh & (1 << ch)){        //pwm 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;
            }
            _pwmCh = _pwmCh & ~(1 << ch);
            
        }
    }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;
            }
            _p[ch]->period(_PWMperiod);
            _pwmCh = _pwmCh | (1 << ch);
        }
    }
    

}

void BridgeDriver::diagnostic(TextLCD_I2C *lcd){
    lcd->setAddress(0,1);
    lcd->printf("PWM:%d,%d,%d,%d    ", _pwm[0], _pwm[1], _pwm[2], _pwm[3]);
    lcd->setAddress(0,2);
    lcd->printf("pCH:%i%i%i%i%i%i%i%i", !!(_pwmCh & (1 << 0)),
                                        !!(_pwmCh & (1 << 1)),
                                        !!(_pwmCh & (1 << 2)),
                                        !!(_pwmCh & (1 << 3)),
                                        !!(_pwmCh & (1 << 4)),
                                        !!(_pwmCh & (1 << 5)),
                                        !!(_pwmCh & (1 << 6)),
                                        !!(_pwmCh & (1 << 7))
                                        );
    lcd->setAddress(0,3);
    lcd->printf("dir:%+d,%+d,%+d,%+d    ", _dir[0], _dir[1], _dir[2], _dir[3]);
}

void BridgeDriver::setPWMperiod(float newPWMperiod){
    _PWMperiod = newPWMperiod;
}