some change to check crossload

ccs/main.c

@@ -10,113 +10,124 @@
 #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// Slower speed (approx 10% duty of 15000)
+#define BASE_SPEED 1500
-#define MAX_DRV 3000   // Max speed limit (20% duty)
+#define MAX_DRV 3000
 
-void main() {
+#define PERIOD_MS 20
+
+enum State current_state = STATE_ALIGNING;
+
+int main() {
     Clock_Init48MHz();
     ir_init();
     motor_init();
 
-    Clock_Delay1ms(1000);// 휴식
+    // 초기 안정화 대기
-    motor_stop();
+    Clock_Delay1ms(500);
 
-    int error = 0;
+    while(1) {
-    int prev_error = 0;
+        switch(current_state) {
-    float pid_out = 0;
+            case STATE_ALIGNING:
-    int speed_l, speed_r;
+                {
+                    // 1. 센서 값 읽기
+                    uint8_t raw_val;
+                    ir_read_ready();
+                    Clock_Delay1us(1000);
+                    ir_read_value(&raw_val);
+                    ir_read_off();
 
-    HistoryEntry entry;
+                    // 2. 에러 계산
-    HistoryBuffer hisbuf;
+                    int error = get_error(raw_val);
-    hisbuf_init(&hisbuf);
+                    
+                    // 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;
+                    }
 
-    int black_cnt = 0;
+                    if (aligned_count > 25) { 
+                        motor_stop(); // 잠시 대기
+                        current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
+                        break;
+                    }
 
-    int trace_mode = 0;// 0: Normal, 1: Traceback
+                    // 4. 제자리 회전 (Pivot Turn)
-    // 2. Main Loop
+                    // BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
-    while (1) {
+                    float pid_out = Kp * error; 
-        // A. Read Sensor
+                    
-        uint8_t raw_val;
+                    int speed_l = (int)pid_out;
-        ir_read_ready();
+                    int speed_r = -(int)pid_out;
-        Clock_Delay1us(1000);// Wait for IR LED
-        ir_read_value(&raw_val);
-        ir_read_off();
 
-        // B. Line Lost Handling (White Area) -> Replay History from Beginning (FIFO)
+                    // 최소 기동 토크 보정 (Deadzone Compensation)
-        if (raw_val == 0x00 || trace_mode == 1) {
+                    // 모터가 돌기 시작하는 최소 PWM 값을 더해줌
-            trace_mode = 1;// Enter Traceback Mode
+                    int min_speed = 1000; 
-            motor_stop();
+                    if (speed_l > 0) speed_l += min_speed;
-            Clock_Delay1ms(1000);// Wait a bit before replay
+                    else if (speed_l < 0) speed_l -= min_speed;
-            
+                    
-            if (raw_val != 0x00) {
+                    if (speed_r > 0) speed_r += min_speed;
-                black_cnt ++;
+                    else if (speed_r < 0) speed_r -= min_speed;
-            } else {
-                black_cnt = 0;
-            }
-            if (black_cnt >= 3) {
-                goto finish;
-            }
-            
-            // Replay entire history from the beginning (FIFO)
-            static int replay_prev_error = 0;
 
-            if (hisbuf_pop_data(&hisbuf, &entry)) {
+                    // 최대 속도 제한
-                // Reconstruct PID
+                    if (speed_l > MAX_DRV) speed_l = MAX_DRV;
-                float rec_pid = (Kp * entry.error) + (Kd * (entry.error - replay_prev_error));
+                    if (speed_l < -MAX_DRV) speed_l = -MAX_DRV;
-                replay_prev_error = entry.error;
+                    if (speed_r > MAX_DRV) speed_r = MAX_DRV;
+                    if (speed_r < -MAX_DRV) speed_r = -MAX_DRV;
 
-                int rec_speed_l = BASE_SPEED + (int) rec_pid;
+                    motor_move(speed_l, speed_r);
-                int rec_speed_r = BASE_SPEED - (int) rec_pid;
+                    
+                    Clock_Delay1ms(PERIOD_MS);
+                }
+                break;
 
-                // Limit
+            case STATE_ALIGNED:
-                if (rec_speed_l > MAX_DRV) rec_speed_l = MAX_DRV;
+                motor_stop();
-                if (rec_speed_l < -MAX_DRV) rec_speed_l = -MAX_DRV;
+                Clock_Delay1ms(500); // 잠시 대기
-                if (rec_speed_r > MAX_DRV) rec_speed_r = MAX_DRV;
+                current_state = STATE_SHOOTING;
-                if (rec_speed_r < -MAX_DRV) rec_speed_r = -MAX_DRV;
+                break;
 
-                // Move Forward (Replay)
+            case STATE_SHOOTING:
-                motor_move(rec_speed_l, rec_speed_r);
+                {
+                    uint8_t raw_val;
+                    ir_read_ready();
+                    Clock_Delay1us(1000);
+                    ir_read_value(&raw_val);
+                    ir_read_off();
 
-                Clock_Delay1ms(2);
+                    // 2. 직진 (PID 없이 등속 주행)
-            }
+                    motor_move(BASE_SPEED, BASE_SPEED);
 
-        } else {
+                    // 3. 왼쪽 라인(검은색) 감지 시 정지
-            if (is_crossroad_robust(&hisbuf)) {
+                    // 출발 직후(오른쪽 라인)에는 감지될 수 있으므로, 
-                goto finish;
+                    // 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요
-            }
+                    if (is_crossroad(raw_val)) {
-            // C. Normal PID Control
+                        current_state = STATE_STOP;
-            error = get_error(raw_val);
+                    }
-            pid_out = (Kp * error) + (Kd * (error - prev_error));
+                    Clock_Delay1ms(PERIOD_MS);
-            prev_error = error;
+                }
+                break;
 
-            // D. Motor Control
+            case STATE_STOP:
-            speed_l = BASE_SPEED + (int) pid_out;
+                motor_stop();
-            speed_r = BASE_SPEED - (int) pid_out;
+                break;
-
-            // 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(2);
     }
-
-finish:
-    motor_stop();
-    while(1);
 }