From f176192dce2e5ab78669ef4e5ac0555072c6ac11 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Sun, 29 Oct 2023 20:26:13 +0800
Subject: [PATCH] PID 99%, suspect wheel issue as if I swap the wheels the car
 slants opposite direction, for a bit; but generally straight for about 1m,
 should be enough for the map

---
 frtos/motor/motor_init.h | 11 ++++-------
 frtos/motor/motor_pid.h  |  4 +---
 frtos/motor/motor_test.c |  4 ++--
 3 files changed, 7 insertions(+), 12 deletions(-)

diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h
index 5a4f2ed..56cb390 100644
--- a/frtos/motor/motor_init.h
+++ b/frtos/motor/motor_init.h
@@ -21,17 +21,14 @@
 
 motor_t g_motor_left  = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_A,
-                          .speed.distance_cm = 0.0f,
-                          .pid.kp_value      = 8.4f,
-                          .pid.ki_value      = 0.021f,
-                          .pid.kd_value      = 42.f,};
+                          .speed.distance_cm = 0.0f };
 
 motor_t g_motor_right = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_B,
                           .speed.distance_cm = 0.0f,
-                          .pid.kp_value      = 8.4f,
-                          .pid.ki_value      = 0.021f,
-                          .pid.kd_value      = 42.0f,};
+                          .pid.kp_value      = 1000.f,
+                          .pid.ki_value      = 0.0f,
+                          .pid.kd_value      = 10000.0f,};
 
 void
 motor_init(void)
diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h
index af112da..c741ef6 100644
--- a/frtos/motor/motor_pid.h
+++ b/frtos/motor/motor_pid.h
@@ -21,8 +21,6 @@ compute_pid(float *integral, float *prev_error)
     float error
         = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
 
-    printf("error: %f\n", error);
-
     *integral += error;
 
     float derivative = error - *prev_error;
@@ -56,7 +54,7 @@ motor_pid_task(__unused void *p_param)
 
         float control_signal = compute_pid(&integral, &prev_error);
 
-        float temp = (float) g_motor_right.pwm.level + control_signal;
+        float temp = (float) g_motor_right.pwm.level + control_signal * 0.05f;
 
         if (temp > MAX_SPEED)
         {
diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c
index 4da2f21..a5f16ff 100644
--- a/frtos/motor/motor_test.c
+++ b/frtos/motor/motor_test.c
@@ -20,7 +20,7 @@ launch()
     irq_set_enabled(IO_IRQ_BANK0, true);
 
     // Set wheel speed
-    set_wheel_speed(3000);
+    set_wheel_speed(3500);
 
     // Left wheel
     //
@@ -60,7 +60,7 @@ main(void)
 {
     stdio_usb_init();
 
-    sleep_ms(2000);
+    sleep_ms(4000);
     printf("Test started!\n");
 
     motor_init();