update motor module

ccs/motor.c

@@ -26,8 +26,30 @@ void motor_init(void) {
     P2->OUT  &= ~0xc0;
 }
 
-void move_stop() {
+static void set_dir_l(char dir) { // f: forward, b: backward
-    P2->OUT  &= ~0xc0;
+    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();
+    set_dir_l('f');
-    move_stop();
+    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();
+    set_dir_l('f');
-    move_stop();
+    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);
+
+    }
 }

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