frequency change?
Fork of BridgeDriver by
BridgeDriver.cpp
- Committer:
- jason701802
- Date:
- 2014-07-24
- Revision:
- 7:7dabd934ebe4
- Parent:
- 6:d4afd3284a04
- Child:
- 8:36e2cd31ccf3
File content as of revision 7:7dabd934ebe4:
#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) { _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(PWM_PERIOD); _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]); }