Mitchell Hatfield / BridgeDriver2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of BridgeDriver by Jason T

BridgeDriver.cpp@13:6561ddb98d6e, 2014-11-10 (annotated)

Committer:
mehatfie
Date:
Mon Nov 10 22:55:46 2014 +0000
Revision:
13:6561ddb98d6e
Parent:
8:36e2cd31ccf3 
don't know

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 }