diff --git a/.gitignore b/.gitignore index d00ed28..7f1712f 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,5 @@ -res \ No newline at end of file +res +.metadata +.settings +Debug +Release \ No newline at end of file diff --git a/ccs/.vscode/settings.json b/ccs/.vscode/settings.json index e5d34c0..c1d4ce5 100644 --- a/ccs/.vscode/settings.json +++ b/ccs/.vscode/settings.json @@ -1,5 +1,6 @@ { "files.associations": { - "msp.h": "c" + "msp.h": "c", + "clock.h": "c" } } \ No newline at end of file diff --git a/ccs/ir.c b/ccs/ir.c index 922821a..7c72e12 100644 --- a/ccs/ir.c +++ b/ccs/ir.c @@ -25,7 +25,7 @@ void ir_read_ready() { Clock_Delay1us(1000); } -void ir_read_value(char *value) { +void ir_read_value(uint8_t *value) { int i; uint8_t port_value = P7->IN; for (i = 0; i < 8; i++) { diff --git a/ccs/ir.h b/ccs/ir.h index 250b5c9..ded8d1c 100644 --- a/ccs/ir.h +++ b/ccs/ir.h @@ -7,6 +7,6 @@ void ir_init(void); void ir_read_ready(void); -void ir_read_value(char *value); +void ir_read_value(uint8_t *value); void ir_read_off(void); \ No newline at end of file diff --git a/ccs/main.c b/ccs/main.c index 911179e..f20d2b6 100644 --- a/ccs/main.c +++ b/ccs/main.c @@ -1,4 +1,6 @@ #include +#include +#include #include "Clock.h" #include "msp.h" @@ -9,11 +11,167 @@ #include "util.h" #define LOOP_PERIOD_MS 20 +#define REC_INTERVAL 5 +#define MAX_REC_SIZE 1000 -const float Kp = 0.8f; -const float Kd = 4.5f; +// PID gains tuned for speed range [-20, 20] +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 -void main(void) { +#define Practice1 + + +#ifdef Practice1 + +static int pid_history[MAX_REC_SIZE]; +static int history_idx = 0; + +void LineTracerPrac1(void) { + // ------------------------------------------------ + // 1. 초기화 + // ------------------------------------------------ + 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); // 무한 대기 +} + +#endif + +#ifdef Practice2 +void LineTracerPrac(void) { Clock_Init48MHz(); printf("Init Phase Begin\n"); @@ -42,9 +200,9 @@ void main(void) { int i, j; senbuf_push(&sb, ir_value); - if(check_crossroad_robust(&sb)) { + if (is_crossroad_robust(&sb)) { // 교차로 처리 로직 - move_stop(); + motor_move_stop(); while(1) {} // succ } @@ -52,14 +210,30 @@ void main(void) { error = get_error(ir_value); output_pid = (int)(Kp * error + Kd * (error - prev_error)); + prev_error = error; - // clamp; + // 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; - // motor driverev_error = error; + // 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); - } } + +#endif + +int main(void) { + LineTracerPrac(); +} \ No newline at end of file diff --git a/ccs/motor.h b/ccs/motor.h index ef88a61..223cf3f 100644 --- a/ccs/motor.h +++ b/ccs/motor.h @@ -1,6 +1,8 @@ #ifndef _motor_h_ #define _motor_h_ +#include "msp.h" + #define PERIOD_ms 20 #define MAX_SPEED PERIOD_ms diff --git a/ccs/util.c b/ccs/util.c index d81f197..c2bce2a 100644 --- a/ccs/util.c +++ b/ccs/util.c @@ -45,8 +45,8 @@ int get_error(uint8_t raw) { // 3. 가중 평균 계산 (Weighted Average) long sum_weighted = 0; int sum_active = 0; - - for (int i = 0; i < 8; i++) { + int i; + for (i = 0; i < 8; i++) { // i번째 비트가 1인지 확인 if ((filtered_raw >> (7 - i)) & 1) { sum_weighted += SENSOR_WEIGHTS[i]; @@ -69,7 +69,8 @@ int get_error(uint8_t raw) { // 센서 비트 중 1의 개수를 세서 T자/십자 여부 판단 int is_crossroad(uint8_t raw) { int count = 0; - for (int i = 0; i < 8; i++) { + int i; + for (i = 0; i < 8; i++) { if ((raw >> i) & 1) {// i번째 비트가 1이면 카운트 count++; } @@ -83,8 +84,8 @@ int is_crossroad(uint8_t raw) { int is_crossroad_robust(SensorBuffer *sb) { int crossroad_count = 0; int total_count = sb->size; - - for (int i = 0; i < 3; i++) { + int i; + for (i = 0; i < 3; i++) { uint8_t sensor_value; senbuf_get(sb, &sensor_value, i); if (!is_crossroad(sensor_value)) {