yenru0/2025-02-MPX
SHA256
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

assign ALIGN V2

Browse Source
This commit is contained in:
yenru0
2025-12-10 02:48:51 +09:00
parent 3d8b12c321
commit 387f483951
1 changed files with 103 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
ccs/main.c
View File

@@ -10,96 +10,85 @@
#include "motor.h"
#include "util.h"
enum State {
STATE_ALIGNING,
STATE_ALIGNED,
STATE_SHOOTING,
STATE_STOP,
};
// PID Control Constants
// Error range: -14 ~ +14 (Max 20)
// PWM Period: 15000
const float Kp = 150.0f;// P gain: 150 * 14 = 2100 (Significant correction)
const float Kd = 600.0f;// D gain
#define BASE_SPEED 1500// Slower speed (approx 10% duty of 15000)
#define MAX_DRV 3000 // Max speed limit (20% duty)
#define BASE_SPEED 1500
#define MAX_DRV 3000
void main() {
#define PERIOD_MS 20
enum State current_state = STATE_ALIGNING;
int main() {
Clock_Init48MHz();
ir_init();
motor_init();
Clock_Delay1ms(1000);// 휴식
motor_stop();
// 초기 안정화 대기
Clock_Delay1ms(500);
int error = 0;
int prev_error = 0;
float pid_out = 0;
int speed_l, speed_r;
int align_phase = 0; // 0: 출발 라인 벗어나기, 1: 목표 라인 찾기
HistoryEntry entry;
HistoryBuffer hisbuf;
hisbuf_init(&hisbuf);
int black_cnt = 0;
int trace_mode = 0;// 0: Normal, 1: Traceback
// 2. Main Loop
while (1) {
// A. Read Sensor
while(1) {
switch(current_state) {
case STATE_ALIGNING:
{
// 1. 센서 값 읽기
uint8_t raw_val;
ir_read_ready();
Clock_Delay1us(1000);// Wait for IR LED
Clock_Delay1us(1000);
ir_read_value(&raw_val);
ir_read_off();
// B. Line Lost Handling (White Area) -> Replay History from Beginning (FIFO)
if (raw_val == 0x00 || trace_mode == 1) {
trace_mode = 1;// Enter Traceback Mode
motor_stop();
Clock_Delay1ms(1000);// Wait a bit before replay
// 2. 에러 계산
int error = get_error(raw_val);
if (raw_val != 0x00) {
black_cnt ++;
// 3. 정렬 완료 조건 확인 (에러가 거의 0인 상태 유지)
// util.c의 가중치: {-14, ..., 14}
// error 0: 완전 중앙, error +/- 2: 약간 벗어남
static int aligned_count = 0;
if (abs(error) <= 2) {
aligned_count++;
} else {
black_cnt = 0;
}
if (black_cnt >= 3) {
goto finish;
aligned_count = 0;
}
// Replay entire history from the beginning (FIFO)
static int replay_prev_error = 0;
if (hisbuf_pop_data(&hisbuf, &entry)) {
// Reconstruct PID
float rec_pid = (Kp * entry.error) + (Kd * (entry.error - replay_prev_error));
replay_prev_error = entry.error;
int rec_speed_l = BASE_SPEED + (int) rec_pid;
int rec_speed_r = BASE_SPEED - (int) rec_pid;
// Limit
if (rec_speed_l > MAX_DRV) rec_speed_l = MAX_DRV;
if (rec_speed_l < -MAX_DRV) rec_speed_l = -MAX_DRV;
if (rec_speed_r > MAX_DRV) rec_speed_r = MAX_DRV;
if (rec_speed_r < -MAX_DRV) rec_speed_r = -MAX_DRV;
// Move Forward (Replay)
motor_move(rec_speed_l, rec_speed_r);
Clock_Delay1ms(2);
if (aligned_count > 25) {
motor_stop(); // 잠시 대기
current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
break;
}
} else {
if (is_crossroad_robust(&hisbuf)) {
goto finish;
}
// C. Normal PID Control
error = get_error(raw_val);
pid_out = (Kp * error) + (Kd * (error - prev_error));
prev_error = error;
// 4. 제자리 회전 (Pivot Turn)
// BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
float pid_out = Kp * error;
// D. Motor Control
speed_l = BASE_SPEED + (int) pid_out;
speed_r = BASE_SPEED - (int) pid_out;
int speed_l = (int)pid_out;
int speed_r = -(int)pid_out;
// E. Speed Limiting
// 최소 기동 토크 보정 (Deadzone Compensation)
// 모터가 돌기 시작하는 최소 PWM 값을 더해줌
int min_speed = 1000;
if (speed_l > 0) speed_l += min_speed;
else if (speed_l < 0) speed_l -= min_speed;
if (speed_r > 0) speed_r += min_speed;
else if (speed_r < 0) speed_r -= min_speed;
// 최대 속도 제한
if (speed_l > MAX_DRV) speed_l = MAX_DRV;
if (speed_l < -MAX_DRV) speed_l = -MAX_DRV;
if (speed_r > MAX_DRV) speed_r = MAX_DRV;
@@ -107,16 +96,47 @@ void main() {
motor_move(speed_l, speed_r);
// Save to History
HistoryEntry entry = {raw_val, error};
hisbuf_push(&hisbuf, entry);
Clock_Delay1ms(PERIOD_MS);
}
break;
// F. Loop Delay (Sampling Time)
Clock_Delay1ms(2);
}
finish:
case STATE_ALIGNED:
motor_stop();
while(1);
Clock_Delay1ms(500); // 잠시 대기
current_state = STATE_SHOOTING;
break;
case STATE_SHOOTING:
{
uint8_t raw_val;
ir_read_ready();
Clock_Delay1us(1000);
ir_read_value(&raw_val);
ir_read_off();
// 2. 직진 (PID 없이 등속 주행)
motor_move(BASE_SPEED, BASE_SPEED);
// 3. 왼쪽 라인(검은색) 감지 시 정지
// 출발 직후(오른쪽 라인)에는 감지될 수 있으므로,
// 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요
static int left_line_phase = 0;
if (left_line_phase == 0) {
if (raw_val == 0x00) left_line_phase = 1; // 출발 라인 벗어남
} else if (left_line_phase == 1) {
if (raw_val != 0x00) { // 목표 라인 도착
motor_stop();
current_state = STATE_STOP;
}
}
Clock_Delay1ms(PERIOD_MS);
}
break;
case STATE_STOP:
motor_stop();
break;
}
}
}