diff --git a/ccs/main.c b/ccs/main.c index 13af0cb..eed47bc 100755 --- a/ccs/main.c +++ b/ccs/main.c @@ -10,113 +10,133 @@ #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 MAX_DRV 3000 // Max speed limit (20% duty) +#define BASE_SPEED 1500 +#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; - int prev_error = 0; - float pid_out = 0; - int speed_l, speed_r; + int align_phase = 0; // 0: 출발 라인 벗어나기, 1: 목표 라인 찾기 - HistoryEntry entry; - HistoryBuffer hisbuf; - hisbuf_init(&hisbuf); + 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 black_cnt = 0; + // 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; + } - 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(); + if (aligned_count > 25) { + motor_stop(); // 잠시 대기 + current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동 + break; + } - // 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; + // 4. 제자리 회전 (Pivot Turn) + // BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤 + float pid_out = Kp * error; + + int speed_l = (int)pid_out; + int speed_r = -(int)pid_out; - 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; + // 최소 기동 토크 보정 (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; - int rec_speed_l = BASE_SPEED + (int) rec_pid; - int rec_speed_r = BASE_SPEED - (int) rec_pid; + // 최대 속도 제한 + 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; - // 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; + motor_move(speed_l, speed_r); + + Clock_Delay1ms(PERIOD_MS); + } + break; - // Move Forward (Replay) - motor_move(rec_speed_l, rec_speed_r); + case STATE_ALIGNED: + motor_stop(); + Clock_Delay1ms(500); // 잠시 대기 + current_state = STATE_SHOOTING; + break; - Clock_Delay1ms(2); - } + case STATE_SHOOTING: + { + uint8_t raw_val; + ir_read_ready(); + Clock_Delay1us(1000); + ir_read_value(&raw_val); + ir_read_off(); - } 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; + // 2. 직진 (PID 없이 등속 주행) + motor_move(BASE_SPEED, BASE_SPEED); - // D. Motor Control - speed_l = BASE_SPEED + (int) pid_out; - speed_r = BASE_SPEED - (int) pid_out; + // 3. 왼쪽 라인(검은색) 감지 시 정지 + // 출발 직후(오른쪽 라인)에는 감지될 수 있으므로, + // 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요 + static int left_line_phase = 0; + + if (left_line_phase == 0) { + if (raw_val == 0x00) left_line_phase = 1; // 출발 라인 벗어남 + } else if (left_line_phase == 1) { + if (raw_val != 0x00) { // 목표 라인 도착 + motor_stop(); + current_state = STATE_STOP; + } + } + Clock_Delay1ms(PERIOD_MS); + } + 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); + case STATE_STOP: + motor_stop(); + break; } - - // F. Loop Delay (Sampling Time) - Clock_Delay1ms(2); } - -finish: - motor_stop(); - while(1); } \ No newline at end of file