From a1da126ee03ec52b47a67813a3e48ca0b4a25e34c300f260ea84de6bbb81220a Mon Sep 17 00:00:00 2001 From: yenru0 Date: Wed, 10 Dec 2025 15:42:06 +0900 Subject: [PATCH] =?UTF-8?q?get=5Ferror=20=EB=A1=A4=EB=B0=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ccs/history.h | 2 +- ccs/motor.c | 4 +-- ccs/util.c | 76 +++++++++++++++------------------------------------ 3 files changed, 25 insertions(+), 57 deletions(-) diff --git a/ccs/history.h b/ccs/history.h index 91e948e..6e09c73 100755 --- a/ccs/history.h +++ b/ccs/history.h @@ -3,7 +3,7 @@ #include -#define HISTORY_SIZE 20000 +#define HISTORY_SIZE 4800 typedef struct HistoryEntry { uint8_t sensor; diff --git a/ccs/motor.c b/ccs/motor.c index dcfdb32..7e7f3b0 100755 --- a/ccs/motor.c +++ b/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); } diff --git a/ccs/util.c b/ccs/util.c index 5707b78..737fc1a 100755 --- a/ccs/util.c +++ b/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;