Mitchell Hatfield / BridgeDriver

- Don't think anything was changed or updated.... simply viewed the class

Dependents:   Component_Test_Interface

Fork of BridgeDriver by Jason T

BridgeDriver.cpp@15:59789a7f1ec1, 2014-11-07 (annotated)

Committer:
mehatfie
Date:
Fri Nov 07 23:35:34 2014 +0000
Revision:
15:59789a7f1ec1
Parent:
8:36e2cd31ccf3 
what is wrong.... everything was going so so well....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jason701802 0:b1e3fa917367 1
jason701802 0:b1e3fa917367 2 #include "BridgeDriver.h"
jason701802 0:b1e3fa917367 3 #include "mbed.h"
jason701802 1:c8e328389a98 4 #include "TextLCD.h" // if using diagnostic
jason701802 0:b1e3fa917367 5
jason701802 0:b1e3fa917367 6 #include "MCP23017.h"
jason701802 0:b1e3fa917367 7
jason701802 0:b1e3fa917367 8
jason701802 1:c8e328389a98 9 BridgeDriver::BridgeDriver( I2C *i2c, uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD, uint8_t enAddr, uint8_t ledAddr) :
jason701802 0:b1e3fa917367 10 _i2c(i2c),
jason701802 0:b1e3fa917367 11 _enAddr(enAddr),
jason701802 0:b1e3fa917367 12 _ledAddr(ledAddr),
jason701802 1:c8e328389a98 13 _pwmCh(0x00),
jason701802 8:36e2cd31ccf3 14 _oldLedState(0x00),
jason701802 8:36e2cd31ccf3 15 _PWMperiod(DEFAULT_PWM_PERIOD)
jason701802 0:b1e3fa917367 16 {
jason701802 0:b1e3fa917367 17
jason701802 0:b1e3fa917367 18
jason701802 0:b1e3fa917367 19 _pwm[0] = enPwmA;
jason701802 0:b1e3fa917367 20 _pwm[1] = enPwmB;
jason701802 0:b1e3fa917367 21 _pwm[2] = enPwmC;
jason701802 0:b1e3fa917367 22 _pwm[3] = enPwmD;
jason701802 0:b1e3fa917367 23
jason701802 0:b1e3fa917367 24 for(int i = 0; i<4; i++){
jason701802 0:b1e3fa917367 25 _dir[i] = 0;
jason701802 0:b1e3fa917367 26 _braking[i] = 0;
jason701802 0:b1e3fa917367 27 }
jason701802 0:b1e3fa917367 28
jason701802 0:b1e3fa917367 29
jason701802 0:b1e3fa917367 30 _EnCtl = new MCP23017(*_i2c, _enAddr); //MCP23008 -Only use PORT_A
jason701802 0:b1e3fa917367 31 _IO = new MCP23017(*_i2c, _ledAddr);
jason701802 0:b1e3fa917367 32
jason701802 0:b1e3fa917367 33 _IO->direction(PORT_B, 0x00); //LEDs are outputs
jason701802 0:b1e3fa917367 34 _IO->write(PORT_B, ~0x00); //bitwise not (~) b/c leds are active low
jason701802 0:b1e3fa917367 35 _EnCtl->configureBanked(BNK);
jason701802 0:b1e3fa917367 36 _EnCtl->direction(PORT_A, 0x00); //all outputs
jason701802 0:b1e3fa917367 37 _EnCtl->write(PORT_A, 0x00); //leave all channels disabled
jason701802 0:b1e3fa917367 38
jason701802 0:b1e3fa917367 39
jason701802 0:b1e3fa917367 40
jason701802 0:b1e3fa917367 41 _d[0] = new DigitalOut(PIN_CH1);
jason701802 0:b1e3fa917367 42 _d[1] = new DigitalOut(PIN_CH2);
jason701802 0:b1e3fa917367 43 _d[2] = new DigitalOut(PIN_CH3);
jason701802 0:b1e3fa917367 44 _d[3] = new DigitalOut(PIN_CH4);
jason701802 0:b1e3fa917367 45 _d[4] = new DigitalOut(PIN_CH5);
jason701802 0:b1e3fa917367 46 _d[5] = new DigitalOut(PIN_CH6);
jason701802 0:b1e3fa917367 47 _d[6] = new DigitalOut(PIN_CH7);
jason701802 0:b1e3fa917367 48 _d[7] = new DigitalOut(PIN_CH8);
jason701802 0:b1e3fa917367 49
jason701802 0:b1e3fa917367 50
jason701802 0:b1e3fa917367 51
jason701802 0:b1e3fa917367 52 }
jason701802 6:d4afd3284a04 53
jason701802 0:b1e3fa917367 54 BridgeDriver::~BridgeDriver(){
jason701802 0:b1e3fa917367 55 for(int i=0; i<4; i++){
jason701802 0:b1e3fa917367 56 if(_pwm[i]){
jason701802 0:b1e3fa917367 57 switch(_dir[i]){
jason701802 0:b1e3fa917367 58 case 1:
jason701802 0:b1e3fa917367 59 delete _p[2*i];
jason701802 0:b1e3fa917367 60 delete _d[2*i + 1];
jason701802 0:b1e3fa917367 61 break;
jason701802 0:b1e3fa917367 62 case -1:
jason701802 0:b1e3fa917367 63 delete _d[2*i];
jason701802 0:b1e3fa917367 64 delete _p[2*i + 1];
jason701802 0:b1e3fa917367 65 break;
jason701802 0:b1e3fa917367 66 case 0:
jason701802 0:b1e3fa917367 67 delete _d[2*i];
jason701802 0:b1e3fa917367 68 delete _d[2*i + 1];
jason701802 0:b1e3fa917367 69 break;
jason701802 0:b1e3fa917367 70 }
jason701802 0:b1e3fa917367 71 }else{
jason701802 0:b1e3fa917367 72 delete _d[2*i];
jason701802 0:b1e3fa917367 73 delete _d[2*i + 1];
jason701802 0:b1e3fa917367 74 }
jason701802 0:b1e3fa917367 75 }
jason701802 0:b1e3fa917367 76 }
jason701802 0:b1e3fa917367 77
jason701802 0:b1e3fa917367 78 void BridgeDriver::enablePwm(uint8_t enPwmA, uint8_t enPwmB, uint8_t enPwmC, uint8_t enPwmD){
jason701802 7:7dabd934ebe4 79 enablePwm(MOTOR_A, enPwmA);
jason701802 7:7dabd934ebe4 80 enablePwm(MOTOR_B, enPwmB);
jason701802 7:7dabd934ebe4 81 enablePwm(MOTOR_C, enPwmC);
jason701802 7:7dabd934ebe4 82 enablePwm(MOTOR_D, enPwmD);
jason701802 0:b1e3fa917367 83 }
jason701802 6:d4afd3284a04 84
jason701802 7:7dabd934ebe4 85 void BridgeDriver::enablePwm(Motors motor, uint8_t enPwm){
jason701802 7:7dabd934ebe4 86 int bNum = static_cast<int>(motor); //numeric motor
jason701802 0:b1e3fa917367 87 if(enPwm == _pwm[bNum]){
jason701802 0:b1e3fa917367 88 return;
jason701802 0:b1e3fa917367 89 }else if(enPwm == 0){ //disable pwm
jason701802 0:b1e3fa917367 90 setPwm(2*bNum, 0); //channels are disabled in setPwm()
jason701802 0:b1e3fa917367 91 setPwm(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 92 _pwm[bNum] = 0;
jason701802 0:b1e3fa917367 93 }else{ //enable pwm
jason701802 0:b1e3fa917367 94 enableCh(2*bNum, 0); //disable channels
jason701802 0:b1e3fa917367 95 enableCh(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 96 _pwm[bNum] = 1; //pwm for individual channels is turned on as necessary when running motor
jason701802 0:b1e3fa917367 97 _dir[bNum] = 0;
jason701802 0:b1e3fa917367 98 }
jason701802 0:b1e3fa917367 99 }
jason701802 0:b1e3fa917367 100
jason701802 0:b1e3fa917367 101 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
jason701802 7:7dabd934ebe4 102 enableBraking(MOTOR_A, enBrakeA);
jason701802 7:7dabd934ebe4 103 enableBraking(MOTOR_B, enBrakeB);
jason701802 7:7dabd934ebe4 104 enableBraking(MOTOR_C, enBrakeC);
jason701802 7:7dabd934ebe4 105 enableBraking(MOTOR_D, enBrakeD);
jason701802 0:b1e3fa917367 106 }
jason701802 6:d4afd3284a04 107
jason701802 7:7dabd934ebe4 108 void BridgeDriver::enableBraking(Motors motor, uint8_t enBrake) {
jason701802 7:7dabd934ebe4 109 _braking[static_cast<int>(motor)] = enBrake;
jason701802 0:b1e3fa917367 110 }
jason701802 0:b1e3fa917367 111
jason701802 0:b1e3fa917367 112 int BridgeDriver::forceBrake(uint8_t ch){ //force a specific channel to GND without changing braking default
jason701802 0:b1e3fa917367 113 if( _pwm[(ch-1)/2]){
jason701802 0:b1e3fa917367 114 return -1;
jason701802 0:b1e3fa917367 115 }else{
jason701802 0:b1e3fa917367 116 *_d[ch-1] = 0;
jason701802 0:b1e3fa917367 117 enableCh(ch-1, 1);
jason701802 0:b1e3fa917367 118 setLed(ch-1, 0);
jason701802 0:b1e3fa917367 119 return 0;
jason701802 0:b1e3fa917367 120 }
jason701802 0:b1e3fa917367 121 }
jason701802 6:d4afd3284a04 122
jason701802 7:7dabd934ebe4 123 int BridgeDriver::forceBrake(Motors motor){ //force a specific motor to GND without changing braking default
jason701802 7:7dabd934ebe4 124 return static_cast<int>(drive(motor, 0, 0));
jason701802 0:b1e3fa917367 125 }
jason701802 6:d4afd3284a04 126
jason701802 0:b1e3fa917367 127 int BridgeDriver::forceFloat(uint8_t ch){ //force a specific channel to float without changing braking default
jason701802 7:7dabd934ebe4 128 int bNum = (ch-1)/2;
jason701802 7:7dabd934ebe4 129 if( _pwm[bNum] ){
jason701802 7:7dabd934ebe4 130 setPwm(2*bNum, 0); //channel is disabled in setPwm()
jason701802 7:7dabd934ebe4 131 setPwm(2*bNum + 1, 0); //channel is disabled in setPwm()
jason701802 7:7dabd934ebe4 132 _dir[bNum] = 0;
jason701802 7:7dabd934ebe4 133 setLed(2*bNum, 0);
jason701802 7:7dabd934ebe4 134 setLed(2*bNum + 1, 0);
jason701802 7:7dabd934ebe4 135 *_d[2*bNum] = 0;
jason701802 7:7dabd934ebe4 136 return *_d[2*bNum + 1] = 0;
jason701802 0:b1e3fa917367 137 }else{
jason701802 0:b1e3fa917367 138 enableCh(ch-1, 0);
jason701802 0:b1e3fa917367 139 setLed(ch-1, 0);
jason701802 0:b1e3fa917367 140 return *_d[ch-1] = 0;
jason701802 0:b1e3fa917367 141 }
jason701802 0:b1e3fa917367 142 }
jason701802 6:d4afd3284a04 143
jason701802 7:7dabd934ebe4 144 int BridgeDriver::forceFloat(Motors motor){ //force a specific motor to float without changing braking default
jason701802 7:7dabd934ebe4 145 int bNum = static_cast<int>(motor); //numeric motor
jason701802 0:b1e3fa917367 146
jason701802 0:b1e3fa917367 147 if(_pwm[bNum] == 0){
jason701802 0:b1e3fa917367 148 return -1;
jason701802 0:b1e3fa917367 149 }else{
jason701802 4:87768972db3f 150 setPwm(2*bNum, 0); //channel is disabled in setPwm()
jason701802 4:87768972db3f 151 setPwm(2*bNum + 1, 0); //channel is disabled in setPwm()
jason701802 4:87768972db3f 152 _dir[bNum] = 0;
jason701802 0:b1e3fa917367 153 setLed(2*bNum, 0);
jason701802 0:b1e3fa917367 154 setLed(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 155 *_d[2*bNum] = 0;
jason701802 0:b1e3fa917367 156 return *_d[2*bNum + 1] = 0;
jason701802 0:b1e3fa917367 157 }
jason701802 0:b1e3fa917367 158 }
jason701802 0:b1e3fa917367 159
jason701802 0:b1e3fa917367 160 int BridgeDriver::drive(uint8_t state){
jason701802 0:b1e3fa917367 161 if(_pwm[0] || _pwm[1] || _pwm[2] || _pwm[3]){
jason701802 0:b1e3fa917367 162 return -1;
jason701802 0:b1e3fa917367 163 }else{
jason701802 0:b1e3fa917367 164 setLed(state);
jason701802 0:b1e3fa917367 165 for(int i=0; i<8; i++){
jason701802 0:b1e3fa917367 166 if(state & (1 << i)){ //channel i should be on
jason701802 0:b1e3fa917367 167 *_d[i] = 1;
jason701802 0:b1e3fa917367 168 enableCh(i, 1);
jason701802 0:b1e3fa917367 169 }else{ //channel i should be off
jason701802 3:247939286d2c 170 if(_braking[i/2]){
jason701802 0:b1e3fa917367 171 *_d[i] = 0; //brake output
jason701802 0:b1e3fa917367 172 enableCh(i, 1);
jason701802 0:b1e3fa917367 173 }else{
jason701802 0:b1e3fa917367 174 enableCh(i, 0);
jason701802 4:87768972db3f 175 *_d[i] = 0; //float/coast output
jason701802 0:b1e3fa917367 176 }
jason701802 0:b1e3fa917367 177 }
jason701802 0:b1e3fa917367 178 }
jason701802 0:b1e3fa917367 179 return !!state;
jason701802 0:b1e3fa917367 180 }
jason701802 0:b1e3fa917367 181 }
jason701802 6:d4afd3284a04 182
jason701802 0:b1e3fa917367 183 int BridgeDriver::drive(uint8_t ch, uint8_t on){
jason701802 0:b1e3fa917367 184 if( _pwm[(ch-1)/2]){
jason701802 0:b1e3fa917367 185 return -1;
jason701802 0:b1e3fa917367 186 }else if(on){ //on
jason701802 4:87768972db3f 187 *_d[ch-1] = 1;
jason701802 0:b1e3fa917367 188 enableCh(ch-1, 1);
jason701802 0:b1e3fa917367 189 setLed(ch-1, 1);
jason701802 4:87768972db3f 190 return 1;
jason701802 4:87768972db3f 191 }else if(_braking[(ch-1)/2]){ //off, brake
jason701802 0:b1e3fa917367 192 *_d[ch-1] = 0;
jason701802 0:b1e3fa917367 193 enableCh(ch-1, 1);
jason701802 0:b1e3fa917367 194 setLed(ch-1, 0);
jason701802 0:b1e3fa917367 195 return 0;
jason701802 0:b1e3fa917367 196 }else{ //off, float
jason701802 0:b1e3fa917367 197 enableCh(ch-1, 0);
jason701802 0:b1e3fa917367 198 setLed(ch-1, 0);
jason701802 0:b1e3fa917367 199 return *_d[ch-1] = 0;
jason701802 0:b1e3fa917367 200 }
jason701802 0:b1e3fa917367 201 }
jason701802 6:d4afd3284a04 202
jason701802 7:7dabd934ebe4 203 float BridgeDriver::drive(Motors motor, float speed){ //speed from -1 to 1, speed of 0 will coast or brake depending on setting of _braking
jason701802 7:7dabd934ebe4 204 int bNum = static_cast<int>(motor); //numeric motor
jason701802 0:b1e3fa917367 205
jason701802 0:b1e3fa917367 206 if(_pwm[bNum] == 0){
jason701802 1:c8e328389a98 207 return -20 - bNum;
jason701802 0:b1e3fa917367 208 }
jason701802 0:b1e3fa917367 209
jason701802 0:b1e3fa917367 210 if(speed == 0){
jason701802 7:7dabd934ebe4 211 return drive(motor, 1, 0);
jason701802 0:b1e3fa917367 212 }else if(speed > 0){
jason701802 7:7dabd934ebe4 213 return drive(motor, 1, speed);
jason701802 0:b1e3fa917367 214 }else{
jason701802 7:7dabd934ebe4 215 return drive(motor, -1, -1*speed);
jason701802 0:b1e3fa917367 216 }
jason701802 0:b1e3fa917367 217 }
jason701802 6:d4afd3284a04 218
jason701802 7:7dabd934ebe4 219 float BridgeDriver::drive(Motors motor, int8_t dir, float speed){
jason701802 0:b1e3fa917367 220 //dir: 1=fwd, -1=rev, 0=brake (regardless of setting of _braking
jason701802 0:b1e3fa917367 221 //speed from 0 to 1, speed of 0 will coast or brake depending on setting of _braking
jason701802 0:b1e3fa917367 222
jason701802 7:7dabd934ebe4 223 int bNum = static_cast<uint8_t>(motor); //numeric motor
jason701802 0:b1e3fa917367 224
jason701802 0:b1e3fa917367 225 if(_pwm[bNum] == 0){
jason701802 1:c8e328389a98 226 return -10 - bNum;
jason701802 0:b1e3fa917367 227 }
jason701802 0:b1e3fa917367 228 if(speed < -1 || speed > 1){ //maybe this should be bounding instead?
jason701802 0:b1e3fa917367 229 return -1;
jason701802 0:b1e3fa917367 230 }
jason701802 0:b1e3fa917367 231
jason701802 0:b1e3fa917367 232 if(dir == 0 || (speed == 0 && _braking[bNum])){ //brake
jason701802 0:b1e3fa917367 233 if(_dir[bNum] != 0){
jason701802 0:b1e3fa917367 234 setPwm(2*bNum, 0);
jason701802 0:b1e3fa917367 235 setPwm(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 236 _dir[bNum] = 0;
jason701802 0:b1e3fa917367 237 }
jason701802 0:b1e3fa917367 238 *_d[2*bNum] = 0;
jason701802 0:b1e3fa917367 239 *_d[2*bNum + 1] = 0;
jason701802 0:b1e3fa917367 240 enableCh(2*bNum, 1); //enable channels for braking
jason701802 0:b1e3fa917367 241 enableCh(2*bNum + 1, 1);
jason701802 4:87768972db3f 242 setLed(2*bNum, 0);
jason701802 4:87768972db3f 243 setLed(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 244
jason701802 0:b1e3fa917367 245 return 0;
jason701802 0:b1e3fa917367 246
jason701802 0:b1e3fa917367 247 }else if(speed == 0){ //coast
jason701802 4:87768972db3f 248 setPwm(2*bNum, 0); //channel is disabled in setPwm()
jason701802 4:87768972db3f 249 setPwm(2*bNum + 1, 0); //channel is disabled in setPwm()
jason701802 4:87768972db3f 250 _dir[bNum] = 0;
jason701802 0:b1e3fa917367 251 setLed(2*bNum, 0);
jason701802 0:b1e3fa917367 252 setLed(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 253 *_d[2*bNum] = 0;
jason701802 0:b1e3fa917367 254 return *_d[2*bNum + 1] = 0;
jason701802 4:87768972db3f 255 }else if(dir == 1){ //forward
jason701802 0:b1e3fa917367 256 if(_dir[bNum] != 1){
jason701802 0:b1e3fa917367 257 setPwm(2*bNum, 1);
jason701802 0:b1e3fa917367 258 setPwm(2*bNum + 1, 0);
jason701802 1:c8e328389a98 259 _dir[bNum] = 1;
jason701802 0:b1e3fa917367 260 }
jason701802 0:b1e3fa917367 261 *_p[2*bNum] = speed;
jason701802 0:b1e3fa917367 262 *_d[2*bNum + 1] = 0;
jason701802 0:b1e3fa917367 263 enableCh(2*bNum, 1); //enable channels
jason701802 0:b1e3fa917367 264 enableCh(2*bNum + 1, 1);
jason701802 4:87768972db3f 265 setLed(2*bNum, 1);
jason701802 4:87768972db3f 266 setLed(2*bNum + 1, 0);
jason701802 0:b1e3fa917367 267 return speed;
jason701802 4:87768972db3f 268 }else if(dir == -1){ //reverse
jason701802 1:c8e328389a98 269 if(_dir[bNum] != -1){
jason701802 0:b1e3fa917367 270 setPwm(2*bNum, 0);
jason701802 0:b1e3fa917367 271 setPwm(2*bNum + 1, 1);
jason701802 1:c8e328389a98 272 _dir[bNum] = -1;
jason701802 0:b1e3fa917367 273 }
jason701802 0:b1e3fa917367 274 *_d[2*bNum] = 0;
jason701802 0:b1e3fa917367 275 *_p[2*bNum + 1] = speed;
jason701802 0:b1e3fa917367 276 enableCh(2*bNum, 1); //enable channels
jason701802 0:b1e3fa917367 277 enableCh(2*bNum + 1, 1);
jason701802 4:87768972db3f 278 setLed(2*bNum, 0);
jason701802 4:87768972db3f 279 setLed(2*bNum + 1, 1);
jason701802 0:b1e3fa917367 280 return speed;
jason701802 0:b1e3fa917367 281 }
jason701802 1:c8e328389a98 282 return -2;
jason701802 0:b1e3fa917367 283 }
jason701802 0:b1e3fa917367 284
jason701802 0:b1e3fa917367 285 void BridgeDriver::enableCh(uint8_t ch, uint8_t en){
jason701802 0:b1e3fa917367 286 static uint8_t state = 0x00;
jason701802 0:b1e3fa917367 287 static uint8_t oldState = 0x00;
jason701802 0:b1e3fa917367 288
jason701802 0:b1e3fa917367 289 if(en){
jason701802 0:b1e3fa917367 290 state = state | (1 << ch);
jason701802 0:b1e3fa917367 291 }else{
jason701802 0:b1e3fa917367 292 state = state & ~(1 << ch);
jason701802 0:b1e3fa917367 293 }
jason701802 0:b1e3fa917367 294
jason701802 0:b1e3fa917367 295 if(state != oldState){
jason701802 0:b1e3fa917367 296 _EnCtl->write(PORT_A, state);
jason701802 0:b1e3fa917367 297 oldState = state;
jason701802 0:b1e3fa917367 298 }
jason701802 0:b1e3fa917367 299 }
jason701802 0:b1e3fa917367 300
jason701802 0:b1e3fa917367 301 void BridgeDriver::setLed(uint8_t ch, uint8_t en){
jason701802 0:b1e3fa917367 302 uint8_t state;
jason701802 0:b1e3fa917367 303 if(en){
jason701802 0:b1e3fa917367 304 state = _oldLedState | (1 << ch);
jason701802 0:b1e3fa917367 305 }else{
jason701802 0:b1e3fa917367 306 state = _oldLedState & ~(1 << ch);
jason701802 0:b1e3fa917367 307 }
jason701802 0:b1e3fa917367 308
jason701802 0:b1e3fa917367 309 if(state != _oldLedState){
jason701802 0:b1e3fa917367 310 _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low
jason701802 0:b1e3fa917367 311 _oldLedState = state;
jason701802 0:b1e3fa917367 312 }
jason701802 0:b1e3fa917367 313 }
jason701802 0:b1e3fa917367 314
jason701802 0:b1e3fa917367 315 void BridgeDriver::setLed(uint8_t state){
jason701802 0:b1e3fa917367 316 if(state != _oldLedState){
jason701802 0:b1e3fa917367 317 _IO->write(PORT_B, ~state); //bitwise not (~) b/c leds are active low
jason701802 0:b1e3fa917367 318 _oldLedState = state;
jason701802 0:b1e3fa917367 319 }
jason701802 0:b1e3fa917367 320 }
jason701802 0:b1e3fa917367 321
jason701802 0:b1e3fa917367 322 void BridgeDriver::setPwm(uint8_t ch, uint8_t en){
jason701802 0:b1e3fa917367 323 enableCh(ch, 0);
jason701802 0:b1e3fa917367 324
jason701802 1:c8e328389a98 325
jason701802 1:c8e328389a98 326 if(_pwmCh & (1 << ch)){ //pwm is currently enabled on this channel
jason701802 0:b1e3fa917367 327 if(en == 0){
jason701802 0:b1e3fa917367 328 delete _p[ch];
jason701802 0:b1e3fa917367 329 switch(ch){
jason701802 1:c8e328389a98 330 case 0:
jason701802 1:c8e328389a98 331 _d[ch] = new DigitalOut(PIN_CH1);
jason701802 1:c8e328389a98 332 break;
jason701802 1:c8e328389a98 333 case 1:
jason701802 1:c8e328389a98 334 _d[ch] = new DigitalOut(PIN_CH2);
jason701802 1:c8e328389a98 335 break;
jason701802 1:c8e328389a98 336 case 2:
jason701802 1:c8e328389a98 337 _d[ch] = new DigitalOut(PIN_CH3);
jason701802 1:c8e328389a98 338 break;
jason701802 1:c8e328389a98 339 case 3:
jason701802 1:c8e328389a98 340 _d[ch] = new DigitalOut(PIN_CH4);
jason701802 1:c8e328389a98 341 break;
jason701802 1:c8e328389a98 342 case 4:
jason701802 1:c8e328389a98 343 _d[ch] = new DigitalOut(PIN_CH5);
jason701802 1:c8e328389a98 344 break;
jason701802 1:c8e328389a98 345 case 5:
jason701802 1:c8e328389a98 346 _d[ch] = new DigitalOut(PIN_CH6);
jason701802 1:c8e328389a98 347 break;
jason701802 1:c8e328389a98 348 case 6:
jason701802 1:c8e328389a98 349 _d[ch] = new DigitalOut(PIN_CH7);
jason701802 1:c8e328389a98 350 break;
jason701802 1:c8e328389a98 351 case 7:
jason701802 1:c8e328389a98 352 _d[ch] = new DigitalOut(PIN_CH8);
jason701802 1:c8e328389a98 353 break;
jason701802 0:b1e3fa917367 354 }
jason701802 1:c8e328389a98 355 _pwmCh = _pwmCh & ~(1 << ch);
jason701802 1:c8e328389a98 356
jason701802 0:b1e3fa917367 357 }
jason701802 0:b1e3fa917367 358 }else{ //pwm is currently disabled on this channel
jason701802 0:b1e3fa917367 359 if(en == 1){
jason701802 0:b1e3fa917367 360 delete _d[ch];
jason701802 0:b1e3fa917367 361 switch(ch){
jason701802 1:c8e328389a98 362 case 0:
jason701802 1:c8e328389a98 363 _p[ch] = new PwmOut(PIN_CH1);
jason701802 1:c8e328389a98 364 break;
jason701802 1:c8e328389a98 365 case 1:
jason701802 1:c8e328389a98 366 _p[ch] = new PwmOut(PIN_CH2);
jason701802 1:c8e328389a98 367 break;
jason701802 1:c8e328389a98 368 case 2:
jason701802 1:c8e328389a98 369 _p[ch] = new PwmOut(PIN_CH3);
jason701802 1:c8e328389a98 370 break;
jason701802 1:c8e328389a98 371 case 3:
jason701802 1:c8e328389a98 372 _p[ch] = new PwmOut(PIN_CH4);
jason701802 1:c8e328389a98 373 break;
jason701802 1:c8e328389a98 374 case 4:
jason701802 1:c8e328389a98 375 _p[ch] = new PwmOut(PIN_CH5);
jason701802 1:c8e328389a98 376 break;
jason701802 1:c8e328389a98 377 case 5:
jason701802 1:c8e328389a98 378 _p[ch] = new PwmOut(PIN_CH6);
jason701802 1:c8e328389a98 379 break;
jason701802 1:c8e328389a98 380 case 6:
jason701802 1:c8e328389a98 381 _p[ch] = new PwmOut(PIN_CH7);
jason701802 1:c8e328389a98 382 break;
jason701802 1:c8e328389a98 383 case 7:
jason701802 1:c8e328389a98 384 _p[ch] = new PwmOut(PIN_CH8);
jason701802 1:c8e328389a98 385 break;
jason701802 0:b1e3fa917367 386 }
jason701802 8:36e2cd31ccf3 387 _p[ch]->period(_PWMperiod);
jason701802 1:c8e328389a98 388 _pwmCh = _pwmCh | (1 << ch);
jason701802 0:b1e3fa917367 389 }
jason701802 0:b1e3fa917367 390 }
jason701802 0:b1e3fa917367 391
jason701802 0:b1e3fa917367 392
jason701802 0:b1e3fa917367 393 }
jason701802 0:b1e3fa917367 394
jason701802 1:c8e328389a98 395 void BridgeDriver::diagnostic(TextLCD_I2C *lcd){
jason701802 1:c8e328389a98 396 lcd->setAddress(0,1);
jason701802 1:c8e328389a98 397 lcd->printf("PWM:%d,%d,%d,%d ", _pwm[0], _pwm[1], _pwm[2], _pwm[3]);
jason701802 1:c8e328389a98 398 lcd->setAddress(0,2);
jason701802 1:c8e328389a98 399 lcd->printf("pCH:%i%i%i%i%i%i%i%i", !!(_pwmCh & (1 << 0)),
jason701802 1:c8e328389a98 400 !!(_pwmCh & (1 << 1)),
jason701802 1:c8e328389a98 401 !!(_pwmCh & (1 << 2)),
jason701802 1:c8e328389a98 402 !!(_pwmCh & (1 << 3)),
jason701802 1:c8e328389a98 403 !!(_pwmCh & (1 << 4)),
jason701802 1:c8e328389a98 404 !!(_pwmCh & (1 << 5)),
jason701802 1:c8e328389a98 405 !!(_pwmCh & (1 << 6)),
jason701802 1:c8e328389a98 406 !!(_pwmCh & (1 << 7))
jason701802 1:c8e328389a98 407 );
jason701802 1:c8e328389a98 408 lcd->setAddress(0,3);
jason701802 1:c8e328389a98 409 lcd->printf("dir:%+d,%+d,%+d,%+d ", _dir[0], _dir[1], _dir[2], _dir[3]);
jason701802 8:36e2cd31ccf3 410 }
jason701802 8:36e2cd31ccf3 411
jason701802 8:36e2cd31ccf3 412 void BridgeDriver::setPWMperiod(float newPWMperiod){
jason701802 8:36e2cd31ccf3 413 _PWMperiod = newPWMperiod;
jason701802 1:c8e328389a98 414 }