142 lines
4.6 KiB
C
Executable File
142 lines
4.6 KiB
C
Executable File
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "Clock.h"
|
|
#include "msp.h"
|
|
|
|
#include "history.h"
|
|
#include "ir.h"
|
|
#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
|
|
#define MAX_DRV 3000
|
|
|
|
#define PERIOD_MS 20
|
|
|
|
enum State current_state = STATE_ALIGNING;
|
|
|
|
int main() {
|
|
Clock_Init48MHz();
|
|
ir_init();
|
|
motor_init();
|
|
|
|
// 초기 안정화 대기
|
|
Clock_Delay1ms(500);
|
|
|
|
int align_phase = 0; // 0: 출발 라인 벗어나기, 1: 목표 라인 찾기
|
|
|
|
while(1) {
|
|
switch(current_state) {
|
|
case STATE_ALIGNING:
|
|
{
|
|
// 1. 센서 값 읽기
|
|
uint8_t raw_val;
|
|
ir_read_ready();
|
|
Clock_Delay1us(1000);
|
|
ir_read_value(&raw_val);
|
|
ir_read_off();
|
|
|
|
// 2. 에러 계산
|
|
int error = get_error(raw_val);
|
|
|
|
// 3. 정렬 완료 조건 확인 (에러가 거의 0인 상태 유지)
|
|
// util.c의 가중치: {-14, ..., 14}
|
|
// error 0: 완전 중앙, error +/- 2: 약간 벗어남
|
|
static int aligned_count = 0;
|
|
if (abs(error) <= 2) {
|
|
aligned_count++;
|
|
} else {
|
|
aligned_count = 0;
|
|
}
|
|
|
|
if (aligned_count > 25) {
|
|
motor_stop(); // 잠시 대기
|
|
current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
|
|
break;
|
|
}
|
|
|
|
// 4. 제자리 회전 (Pivot Turn)
|
|
// BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
|
|
float pid_out = Kp * error;
|
|
|
|
int speed_l = (int)pid_out;
|
|
int speed_r = -(int)pid_out;
|
|
|
|
// 최소 기동 토크 보정 (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;
|
|
if (speed_r < -MAX_DRV) speed_r = -MAX_DRV;
|
|
|
|
motor_move(speed_l, speed_r);
|
|
|
|
Clock_Delay1ms(PERIOD_MS);
|
|
}
|
|
break;
|
|
|
|
case STATE_ALIGNED:
|
|
motor_stop();
|
|
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;
|
|
}
|
|
}
|
|
} |