Mitchell Hatfield
frequency change?
Fork of
BridgeDriver
by 
Jason T
Diff: BridgeDriver.cpp
- Revision:
- 0:b1e3fa917367
- Child:
- 1:c8e328389a98
--- /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;
+ }
+ }
+ }
+
+
+}
+
+
+
+