Skip to content

Commit 8b44473

Browse files
askuricaskuric
committed
FEAT esp32 and stm32 stable 6PWM configuration
1 parent 878283d commit 8b44473

File tree

5 files changed

+82
-82
lines changed

5 files changed

+82
-82
lines changed

‎src/drivers/BLDCDriver6PWM.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -69,9 +69,9 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {
6969
Uc = _constrain(Uc, -voltage_limit, voltage_limit);
7070
// calculate duty cycle
7171
// limited in [0,1]
72-
float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 );
73-
float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 );
74-
float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 );
72+
float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 );
73+
float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 );
74+
float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 );
7575
// hardware specific writing
7676
// hardware specific function - depending on driver and mcu
7777
_writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);

‎src/drivers/StepperDriver4PWM.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ int StepperDriver4PWM::init() {
5353

5454
// Set the pwm frequency to the pins
5555
// hardware specific function - depending on driver and mcu
56-
_configure4PWM(pwm_frequency, pwm1A, pwm2A, pwm1B, pwm2B);
56+
_configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
5757
return 0;
5858
}
5959

‎src/drivers/hardware_specific/atmega328_mcu.cpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
6666

6767

6868
// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
69-
int _configurePair(int pinH, int pinL) {
69+
int _configureComplementaryPair(int pinH, int pinL) {
7070
if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){
7171
// configure the pwm phase-corrected mode
7272
TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
@@ -101,9 +101,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
101101
// High PWM frequency
102102
// - always max 32kHz
103103
int ret_flag = 0;
104-
ret_flag += _configurePair(pinA_h, pinA_l);
105-
ret_flag += _configurePair(pinB_h, pinB_l);
106-
ret_flag += _configurePair(pinC_h, pinC_l);
104+
ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
105+
ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
106+
ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
107107
return ret_flag; // returns -1 if not well configured
108108
}
109109

‎src/drivers/hardware_specific/esp32_mcu.cpp

Lines changed: 29 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -68,41 +68,27 @@ bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = {
6868
};
6969

7070
// configuring high frequency pwm timer
71+
// a lot of help from this post from Paul Gauld
7172
// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller
72-
void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){
73+
void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){
7374

7475
mcpwm_config_t pwm_config;
7576
pwm_config.frequency = pwm_frequency; //frequency
76-
pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 50.0%
77-
pwm_config.cmpr_b = 0; //duty cycle of PWMxB = 50.0%
7877
pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
7978
pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH
8079
mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
8180
mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings
8281
mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings
8382

84-
_delay(100);
85-
86-
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
87-
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
88-
mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
89-
mcpwm_num->clk_cfg.prescale = 0;
83+
if (dead_zone != NOT_SET){
84+
// dead zone is configured in dead_time x 100 nanoseconds
85+
float dead_time = (float)(1e7 / pwm_frequency) * dead_zone;
86+
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time);
87+
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time);
88+
mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time);
89+
}
9090

91-
mcpwm_num->timer[0].period.prescale = 4;
92-
mcpwm_num->timer[1].period.prescale = 4;
93-
mcpwm_num->timer[2].period.prescale = 4;
94-
_delay(1);
95-
mcpwm_num->timer[0].period.period = 2048;
96-
mcpwm_num->timer[1].period.period = 2048;
97-
mcpwm_num->timer[2].period.period = 2048;
98-
_delay(1);
99-
mcpwm_num->timer[0].period.upmethod = 0;
100-
mcpwm_num->timer[1].period.upmethod = 0;
101-
mcpwm_num->timer[2].period.upmethod = 0;
102-
_delay(1);
103-
mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
104-
mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
105-
mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
91+
_delay(100);
10692

10793
mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0);
10894
mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0);
@@ -121,7 +107,7 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm
121107
void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
122108

123109
if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency
124-
else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency
110+
else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency
125111

126112
bldc_3pwm_motor_slots_t m_slot = {};
127113

@@ -164,7 +150,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int
164150
void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
165151

166152
if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency
167-
else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency
153+
else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency
168154
stepper_motor_slots_t m_slot = {};
169155
// determine which motor are we connecting
170156
// and set the appropriate configuration parameters
@@ -210,10 +196,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB
210196
for(int i = 0; i < 4; i++){
211197
if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found
212198
// se the PWM on the slot timers
213-
// transform duty cycle from [0,1] to [0,2047]
214-
esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047;
215-
esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047;
216-
esp32_bldc_3pwm_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_3pwm_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047;
199+
// transform duty cycle from [0,1] to [0,100]
200+
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
201+
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
202+
mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0);
217203
break;
218204
}
219205
}
@@ -228,11 +214,11 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
228214
for(int i = 0; i < 2; i++){
229215
if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found
230216
// se the PWM on the slot timers
231-
// transform duty cycle from [0,1] to [0,2047]
232-
esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1a*2047;
233-
esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1b*2047;
234-
esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2a*2047;
235-
esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2b*2047;
217+
// transform duty cycle from [0,1] to [0,100]
218+
mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1a*100.0);
219+
mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1b*100.0);
220+
mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2a*100.0);
221+
mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2b*100.0);
236222
break;
237223
}
238224
}
@@ -257,7 +243,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
257243
}
258244
}
259245
// if no slots available
260-
if(!m_slot.mcpwm_unit) return -1;
246+
if(slot_num >= 2) return -1;
261247

262248
// disable all the slots with the same MCPWM
263249
if( slot_num == 0 ){
@@ -283,7 +269,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
283269
mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l);
284270

285271
// configure the timer
286-
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
272+
_configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone);
287273
// return
288274
return 0;
289275
}
@@ -297,18 +283,12 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i
297283
if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
298284
// se the PWM on the slot timers
299285
// transform duty cycle from [0,1] to [0,100.0]
300-
float dc_ah = _constrain(dc_a - dead_zone/2.0 , 0, 1)*100.0;
301-
float dc_bh = _constrain(dc_b - dead_zone/2.0 , 0, 1)*100.0;
302-
float dc_ch = _constrain(dc_c - dead_zone/2.0 , 0, 1)*100.0;
303-
float dc_al = _constrain(dc_a + dead_zone/2.0 , 0, 1)*100.0;
304-
float dc_bl = _constrain(dc_b + dead_zone/2.0 , 0, 1)*100.0;
305-
float dc_cl = _constrain(dc_c + dead_zone/2.0 , 0, 1)*100.0;
306-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_ah);
307-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_al);
308-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_bh);
309-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_bl);
310-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_ch);
311-
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_cl);
286+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100);
287+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100);
288+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100);
289+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100);
290+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100);
291+
mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100);
312292
break;
313293
}
314294
}

‎src/drivers/hardware_specific/stm32_mcu.cpp

Lines changed: 45 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
6565
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
6666
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
6767
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
68-
TIM_OC_InitTypeDef sConfigOC = {0};
68+
TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
6969
sConfigOC.OCMode = TIM_OCMODE_PWM2;
7070
sConfigOC.Pulse = 100;
7171
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
@@ -117,7 +117,7 @@ void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,Ha
117117
HT4->resume();
118118
}
119119

120-
HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, int dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
120+
HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
121121
{
122122
PinName uhPinName = digitalPinToPinName(pinA_h);
123123
PinName ulPinName = digitalPinToPinName(pinA_l);
@@ -144,8 +144,11 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, int dead_zone, int
144144
HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
145145
HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
146146
HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
147-
148-
LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, 100); // deadtime is non linear!
147+
148+
// dead time is set in nanoseconds
149+
uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone;
150+
uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
151+
LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
149152
LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
150153

151154
HT->pause();
@@ -237,35 +240,52 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in
237240
// - hardware specific
238241
int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
239242
if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
240-
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max
243+
else pwm_frequency = _constrain(pwm_frequency, 0, 100000); // constrain to 100kHz max
241244

245+
// find configuration
242246
int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
243-
244-
if(config == _HARDWARE_6PWM){
245-
_initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
246-
}else if(config == _SOFTWARE_6PWM){
247-
HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
248-
_initPinPWMLow(pwm_frequency, pinA_l);
249-
HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
250-
_initPinPWMLow(pwm_frequency, pinB_l);
251-
HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
252-
_initPinPWMLow(pwm_frequency, pinC_l);
253-
_alignPWMTimers(HT1, HT2, HT3);
254-
}else{
255-
return -1;
247+
// configure accordingly
248+
switch(config){
249+
case _ERROR_6PWM:
250+
return -1; // fail
251+
break;
252+
case _HARDWARE_6PWM:
253+
_initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
254+
break;
255+
case _SOFTWARE_6PWM:
256+
HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
257+
_initPinPWMLow(pwm_frequency, pinA_l);
258+
HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
259+
_initPinPWMLow(pwm_frequency, pinB_l);
260+
HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
261+
_initPinPWMLow(pwm_frequency, pinC_l);
262+
_alignPWMTimers(HT1, HT2, HT3);
263+
break;
256264
}
257-
return 0;
265+
return 0; // success
258266
}
259267

260268
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
261269
// - BLDC driver - 6PWM setting
262270
// - hardware specific
263271
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){
264-
_setPwm(pinA_l, _constrain(dc_a + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM
265-
_setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
266-
_setPwm(pinB_l, _constrain(dc_b + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM
267-
_setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
268-
_setPwm(pinC_l, _constrain(dc_c + dead_zone,0,1)*_PWM_RANGE, _PWM_RESOLUTION); // not taking effect for _HARDWARE_6PWM
269-
_setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
272+
// find configuration
273+
int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
274+
// set pwm accordingly
275+
switch(config){
276+
case _HARDWARE_6PWM:
277+
_setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
278+
_setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
279+
_setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
280+
break;
281+
case _SOFTWARE_6PWM:
282+
_setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
283+
_setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
284+
_setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
285+
_setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
286+
_setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
287+
_setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
288+
break;
289+
}
270290
}
271291
#endif

0 commit comments

Comments
 (0)