void Esp32ServoController::init() {
// Setup ledc servos
for (byte i = 0; i < LEDC_N_SERVOS; i++) {
byte channel = i;
byte servoPin = ledcServoPins[i];
ledcSetup(channel, 50, 16); // channel X, 50 Hz, 16-bit depth
ledcAttachPin(servoPin, channel); // GPIO servoPin on channel X
ledcWrite(channel, servoMid); // Set initial (midpoint) position
}
// Setup MCPWM servos
// Setup which MCPWM units are linked to which pins
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, mcpwmServoPins[0]);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, mcpwmServoPins[1]);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, mcpwmServoPins[2]);
// Configure MCPWM parameters
mcpwm_config_t pwm_config;
pwm_config.frequency = 50; //frequency = 50Hz, i.e. for every servo motor time period should be 20ms
pwm_config.cmpr_a = MCPWM_DUTY_MID; //duty cycle of PWMxA
pwm_config.cmpr_b = MCPWM_DUTY_MID; //duty cycle of PWMxb
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with same settings
// Set some initial positions
// TODO: surely this shouldn't be required?
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, MCPWM_DUTY_MID);
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, MCPWM_DUTY_MID);
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, MCPWM_DUTY_MID);
}
static void mcpwm_example_gpio_initialize()
{
printf("initializing mcpwm bldc control gpio...\n");
#if MCPWM_GPIO_INIT
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_PWM0A_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, GPIO_PWM0B_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, GPIO_PWM1A_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, GPIO_PWM1B_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, GPIO_PWM2A_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, GPIO_PWM2B_OUT);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, GPIO_CAP0_IN);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_1, GPIO_CAP1_IN);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_2, GPIO_CAP2_IN);
#else
mcpwm_pin_config_t pin_config = {
.mcpwm0a_out_num = GPIO_PWM0A_OUT,
.mcpwm0b_out_num = GPIO_PWM0B_OUT,
.mcpwm1a_out_num = GPIO_PWM1A_OUT,
.mcpwm1b_out_num = GPIO_PWM1B_OUT,
.mcpwm2a_out_num = GPIO_PWM2A_OUT,
.mcpwm2b_out_num = GPIO_PWM2B_OUT,
.mcpwm_cap0_in_num = GPIO_CAP0_IN,
.mcpwm_cap1_in_num = GPIO_CAP1_IN,
.mcpwm_cap2_in_num = GPIO_CAP2_IN,
.mcpwm_sync0_in_num = -1, //Not used
.mcpwm_sync1_in_num = -1, //Not used
.mcpwm_sync2_in_num = -1, //Not used
.mcpwm_fault0_in_num = -1, //Not used
.mcpwm_fault1_in_num = -1, //Not used
.mcpwm_fault2_in_num = -1 //Not used
};
mcpwm_set_pin(MCPWM_UNIT_0, &pin_config);
#endif
gpio_pulldown_en(GPIO_CAP0_IN); //Enable pull down on CAP0 signal
gpio_pulldown_en(GPIO_CAP1_IN); //Enable pull down on CAP1 signal
gpio_pulldown_en(GPIO_CAP2_IN); //Enable pull down on CAP2 signal
}
#if GPIO_HALL_TEST_SIGNAL
/**
* @brief Set gpio 13, 12, 14 as our test signal of hall sensors, that generates high-low waveform continuously
* Attach this pins to GPIO 27, 26, 25 respectively for capture unit
*/
static void gpio_test_signal(void *arg)
{
printf("intializing test signal...\n");
gpio_config_t gp;
gp.intr_type = GPIO_INTR_DISABLE;
gp.mode = GPIO_MODE_OUTPUT;
gp.pin_bit_mask = GPIO_SEL_13 | GPIO_SEL_12 | GPIO_SEL_14;
gpio_config(&gp);
while (1) {
gpio_set_level(GPIO_NUM_13, 1); //Set H1 high
gpio_set_level(GPIO_NUM_12, 0); //Set H2 low
gpio_set_level(GPIO_NUM_14, 1); //Set H3 high
vTaskDelay(1);
gpio_set_level(GPIO_NUM_14, 0); //Set H3 low
vTaskDelay(1);
gpio_set_level(GPIO_NUM_12, 1); //Set H2 high
vTaskDelay(1);
gpio_set_level(GPIO_NUM_13, 0); //Set H1 low
vTaskDelay(1);
gpio_set_level(GPIO_NUM_14, 1); //Set H3 high
vTaskDelay(1);
gpio_set_level(GPIO_NUM_12, 0); //Set H2 high
vTaskDelay(1);
}
}
static void mcpwm_example_gpio_initialize()
{
printf("initializing mcpwm servo control gpio......\n");
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, 18); //Set GPIO 18 as PWM0A, to which servo is connected
}
