Skip to content
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

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

Open
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 all 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
30 changes: 15 additions & 15 deletions include/servo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,33 +21,33 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#ifndef SERVO_H
#define SERVO_H

#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"
#include "esp_err.h"
#include "pin_defs.h"

typedef struct
{
int servo_pin;
int min_pulse_width;
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 All @@ -65,14 +65,14 @@ typedef struct
* @var servo_config::gen
* Member 'gen' contains MCPWM operator to use
*/

/**
* @brief Enables Servo port on the sra board, sets up PWM for the three pins in servo port.
*
* @return esp_err_t - returns ESP_OK if servo pins initialised, else it returns ESP_ERR_INVALID_ARG
**/
esp_err_t enable_servo();

/**
* @brief Set the angle of the servos attached to the servo port of SRA Board
*
Expand All @@ -81,11 +81,11 @@ esp_err_t enable_servo();
* @return esp_err_t
*/
esp_err_t set_angle_servo(servo_config *config, unsigned int degree_of_rotation);

/**
* @brief Get the angle of the servos
* @return esp_err_t
*/
int read_servo(servo_config *config);

#endif
#endif
Copy link
Member

Choose a reason for hiding this comment

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

Please fix these types of newline diffs. They are unnecessary.

217 changes: 169 additions & 48 deletions src/servo.c
View file
Open in desktop
Copy link
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
Expand Up @@ -23,78 +23,198 @@
*/

#include "servo.h"

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");

return ESP_OK;
}
else
{
enabled_servo_flag = 0;
return ESP_FAIL;
}
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));

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_a 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_c, &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_d, &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
{
ESP_LOGE(TAG_SERVO, "error: servo at pin %d: %s", servo_pin, esp_err_to_name(err));
}

return err;
}

esp_err_t set_angle_servo(servo_config *config, unsigned int degree_of_rotation)
{
if (enabled_servo_flag)
{
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,7 +228,8 @@ 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;
Expand Down