From 2c503a68307b06b0916ac9ad2bb62bdf96c1719a10c9631da2fac33d0d7aaf51 Mon Sep 17 00:00:00 2001
From: yenru0 <yenru0604@gmail.com>
Date: Wed, 3 Dec 2025 05:01:31 +0900
Subject: [PATCH] update motor module

---
 ccs/motor.c | 88 +++++++++++++++++++++++++++++++++++++++++++++--------
 ccs/motor.h | 12 ++------
 2 files changed, 78 insertions(+), 22 deletions(-)

diff --git a/ccs/motor.c b/ccs/motor.c
index eb096ad..0045d45 100644
--- a/ccs/motor.c
+++ b/ccs/motor.c
@@ -26,8 +26,30 @@ void motor_init(void) {
     P2->OUT  &= ~0xc0;
 }
 
-void move_stop() {
-    P2->OUT  &= ~0xc0;
+static void set_dir_l(char dir) { // f: forward, b: backward
+    if      (dir == 'f') P5->OUT &= ~0b00010000;
+    else if (dir == 'b') P5->OUT |=  0b00010000;
+}
+
+static void set_dir_r(char dir) { // f: forward, b: backward
+    if      (dir == 'f') P5->OUT &= ~0b00100000;
+    else if (dir == 'b') P5->OUT |=  0b00100000;
+}
+
+static void set_en_l(int enable) { // 1: on, 0: off
+    if      (enable == 1) P2->OUT |=  0b10000000;
+    else if (enable == 0) P2->OUT &= ~0b10000000;
+}
+
+static void set_en_r(int enable) { // 1: on, 0: off
+    if      (enable == 1) P2->OUT |=  0b01000000;
+    else if (enable == 0) P2->OUT &= ~0b01000000;
+}
+
+void motor_move_stop() {
+    // P2->OUT  &= ~0xc0;
+    set_en_l(0);
+    set_en_r(0);
 }
 
 static void move_forward() {
@@ -74,7 +96,7 @@ void motor_move_forward(int speed) {
     move_forward();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 }
 
@@ -84,7 +106,7 @@ void motor_move_backward(int speed) {
     move_backward();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 }
 
@@ -94,7 +116,7 @@ void motor_turn_left(int speed) {
     turn_left();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 }
 
@@ -104,7 +126,7 @@ void motor_turn_right(int speed) {
     turn_right();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 }
 
@@ -114,11 +136,13 @@ void motor_rotate_clockwise(int speed) {
     rotate_clockwise();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 
-    move_forward();
-    move_stop();
+    set_dir_l('f');
+    set_dir_r('f');
+    // move_forward();
+    // motor_move_stop();
 }
 
 void motor_rotate_cclockwise(int speed) {
@@ -127,9 +151,49 @@ void motor_rotate_cclockwise(int speed) {
     rotate_cclockwise();
     Clock_Delay1ms(speed);
 
-    move_stop();
+    motor_move_stop();
     Clock_Delay1ms(PERIOD_ms - speed);
 
-    move_forward();
-    move_stop();
+    set_dir_l('f');
+    set_dir_r('f');
+    // move_forward();
+    // motor_move_stop();
+}
+
+void motor_move_custom(int left_speed, int right_speed) { // positive will be forward, negative will be backward
+    int l_speed = left_speed > 0 ? left_speed : -left_speed;
+    int r_speed = right_speed > 0 ? right_speed : -right_speed;
+    normalize_speed(&l_speed);
+    normalize_speed(&r_speed);
+
+    if (left_speed >= 0) set_dir_l('f');
+	else if (left_speed < 0) set_dir_l('b');
+
+    if (right_speed >= 0) set_dir_r('f');
+	else if (right_speed < 0) set_dir_r('b');
+
+
+    if (l_speed >= r_speed) {
+        set_en_l(1);
+        set_en_r(1);
+        Clock_Delay1ms(r_speed);
+
+        set_en_r(0);
+        Clock_Delay1ms(l_speed - r_speed);
+
+        set_en_l(0);
+        Clock_Delay1ms(PERIOD_ms - l_speed);
+
+    } else if (l_speed < r_speed) {
+        set_en_l(1);
+        set_en_r(1);
+        Clock_Delay1ms(l_speed);
+
+        set_en_l(0);
+        Clock_Delay1ms(r_speed - l_speed);
+
+        set_en_r(0);
+        Clock_Delay1ms(PERIOD_ms - r_speed);
+
+    }
 }
diff --git a/ccs/motor.h b/ccs/motor.h
index 2cdcb70..ef88a61 100644
--- a/ccs/motor.h
+++ b/ccs/motor.h
@@ -1,25 +1,17 @@
 #ifndef _motor_h_
 #define _motor_h_
 
-#include "msp.h"
-
 #define PERIOD_ms 20
 #define MAX_SPEED PERIOD_ms
 
 void motor_init();
-
-void move_stop();
-
+void motor_move_stop();
 void motor_move_forward(int speed);
-
 void motor_move_backward(int speed);
-
 void motor_turn_left(int speed);
-
 void motor_turn_right(int speed);
-
 void motor_rotate_clockwise(int speed);
-
 void motor_rotate_cclockwise(int speed);
+void motor_move_custom(int left_speed, int right_speed);
 
 #endif