some change to check crossload

ccs/history.c

@@ -0,0 +1,48 @@
+#include "history.h"
+
+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, HistoryEntry entry) {
+    if (sb->size < HISTORY_SIZE) {
+        sb->data[sb->rear] = entry;
+        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
+}
+
+int hisbuf_pop_data(HistoryBuffer *sb, HistoryEntry *out) {
+    if (sb->size > 0) {
+        *out = sb->data[sb->front];
+        sb->front = (sb->front + 1) % HISTORY_SIZE;
+        sb->size--;
+        return 1; // Success
+    }
+    return 0; // Buffer empty
+}
+
+void hisbuf_get(HistoryBuffer *sb, HistoryEntry *out, int lookahead) {
+    if (lookahead >= 0 && lookahead < sb->size) {
+        // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead
+        int index = (sb->rear - 1 - lookahead + HISTORY_SIZE) % HISTORY_SIZE;
+        *out = sb->data[index];
+    }
+}

ccs/history.h

@@ -0,0 +1,33 @@
+#ifndef _history_h_
+#define _history_h_
+
+#include <stdint.h>
+
+#define HISTORY_SIZE 20000
+
+typedef struct HistoryEntry {
+    uint8_t sensor;
+    int error;
+} HistoryEntry;
+
+typedef struct {
+    HistoryEntry data[HISTORY_SIZE];
+    int errors[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, HistoryEntry entry);
+
+int hisbuf_pop(HistoryBuffer *sb);
+
+int hisbuf_pop_data(HistoryBuffer *sb, HistoryEntry *out);
+
+void hisbuf_get(HistoryBuffer *sb, HistoryEntry *out, int lookahead);
+
+#endif

ccs/ir.c

@@ -17,12 +17,13 @@ void ir_init(void) {
 }
 
 void ir_read_ready() {
+    P7->DIR = 0xff;
+    P7->OUT &= ~0xff; /* initially offed */
     P5->OUT |= 0x08;
     P9->OUT |= 0x04;
-    P7->DIR = 0xff;
+    P7->OUT |= 0xff; /* init charge the caps */
     Clock_Delay1us(10);
-    P7->DIR = 0x00;
+    P7->DIR = 0x00;  /* set input */
-    Clock_Delay1us(1000);
 }
 
 void ir_read_value(uint8_t *value) {
@@ -40,6 +41,7 @@ void test_ir(void) {
 
     while (1) {
         ir_ready_ready();
+        Clock_Delay1us(1000); // 1ms
         ir_read_value(&ir_value);
         print_binary(ir_value);
         printf("\n");

ccs/ir.h

@@ -1,4 +1,5 @@
-#pragma once
+#ifndef _ir_h_
+#define _ir_h_
 
 #include "msp.h"
 #include "util.h"
@@ -13,3 +14,5 @@ void ir_read_value(uint8_t *value);
 void ir_read_off(void);
 
 void test_ir(void);
+
+#endif

ccs/main.c

@@ -1,574 +1,133 @@
-#include <stdio.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <stdlib.h>
 
 #include "Clock.h"
 #include "msp.h"
 
+#include "history.h"
 #include "ir.h"
 #include "motor.h"
-#include "state.h"
 #include "util.h"
 
-#define TRUE 1
+enum State {
-#define FALSE 0
+    STATE_ALIGNING,
+    STATE_ALIGNED,
+    STATE_SHOOTING,
+    STATE_STOP,
+};
 
-#define PRACTICE 4
 
-#if PRACTICE == 1
 
-#define LOOP_PERIOD_MS 20
+// PID Control Constants
-#define REC_INTERVAL 5
+// Error range: -14 ~ +14 (Max 20)
-#define MAX_REC_SIZE 1000
+// PWM Period: 15000
+const float Kp = 150.0f;// P gain: 150 * 14 = 2100 (Significant correction)
+const float Kd = 600.0f;// D gain
 
-const float Kp = 0.005f;   // proportional gain
+#define BASE_SPEED 1500
-const float Kd = 0.02f;    // derivative gain
+#define MAX_DRV 3000
-#define BASE_SPEED 10       // nominal forward speed (0~20)
-#define MAX_DRV 20          // absolute max speed
 
-static int pid_history[MAX_REC_SIZE];
+#define PERIOD_MS 20
-static int history_idx = 0;
 
-void LineTracerPrac1(void) {
+enum State current_state = STATE_ALIGNING;
-    motor_init();
+
+int main() {
+    Clock_Init48MHz();
     ir_init();
+    motor_init();
 
-    SensorBuffer sb;
+    // 초기 안정화 대기
-    senbuf_init(&sb);
+    Clock_Delay1ms(500);
-
-    int error = 0;
-    int prev_error = 0;
-    float pid_out = 0;
-    int speed_l, speed_r;
-    int loop_cnt = 0; // 다운샘플링용 카운터
-
-    // ------------------------------------------------
-    // 2. [PHASE 1] 출발 및 기록 (Forward & Record)
-    // ------------------------------------------------
-    while (1) {
-        // A. 센서 값 읽기
-        uint8_t raw_val;
-        ir_read_ready();       // 적외선 발광 시작
-        delay_ms_simple(1);    // 안정화 대기
-        ir_read_value(&raw_val);
-        ir_read_off();         // 절전
-
-        // B. 버퍼에 저장 및 T자 감지
-        senbuf_push(&sb, raw_val);
-        
-        // T자형 교차로(끝)를 만나면 루프 탈출
-        if (is_crossroad_robust(&sb)) {
-            motor_move_stop();
-            delay_ms_simple(1000); // 1초 정지
-            break;
-        }
-
-        // C. PID 제어 계산
-        error = get_error(raw_val);
-        pid_out = (Kp * error) + (Kd * (error - prev_error));
-        prev_error = error;
-
-        // D. 모터 구동
-        // 보정값이 양수(우측 쏠림) -> 우회전 필요 -> 왼쪽 가속, 오른쪽 감속
-        speed_l = BASE_SPEED + (int)pid_out;
-        speed_r = BASE_SPEED - (int)pid_out;
-
-        // 속도 범위 제한 (0 ~ 20) - motor.h의 MAX_SPEED 참조
-        if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
-        if (speed_l < 0) speed_l = 0;
-        if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
-        if (speed_r < 0) speed_r = 0;
-
-        motor_move_custom(speed_l, speed_r);
-
-        // E. 경로 기록 (Downsampling)
-        loop_cnt++;
-        if (loop_cnt >= REC_INTERVAL) {
-            if (history_idx < MAX_REC_SIZE) {
-                // 현재 적용된 PID 보정값을 저장
-                pid_history[history_idx++] = (int)pid_out;
-            }
-            loop_cnt = 0;
-        }
-
-        delay_ms_simple(2); // 루프 주기
-    }
-
-    // ------------------------------------------------
-    // 3. [PHASE 2] U턴 (180도 회전)
-    // ------------------------------------------------
-    // T자 라인을 벗어나기 위해 살짝 후진 (선택 사항)
-    motor_move_backward(10);
-    delay_ms_simple(200);
-    
-    // 제자리 회전 (Pivot Turn)
-    // 왼쪽: 20, 오른쪽: -20 (motor_rotate_clockwise 함수 활용)
-    motor_rotate_clockwise(15);
-    
-    // 회전 시간 제어: 라인을 찾을 때까지 돔
-    // (처음 0.5초는 무조건 돌아서 기존 라인을 벗어남)
-    delay_ms_simple(500); 
 
     while(1) {
-        uint8_t raw_turn;
+        switch(current_state) {
-        ir_read_ready();
+            case STATE_ALIGNING:
-        ir_read_value(&raw_turn);
+                {
-        ir_read_off();
+                    // 1. 센서 값 읽기
+                    uint8_t raw_val;
+                    ir_read_ready();
+                    Clock_Delay1us(1000);
+                    ir_read_value(&raw_val);
+                    ir_read_off();
 
-        // 중앙 센서(00011000)가 감지되면 정지
+                    // 2. 에러 계산
-        if (raw_turn & 0b00011000) {
+                    int error = get_error(raw_val);
-            motor_move_stop();
+                    
-            delay_ms_simple(500); // 안정화
+                    // 3. 정렬 완료 조건 확인 (에러가 거의 0인 상태 유지)
-            break;
+                    // util.c의 가중치: {-14, ..., 14}
+                    // error 0: 완전 중앙, error +/- 2: 약간 벗어남
+                    static int aligned_count = 0;
+                    if (abs(error) <= 2) { 
+                        aligned_count++;
+                    } else {
+                        aligned_count = 0;
+                    }
+
+                    if (aligned_count > 25) { 
+                        motor_stop(); // 잠시 대기
+                        current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
+                        break;
+                    }
+
+                    // 4. 제자리 회전 (Pivot Turn)
+                    // BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
+                    float pid_out = Kp * error; 
+                    
+                    int speed_l = (int)pid_out;
+                    int speed_r = -(int)pid_out;
+
+                    // 최소 기동 토크 보정 (Deadzone Compensation)
+                    // 모터가 돌기 시작하는 최소 PWM 값을 더해줌
+                    int min_speed = 1000; 
+                    if (speed_l > 0) speed_l += min_speed;
+                    else if (speed_l < 0) speed_l -= min_speed;
+                    
+                    if (speed_r > 0) speed_r += min_speed;
+                    else if (speed_r < 0) speed_r -= min_speed;
+
+                    // 최대 속도 제한
+                    if (speed_l > MAX_DRV) speed_l = MAX_DRV;
+                    if (speed_l < -MAX_DRV) speed_l = -MAX_DRV;
+                    if (speed_r > MAX_DRV) speed_r = MAX_DRV;
+                    if (speed_r < -MAX_DRV) speed_r = -MAX_DRV;
+
+                    motor_move(speed_l, speed_r);
+                    
+                    Clock_Delay1ms(PERIOD_MS);
+                }
+                break;
+
+            case STATE_ALIGNED:
+                motor_stop();
+                Clock_Delay1ms(500); // 잠시 대기
+                current_state = STATE_SHOOTING;
+                break;
+
+            case STATE_SHOOTING:
+                {
+                    uint8_t raw_val;
+                    ir_read_ready();
+                    Clock_Delay1us(1000);
+                    ir_read_value(&raw_val);
+                    ir_read_off();
+
+                    // 2. 직진 (PID 없이 등속 주행)
+                    motor_move(BASE_SPEED, BASE_SPEED);
+
+                    // 3. 왼쪽 라인(검은색) 감지 시 정지
+                    // 출발 직후(오른쪽 라인)에는 감지될 수 있으므로, 
+                    // 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요
+                    if (is_crossroad(raw_val)) {
+                        current_state = STATE_STOP;
+                    }
+                    Clock_Delay1ms(PERIOD_MS);
+                }
+                break;
+
+            case STATE_STOP:
+                motor_stop();
+                break;
         }
     }
-
-    // ------------------------------------------------
-    // 4. [PHASE 3] 되돌아가기 (Replay)
-    // ------------------------------------------------
-    // 기록된 인덱스의 끝부터 역순으로 재생
-    loop_cnt = 0;
-
-    while (history_idx >= 0) {
-        // 타이밍 맞추기 (기록할 때와 같은 속도로 읽기)
-        loop_cnt++;
-        if (loop_cnt >= REC_INTERVAL) {
-            history_idx--; // 인덱스 감소 (Stack Pop)
-            loop_cnt = 0;
-        }
-        
-        // 데이터가 끝났으면 종료
-        if (history_idx < 0) break;
-
-        // ★ 핵심 로직: 부호 반전
-        // 갈 때: 왼쪽으로 5만큼 꺾음 (PID = +5)
-        // 올 때: 로봇이 뒤집혔으므로 오른쪽으로 5만큼 꺾어야 함 (Replay PID = -5)
-        int replay_pid =    pid_history[history_idx];
-
-        speed_l = BASE_SPEED + replay_pid;
-        speed_r = BASE_SPEED - replay_pid;
-
-        // 속도 제한
-        if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
-        if (speed_l < 0) speed_l = 0;
-        if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
-        if (speed_r < 0) speed_r = 0;
-
-        motor_move_custom(speed_l, speed_r);
-        
-        delay_ms_simple(2); // 갈 때와 동일한 루프 딜레이
-    }
-
-    // ------------------------------------------------
-    // 5. 종료
-    // ------------------------------------------------
-    motor_move_stop();
-    while(1); // 무한 대기
-}
-
-#elif PRACTICE == 2
-
-#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");
-
-    ir_init();
-    motor_init();
-
-    SensorBuffer sb;
-    senbuf_init(&sb);
-
-    printf("Init Phase End\n");
-
-    uint8_t ir_value;
-
-    int error = 888481557;
-    int prev_error = 0;
-
-    int output_pid = 0;
-
-    int speed_left;
-    int speed_right;
-
-    while (1) {
-        ir_read_ready();
-        ir_read_value(ir_value);
-        ir_read_off();
-        int i, j;
-
-        senbuf_push(&sb, ir_value);
-        if (is_crossroad_robust(&sb)) {
-            // 교차로 처리 로직
-            motor_move_stop();
-            while(1) {} // succ
-        }
-
-
-
-        error = get_error(ir_value);
-        output_pid = (int)(Kp * error + Kd * (error - prev_error));
-        prev_error = error;
-
-
-        // Differential drive speeds based on PID correction
-        // Left slows down when error positive (line to right), right speeds up
-        speed_left  = BASE_SPEED - output_pid;
-        speed_right = BASE_SPEED + output_pid;
-
-        // Clamp to allowed range [-20, 20]
-        if (speed_left  >  MAX_DRV) speed_left  =  MAX_DRV;
-        if (speed_left  < -MAX_DRV) speed_left  = -MAX_DRV;
-        if (speed_right >  MAX_DRV) speed_right =  MAX_DRV;
-        if (speed_right < -MAX_DRV) speed_right = -MAX_DRV;
-
-        // Move motors with custom differential speeds
-        motor_move_custom(speed_left, speed_right);
-
-        Clock_Delay1ms(LOOP_PERIOD_MS);
-
-    }
-}
-
-#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;
-}
-
-
-#elif PRACTICE == 4
-
-void LineTracerPrac4(void) {
-    Clock_Init48MHz();
-    ir_init();
-    motor_init();
-
-    // PID Constants
-    const float Kp = 0.005f;
-    const float Kd = 0.02f;
-    const int BASE_SPEED = 10;
-    const int MAX_DRV = 20;
-
-    int error = 0;
-    int prev_error = 0;
-    float pid_out = 0;
-    int speed_l, speed_r;
-
-    while (1) {
-        uint8_t raw_val;
-        ir_read_ready();
-        Clock_Delay1ms(1);
-        ir_read_value(&raw_val);
-        ir_read_off();
-
-        error = get_error(raw_val);
-        pid_out = (Kp * error) + (Kd * (error - prev_error));
-        prev_error = error;
-
-        speed_l = BASE_SPEED + (int)pid_out;
-        speed_r = BASE_SPEED - (int)pid_out;
-
-        if (speed_l > MAX_DRV) speed_l = MAX_DRV;
-        if (speed_l < 0) speed_l = 0;
-        if (speed_r > MAX_DRV) speed_r = MAX_DRV;
-        if (speed_r < 0) speed_r = 0;
-
-        motor_move_custom(speed_l, speed_r);
-
-        Clock_Delay1ms(2);
-    }
-}
-#endif
-
-int main(void) {
-    #if PRACTICE == 1
-    LineTracerPrac1();
-    #elif PRACTICE == 2
-    LineTracerPrac2();
-    #elif PRACTICE == 3
-    LineTracerPrac3();
-    #elif PRACTICE == 4
-    LineTracerPrac4();
-    #else
-    printf("No practice selected.\n");
-    #endif
-    return 0;
 }

ccs/motor.c

@@ -2,198 +2,60 @@
 #include "Clock.h"
 #include <stdio.h>
 
-// Period: 20ms
-// MAX speed: 20
 
-static void normalize_speed(int *speed) {
-    if (*speed <= 0) *speed = 0;
-    else if (*speed > MAX_SPEED) *speed = MAX_SPEED;
-}
 
 void motor_init(void) {
+    P2->SEL0 &= ~0xc0;
+    P2->SEL1 &= ~0xc0;
+    P2->DIR |= 0xc0; // output
+    P2->OUT &= ~0xc0;// clear
+
     P3->SEL0 &= ~0xc0;
     P3->SEL1 &= ~0xc0;
-    P3->DIR  |=  0xc0;
+    P3->DIR |= 0xc0; // output
-    P3->OUT  &= ~0xc0;
+    P3->OUT &= ~0xc0;// clear
 
     P5->SEL0 &= ~0x30;
     P5->SEL1 &= ~0x30;
-    P5->DIR  |=  0x30;
+    P5->DIR |= 0x30;
-    P5->OUT  &= ~0x30;
+    P5->OUT &= ~0x30;
 
-    P2->SEL0 &= ~0xc0;
+    pwm_init_for_motors(MOTOR_PERIOD);
-    P2->SEL1 &= ~0xc0;
-    P2->DIR  |=  0xc0;
-    P2->OUT  &= ~0xc0;
 }
 
-static void set_dir_l(char dir) { // f: forward, b: backward
+void motor_move(int32_t left, int32_t right) {
-    if      (dir == 'f') P5->OUT &= ~0b00010000;
+    if (left == 0 && right == 0) {
-    else if (dir == 'b') P5->OUT |=  0b00010000;
+        pwm_set_duty_left(0);
-}
+        pwm_set_duty_right(0);
-
+    } else if (left >= 0 && right >= 0) {
-static void set_dir_r(char dir) { // f: forward, b: backward
+        // fwd fwd
-    if      (dir == 'f') P5->OUT &= ~0b00100000;
+        P3->OUT |= 0xc0;
-    else if (dir == 'b') P5->OUT |=  0b00100000;
+        P5->OUT &= ~0x30;
-}
+        pwm_set_duty_left((uint16_t) left);
-
+        pwm_set_duty_right((uint16_t) right);
-static void set_en_l(int enable) { // 1: on, 0: off
+    } else if (left < 0 && right < 0) {
-    if      (enable == 1) P2->OUT |=  0b10000000;
+        // back back
-    else if (enable == 0) P2->OUT &= ~0b10000000;
+        P3->OUT |= 0xc0;
-}
+        P5->OUT = 0x30;
-
+        pwm_set_duty_left((uint16_t) (-left));
-static void set_en_r(int enable) { // 1: on, 0: off
+        pwm_set_duty_right((uint16_t) (-right));
-    if      (enable == 1) P2->OUT |=  0b01000000;
+    } else if (left >= 0 && right < 0) {
-    else if (enable == 0) P2->OUT &= ~0b01000000;
+        // fwd back
-}
+        P3->OUT |= 0xc0;
-
+        P5->OUT |= (P5->OUT & (~0x30)) | 0x10;
-void motor_move_stop() {
+        pwm_set_duty_left((uint16_t) left);
-    // P2->OUT  &= ~0xc0;
+        pwm_set_duty_right((uint16_t) (-right));
-    set_en_l(0);
+    } else if (left < 0 && right >= 0) {
-    set_en_r(0);
+        // back fwd
-}
+        P3->OUT |= 0xc0;
-
+        P5->OUT |= (P5->OUT & (~0x30)) | 0x20;
-static void move_forward() {
+        pwm_set_duty_left((uint16_t) (-left));
-    P5->OUT  &= ~0b00110000;
+        pwm_set_duty_right((uint16_t) right);
-    P3->OUT  |=  0b11000000;
-    P2->OUT  |=  0b11000000;
-}
-
-static void move_backward() {
-    P5->OUT  |=  0b00110000;
-    P3->OUT  |=  0b11000000;
-    P2->OUT  |=  0b11000000;
-}
-
-static void turn_left() {
-    P3->OUT  |=  0b11000000;
-    P2->OUT  &= ~0b10000000;
-    P2->OUT  |=  0b01000000;
-}
-
-static void turn_right() {
-    P3->OUT  |=  0b11000000;
-    P2->OUT  &= ~0b01000000;
-    P2->OUT  |=  0b10000000;
-}
-
-static void rotate_clockwise() {
-    P5->OUT  &= ~0b00110000;
-    P5->OUT  |=  0b00100000;
-    P3->OUT  |=  0b11000000;
-    P2->OUT  |=  0b11000000;
-}
-
-static void rotate_cclockwise() {
-    P5->OUT  &= ~0b00110000;
-    P5->OUT  |=  0b00010000;
-    P3->OUT  |=  0b11000000;
-    P2->OUT  |=  0b11000000;
-}
-
-void motor_move_forward(int speed) {
-    normalize_speed(&speed);
-
-    move_forward();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-}
-
-void motor_move_backward(int speed) {
-    normalize_speed(&speed);
-
-    move_backward();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-}
-
-void motor_turn_left(int speed) {
-    normalize_speed(&speed);
-
-    turn_left();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-}
-
-void motor_turn_right(int speed) {
-    normalize_speed(&speed);
-
-    turn_right();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-}
-
-void motor_rotate_clockwise(int speed) {
-    normalize_speed(&speed);
-
-    rotate_clockwise();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-
-    set_dir_l('f');
-    set_dir_r('f');
-    // move_forward();
-    // motor_move_stop();
-}
-
-void motor_rotate_cclockwise(int speed) {
-    normalize_speed(&speed);
-
-    rotate_cclockwise();
-    Clock_Delay1ms(speed);
-
-    motor_move_stop();
-    Clock_Delay1ms(PERIOD_ms - speed);
-
-    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);
-
     }
 }
+
+void motor_stop(void) {
+    pwm_set_duty_left(0);
+    pwm_set_duty_right(0);
+    P3->OUT &= ~0xc0;
+}

ccs/motor.h

@@ -2,18 +2,26 @@
 #define _motor_h_
 
 #include "msp.h"
+#include "pwm.h"
+#include <stdint.h>
 
-#define PERIOD_ms 20
+#define MOTOR_PERIOD 15000
-#define MAX_SPEED PERIOD_ms
 
-void motor_init();
+/**
-void motor_move_stop();
+ * @brief 모터 핀과 PWM 초기화
-void motor_move_forward(int speed);
+ * P2.6 P2.7 : PWM 출력
-void motor_move_backward(int speed);
+ * P5.4 P5.5 : 방향 제어
-void motor_turn_left(int speed);
+ * P3.6 P3.7 : 전원 공급
-void motor_turn_right(int speed);
+ */
-void motor_rotate_clockwise(int speed);
+void motor_init(void);
-void motor_rotate_cclockwise(int speed);
+
-void motor_move_custom(int left_speed, int right_speed);
+/**
+ * @brief Set motor speeds.
+ * @param left 왼쪽 모터 PWM
+ * @param right 오른쪽 모터 PWM (-1000 ~ 1000)
+ */
+void motor_move(int32_t left, int32_t right);
+
+void motor_stop(void);
 
 #endif

ccs/pwm.c

@@ -0,0 +1,31 @@
+
+#include "pwm.h"
+
+void pwm_init_for_motors(uint16_t period) {
+    P2->DIR |= 0xc0;
+    P2->SEL0 |= 0xc0;
+    P2->SEL1 &= ~0xc0;
+
+    // 전체 주기 설정
+    TIMER_A0->CCTL[0] = 0x0080;
+    TIMER_A0->CCR[0] = period;
+    TIMER_A0->EX0 = 0x00000;
+    // 세부 주기 및 출력 설정
+    TIMER_A0->CCTL[3] = 0x0040;
+    TIMER_A0->CCR[3] = 0; // set duty left
+    TIMER_A0->CCTL[4] = 0x0040;
+    TIMER_A0->CCR[4] = 0; // set duty right
+    // 모드
+    TIMER_A0->CTL = 0x02f0;
+    // SMCLK 12MHz, divide by 8, up-down mode
+}
+
+void pwm_set_duty_right(uint16_t duty) {
+    if (duty > TIMER_A0->CCR[0]) duty = TIMER_A0->CCR[0];
+    TIMER_A0->CCR[3] = duty;
+}
+
+void pwm_set_duty_left(uint16_t duty) {
+    if (duty > TIMER_A0->CCR[0]) duty = TIMER_A0->CCR[0];
+    TIMER_A0->CCR[4] = duty;
+}

ccs/pwm.h

@@ -0,0 +1,11 @@
+#pragma once
+#include "msp.h"
+#include <stdint.h>
+
+/**
+ * @brief Initialize PWM for motor control with the specified period.
+ * @note P2.6 과 P2.7을 PWM 출력으로 설정합니다.
+ * @param period PWM 주기 (타이머 카운트 값)
+ */
+void pwm_init_for_motors(uint16_t period);
+

ccs/state.c

@@ -1,77 +0,0 @@
-#include "state.h"
-
-void senbuf_init(SensorBuffer *sb) {
-    sb->front = 0;
-    sb->rear = 0;
-    sb->size = 0;
-}
-
-int senbuf_is_full(SensorBuffer *sb) {
-    return sb->size == STATE_SENSOR_BUFFER_SIZE;
-}
-
-int senbuf_push(SensorBuffer *sb, uint8_t value) {
-    if (sb->size < STATE_SENSOR_BUFFER_SIZE) {
-        sb->buffer[sb->rear] = value;
-        sb->rear = (sb->rear + 1) % STATE_SENSOR_BUFFER_SIZE;
-        sb->size++;
-        return 1; // Success
-    }
-    return 0; // Buffer full
-}
-
-int senbuf_pop(SensorBuffer *sb) {
-    if (sb->size > 0) {
-        sb->front = (sb->front + 1) % STATE_SENSOR_BUFFER_SIZE;
-        sb->size--;
-        return 1; // Success
-    }
-    return 0; // Buffer empty
-}
-
-void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead) {
-    if (lookahead >= 0 && lookahead < sb->size) {
-        // 가장 최근에 넣은 것(rear - 1)을 기준으로 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

@@ -1,45 +0,0 @@
-#pragma once
-
-#include <stdint.h>
-
-#define STATE_SENSOR_BUFFER_SIZE 8
-
-typedef struct {
-    uint8_t buffer[STATE_SENSOR_BUFFER_SIZE];
-    int front;
-    int rear;
-    int size;
-} SensorBuffer;
-
-void senbuf_init(SensorBuffer *sb);
-
-int senbuf_is_full(SensorBuffer *sb);
-
-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

@@ -3,44 +3,77 @@
 #include <math.h>
 #include <stdint.h>
 
-// 가중치 설정 (-4000 ~ +4000)
+// 가중치 설정 (-14 ~ +14)
 // 중앙(3번과 4번 사이)이 0이 되도록 설정
 // [0] [1] [2] [3] | [4] [5] [6] [7]
-const int SENSOR_WEIGHTS[8] = {-4000, -3000, -2000, -1000, 1000, 2000, 3000, 4000};
+const int SENSOR_WEIGHTS[8] = {-14, -10, -6, -2, 2, 6, 10, 14};
 
 // 선을 완전히 벗어났을 때 반환할 최대 에러 값
-const int MAX_ERROR = 5000;
+const int MAX_ERROR = 20;
 
 int get_error(uint8_t raw) {
     // static 변수: 함수가 끝나도 값이 사라지지 않고 기억됨 (로봇의 마지막 위치)
     static int last_valid_error = 0;
 
-    // 1. 선이 아예 안 보이는 경우 (Line Lost)
-    if (raw == 0x00) {
-        // 마지막에 왼쪽(-값)에 있었다면 -> 계속 왼쪽으로 돌아라 (-MAX_ERROR)
-        if (last_valid_error < 0) return -MAX_ERROR;
-        // 마지막에 오른쪽(+값)에 있었다면 -> 계속 오른쪽으로 돌아라 (+MAX_ERROR)
-        else
-            return MAX_ERROR;
-    }
-
     // 2. 노이즈/갈림길 처리 (Masking) - 사용자가 걱정했던 '1 0 0 0 1 1 0 0' 상황
     // 간단한 로직: 마지막 위치가 양수(오른쪽)였다면, 왼쪽 끝(0,1번) 센서는 노이즈일 확률이 높음 -> 무시
-    // 반대의 경우도 마찬가지.
+    // 2. 노이즈 제거 및 보정
 
-    uint8_t filtered_raw = raw;
+    // [Step 1] Gap Filling (구멍 메우기)
-
+    // 센서 불량이나 바닥 상태로 인해 라인 중간이 끊겨 보이는 경우(101)를 보정
-    // 로봇이 확실히 오른쪽(>1000)에 있었는데, 갑자기 왼쪽 끝(0,1번 비트)이 켜짐?
+    // 예: 01011000 -> 01111000 (중간의 0을 1로 채움)
-    if (last_valid_error > 1000 && (raw & 0b00000011)) {
+    uint8_t filled_raw = raw;
-        filtered_raw &= ~0b00000011;// 0, 1번 비트 강제 삭제 (Masking)
+    int k;
-    }
+    for (k = 1; k < 7; k++) {
-    // 로봇이 확실히 왼쪽(<-1000)에 있었는데, 갑자기 오른쪽 끝(6,7번 비트)이 켜짐?
+        // 현재 비트가 0이고, 좌우 비트가 모두 1인 경우
-    else if (last_valid_error < -1000 && (raw & 0b11000000)) {
+        if ( !((raw >> k) & 1) && ((raw >> (k+1)) & 1) && ((raw >> (k-1)) & 1) ) {
-        filtered_raw &= ~0b11000000;// 6, 7번 비트 강제 삭제
+            filled_raw |= (1 << k);
+        }
     }
 
-    // 만약 필터링했더니 남는 게 없다면? (노이즈만 떴던 경우) -> 원본 사용 or 이전 값 리턴
+    // [Step 2] Largest Segment Selection (가장 큰 덩어리 선택)
-    if (filtered_raw == 0x00) filtered_raw = raw;
+    // 보정된 데이터(filled_raw)를 기준으로 가장 큰 덩어리만 남김
+    
+    uint8_t filtered_raw = 0;
+    int max_consecutive = -1;
+    int best_mask = 0;
+    
+    int current_consecutive = 0;
+    int current_mask = 0;
+    int i;
+
+    for (i = 0; i < 8; i++) {
+        // 7번 비트(왼쪽)부터 0번 비트(오른쪽) 순으로 검사
+        if ((filled_raw >> (7 - i)) & 1) {
+            current_consecutive++;
+            current_mask |= (1 << (7 - i));
+        } else {
+            // 덩어리가 끊긴 경우
+            if (current_consecutive > 0) {
+                if (current_consecutive > max_consecutive) {
+                    max_consecutive = current_consecutive;
+                    best_mask = current_mask;
+                } else if (current_consecutive == max_consecutive) {
+                    // 덩어리 크기가 같다면, 이전 진행 방향(last_valid_error)과 가까운 쪽 선택
+                    if (last_valid_error > 0) best_mask = current_mask;
+                }
+                current_consecutive = 0;
+                current_mask = 0;
+            }
+        }
+    }
+    
+    // 마지막 비트까지 1이었던 경우 처리
+    if (current_consecutive > 0) {
+        if (current_consecutive > max_consecutive) {
+            best_mask = current_mask;
+        } else if (current_consecutive == max_consecutive) {
+            if (last_valid_error > 0) best_mask = current_mask;
+        }
+    }
+
+    filtered_raw = best_mask;
+    if (filtered_raw == 0x00) filtered_raw = filled_raw;
 
     // 3. 가중 평균 계산 (Weighted Average)
     long sum_weighted = 0;
@@ -81,14 +114,17 @@ int is_crossroad(uint8_t raw) {
         return 0;
 }
 
-int is_crossroad_robust(SensorBuffer *sb) {
+int is_crossroad_robust(HistoryBuffer *sb) {
     int crossroad_count = 0;
     int total_count = sb->size;
     int i;
+    if (total_count < 3) {
+        return 0; // 기록이 충분하지 않음
+    }
     for (i = 0; i < 3; i++) {
-        uint8_t sensor_value;
+        HistoryEntry entry;
-        senbuf_get(sb, &sensor_value, i);
+        hisbuf_get(sb, &entry, i);
-        if (!is_crossroad(sensor_value)) {
+        if (!is_crossroad(entry.sensor)) {
             goto fail;
         }
     }

ccs/util.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "state.h"
+#include "history.h"
 
 #include <stdint.h>
 
@@ -8,7 +8,7 @@ int get_error(uint8_t raw);
 
 int is_crossroad(uint8_t raw);
 
-int is_crossroad_robust(SensorBuffer *sb);
+int is_crossroad_robust(HistoryBuffer *sb);
 
 void print_binary(const uint8_t n);
 

docker-compose.yml

@@ -4,7 +4,7 @@ services:
     container_name: ccstudio
     privileged: true             # USB JTAG 장비(XDS110 등) 직접 접근 권한
     network_mode: host           # GUI 디스플레이 호환성을 위해 호스트 네트워크 사용
-    user: "1000:1000"
+    user: "0:0"
     environment:
       - TZ=Asia/Seoul
       - HOME=/workspaces