From 389c4ecd92e013169412ab4906d132fabf6f7b93 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Mon, 6 Nov 2023 20:45:42 +0800
Subject: [PATCH] add function to revert wheel direction

---
 frtos/config/motor_config.h   |  2 ++
 frtos/motor/motor_direction.h | 20 +++++++++++++++-----
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h
index 07b6578..509f9f9 100644
--- a/frtos/config/motor_config.h
+++ b/frtos/config/motor_config.h
@@ -23,6 +23,8 @@
 #define DIRECTION_LEFT     (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
 #define DIRECTION_RIGHT    (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
 
+#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD)
+
 #define SPEED_PIN_RIGHT 15U
 #define SPEED_PIN_LEFT  16U
 
diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index e315635..a5ad873 100644
--- a/frtos/motor/motor_direction.h
+++ b/frtos/motor/motor_direction.h
@@ -19,14 +19,24 @@
 void
 set_wheel_direction(uint32_t direction)
 {
-    static const uint32_t mask
-        = DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD
-          | DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD;
-
-    gpio_put_masked(mask, 0U);
+    gpio_put_masked(DIRECTION_MASK, 0U);
     gpio_set_mask(direction);
 }
 
+/*!
+ * @brief Set the direction of the wheel to opposite direction using bit mask
+ */
+void
+revert_wheel_direction()
+{
+    uint32_t current_direction = gpio_get_all();
+
+    uint32_t reverted_direction = current_direction ^ DIRECTION_MASK;
+
+    gpio_put_masked(DIRECTION_MASK, 0U);
+    gpio_set_mask(reverted_direction & DIRECTION_MASK);
+}
+
 void
 turn_left_90()
 {