get_error 롤백

ccs/history.h

@@ -3,7 +3,7 @@
 
 #include <stdint.h>
 
-#define HISTORY_SIZE 20000
+#define HISTORY_SIZE 4800
 
 typedef struct HistoryEntry {
     uint8_t sensor;

ccs/main.c

@@ -10,124 +10,115 @@
 #include "motor.h"
 #include "util.h"
 
-enum State {
-    STATE_ALIGNING,
-    STATE_ALIGNED,
-    STATE_SHOOTING,
-    STATE_STOP,
-};
-
-
-
 // PID Control Constants
 // Error range: -14 ~ +14 (Max 20)
 // PWM Period: 15000
 const float Kp = 150.0f;// P gain: 150 * 14 = 2100 (Significant correction)
 const float Kd = 600.0f;// D gain
 
-#define BASE_SPEED 1500
-#define MAX_DRV 3000
+#define BASE_SPEED 1500// Slower speed (approx 10% duty of 15000)
+#define MAX_DRV 3000   // Max speed limit (20% duty)
 
 #define PERIOD_MS 20
 
-enum State current_state = STATE_ALIGNING;
-
-int main() {
+void main() {
     Clock_Init48MHz();
     ir_init();
     motor_init();
 
-    // 초기 안정화 대기
-    Clock_Delay1ms(500);
+    Clock_Delay1ms(1000);// 휴식
+    motor_stop();
 
-    while(1) {
-        switch(current_state) {
-            case STATE_ALIGNING:
-                {
-                    // 1. 센서 값 읽기
-                    uint8_t raw_val;
-                    ir_read_ready();
-                    Clock_Delay1us(1000);
-                    ir_read_value(&raw_val);
-                    ir_read_off();
+    int error = 0;
+    int prev_error = 0;
+    float pid_out = 0;
+    int speed_l, speed_r;
 
-                    // 2. 에러 계산
-                    int error = get_error(raw_val);
-                    
-                    // 3. 정렬 완료 조건 확인 (에러가 거의 0인 상태 유지)
-                    // util.c의 가중치: {-14, ..., 14}
-                    // error 0: 완전 중앙, error +/- 2: 약간 벗어남
-                    static int aligned_count = 0;
-                    if (abs(error) <= 2) { 
-                        aligned_count++;
-                    } else {
-                        aligned_count = 0;
-                    }
+    HistoryEntry entry;
+    HistoryBuffer hisbuf;
+    hisbuf_init(&hisbuf);
 
-                    if (aligned_count > 25) { 
-                        motor_stop(); // 잠시 대기
-                        current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
-                        break;
-                    }
+    int black_cnt = 0;
 
-                    // 4. 제자리 회전 (Pivot Turn)
-                    // BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
-                    float pid_out = Kp * error; 
-                    
-                    int speed_l = (int)pid_out;
-                    int speed_r = -(int)pid_out;
+    int trace_mode = 0;// 0: Normal, 1: Traceback
+    // 2. Main Loop
+    while (1) {
+        // A. Read Sensor
+        uint8_t raw_val;
+        ir_read_ready();
+        Clock_Delay1us(1000);// Wait for IR LED
+        ir_read_value(&raw_val);
+        ir_read_off();
 
-                    // 최소 기동 토크 보정 (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;
+        // B. Line Lost Handling (White Area) -> Replay History from Beginning (FIFO)
+        if (raw_val == 0x00 || trace_mode == 1) {
+            trace_mode = 1;// Enter Traceback Mode
+            motor_stop();
+            Clock_Delay1ms(1000);// Wait a bit before replay
+            
+            if (raw_val != 0x00) {
+                black_cnt ++;
+            } else {
+                black_cnt = 0;
+            }
+            if (black_cnt >= 3) {
+                goto finish;
+            }
+            
+            // Replay entire history from the beginning (FIFO)
+            static int replay_prev_error = 0;
 
-                    // 최대 속도 제한
-                    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;
+            if (hisbuf_pop_data(&hisbuf, &entry)) {
+                // Reconstruct PID
+                float rec_pid = (Kp * entry.error) + (Kd * (entry.error - replay_prev_error));
+                replay_prev_error = entry.error;
 
-                    motor_move(speed_l, speed_r);
-                    
-                    Clock_Delay1ms(PERIOD_MS);
-                }
-                break;
+                int rec_speed_l = BASE_SPEED + (int) rec_pid;
+                int rec_speed_r = BASE_SPEED - (int) rec_pid;
 
-            case STATE_ALIGNED:
-                motor_stop();
-                Clock_Delay1ms(500); // 잠시 대기
-                current_state = STATE_SHOOTING;
-                break;
+                // Limit
+                if (rec_speed_l > MAX_DRV) rec_speed_l = MAX_DRV;
+                if (rec_speed_l < -MAX_DRV) rec_speed_l = -MAX_DRV;
+                if (rec_speed_r > MAX_DRV) rec_speed_r = MAX_DRV;
+                if (rec_speed_r < -MAX_DRV) rec_speed_r = -MAX_DRV;
 
-            case STATE_SHOOTING:
-                {
-                    uint8_t raw_val;
-                    ir_read_ready();
-                    Clock_Delay1us(1000);
-                    ir_read_value(&raw_val);
-                    ir_read_off();
+                // Move Forward (Replay)
+                motor_move(rec_speed_l, rec_speed_r);
 
-                    // 2. 직진 (PID 없이 등속 주행)
-                    motor_move(BASE_SPEED, BASE_SPEED);
+                Clock_Delay1ms(2);
+            }
 
-                    // 3. 왼쪽 라인(검은색) 감지 시 정지
-                    // 출발 직후(오른쪽 라인)에는 감지될 수 있으므로, 
-                    // 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요
-                    if (is_crossroad(raw_val)) {
-                        current_state = STATE_STOP;
-                    }
-                    Clock_Delay1ms(PERIOD_MS);
-                }
-                break;
+        } else {
+            if (is_crossroad_robust(&hisbuf)) {
+                goto finish;
+            }
+            // C. Normal PID Control
+            error = get_error(raw_val);
+            pid_out = (Kp * error) + (Kd * (error - prev_error));
+            prev_error = error;
 
-            case STATE_STOP:
-                motor_stop();
-                break;
+            // D. Motor Control
+            speed_l = BASE_SPEED + (int) pid_out;
+            speed_r = BASE_SPEED - (int) pid_out;
+
+            // E. Speed Limiting
+            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);
+
+            // Save to History
+            HistoryEntry entry = {raw_val, error};
+            hisbuf_push(&hisbuf, entry);
         }
+
+        // F. Loop Delay (Sampling Time)
+        Clock_Delay1ms(PERIOD_MS);
     }
+
+finish:
+    motor_stop();
+    while(1);
 }

ccs/motor.c

@@ -42,13 +42,13 @@ void motor_move(int32_t left, int32_t right) {
     } else if (left >= 0 && right < 0) {
         // fwd back
         P3->OUT |= 0xc0;
-        P5->OUT |= (P5->OUT & (~0x30)) | 0x10;
+        P5->OUT |= (P5->OUT & (~0x30)) | 0x20;
         pwm_set_duty_left((uint16_t) left);
         pwm_set_duty_right((uint16_t) (-right));
     } else if (left < 0 && right >= 0) {
         // back fwd
         P3->OUT |= 0xc0;
-        P5->OUT |= (P5->OUT & (~0x30)) | 0x20;
+        P5->OUT |= (P5->OUT & (~0x30)) | 0x10;
         pwm_set_duty_left((uint16_t) (-left));
         pwm_set_duty_right((uint16_t) right);
     }

ccs/util.c

@@ -15,65 +15,32 @@ 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. 노이즈 제거 및 보정
+    // 반대의 경우도 마찬가지.
 
-    // [Step 1] Gap Filling (구멍 메우기)
-    // 센서 불량이나 바닥 상태로 인해 라인 중간이 끊겨 보이는 경우(101)를 보정
-    // 예: 01011000 -> 01111000 (중간의 0을 1로 채움)
-    uint8_t filled_raw = raw;
-    int k;
-    for (k = 1; k < 7; k++) {
-        // 현재 비트가 0이고, 좌우 비트가 모두 1인 경우
-        if ( !((raw >> k) & 1) && ((raw >> (k+1)) & 1) && ((raw >> (k-1)) & 1) ) {
-            filled_raw |= (1 << k);
-        }
+    uint8_t filtered_raw = raw;
+
+    // 로봇이 확실히 오른쪽(>1000)에 있었는데, 갑자기 왼쪽 끝(0,1번 비트)이 켜짐?
+    if (last_valid_error > 1000 && (raw & 0b00000011)) {
+        filtered_raw &= ~0b00000011;// 0, 1번 비트 강제 삭제 (Masking)
+    }
+    // 로봇이 확실히 왼쪽(<-1000)에 있었는데, 갑자기 오른쪽 끝(6,7번 비트)이 켜짐?
+    else if (last_valid_error < -1000 && (raw & 0b11000000)) {
+        filtered_raw &= ~0b11000000;// 6, 7번 비트 강제 삭제
     }
 
-    // [Step 2] Largest Segment Selection (가장 큰 덩어리 선택)
-    // 보정된 데이터(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;
+    // 만약 필터링했더니 남는 게 없다면? (노이즈만 떴던 경우) -> 원본 사용 or 이전 값 리턴
+    if (filtered_raw == 0x00) filtered_raw = raw;
 
     // 3. 가중 평균 계산 (Weighted Average)
     long sum_weighted = 0;
@@ -99,6 +66,7 @@ int get_error(uint8_t raw) {
     return current_error;
 }
 
+
 // 센서 비트 중 1의 개수를 세서 T자/십자 여부 판단
 int is_crossroad(uint8_t raw) {
     int count = 0;