Skip to content

Updated MCPWM Implementation for servo control to support ESP-IDF 5.0 (#90) #94

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
purviyeshi wants to merge 13 commits into
base: main
Choose a base branch
from
Open

Updated MCPWM Implementation for servo control to support ESP-IDF 5.0 (#90) #94

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
e668d21
start porting servo.c code into esp idf v5.1
purviyeshi Apr 5, 2024
5505b86
start porting servo.h code into esp idf v5.1
purviyeshi Apr 5, 2024
47fee7a
start porting servo.c code into esp idf v5.1
purviyeshi Apr 5, 2024
ad77332
Port servo.c code into esp idf v5.1
purviyeshi Apr 7, 2024
70fa202
Port servo.h code into esp idf v5.1
purviyeshi Apr 7, 2024
98e2a57
modify servo.c code to identify generators for 4 servos
purviyeshi Apr 11, 2024
c15ace8
add separate comparator for each servo
purviyeshi Apr 13, 2024
4dbfa51
remove cmp_num variable and #endif line
purviyeshi Apr 13, 2024
3606958
correct a silly mistake
purviyeshi Apr 13, 2024
994cef4
Merge branch 'SRA-VJTI:main' into main
purviyeshi Apr 13, 2024
a3144e5
Add tested code
purviyeshi Apr 13, 2024
cc7b4ea
Add tested code
purviyeshi Apr 13, 2024
a763759
add license which was removed
purviyeshi Apr 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 5 additions & 7 deletions include/servo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,9 @@

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "driver/mcpwm.h"
#include "soc/mcpwm_periph.h"
#include "esp_attr.h"
#include "driver/mcpwm_prelude.h"


#include "sdkconfig.h"
#include "esp_log.h"
Expand All @@ -43,11 +43,11 @@ typedef struct
int max_pulse_width;
int max_degree;
int angle;
mcpwm_unit_t mcpwm_num;
mcpwm_timer_t timer_num;
mcpwm_generator_t gen;
} servo_config;




/** @struct servo_config
* @brief This structure contains the configuration of servos
* @var servo_config::servo_pin
Expand Down Expand Up @@ -87,5 +87,3 @@ esp_err_t set_angle_servo(servo_config *config, unsigned int degree_of_rotation)
* @return esp_err_t
*/
int read_servo(servo_config *config);

#endif
207 changes: 162 additions & 45 deletions src/servo.c
View file
Open in desktop

@SuperChamp234 SuperChamp234 Apr 12, 2024

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There is a lot of repetitive logic in this code. Can’t we just create an object of the servo for three different GPIOs? Hardcoding this seems unnecessary.

Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
/*
/
* MIT License
*
* Copyright (c) 2021 Society of Robotics and Automation
Expand Down Expand Up @@ -26,58 +26,180 @@

static const char *TAG_SERVO = "servo";
static int enabled_servo_flag = 0;
#define STR(A) #A

#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick
#define SERVO_TIMEBASE_PERIOD 20000 // 20000 ticks, 20ms

mcpwm_cmpr_handle_t comparator_a= NULL;
mcpwm_cmpr_handle_t comparator_b= NULL;
mcpwm_cmpr_handle_t comparator_c= NULL;
mcpwm_cmpr_handle_t comparator_d= NULL;

esp_err_t enable_servo()
{
esp_err_t err;
CHECK_LOGE(err, mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, SERVO_A), TAG_SERVO, "error: servo A: %s", esp_err_to_name(err));
CHECK_LOGE(err, mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, SERVO_B), TAG_SERVO, "error: servo B: %s", esp_err_to_name(err));
CHECK_LOGE(err, mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, SERVO_C), TAG_SERVO, "error: servo C: %s", esp_err_to_name(err));
CHECK_LOGE(err, mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, SERVO_D), TAG_SERVO, "error: servo D: %s", esp_err_to_name(err));

mcpwm_config_t pwm_config;
// sets the pwm frequency = 50
pwm_config.frequency = 50;
// sets the initial duty cycle of PWMxA = 0
pwm_config.cmpr_a = 0;
// sets the initial duty cycle of PWMxB = 0
pwm_config.cmpr_b = 0;
// sets the pwm counter mode
pwm_config.counter_mode = MCPWM_UP_COUNTER;
// sets the pwm duty mode
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;

// init pwm 0a, 1a, 2a with the above settings

esp_err_t err_A = mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);
esp_err_t err_B = mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config);

if (err_A == ESP_OK && err_B == ESP_OK)
{
enabled_servo_flag = 1;
ESP_LOGI(TAG_SERVO, "enabled servos");
ESP_LOGI(TAG_SERVO, "Create timer and operator for servos A and B");
mcpwm_timer_handle_t timer = NULL;
mcpwm_timer_config_t timer_config = {
.group_id = 0,
.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
.resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ,
.period_ticks = SERVO_TIMEBASE_PERIOD,
.count_mode = MCPWM_TIMER_COUNT_MODE_UP,
};
ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config, &timer));

return ESP_OK;
}
else
{
enabled_servo_flag = 0;
return ESP_FAIL;
}
mcpwm_oper_handle_t oper = NULL;
mcpwm_operator_config_t operator_config = {
.group_id = 0, // operator must be in the same group as the timer
};
ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config, &oper));

ESP_LOGI(TAG_SERVO, "Connect timer and operator for servos A and B");
ESP_ERROR_CHECK(mcpwm_operator_connect_timer(oper, timer));

ESP_LOGI(TAG_SERVO, "Create comparator_0 and generator from the operator for servos A and B");

mcpwm_comparator_config_t comparator_config_a = {
.flags.update_cmp_on_tez = true,
};
ESP_ERROR_CHECK(mcpwm_new_comparator(oper, &comparator_config_a, &comparator_a));

mcpwm_comparator_config_t comparator_config_b = {
.flags.update_cmp_on_tez = true,
};
ESP_ERROR_CHECK(mcpwm_new_comparator(oper, &comparator_config_b, &comparator_b));

mcpwm_gen_handle_t generator_a = NULL;
mcpwm_generator_config_t generator_config_a = {
.gen_gpio_num = SERVO_A,
};
ESP_ERROR_CHECK(mcpwm_new_generator(oper, &generator_config_a, &generator_a));

// Similarly, create generator B for SERVO_B
mcpwm_gen_handle_t generator_b = NULL;
mcpwm_generator_config_t generator_config_b = {
.gen_gpio_num = SERVO_B,
};
ESP_ERROR_CHECK(mcpwm_new_generator(oper, &generator_config_b, &generator_b));

// Set actions for generator A and B
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator_a,
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator_a,
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator_a, MCPWM_GEN_ACTION_LOW)));

ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator_b,
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator_b,
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator_b, MCPWM_GEN_ACTION_LOW)));


// Now, create a new timer, operator, and generators for servos C and D
ESP_LOGI(TAG_SERVO, "Create timer and operator for servos C and D");
mcpwm_timer_handle_t timer_1 = NULL;
mcpwm_timer_config_t timer_config_1 = {
.group_id = 1,
.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
.resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ,
.period_ticks = SERVO_TIMEBASE_PERIOD,
.count_mode = MCPWM_TIMER_COUNT_MODE_UP,
};
ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config_1, &timer_1));

mcpwm_oper_handle_t oper_1 = NULL;
mcpwm_operator_config_t operator_config_1 = {
.group_id = 1, // operator must be in the same group as the timer
};
ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config_1, &oper_1));

ESP_LOGI(TAG_SERVO, "Connect timer and operator for servos C and D");
ESP_ERROR_CHECK(mcpwm_operator_connect_timer(oper_1, timer_1));

ESP_LOGI(TAG_SERVO, "Create comparator and generator from the operator for servos C and D");

mcpwm_comparator_config_t comparator_config_c = {
.flags.update_cmp_on_tez = true,
};
ESP_ERROR_CHECK(mcpwm_new_comparator(oper_1, &comparator_config_1, &comparator_c));

mcpwm_comparator_config_t comparator_config_d = {
.flags.update_cmp_on_tez = true,
};
ESP_ERROR_CHECK(mcpwm_new_comparator(oper_1, &comparator_config_1, &comparator_d));


// Similarly, create generator C for SERVO_C
mcpwm_gen_handle_t generator_c = NULL;
mcpwm_generator_config_t generator_config_c = {
.gen_gpio_num = SERVO_C,
};

ESP_ERROR_CHECK(mcpwm_new_generator(oper_1, &generator_config_c, &generator_c));

// Similarly, create generator D for SERVO_D
mcpwm_gen_handle_t generator_d = NULL;
mcpwm_generator_config_t generator_config_d = {
.gen_gpio_num = SERVO_D,
};
ESP_ERROR_CHECK(mcpwm_new_generator(oper_1, &generator_config_d, &generator_d));

// Set actions for generator C and D
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator_c,
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator_c,
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator_c, MCPWM_GEN_ACTION_LOW)));

ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator_d,
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator_d,
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator_d, MCPWM_GEN_ACTION_LOW)));


// Enable and start both timers
ESP_LOGI(TAG_SERVO, "Enable and start timers");
ESP_ERROR_CHECK(mcpwm_timer_enable(timer));
ESP_ERROR_CHECK(mcpwm_timer_enable(timer_1));

ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP));
ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer_1, MCPWM_TIMER_START_NO_STOP));

// Set the flag to indicate that servos are enabled
enabled_servo_flag = 1;

return ESP_OK;
}

static esp_err_t set_angle_servo_helper(int servo_pin, int servo_max, int servo_min_pulsewidth, int servo_max_pulsewidth, unsigned int degree_of_rotation, mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num, mcpwm_generator_t gen)
static esp_err_t set_angle_servo_helper(int servo_pin, int servo_max, int servo_min_pulsewidth, int servo_max_pulsewidth, unsigned int degree_of_rotation)
{
degree_of_rotation = degree_of_rotation > servo_max ? servo_max : degree_of_rotation;

uint32_t cal_pulsewidth = 0;
cal_pulsewidth = (servo_min_pulsewidth + ((servo_max_pulsewidth - servo_min_pulsewidth) * (degree_of_rotation)) / (servo_max));

esp_err_t err = mcpwm_set_duty_in_us(mcpwm_num, timer_num, gen, cal_pulsewidth);


esp_err_t err;
switch(servo_pin) {
case SERVO_A:
err = mcpwm_comparator_set_compare_value(comparator_a, cal_pulsewidth);
break;
case SERVO_B:
err = mcpwm_comparator_set_compare_value(comparator_b, cal_pulsewidth);
break;
case SERVO_C:
err = mcpwm_comparator_set_compare_value(comparator_c, cal_pulsewidth);
break;
case SERVO_D:
err = mcpwm_comparator_set_compare_value(comparator_d, cal_pulsewidth);
break;
default:
err = ESP_ERR_INVALID_ARG;
break;
}

if (err == ESP_OK)
{
ESP_LOGI(TAG_SERVO, "set servo at pin %d: %ud", servo_pin, degree_of_rotation);
ESP_LOGI(TAG_SERVO, "set servo at pin %d: %d", servo_pin, degree_of_rotation);
}
else
{
Expand All @@ -94,7 +216,7 @@ esp_err_t set_angle_servo(servo_config *config, unsigned int degree_of_rotation)
if (config->servo_pin)
{
config->angle = degree_of_rotation;
return set_angle_servo_helper(config->servo_pin, config->max_degree, config->min_pulse_width, config->max_pulse_width, degree_of_rotation, config->mcpwm_num, config->timer_num, config->gen);
return set_angle_servo_helper(config->servo_pin, config->max_degree, config->min_pulse_width, config->max_pulse_width, degree_of_rotation);
}
else
{
Expand All @@ -108,8 +230,3 @@ esp_err_t set_angle_servo(servo_config *config, unsigned int degree_of_rotation)
return ESP_FAIL;
}
}

int read_servo(servo_config *config)
{
return config->angle;
}