144 lines
4.5 KiB
C
Executable File
144 lines
4.5 KiB
C
Executable File
#include "util.h"
|
|
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
|
|
// 가중치 설정 (-14 ~ +14)
|
|
// 중앙(3번과 4번 사이)이 0이 되도록 설정
|
|
// [0] [1] [2] [3] | [4] [5] [6] [7]
|
|
const int SENSOR_WEIGHTS[8] = {-14, -10, -6, -2, 2, 6, 10, 14};
|
|
|
|
// 선을 완전히 벗어났을 때 반환할 최대 에러 값
|
|
const int MAX_ERROR = 20;
|
|
|
|
int get_error(uint8_t raw) {
|
|
// static 변수: 함수가 끝나도 값이 사라지지 않고 기억됨 (로봇의 마지막 위치)
|
|
static int last_valid_error = 0;
|
|
|
|
// 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);
|
|
}
|
|
}
|
|
|
|
// [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;
|
|
|
|
// 3. 가중 평균 계산 (Weighted Average)
|
|
long sum_weighted = 0;
|
|
int sum_active = 0;
|
|
int i;
|
|
for (i = 0; i < 8; i++) {
|
|
// i번째 비트가 1인지 확인
|
|
if ((filtered_raw >> (7 - i)) & 1) {
|
|
sum_weighted += SENSOR_WEIGHTS[i];
|
|
sum_active++;
|
|
}
|
|
}
|
|
|
|
// 예외 처리: 계산 중 비트가 다 사라진 경우 (혹시 모를 상황)
|
|
if (sum_active == 0) return last_valid_error;
|
|
|
|
// 최종 에러 값 계산
|
|
int current_error = sum_weighted / sum_active;
|
|
|
|
// 4. 다음 계산을 위해 현재 위치 기억
|
|
last_valid_error = current_error;
|
|
|
|
return current_error;
|
|
}
|
|
|
|
// 센서 비트 중 1의 개수를 세서 T자/십자 여부 판단
|
|
int is_crossroad(uint8_t raw) {
|
|
int count = 0;
|
|
int i;
|
|
for (i = 0; i < 8; i++) {
|
|
if ((raw >> i) & 1) {// i번째 비트가 1이면 카운트
|
|
count++;
|
|
}
|
|
}
|
|
// 8개 중 6개 이상이 검은색이면 교차로(T자)로 인정
|
|
if (count >= 7) return 1;
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
int is_crossroad_robust(HistoryBuffer *sb) {
|
|
int crossroad_count = 0;
|
|
int total_count = sb->size;
|
|
int i;
|
|
if (total_count < 3) {
|
|
return 0; // 기록이 충분하지 않음
|
|
}
|
|
for (i = 0; i < 3; i++) {
|
|
HistoryEntry entry;
|
|
hisbuf_get(sb, &entry, i);
|
|
if (!is_crossroad(entry.sensor)) {
|
|
goto fail;
|
|
}
|
|
}
|
|
return 1;
|
|
fail:
|
|
return 0;
|
|
}
|
|
|
|
void print_binary(const uint8_t n) {
|
|
int i;
|
|
for (i = 7; i >= 0; i--) printf("%d", (n >> i) & 0b1);
|
|
}
|
|
|
|
uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value) {
|
|
return mask & ir_value;
|
|
}
|