Skip to content

Commit

Permalink
ESP-IDF 5 MCPWM Implementation Updates for Wall-E (#138)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@SuperChamp234
SuperChamp234 authored Feb 1, 2024
1 parent 2754499 commit 0fbb69d
Show file tree
Hide file tree
Showing 7 changed files with 106 additions and 106 deletions.
16 changes: 9 additions & 7 deletions 4_switch_controlled_motor_normal/main/switch_control_motor_normal.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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))
Expand All @@ -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
Expand Down
103 changes: 29 additions & 74 deletions 5_PWM/main/pwm_test.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down
16 changes: 9 additions & 7 deletions 6_line_following/main/line_following.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;

/*
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
10 changes: 10 additions & 0 deletions 7_self_balancing/main/include/tuning_websocket_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
44 changes: 30 additions & 14 deletions 7_self_balancing/main/self_balancing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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)
{
/**
Expand All @@ -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);
Expand All @@ -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);
}
21 changes: 18 additions & 3 deletions 7_self_balancing/main/tuning_websocket_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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);
}
2 changes: 1 addition & 1 deletion components/sra-board-component
Submodule sra-board-component updated 9 files
+3 −1 CMakeLists.txt
+13 −19 examples/motor_driver_normal/main/main.c
+0 −1,115 examples/servos/sdkconfig
+136 −0 include/mcpwm_interface.h
+26 −44 include/motor_driver.h
+1 −0 include/sra_board.h
+5 −11 src/bar_graph.c
+201 −0 src/mcpwm_interface.c
+48 −142 src/motor_driver.c

0 comments on commit 0fbb69d

Please sign in to comment.