약간의 수정

ccs/.vscode/c_cpp_properties.json

@@ -5,7 +5,7 @@
             "includePath": [
                 "${workspaceFolder}/**",
                 "/opt/ti/ccs/tools/compiler/ti-cgt-armllvm_3.2.2.LTS/include/**",
-                "/opt/ti/ccs/ccs_base/arm/include/**",
+                "/opt/ti/ccs/ccs_base/arm/include/**"
             ],
             "defines": ["__MSP432P401R__"],
             "compilerPath": "/opt/ti/ccs/tools/compiler/ti-cgt-armllvm_3.2.2.LTS/bin/tiarmclang",

ccs/ir.c

@@ -27,13 +27,22 @@ void ir_read_ready() {
 
 void ir_read_value(uint8_t *value) {
     int i;
-    uint8_t port_value = P7->IN;
-    for (i = 0; i < 8; i++) {
-        value[i] = !!(port_value & (0x01 << i));
-    }
+    *value = P7->IN;
 }
 
 void ir_read_off() {
     P5->OUT &= ~0x08;
     P9->OUT &= ~0x04;
 }
+
+void test_ir(void) {
+    uint8_t ir_value;
+
+    while (1) {
+        ir_ready_ready();
+        ir_read_value(&ir_value);
+        print_binary(ir_value);
+        printf("\n";)
+        ir_read_off();
+    }
+}

ccs/ir.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "msp.h"
+#include "util.h"
 #include <stdint.h>
 
 void ir_init(void);
@@ -9,4 +10,6 @@ void ir_read_ready(void);
 
 void ir_read_value(uint8_t *value);
 
-void ir_read_off(void);
+void ir_read_off(void);
+
+void test_ir(void);

ccs/main.c

@@ -10,28 +10,26 @@
 #include "state.h"
 #include "util.h"
 
+#define TRUE 1
+#define FALSE 0
+
+#define PRACTICE 3
+
+#if PRACTICE == 1
+
 #define LOOP_PERIOD_MS 20
 #define REC_INTERVAL 5
 #define MAX_REC_SIZE 1000
 
-// PID gains tuned for speed range [-20, 20]
 const float Kp = 0.005f;   // proportional gain
 const float Kd = 0.02f;    // derivative gain
 #define BASE_SPEED 10       // nominal forward speed (0~20)
 #define MAX_DRV 20          // absolute max speed
 
-#define Practice1
-
-
-#ifdef Practice1
-
 static int pid_history[MAX_REC_SIZE];
 static int history_idx = 0;
 
 void LineTracerPrac1(void) {
-    // ------------------------------------------------
-    // 1. 초기화
-    // ------------------------------------------------
     motor_init();
     ir_init();
     
@@ -168,10 +166,21 @@ void LineTracerPrac1(void) {
     while(1); // 무한 대기
 }
 
-#endif
+#elif PRACTICE == 2
 
-#ifdef Practice2
-void LineTracerPrac(void) {
+#define LOOP_PERIOD_MS 20
+#define REC_INTERVAL 5
+#define MAX_REC_SIZE 1000
+
+const float Kp = 0.005f;   // proportional gain
+const float Kd = 0.02f;    // derivative gain
+#define BASE_SPEED 10       // nominal forward speed (0~20)
+#define MAX_DRV 20          // absolute max speed
+
+static int pid_history[MAX_REC_SIZE];
+static int history_idx = 0;
+
+void LineTracerPrac2(void) {
     Clock_Init48MHz();
     printf("Init Phase Begin\n");
 
@@ -232,8 +241,290 @@ void LineTracerPrac(void) {
     }
 }
 
+#elif PRACTICE == 3
+
+#define TEST_IR TRUE
+#define PRINT_DEBUG FALSE
+#define DELAY_FOR_PUT_DOWN_s 5
+#define POSITION_INIT_MOTOR TRUE
+#define POSITION_INIT_LED TRUE
+#define TRACK_NUMBER 1
+// 1: Half Circle
+// 2: Worm-Like
+// 3: Dots Line
+// 4: Long Term Line
+
+#if TRACK_NUMBER == 1 // Half Circle
+    #define STRAIGHT_SPEED 5
+    #define TURN_SPEED 5
+    #define ROTATE_SPEED 3
+
+#elif TRACK_NUMBER == 2 // Worm-Like
+    #define STRAIGHT_SPEED_BEFORE_ROTATE 1
+    #define STRAIGHT_SPEED 5
+    #define TURN_SPEED 5
+    #define ROTATE_SPEED 3
+    #define ROTATE_SPEED_90DEG 5
+
+#elif TRACK_NUMBER == 3 // Dots Line
+    #define STRAIGHT_SPEED 5
+    #define TURN_SPEED 5
+    #define ROTATE_SPEED 3
+
+#elif TRACK_NUMBER == 4 // Long Term Line
+    #define STRAIGHT_SPEED 5
+    #define TURN_SPEED 5
+    #define ROTATE_SPEED 3
+#endif
+
+
+void position_init_motor(void);
+void position_init_led(void);
+
+
+// global var
+HistoryBuffer hisbuf;
+int buf_lookahead;
+int use_buf = 0;
+//int set_pre = 1;
+
+int LineTracerPrac3(void) {
+    // init
+    Clock_Init48MHz();
+    ir_init();
+    hisbuf_init(&hisbuf);
+
+#if TEST_IR
+    test_ir();
+#endif
+
+    // position init
+#if POSITION_INIT_MOTOR
+    Clock_Delay1ms(1000 * DELAY_FOR_PUT_DOWN_s); // delay for init
+    motor_init();
+    position_init_motor();
+#endif
+#if POSITION_INIT_LED
+    position_init_led();
+#endif
+
+
+    // local var for running
+    uint8_t ir_value;
+    uint8_t target_ir_value;
+//    uint8_t pre_target_ir_value = 0b00011000;
+    while (1) {
+        ir_read_ready();
+        ir_read_value(&ir_value);
+        ir_read_off();
+        target_ir_value = extract_ir_value(0b00111100, ir_value);
+
+        /* when using buffer */
+        if (use_buf) {
+            if (target_ir_value != 0b00000000) {
+                while (hisbuf_pop(&hisbuf)); // make buf empty
+                buf_lookahead = 0;
+                use_buf = 0;
+            } else {
+                hisbuf_get(&hisbuf, &target_ir_value, buf_lookahead);
+//                set_pre = target_ir_value & 1;
+                target_ir_value = extract_ir_value(0b00111100, target_ir_value);
+                buf_lookahead = (buf_lookahead + 1) % hisbuf.size;
+            }
+        }
+        /* end when using buffer */
+
+#if TRACK_NUMBER == 1 // Half Circle
+        if (use_buf && ir_value) return 0;
+        switch (target_ir_value) {
+            case 0b00011000:
+                motor_move_forward(STRAIGHT_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00110000: // left-biased a lot
+                motor_rotate_cclockwise(ROTATE_SPEED);
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001100: // right-biased a lot
+                motor_rotate_clockwise(ROTATE_SPEED);
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00010000: // left-biased
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001000: // right-biased
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00000000: // no line to trace
+                buf_lookahead = 0;
+                use_buf = 1;
+                break;
+        }
+
+#elif TRACK_NUMBER == 2 // Worm-Like
+        switch (target_ir_value) {
+            case 0b00011000:
+                motor_move_forward(STRAIGHT_SPEED);
+                break;
+            case 0b00111000: // left-right angle
+				motor_move_forward(STRAIGHT_SPEED_BEFORE_ROTATE);
+                motor_rotate_cclockwise(ROTATE_SPEED_90DEG);
+                break;
+            case 0b00011100: // right-right angle
+                motor_move_forward(STRAIGHT_SPEED_BEFORE_ROTATE);
+                motor_rotate_clockwise(ROTATE_SPEED_90DEG);
+                break;
+            case 0b00110000: // left-biased a lot
+                motor_rotate_cclockwise(ROTATE_SPEED);
+                motor_turn_left(TURN_SPEED);
+                break;
+            case 0b00001100: // right-biased a lot
+                motor_rotate_clockwise(ROTATE_SPEED);
+                motor_turn_right(TURN_SPEED);
+                break;
+            case 0b00010000: // left-biased
+                motor_turn_left(TURN_SPEED);
+                break;
+            case 0b00001000: // right-biased
+                motor_turn_right(TURN_SPEED);
+                break;
+            case 0b00000000: // no line to trace
+                buf_lookahead = 0;
+                break;
+        }
+
+#elif TRACK_NUMBER == 3 // Dots Line
+        switch (target_ir_value) {
+            case 0b00111100:
+                return 0;
+            case 0b00011000:
+                motor_move_forward(STRAIGHT_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00110000: // left-biased a lot
+                motor_rotate_cclockwise(ROTATE_SPEED);
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001100: // right-biased a lot
+                motor_rotate_clockwise(ROTATE_SPEED);
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00010000: // left-biased
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001000: // right-biased
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00000000: // no line to trace
+                buf_lookahead = 0;
+                use_buf = 1;
+                break;
+        }
+
+#elif TRACK_NUMBER == 4 // Long Term Line
+        switch (target_ir_value) {
+            case 0b00111100:
+                return 0;
+            case 0b00011000:
+                motor_move_forward(STRAIGHT_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00110000: // left-biased a lot
+                motor_rotate_cclockwise(ROTATE_SPEED);
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001100: // right-biased a lot
+                motor_rotate_clockwise(ROTATE_SPEED);
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00010000: // left-biased
+                motor_turn_left(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00001000: // right-biased
+                motor_turn_right(TURN_SPEED);
+                use_buf = 0;
+                break;
+            case 0b00000000: // no line to trace
+                buf_lookahead = 0;
+                use_buf = 1;
+                break;
+        }
+#endif
+
+//        if (set_pre) pre_target_ir_value = target_ir_value;
+        if (!use_buf) hisbuf_push(&hisbuf, target_ir_value); // when use_buf, record track
+    }
+
+    return 0;
+}
+
+void position_init_motor(void) {
+	uint8_t ir_value, target_ir_value;
+
+    while (1) {
+        ir_read_ready();
+        ir_read_value(&ir_value);
+        ir_read_off();
+        target_ir_value = extract_ir_value(0b00111100, ir_value);
+
+        if (target_ir_value == 0b00011000) break;
+    }
+}
+void position_init_led(void) {
+	uint8_t ir_value, target_ir_value;
+
+    // led init
+    P2->SEL0 &= ~0x07;
+    P2->SEL1 &= ~0x07;
+    P2->DIR |= 0x07;
+    P2->OUT &= ~0x07;
+
+    int cnt = 0;
+    while (1) {
+        ir_read_ready();
+        ir_read_value(&ir_value);
+        ir_read_off();
+        target_ir_value = extract_ir_value(0b00111100, ir_value);
+
+        if (target_ir_value == 0b00011000) {
+            P2->OUT &= ~0b111;
+            P2->OUT |=  0b001;
+            cnt++;
+        } else {
+            P2->OUT &= ~0b111;
+            cnt = 0;
+        }
+
+        // Hard Coded
+        if (cnt == DELAY_FOR_PUT_DOWN_s * 50) break;
+        Clock_Delay1ms(20);
+    }
+    P2->OUT &= ~0b111;
+}
+
+
 #endif
 
 int main(void) {
-    LineTracerPrac();
+    #if PRACTICE == 1
+    LineTracerPrac1();
+    #elif PRACTICE == 2
+    LineTracerPrac2();
+    #elif PRACTICE == 3
+    LineTracerPrac3();
+    #else
+    printf("No practice selected.\n");
+    #endif
+    return 0;
 }

ccs/motor.c

@@ -1,5 +1,6 @@
 #include "motor.h"
 #include "Clock.h"
+#include <stdio.h>
 
 // Period: 20ms
 // MAX speed: 20
@@ -172,7 +173,6 @@ void motor_move_custom(int left_speed, int right_speed) { // positive will be fo
     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);

ccs/state.c

@@ -35,4 +35,45 @@ void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead) {
         int index = (sb->rear - 1 - lookahead + STATE_SENSOR_BUFFER_SIZE) % STATE_SENSOR_BUFFER_SIZE;
         *out = sb->buffer[index];
     }
-}
+}
+
+
+
+// History Buffer
+
+void hisbuf_init(HistoryBuffer *sb) {
+    sb->front = 0;
+    sb->rear = 0;
+    sb->size = 0;
+}
+
+int hisbuf_is_full(HistoryBuffer *sb) {
+    return sb->size == HISTORY_SIZE;
+}
+
+int hisbuf_push(HistoryBuffer *sb, uint8_t value) {
+    if (sb->size < HISTORY_SIZE) {
+        sb->buffer[sb->rear] = value;
+        sb->rear = (sb->rear + 1) % HISTORY_SIZE;
+        sb->size++;
+        return 1; // Success
+    }
+    return 0; // Buffer full
+}
+
+int hisbuf_pop(HistoryBuffer *sb) {
+    if (sb->size > 0) {
+        sb->front = (sb->front + 1) % HISTORY_SIZE;
+        sb->size--;
+        return 1; // Success
+    }
+    return 0; // Buffer empty
+}
+
+void hisbuf_get(HistoryBuffer *sb, uint8_t *out, int lookahead) {
+    if (lookahead >= 0 && lookahead < sb->size) {
+        // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead
+        int index = (sb->rear - 1 - lookahead + HISTORY_SIZE) % HISTORY_SIZE;
+        *out = sb->buffer[index];
+    }
+}

ccs/state.h

@@ -20,3 +20,26 @@ int senbuf_push(SensorBuffer *sb, uint8_t value);
 int senbuf_pop(SensorBuffer *sb);
 
 void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead);
+
+
+
+// History Buffer
+
+#define HISTORY_SIZE 20000
+
+typedef struct {
+    uint8_t buffer[HISTORY_SIZE];
+    int front;
+    int rear;
+    int size;
+} HistoryBuffer;
+
+void hisbuf_init(HistoryBuffer *sb);
+
+int hisbuf_is_full(HistoryBuffer *sb);
+
+int hisbuf_push(HistoryBuffer *sb, uint8_t value);
+
+int hisbuf_pop(HistoryBuffer *sb);
+
+void hisbuf_get(HistoryBuffer *sb, uint8_t *out, int lookahead);

ccs/util.c

@@ -95,4 +95,13 @@ int is_crossroad_robust(SensorBuffer *sb) {
     return 1;
 fail:
     return 0;
-}
+}
+
+void print_binary(const uint8_t n) {
+    int i;
+    for (i = 7; i >= 0; i--) printf("%d", (n >> i) & 0b1);
+}
+
+uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value) {
+    return mask & ir_value;
+}

ccs/util.h

@@ -8,4 +8,8 @@ int get_error(uint8_t raw);
 
 int is_crossroad(uint8_t raw);
 
-int is_crossroad_robust(SensorBuffer *sb);
+int is_crossroad_robust(SensorBuffer *sb);
+
+void print_binary(const uint8_t n);
+
+uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value);