From ed3c440ebc75ec94c5ad7f7a0314c07860f48d6f Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Mon, 30 Oct 2023 23:24:58 +0800
Subject: [PATCH] add direction control

---
 frtos/config/motor_config.h |  5 +++++
 frtos/motor/motor_speed.h   | 42 +++++++++++++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h
index 267d672..6d2cf0e 100644
--- a/frtos/config/motor_config.h
+++ b/frtos/config/motor_config.h
@@ -18,6 +18,11 @@
 #define DIRECTION_LEFT_FORWARD      (1U << DIRECTION_PIN_LEFT_IN4)
 #define DIRECTION_LEFT_BACKWARD     (1U << DIRECTION_PIN_LEFT_IN3)
 
+#define DIRECTION_FORWARD           (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD)
+#define DIRECTION_BACKWARD          (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD)
+#define DIRECTION_LEFT              (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
+#define DIRECTION_RIGHT             (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
+
 #define SPEED_PIN_RIGHT             15U
 #define SPEED_PIN_LEFT              16U
 
diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h
index 520fbee..d573276 100644
--- a/frtos/motor/motor_speed.h
+++ b/frtos/motor/motor_speed.h
@@ -72,4 +72,46 @@ monitor_wheel_speed_task(void *pvParameters)
             p_motor->speed.current_cms = 0.f;
         }
     }
+}
+
+
+/*!
+ * @brief Set the distance to travel before stopping, must be called after
+ * setting the speed and direction.
+ * @param distance_cm distance to travel in cm
+ */
+void
+distance_to_stop(float distance_cm)
+{
+    float initial = g_motor_right.speed.distance_cm;
+    printf("initial: %f\n", initial);
+
+    for (;;)
+    {
+        if (g_motor_right.speed.distance_cm - initial >= distance_cm)
+        {
+            g_motor_right.pwm.level = 0;
+            g_motor_left.pwm.level  = 0;
+            break;
+        }
+        // vTaskDelay(pdMS_TO_TICKS(50));
+    }
+}
+
+/*!
+ * @brief Set the speed of the wheels
+ * @param pwm_level The pwm_level of the wheels, from 0 to 5000
+ */
+void
+set_wheel_speed(uint32_t pwm_level)
+{
+    g_motor_right.pwm.level = pwm_level;
+    g_motor_left.pwm.level  = pwm_level;
+
+    pwm_set_chan_level(g_motor_right.pwm.slice_num,
+                       g_motor_right.pwm.channel,
+                       g_motor_right.pwm.level);
+    pwm_set_chan_level(g_motor_left.pwm.slice_num,
+                       g_motor_left.pwm.channel,
+                       g_motor_left.pwm.level);
 }
\ No newline at end of file