122 lines
3.4 KiB
C
Executable File
122 lines
3.4 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"
|
|
|
|
// 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)
|
|
|
|
void main() {
|
|
Clock_Init48MHz();
|
|
ir_init();
|
|
motor_init();
|
|
|
|
Clock_Delay1ms(1000);// 휴식
|
|
motor_stop();
|
|
|
|
int error = 0;
|
|
int prev_error = 0;
|
|
float pid_out = 0;
|
|
int speed_l, speed_r;
|
|
|
|
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
|
|
uint8_t raw_val;
|
|
ir_read_ready();
|
|
Clock_Delay1us(1000);// Wait for IR LED
|
|
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
|
|
|
|
if (raw_val != 0x00) {
|
|
black_cnt ++;
|
|
} else {
|
|
black_cnt = 0;
|
|
}
|
|
if (black_cnt >= 3) {
|
|
goto finish;
|
|
}
|
|
|
|
// 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);
|
|
}
|
|
|
|
} 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;
|
|
|
|
// D. Motor Control
|
|
speed_l = BASE_SPEED + (int) pid_out;
|
|
speed_r = BASE_SPEED - (int) pid_out;
|
|
|
|
// E. Speed Limiting
|
|
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);
|
|
|
|
// Save to History
|
|
HistoryEntry entry = {raw_val, error};
|
|
hisbuf_push(&hisbuf, entry);
|
|
}
|
|
|
|
// F. Loop Delay (Sampling Time)
|
|
Clock_Delay1ms(2);
|
|
}
|
|
|
|
finish:
|
|
motor_stop();
|
|
while(1);
|
|
} |