From 0fbb69d06c96ab85b15cc91711a140601b4ecfb3 Mon Sep 17 00:00:00 2001 From: Zain Date: Thu, 1 Feb 2024 15:51:09 +0530 Subject: [PATCH] ESP-IDF 5 MCPWM Implementation Updates for Wall-E (#138) * Update motor driver configuration and PID constants * Add plot_graph_data struct and plot_graph_task, pinned server and balancetask to different cores. * Fix function parameter spacing in tuning_websocket_server.h and self_balancing.c, updated comments. * Fix formatting issues in self_balancing.c and tuning_websocket_server.c * Refactor self-balancing tuning parameters and plot graph task * Refactor motor control code to use motor handles * Refactor PWM task to use motor handles, removed PARALLEL mode * Update line following parameters and motor driver initialization using handles * Update subcomponent to latest * Update task priorities in self_balancing.c and tuning_websocket_server.c --- .../main/switch_control_motor_normal.c | 16 +-- 5_PWM/main/pwm_test.c | 103 +++++------------- 6_line_following/main/line_following.c | 16 +-- .../main/include/tuning_websocket_server.h | 10 ++ 7_self_balancing/main/self_balancing.c | 44 +++++--- .../main/tuning_websocket_server.c | 21 +++- components/sra-board-component | 2 +- 7 files changed, 106 insertions(+), 106 deletions(-) diff --git a/4_switch_controlled_motor_normal/main/switch_control_motor_normal.c b/4_switch_controlled_motor_normal/main/switch_control_motor_normal.c index ddcf9609..06e0f8af 100644 --- a/4_switch_controlled_motor_normal/main/switch_control_motor_normal.c +++ b/4_switch_controlled_motor_normal/main/switch_control_motor_normal.c @@ -23,7 +23,9 @@ void drive_task(void *arg) * The ESP has two TB6612FNG drivers which can run upto 2 motors each, the a refers to the first driver * more details can be found in https://github.com/SRA-VJTI/sra-board-component/blob/9f28700759ac816660c18859d65303d6540e8732/src/motor_driver.c#L7-L57 */ - enable_motor_driver(a, NORMAL_MODE); // Enable motor driver A in Normal Mode + motor_handle_t motor_a_0, motor_a_1; + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_0, MOTOR_A_0)); + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_1, MOTOR_A_1)); //infinite loop to get LSA readings continuously while (1) @@ -40,20 +42,20 @@ void drive_task(void *arg) * runs the first motor A_0 for 80 duty cylces forward or backward according to LSA input * more details can be found at https://github.com/SRA-VJTI/sra-board-component/blob/9f28700759ac816660c18859d65303d6540e8732/src/motor_driver.c#L195 */ - set_motor_speed(MOTOR_A_0, MOTOR_FORWARD, 80); + set_motor_speed(motor_a_0, MOTOR_FORWARD, 80); // Display logs - final LSA readings ESP_LOGI(TAG_SWITCH_CONTROL_MOTOR, "MOTOR_A_0 FORWARD"); } else if (read_switch(SWITCH_2)) { - set_motor_speed(MOTOR_A_0, MOTOR_BACKWARD, 80); + set_motor_speed(motor_a_0, MOTOR_BACKWARD, 80); ESP_LOGI(TAG_SWITCH_CONTROL_MOTOR, "MOTOR_A_0 BACKWARD"); } else { // if there is no input from LSA for first motor then stop the motor, this stops the motor if the previous 80 duty cycles are not completed - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); + set_motor_speed(motor_a_0, MOTOR_STOP, 0); } if (read_switch(SWITCH_3)) @@ -62,20 +64,20 @@ void drive_task(void *arg) * runs the second motor A_1 for 80 duty cylces forward or backward according to LSA input * more details can be found at https://github.com/SRA-VJTI/sra-board-component/blob/9f28700759ac816660c18859d65303d6540e8732/src/motor_driver.c#L195 */ - set_motor_speed(MOTOR_A_1, MOTOR_FORWARD, 80); + set_motor_speed(motor_a_1, MOTOR_FORWARD, 80); //Displaying Logs - final LSA readings ESP_LOGI(TAG_SWITCH_CONTROL_MOTOR, "MOTOR_A_1 FORWARD"); } else if (read_switch(SWITCH_4)) { - set_motor_speed(MOTOR_A_1, MOTOR_BACKWARD, 80); + set_motor_speed(motor_a_1, MOTOR_BACKWARD, 80); ESP_LOGI(TAG_SWITCH_CONTROL_MOTOR, "MOTOR_A_1 BACKWARD"); } else { //if there is no input from LSA for the second motor then stop the motor - set_motor_speed(MOTOR_A_1, MOTOR_STOP, 0); + set_motor_speed(motor_a_1, MOTOR_STOP, 0); } /** * Delay the task for a specific amount of freeRTOS ticks diff --git a/5_PWM/main/pwm_test.c b/5_PWM/main/pwm_test.c index c7d97e9c..c87ef33b 100644 --- a/5_PWM/main/pwm_test.c +++ b/5_PWM/main/pwm_test.c @@ -8,101 +8,56 @@ //Components #include "sra_board.h" -// Select the mode needed by uncommenting its definition -#define MODE NORMAL_MODE -// #define MODE PARALLEL_MODE - void pwm_task(void *arg) { + motor_handle_t motor_a_0, motor_a_1; + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_0, MOTOR_A_0)); // Enable motor driver A0 + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_1, MOTOR_A_1)); // Enable motor driver A1 - if (MODE == NORMAL_MODE) + // Make the Motors go forward & backward alternatively, at different PWM from 60 to 100 + while (1) { - enable_motor_driver(a, NORMAL_MODE); // Enable motor driver A in Normal Mode - - // Make the Motors go forward & backward alternatively, at different PWM from 60 to 100 - while (1) + for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) { - for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) - { - // setting motor speed of MOTOR A0 in forward direction with duty cycle - set_motor_speed(MOTOR_A_0, MOTOR_FORWARD, duty_cycle); - - // setting motor speed of MOTOR A1 in forward direction with duty cycle - set_motor_speed(MOTOR_A_1, MOTOR_FORWARD, duty_cycle); + // setting motor speed of MOTOR A0 in forward direction with duty cycle + set_motor_speed(motor_a_0, MOTOR_FORWARD, duty_cycle); - // adding delay of 100ms - vTaskDelay(100 / portTICK_PERIOD_MS); - } - - // stopping the MOTOR A0 - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); - - // stopping the MOTOR A1 - set_motor_speed(MOTOR_A_1, MOTOR_STOP, 0); + // setting motor speed of MOTOR A1 in forward direction with duty cycle + set_motor_speed(motor_a_1, MOTOR_FORWARD, duty_cycle); // adding delay of 100ms vTaskDelay(100 / portTICK_PERIOD_MS); + } - for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) - { - // setting motor speed of MOTOR A0 in backward direction with duty cycle - set_motor_speed(MOTOR_A_0, MOTOR_BACKWARD, duty_cycle); - - // setting motor speed of MOTOR A1 in backward direction with duty cycle - set_motor_speed(MOTOR_A_1, MOTOR_BACKWARD, duty_cycle); - - // adding delay of 100ms - vTaskDelay(100 / portTICK_PERIOD_MS); - } + // stopping the MOTOR A0 + set_motor_speed(motor_a_0, MOTOR_STOP, 0); - // stopping the MOTOR A0 - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); + // stopping the MOTOR A1 + set_motor_speed(motor_a_1, MOTOR_STOP, 0); - // stopping the MOTOR A1 - set_motor_speed(MOTOR_A_1, MOTOR_STOP, 0); + // adding delay of 100ms + vTaskDelay(100 / portTICK_PERIOD_MS); - // adding delay of 100ms - vTaskDelay(100 / portTICK_PERIOD_MS); - } - } - - else if (MODE == PARALLEL_MODE) - { - enable_motor_driver(a, PARALLEL_MODE); // Enable motor driver A in Parallel Mode - - // Make the Motors go forward & backward alternatively, at different PWM from 60 to 100 - while (1) + for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) { - for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) - { - // setting motor speed of motor A in forward direction with duty cycle - set_motor_speed(MOTOR_A_0, MOTOR_FORWARD, duty_cycle); - - // adding delay of 100ms - vTaskDelay(100 / portTICK_PERIOD_MS); - } + // setting motor speed of MOTOR A0 in backward direction with duty cycle + set_motor_speed(motor_a_0, MOTOR_BACKWARD, duty_cycle); - // stopping the motor A - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); + // setting motor speed of MOTOR A1 in backward direction with duty cycle + set_motor_speed(motor_a_1, MOTOR_BACKWARD, duty_cycle); // adding delay of 100ms vTaskDelay(100 / portTICK_PERIOD_MS); + } - for (int duty_cycle = 60; duty_cycle <= 100; duty_cycle++) - { - // setting motor speed of motor A in backward direction with duty cycle - set_motor_speed(MOTOR_A_0, MOTOR_BACKWARD, duty_cycle); - - // adding delay of 100ms - vTaskDelay(100 / 10); - } + // stopping the MOTOR A0 + set_motor_speed(motor_a_0, MOTOR_STOP, 0); - // stopping the motor A - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); + // stopping the MOTOR A1 + set_motor_speed(motor_a_1, MOTOR_STOP, 0); - // adding delay of 100ms - vTaskDelay(100 / portTICK_PERIOD_MS); - } + // adding delay of 100ms + vTaskDelay(100 / portTICK_PERIOD_MS); } } diff --git a/6_line_following/main/line_following.c b/6_line_following/main/line_following.c index 13238d89..96717f03 100644 --- a/6_line_following/main/line_following.c +++ b/6_line_following/main/line_following.c @@ -9,7 +9,7 @@ #define WHITE_MARGIN 0 #define bound_LSA_LOW 0 #define bound_LSA_HIGH 1000 -#define BLACK_BOUNDARY 700 // Boundary value to distinguish between black and white readings +#define BLACK_BOUNDARY 930 // Boundary value to distinguish between black and white readings /* * weights given to respective line sensor @@ -19,9 +19,9 @@ const int weights[5] = {-5, -3, 1, 3, 5}; /* * Motor value boundts */ -int optimum_duty_cycle = 63; -int lower_duty_cycle = 50; -int higher_duty_cycle = 76; +int optimum_duty_cycle = 57; +int lower_duty_cycle = 45; +int higher_duty_cycle = 65; float left_duty_cycle = 0, right_duty_cycle = 0; /* @@ -107,7 +107,9 @@ void calculate_error() void line_follow_task(void* arg) { - ESP_ERROR_CHECK(enable_motor_driver(a, NORMAL_MODE)); + motor_handle_t motor_a_0, motor_a_1; + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_0, MOTOR_A_0)); + ESP_ERROR_CHECK(enable_motor_driver(&motor_a_1, MOTOR_A_1)); ESP_ERROR_CHECK(enable_line_sensor()); ESP_ERROR_CHECK(enable_bar_graph()); #ifdef CONFIG_ENABLE_OLED @@ -137,8 +139,8 @@ void line_follow_task(void* arg) left_duty_cycle = bound((optimum_duty_cycle + correction), lower_duty_cycle, higher_duty_cycle); right_duty_cycle = bound((optimum_duty_cycle - correction), lower_duty_cycle, higher_duty_cycle); - set_motor_speed(MOTOR_A_0, MOTOR_FORWARD, left_duty_cycle); - set_motor_speed(MOTOR_A_1, MOTOR_FORWARD, right_duty_cycle); + set_motor_speed(motor_a_0, MOTOR_FORWARD, left_duty_cycle); + set_motor_speed(motor_a_1, MOTOR_FORWARD, right_duty_cycle); //ESP_LOGI("debug","left_duty_cycle: %f :: right_duty_cycle : %f :: error : %f correction : %f \n",left_duty_cycle, right_duty_cycle, error, correction); diff --git a/7_self_balancing/main/include/tuning_websocket_server.h b/7_self_balancing/main/include/tuning_websocket_server.h index f73611a9..b19fb74d 100644 --- a/7_self_balancing/main/include/tuning_websocket_server.h +++ b/7_self_balancing/main/include/tuning_websocket_server.h @@ -39,6 +39,16 @@ typedef struct pid_const bool val_changed; } pid_const_t; +typedef struct plot_graph_data { + float p_term; + float d_term; + float i_term; + float pitch_corr; + float pitch_err; +} plot_graph_data_t; + +void plot_graph_task(void *plot_data_queue); + pid_const_t read_pid_const(); void reset_val_changed_pid_const(); void start_websocket_server(); diff --git a/7_self_balancing/main/self_balancing.c b/7_self_balancing/main/self_balancing.c index eb252e8e..c195f189 100644 --- a/7_self_balancing/main/self_balancing.c +++ b/7_self_balancing/main/self_balancing.c @@ -16,10 +16,13 @@ #define MAX_PWM (80.0f) #define MIN_PWM (60.0f) -/* Self Balancing Tuning Parameters -float forward_offset = 2.51f; -float forward_buffer = 3.1f; +/** +* Self Balancing Tuning Parameters +* float forward_offset = 2.51f; +* float forward_buffer = 3.1f; */ +plot_graph_data_t pg_data; +QueueHandle_t plot_graph_queue = NULL; // Calculate the motor inputs according to angle of the MPU void calculate_motor_command(const float pitch_error, float *motor_cmd) @@ -57,7 +60,15 @@ void calculate_motor_command(const float pitch_error, float *motor_cmd) pitch_correction = P_term + I_term + D_term; - plot_graph(P_term, D_term, I_term, pitch_correction, pitch_error); + pg_data.p_term = P_term; + pg_data.d_term = D_term; + pg_data.i_term = I_term; + pg_data.pitch_corr = pitch_correction; + pg_data.pitch_err = pitch_error; + + plot_graph_data_t *pg_data_handle = &pg_data; + + xQueueSend(plot_graph_queue, (void *)&pg_data_handle, (TickType_t)0); /** * Calculating absolute value of pitch_correction since duty cycle can't be negative. * Since it is a floating point variable, fabsf was used instead of abs @@ -85,7 +96,7 @@ void balance_task(void *arg) giving actual correction velocity to the motors */ float motor_cmd, motor_pwm = 0.0f; - + motor_handle_t motor_a_0, motor_a_1; // Pitch angle where you want to go - pitch_cmd, setpoint and mpu_offsets are linked to one another float pitch_cmd = 0.0f; #ifdef CONFIG_ENABLE_OLED @@ -101,7 +112,8 @@ void balance_task(void *arg) if (enable_mpu6050() == ESP_OK) { // Function to enable Motor driver A in Normal Mode - enable_motor_driver(a, NORMAL_MODE); + enable_motor_driver(&motor_a_0, MOTOR_A_0); + enable_motor_driver(&motor_a_1, MOTOR_A_1); while (1) { /** @@ -125,27 +137,27 @@ void balance_task(void *arg) if (pitch_error > 1) { // setting motor A0 with definite speed(duty cycle of motor driver PWM) in Backward direction - set_motor_speed(MOTOR_A_0, MOTOR_BACKWARD, motor_pwm); + set_motor_speed(motor_a_0, MOTOR_BACKWARD, motor_pwm); // setting motor A1 with definite speed(duty cycle of motor driver PWM) in Backward direction - set_motor_speed(MOTOR_A_1, MOTOR_BACKWARD, motor_pwm); + set_motor_speed(motor_a_1, MOTOR_BACKWARD, motor_pwm); } // Bot tilts downwards else if (pitch_error < -1) { // setting motor A0 with definite speed(duty cycle of motor driver PWM) in Forward direction - set_motor_speed(MOTOR_A_0, MOTOR_FORWARD, motor_pwm); + set_motor_speed(motor_a_0, MOTOR_FORWARD, motor_pwm); // setting motor A1 with definite speed(duty cycle of motor driver PWM) in Forward direction - set_motor_speed(MOTOR_A_1, MOTOR_FORWARD, motor_pwm); + set_motor_speed(motor_a_1, MOTOR_FORWARD, motor_pwm); } // Bot remains in desired region for vertical balance else { // stopping motor A0 - set_motor_speed(MOTOR_A_0, MOTOR_STOP, 0); + set_motor_speed(motor_a_0, MOTOR_STOP, 0); // stopping motor A1 - set_motor_speed(MOTOR_A_1, MOTOR_STOP, 0); + set_motor_speed(motor_a_1, MOTOR_STOP, 0); } //ESP_LOGI("debug","left_duty_cycle: %f :: right_duty_cycle : %f :: error : %f correction : %f \n",left_duty_cycle, right_duty_cycle, error, correction); @@ -170,9 +182,13 @@ void balance_task(void *arg) void app_main() { - // Starts tuning server for wireless control + // Create a queue to send PID values to plot graph task + plot_graph_queue = xQueueCreate(10, sizeof(&pg_data)); + // Starts tuning server for wireless control start_websocket_server(); + // xTaskCreate -> Create a new task to start plotting the graph + xTaskCreatePinnedToCore(&plot_graph_task, "plot graph task", 4096, (void *)plot_graph_queue, 6, NULL, 1); // xTaskCreate -> Create a new task and add it to the list of tasks that are ready to run - xTaskCreate(&balance_task, "balance task", 4096, NULL, 1, NULL); + xTaskCreatePinnedToCore(&balance_task, "balance task", 4096, NULL, 1, NULL, 0); } \ No newline at end of file diff --git a/7_self_balancing/main/tuning_websocket_server.c b/7_self_balancing/main/tuning_websocket_server.c index 99f4fc27..46a8df8b 100644 --- a/7_self_balancing/main/tuning_websocket_server.c +++ b/7_self_balancing/main/tuning_websocket_server.c @@ -2,7 +2,7 @@ static const char *TAG = "tuning_websocket_server"; -static pid_const_t pid_constants = {.kp = 5.0, .ki = 0.0, .kd = 1.0, .setpoint = 6.0, .offset = 0.0, .val_changed = true}; +static pid_const_t pid_constants = {.kp = 27.0, .ki = 0.0, .kd = 2.0, .setpoint = 2.0, .offset = 0.0, .val_changed = true}; static QueueHandle_t client_queue; const static int client_queue_size = 10; @@ -82,6 +82,21 @@ void websocket_callback(uint8_t num, WEBSOCKET_TYPE_t type, char *msg, uint64_t } } +void plot_graph_task(void *plot_data_queue){ + /** + * Try to read from the queue, if it is not empty and we successfully receive a value then call plot_graph function + * If the queue is empty then wait for 10ms and try again + */ + plot_graph_data_t *pg_data; + while(1){ + if(xQueueReceive((QueueHandle_t)plot_data_queue, &pg_data, (TickType_t)10) == pdPASS){ + if (pg_data != NULL) + plot_graph(pg_data->p_term, pg_data->d_term, pg_data->i_term, pg_data->pitch_corr, pg_data->pitch_err); + } + vTaskDelay(10 / portTICK_PERIOD_MS); + } +} + static void http_server(struct netconn *conn) { const static char HTML_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/html\n\n"; @@ -251,6 +266,6 @@ void start_websocket_server() // ESP_ERROR_CHECK(init_fs()); ws_server_start(); - xTaskCreate(&server_task, "server_task", 3000, NULL, 9, NULL); - xTaskCreate(&server_handle_task, "server_handle_task", 4000, NULL, 6, NULL); + xTaskCreatePinnedToCore(&server_task, "server_task", 3000, NULL, 4, NULL, 1); + xTaskCreatePinnedToCore(&server_handle_task, "server_handle_task", 4000, NULL, 3, NULL, 1); } \ No newline at end of file diff --git a/components/sra-board-component b/components/sra-board-component index d73559b5..619695fc 160000 --- a/components/sra-board-component +++ b/components/sra-board-component @@ -1 +1 @@ -Subproject commit d73559b53d2808842b49e0e8b4f776fd2b2fa7dc +Subproject commit 619695fc33119a7647b7febe024e52022134d8bd