1 #include "../hardware_api.h"
5 #include "driver/mcpwm.h"
6 #include "soc/mcpwm_reg.h"
7 #include "soc/mcpwm_struct.h"
10 #define _EMPTY_SLOT -20
11 #define _TAKEN_SLOT -21
15 #define _MCPWM_FREQ 160e6
18 #define _PWM_RES_DEF 2048
20 #define _PWM_RES_MIN 1500
22 #define _PWM_RES_MAX 3000
28 mcpwm_dev_t* mcpwm_num;
29 mcpwm_unit_t mcpwm_unit;
30 mcpwm_operator_t mcpwm_operator;
31 mcpwm_io_signals_t mcpwm_a;
32 mcpwm_io_signals_t mcpwm_b;
33 mcpwm_io_signals_t mcpwm_c;
34 } bldc_3pwm_motor_slots_t;
37 mcpwm_dev_t* mcpwm_num;
38 mcpwm_unit_t mcpwm_unit;
39 mcpwm_operator_t mcpwm_operator1;
40 mcpwm_operator_t mcpwm_operator2;
41 mcpwm_io_signals_t mcpwm_1a;
42 mcpwm_io_signals_t mcpwm_1b;
43 mcpwm_io_signals_t mcpwm_2a;
44 mcpwm_io_signals_t mcpwm_2b;
45 } stepper_4pwm_motor_slots_t;
48 mcpwm_dev_t* mcpwm_num;
49 mcpwm_unit_t mcpwm_unit;
50 mcpwm_operator_t mcpwm_operator;
51 mcpwm_io_signals_t mcpwm_a;
52 mcpwm_io_signals_t mcpwm_b;
53 } stepper_2pwm_motor_slots_t;
57 mcpwm_dev_t* mcpwm_num;
58 mcpwm_unit_t mcpwm_unit;
59 mcpwm_operator_t mcpwm_operator1;
60 mcpwm_operator_t mcpwm_operator2;
61 mcpwm_io_signals_t mcpwm_ah;
62 mcpwm_io_signals_t mcpwm_bh;
63 mcpwm_io_signals_t mcpwm_ch;
64 mcpwm_io_signals_t mcpwm_al;
65 mcpwm_io_signals_t mcpwm_bl;
66 mcpwm_io_signals_t mcpwm_cl;
67 } bldc_6pwm_motor_slots_t;
70 bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = {
71 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A},
72 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B},
73 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A},
74 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}
78 bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = {
79 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B},
80 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B},
84 stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = {
85 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B},
86 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B},
90 stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = {
91 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A},
92 {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B},
93 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A},
94 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B}
100 void _configureTimerFrequency(
long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit,
float dead_zone =
NOT_SET){
102 mcpwm_config_t pwm_config;
103 pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;
104 pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
105 mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config);
106 mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config);
107 mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config);
111 float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone;
112 mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
113 mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
114 mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0);
118 mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
119 mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
120 mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
123 mcpwm_num->clk_cfg.prescale = 0;
127 int prescaler = ceil((
double)_MCPWM_FREQ / (
double)_PWM_RES_DEF / 2.0 / (
double)pwm_frequency) - 1;
132 int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (
double)pwm_frequency / (double)(prescaler + 1);
134 if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 )
135 resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (
double)pwm_frequency / (double)(--prescaler + 1);
136 resolution_corrected =
_constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX);
139 mcpwm_num->timer[0].period.prescale = prescaler;
140 mcpwm_num->timer[1].period.prescale = prescaler;
141 mcpwm_num->timer[2].period.prescale = prescaler;
144 mcpwm_num->timer[0].period.period = resolution_corrected;
145 mcpwm_num->timer[1].period.period = resolution_corrected;
146 mcpwm_num->timer[2].period.period = resolution_corrected;
148 mcpwm_num->timer[0].period.upmethod = 0;
149 mcpwm_num->timer[1].period.upmethod = 0;
150 mcpwm_num->timer[2].period.upmethod = 0;
153 mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
154 mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
155 mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
158 mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0);
159 mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0);
160 mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0);
162 mcpwm_num->timer[0].sync.out_sel = 1;
164 mcpwm_num->timer[0].sync.out_sel = 0;
171 void _configure2PWM(
long pwm_frequency,
const int pinA,
const int pinB) {
173 if(!pwm_frequency || !
_isset(pwm_frequency) ) pwm_frequency = 20000;
174 else pwm_frequency =
_constrain(pwm_frequency, 0, 40000);
176 stepper_2pwm_motor_slots_t m_slot = {};
181 for(slot_num = 0; slot_num < 4; slot_num++){
182 if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){
183 esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA;
184 m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
191 esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
194 esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
196 esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
199 esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
201 esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
204 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
205 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
208 _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
217 void _configure3PWM(
long pwm_frequency,
const int pinA,
const int pinB,
const int pinC) {
219 if(!pwm_frequency || !
_isset(pwm_frequency) ) pwm_frequency = 20000;
220 else pwm_frequency =
_constrain(pwm_frequency, 0, 40000);
222 bldc_3pwm_motor_slots_t m_slot = {};
227 for(slot_num = 0; slot_num < 4; slot_num++){
228 if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){
229 esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
230 m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
236 esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT;
239 esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
241 esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
244 esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
246 esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
249 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
250 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
251 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
254 _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
262 void _configure4PWM(
long pwm_frequency,
const int pinA,
const int pinB,
const int pinC,
const int pinD) {
264 if(!pwm_frequency || !
_isset(pwm_frequency) ) pwm_frequency = 20000;
265 else pwm_frequency =
_constrain(pwm_frequency, 0, 40000);
266 stepper_4pwm_motor_slots_t m_slot = {};
270 for(slot_num = 0; slot_num < 2; slot_num++){
271 if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){
272 esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA;
273 m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
280 esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
281 esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
283 esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT;
284 esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT;
286 esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
289 esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
290 esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
292 esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT;
293 esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT;
295 esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
299 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA);
300 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB);
301 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC);
302 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD);
305 _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit);
314 for(
int i = 0; i < 4; i++){
315 if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){
318 mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0);
319 mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0);
329 void _writeDutyCycle3PWM(
float dc_a,
float dc_b,
float dc_c,
int pinA,
int pinB,
int pinC){
331 for(
int i = 0; i < 4; i++){
332 if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){
335 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);
336 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);
337 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);
347 void _writeDutyCycle4PWM(
float dc_1a,
float dc_1b,
float dc_2a,
float dc_2b,
int pin1A,
int pin1B,
int pin2A,
int pin2B){
349 for(
int i = 0; i < 2; i++){
350 if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){
353 mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0);
354 mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0);
355 mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0);
356 mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0);
365 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){
367 if(!pwm_frequency || !
_isset(pwm_frequency) ) pwm_frequency = 20000;
368 else pwm_frequency =
_constrain(pwm_frequency, 0, 40000);
369 bldc_6pwm_motor_slots_t m_slot = {};
373 for(slot_num = 0; slot_num < 2; slot_num++){
374 if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){
375 esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
376 m_slot = esp32_bldc_6pwm_motor_slots[slot_num];
381 if(slot_num >= 2)
return -1;
386 esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
387 esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
389 esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
392 esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
393 esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
395 esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
399 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h);
400 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l);
401 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h);
402 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l);
403 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h);
404 mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l);
407 _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone);
415 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){
417 for(
int i = 0; i < 2; i++){
418 if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){
421 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0);
422 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0);
423 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0);
424 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0);
425 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0);
426 mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0);