ESP-IDF 5 MCPWM Implementation Updates for Wall-E

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

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

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

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

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

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