BridgeDriver.cpp

00001 
00002 #include "BridgeDriver.h"
00003 #include "mbed.h"
00004 #include "TextLCD.h" // if using diagnostic
00005 
00006 #include "MCP23017.h"
00007 
00008 
00009 BridgeDriver::BridgeDriver( I2C *i2c, uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD, uint8_t enAddr, uint8_t ledAddr) :
00010                             _i2c(i2c),
00011                             _enAddr(enAddr),
00012                             _ledAddr(ledAddr),
00013                             _pwmCh(0x00),
00014                             _oldLedState(0x00),
00015                             _PWMperiod(DEFAULT_PWM_PERIOD)
00016                           {
00017 
00018 
00019     _pwm[0] = enPwmA;
00020     _pwm[1] = enPwmB;
00021     _pwm[2] = enPwmC;
00022     _pwm[3] = enPwmD;
00023     
00024     for(int i = 0; i<4; i++){
00025         _dir[i] = 0;
00026         _braking[i] = 0;
00027     }
00028     
00029     
00030     _EnCtl = new MCP23017(*_i2c, _enAddr); //MCP23008 -Only use PORT_A
00031     _IO = new MCP23017(*_i2c, _ledAddr);
00032     
00033     _IO->direction(PORT_B, 0x00); //LEDs are outputs
00034     _IO->write(PORT_B, ~0x00); //bitwise not (~) b/c leds are active low
00035     _EnCtl->configureBanked(BNK);
00036     _EnCtl->direction(PORT_A, 0x00); //all outputs
00037     _EnCtl->write(PORT_A, 0x00);  //leave all channels disabled
00038     
00039     
00040     
00041     _d[0] = new DigitalOut(PIN_CH1);
00042     _d[1] = new DigitalOut(PIN_CH2);
00043     _d[2] = new DigitalOut(PIN_CH3);
00044     _d[3] = new DigitalOut(PIN_CH4);
00045     _d[4] = new DigitalOut(PIN_CH5);
00046     _d[5] = new DigitalOut(PIN_CH6);
00047     _d[6] = new DigitalOut(PIN_CH7);
00048     _d[7] = new DigitalOut(PIN_CH8);
00049     
00050 
00051     
00052 }
00053        
00054 BridgeDriver::~BridgeDriver(){
00055     for(int i=0; i<4; i++){
00056         if(_pwm[i]){
00057             switch(_dir[i]){
00058                 case 1:
00059                     delete _p[2*i];
00060                     delete _d[2*i + 1];
00061                 break;
00062                 case -1:
00063                     delete _d[2*i];
00064                     delete _p[2*i + 1];
00065                 break;
00066                 case 0:
00067                     delete _d[2*i];
00068                     delete _d[2*i + 1];
00069                 break;
00070             }
00071         }else{
00072             delete _d[2*i];
00073             delete _d[2*i + 1];
00074         }
00075     }
00076 }
00077 
00078 void BridgeDriver::enablePwm(uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD){
00079     enablePwm(MOTOR_A, enPwmA);
00080     enablePwm(MOTOR_B, enPwmB);
00081     enablePwm(MOTOR_C, enPwmC);
00082     enablePwm(MOTOR_D, enPwmD);
00083 }
00084 
00085 void BridgeDriver::enablePwm(Motors motor, uint8_t enPwm){
00086     int bNum = static_cast<int>(motor); //numeric motor
00087     if(enPwm == _pwm[bNum]){
00088         return;
00089     }else if(enPwm == 0){ //disable pwm
00090         setPwm(2*bNum, 0);      //channels are disabled in setPwm()
00091         setPwm(2*bNum + 1, 0);
00092         _pwm[bNum] = 0;
00093     }else{  //enable pwm
00094         enableCh(2*bNum, 0);        //disable channels
00095         enableCh(2*bNum + 1, 0);
00096         _pwm[bNum] = 1; //pwm for individual channels is turned on as necessary when running motor
00097         _dir[bNum] = 0;
00098     }
00099 }
00100 
00101 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
00102     enableBraking(MOTOR_A, enBrakeA);
00103     enableBraking(MOTOR_B, enBrakeB);
00104     enableBraking(MOTOR_C, enBrakeC);
00105     enableBraking(MOTOR_D, enBrakeD);
00106 }
00107 
00108 void BridgeDriver::enableBraking(Motors motor, uint8_t enBrake) {
00109     _braking[static_cast<int>(motor)] = enBrake;
00110 }
00111 
00112 int BridgeDriver::forceBrake(uint8_t ch){             //force a specific channel to GND without changing braking default
00113     if( _pwm[(ch-1)/2]){
00114         return -1;
00115     }else{
00116         *_d[ch-1] = 0;
00117         enableCh(ch-1, 1);
00118         setLed(ch-1, 0);
00119         return 0;
00120     }
00121 }
00122 
00123 int BridgeDriver::forceBrake(Motors motor){     //force a specific motor to GND without changing braking default
00124     return static_cast<int>(drive(motor, 0, 0));
00125 }
00126 
00127 int BridgeDriver::forceFloat(uint8_t ch){             //force a specific channel to float without changing braking default
00128     int bNum = (ch-1)/2;
00129     if( _pwm[bNum] ){
00130         setPwm(2*bNum, 0);  //channel is disabled in setPwm()
00131         setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
00132         _dir[bNum] = 0;
00133         setLed(2*bNum, 0);
00134         setLed(2*bNum + 1, 0);
00135         *_d[2*bNum] = 0;
00136         return *_d[2*bNum + 1] = 0;
00137     }else{
00138         enableCh(ch-1, 0);
00139         setLed(ch-1, 0);
00140         return *_d[ch-1] = 0;
00141     }
00142 }
00143 
00144 int BridgeDriver::forceFloat(Motors motor){     //force a specific motor to float without changing braking default
00145     int bNum = static_cast<int>(motor); //numeric motor
00146     
00147     if(_pwm[bNum] == 0){
00148         return -1;
00149     }else{
00150         setPwm(2*bNum, 0);  //channel is disabled in setPwm()
00151         setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
00152         _dir[bNum] = 0;
00153         setLed(2*bNum, 0);
00154         setLed(2*bNum + 1, 0);
00155         *_d[2*bNum] = 0;
00156         return *_d[2*bNum + 1] = 0;
00157     }
00158 }
00159 
00160 int BridgeDriver::drive(uint8_t state){
00161     if(_pwm[0] || _pwm[1] || _pwm[2] || _pwm[3]){
00162         return -1;
00163     }else{
00164         setLed(state);
00165         for(int i=0; i<8; i++){
00166             if(state & (1 << i)){   //channel i should be on
00167                 *_d[i] = 1; 
00168                 enableCh(i, 1);
00169             }else{                  //channel i should be off
00170                 if(_braking[i/2]){      
00171                    *_d[i] = 0;         //brake output
00172                     enableCh(i, 1);
00173                 }else{
00174                     enableCh(i, 0);
00175                     *_d[i] = 0;         //float/coast output
00176                 }
00177             }
00178         }
00179         return !!state;             
00180     }
00181 }
00182 
00183 int BridgeDriver::drive(uint8_t ch, uint8_t on){
00184     if( _pwm[(ch-1)/2]){
00185         return -1;
00186     }else if(on){               //on
00187         *_d[ch-1] = 1;
00188         enableCh(ch-1, 1);
00189         setLed(ch-1, 1);
00190         return 1;   
00191     }else if(_braking[(ch-1)/2]){   //off, brake
00192         *_d[ch-1] = 0;
00193         enableCh(ch-1, 1);
00194         setLed(ch-1, 0);
00195         return 0;
00196     }else{                      //off, float
00197         enableCh(ch-1, 0);
00198         setLed(ch-1, 0);
00199         return *_d[ch-1] = 0;
00200     }
00201 }
00202 
00203 float BridgeDriver::drive(Motors motor, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking
00204     int bNum = static_cast<int>(motor); //numeric motor
00205     
00206     if(_pwm[bNum] == 0){
00207         return -20 - bNum;
00208     }
00209     
00210     if(speed == 0){
00211         return drive(motor, 1, 0);
00212     }else if(speed > 0){
00213         return drive(motor, 1, speed);
00214     }else{  
00215         return drive(motor, -1, -1*speed);
00216     }
00217 }
00218 
00219 float BridgeDriver::drive(Motors motor, int8_t dir, float speed){ 
00220     //dir: 1=fwd,  -1=rev, 0=brake (regardless of setting of _braking
00221     //speed from 0 to 1, speed of 0 will coast or brake depending on setting of _braking
00222     
00223     int bNum = static_cast<uint8_t>(motor); //numeric motor
00224     
00225     if(_pwm[bNum] == 0){
00226         return -10 - bNum;
00227     }
00228     if(speed < -1 || speed > 1){        //maybe this should be bounding instead?
00229         return -1;
00230     }
00231     
00232     if(dir == 0 || (speed == 0 && _braking[bNum])){ //brake
00233         if(_dir[bNum] != 0){
00234             setPwm(2*bNum, 0);
00235             setPwm(2*bNum + 1, 0);
00236             _dir[bNum] = 0;
00237         }
00238         *_d[2*bNum] = 0;
00239         *_d[2*bNum + 1] = 0;
00240         enableCh(2*bNum, 1);        //enable channels for braking
00241         enableCh(2*bNum + 1, 1);
00242         setLed(2*bNum, 0);
00243         setLed(2*bNum + 1, 0);
00244         
00245         return 0;
00246         
00247     }else if(speed == 0){       //coast
00248         setPwm(2*bNum, 0);  //channel is disabled in setPwm()
00249         setPwm(2*bNum + 1, 0);  //channel is disabled in setPwm()
00250         _dir[bNum] = 0;
00251         setLed(2*bNum, 0);
00252         setLed(2*bNum + 1, 0);
00253         *_d[2*bNum] = 0;
00254         return *_d[2*bNum + 1] = 0;
00255     }else if(dir == 1){         //forward
00256         if(_dir[bNum] != 1){
00257             setPwm(2*bNum, 1);
00258             setPwm(2*bNum + 1, 0);
00259             _dir[bNum] = 1;
00260         }
00261         *_p[2*bNum] = speed;
00262         *_d[2*bNum + 1] = 0;
00263         enableCh(2*bNum, 1);        //enable channels
00264         enableCh(2*bNum + 1, 1);
00265         setLed(2*bNum, 1);
00266         setLed(2*bNum + 1, 0);
00267         return speed;
00268     }else if(dir == -1){        //reverse
00269         if(_dir[bNum] != -1){
00270             setPwm(2*bNum, 0);
00271             setPwm(2*bNum + 1, 1);
00272             _dir[bNum] = -1;
00273         }
00274         *_d[2*bNum] = 0;
00275         *_p[2*bNum + 1] = speed;
00276         enableCh(2*bNum, 1);        //enable channels
00277         enableCh(2*bNum + 1, 1);
00278         setLed(2*bNum, 0);
00279         setLed(2*bNum + 1, 1);
00280         return speed;
00281     } 
00282     return -2;
00283 }
00284 
00285 void BridgeDriver::enableCh(uint8_t ch, uint8_t en){
00286     static uint8_t state = 0x00;
00287     static uint8_t oldState = 0x00;
00288     
00289     if(en){
00290         state = state | (1 << ch);
00291     }else{
00292         state = state & ~(1 << ch);
00293     }
00294     
00295     if(state != oldState){
00296         _EnCtl->write(PORT_A, state);
00297         oldState = state;
00298     }
00299 }
00300 
00301 void BridgeDriver::setLed(uint8_t ch, uint8_t en){
00302     uint8_t state;
00303     if(en){
00304         state = _oldLedState | (1 << ch);
00305     }else{
00306         state = _oldLedState & ~(1 << ch);
00307     }
00308     
00309     if(state != _oldLedState){
00310          _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low
00311         _oldLedState = state;
00312     }
00313 }
00314 
00315 void BridgeDriver::setLed(uint8_t state){
00316     if(state != _oldLedState){
00317          _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low
00318         _oldLedState = state;
00319     }
00320 }
00321 
00322 void BridgeDriver::setPwm(uint8_t ch, uint8_t en){  
00323     enableCh(ch, 0);
00324     
00325     
00326     if(_pwmCh & (1 << ch)){        //pwm is currently enabled on this channel
00327         if(en == 0){
00328             delete _p[ch];
00329             switch(ch){
00330                 case 0:
00331                     _d[ch] = new DigitalOut(PIN_CH1);
00332                 break;
00333                 case 1:
00334                     _d[ch] = new DigitalOut(PIN_CH2);
00335                 break;
00336                 case 2:
00337                     _d[ch] = new DigitalOut(PIN_CH3);
00338                 break;
00339                 case 3:
00340                     _d[ch] = new DigitalOut(PIN_CH4);
00341                 break;
00342                 case 4:
00343                     _d[ch] = new DigitalOut(PIN_CH5);
00344                 break;
00345                 case 5:
00346                     _d[ch] = new DigitalOut(PIN_CH6);
00347                 break;
00348                 case 6:
00349                     _d[ch] = new DigitalOut(PIN_CH7);
00350                 break;
00351                 case 7:
00352                     _d[ch] = new DigitalOut(PIN_CH8);
00353                 break;
00354             }
00355             _pwmCh = _pwmCh & ~(1 << ch);
00356             
00357         }
00358     }else{ //pwm is currently disabled on this channel
00359         if(en == 1){
00360             delete _d[ch];
00361             switch(ch){
00362                 case 0:
00363                     _p[ch] = new PwmOut(PIN_CH1);
00364                 break;                     
00365                 case 1:                    
00366                     _p[ch] = new PwmOut(PIN_CH2);
00367                 break;                     
00368                 case 2:                    
00369                     _p[ch] = new PwmOut(PIN_CH3);
00370                 break;                     
00371                 case 3:                    
00372                     _p[ch] = new PwmOut(PIN_CH4);
00373                 break;                     
00374                 case 4:                    
00375                     _p[ch] = new PwmOut(PIN_CH5);
00376                 break;                     
00377                 case 5:                    
00378                     _p[ch] = new PwmOut(PIN_CH6);
00379                 break;                     
00380                 case 6:                    
00381                     _p[ch] = new PwmOut(PIN_CH7);
00382                 break;                     
00383                 case 7:                    
00384                     _p[ch] = new PwmOut(PIN_CH8);
00385                 break;
00386             }
00387             _p[ch]->period(_PWMperiod);
00388             _pwmCh = _pwmCh | (1 << ch);
00389         }
00390     }
00391     
00392 
00393 }
00394 
00395 void BridgeDriver::diagnostic(TextLCD_I2C *lcd){
00396     lcd->setAddress(0,1);
00397     lcd->printf("PWM:%d,%d,%d,%d    ", _pwm[0], _pwm[1], _pwm[2], _pwm[3]);
00398     lcd->setAddress(0,2);
00399     lcd->printf("pCH:%i%i%i%i%i%i%i%i", !!(_pwmCh & (1 << 0)),
00400                                         !!(_pwmCh & (1 << 1)),
00401                                         !!(_pwmCh & (1 << 2)),
00402                                         !!(_pwmCh & (1 << 3)),
00403                                         !!(_pwmCh & (1 << 4)),
00404                                         !!(_pwmCh & (1 << 5)),
00405                                         !!(_pwmCh & (1 << 6)),
00406                                         !!(_pwmCh & (1 << 7))
00407                                         );
00408     lcd->setAddress(0,3);
00409     lcd->printf("dir:%+d,%+d,%+d,%+d    ", _dir[0], _dir[1], _dir[2], _dir[3]);
00410 }
00411 
00412 void BridgeDriver::setPWMperiod(float newPWMperiod){
00413     _PWMperiod = newPWMperiod;
00414 }