#include #include #include #include "Clock.h" #include "msp.h" #include "ir.h" #include "motor.h" #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 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 LineTracerPrac1(void) { motor_init(); ir_init(); SensorBuffer sb; senbuf_init(&sb); 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; ir_read_ready(); ir_read_value(&raw_turn); ir_read_off(); // 중앙 센서(00011000)가 감지되면 정지 if (raw_turn & 0b00011000) { motor_move_stop(); delay_ms_simple(500); // 안정화 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; } #endif int main(void) { #if PRACTICE == 1 LineTracerPrac1(); #elif PRACTICE == 2 LineTracerPrac2(); #elif PRACTICE == 3 LineTracerPrac3(); #else printf("No practice selected.\n"); #endif return 0; }