98 lines
3.2 KiB
C
98 lines
3.2 KiB
C
#include "util.h"
|
|
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
|
|
// 가중치 설정 (-4000 ~ +4000)
|
|
// 중앙(3번과 4번 사이)이 0이 되도록 설정
|
|
// [0] [1] [2] [3] | [4] [5] [6] [7]
|
|
const int SENSOR_WEIGHTS[8] = {-4000, -3000, -2000, -1000, 1000, 2000, 3000, 4000};
|
|
|
|
// 선을 완전히 벗어났을 때 반환할 최대 에러 값
|
|
const int MAX_ERROR = 5000;
|
|
|
|
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번) 센서는 노이즈일 확률이 높음 -> 무시
|
|
// 반대의 경우도 마찬가지.
|
|
|
|
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번 비트 강제 삭제
|
|
}
|
|
|
|
// 만약 필터링했더니 남는 게 없다면? (노이즈만 떴던 경우) -> 원본 사용 or 이전 값 리턴
|
|
if (filtered_raw == 0x00) filtered_raw = 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(SensorBuffer *sb) {
|
|
int crossroad_count = 0;
|
|
int total_count = sb->size;
|
|
int i;
|
|
for (i = 0; i < 3; i++) {
|
|
uint8_t sensor_value;
|
|
senbuf_get(sb, &sensor_value, i);
|
|
if (!is_crossroad(sensor_value)) {
|
|
goto fail;
|
|
}
|
|
}
|
|
return 1;
|
|
fail:
|
|
return 0;
|
|
} |