530 lines
15 KiB
C
Executable File
530 lines
15 KiB
C
Executable File
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "Clock.h"
|
|
#include "msp.h"
|
|
|
|
#include "ir.h"
|
|
#include "motor.h"
|
|
#include "state.h"
|
|
#include "util.h"
|
|
|
|
#define TRUE 1
|
|
#define FALSE 0
|
|
|
|
#define PRACTICE 3
|
|
|
|
#if PRACTICE == 1
|
|
|
|
#define LOOP_PERIOD_MS 20
|
|
#define REC_INTERVAL 5
|
|
#define MAX_REC_SIZE 1000
|
|
|
|
const float Kp = 0.005f; // proportional gain
|
|
const float Kd = 0.02f; // derivative gain
|
|
#define BASE_SPEED 10 // nominal forward speed (0~20)
|
|
#define MAX_DRV 20 // absolute max speed
|
|
|
|
static int pid_history[MAX_REC_SIZE];
|
|
static int history_idx = 0;
|
|
|
|
void LineTracerPrac1(void) {
|
|
motor_init();
|
|
ir_init();
|
|
|
|
SensorBuffer sb;
|
|
senbuf_init(&sb);
|
|
|
|
int error = 0;
|
|
int prev_error = 0;
|
|
float pid_out = 0;
|
|
int speed_l, speed_r;
|
|
int loop_cnt = 0; // 다운샘플링용 카운터
|
|
|
|
// ------------------------------------------------
|
|
// 2. [PHASE 1] 출발 및 기록 (Forward & Record)
|
|
// ------------------------------------------------
|
|
while (1) {
|
|
// A. 센서 값 읽기
|
|
uint8_t raw_val;
|
|
ir_read_ready(); // 적외선 발광 시작
|
|
delay_ms_simple(1); // 안정화 대기
|
|
ir_read_value(&raw_val);
|
|
ir_read_off(); // 절전
|
|
|
|
// B. 버퍼에 저장 및 T자 감지
|
|
senbuf_push(&sb, raw_val);
|
|
|
|
// T자형 교차로(끝)를 만나면 루프 탈출
|
|
if (is_crossroad_robust(&sb)) {
|
|
motor_move_stop();
|
|
delay_ms_simple(1000); // 1초 정지
|
|
break;
|
|
}
|
|
|
|
// C. PID 제어 계산
|
|
error = get_error(raw_val);
|
|
pid_out = (Kp * error) + (Kd * (error - prev_error));
|
|
prev_error = error;
|
|
|
|
// D. 모터 구동
|
|
// 보정값이 양수(우측 쏠림) -> 우회전 필요 -> 왼쪽 가속, 오른쪽 감속
|
|
speed_l = BASE_SPEED + (int)pid_out;
|
|
speed_r = BASE_SPEED - (int)pid_out;
|
|
|
|
// 속도 범위 제한 (0 ~ 20) - motor.h의 MAX_SPEED 참조
|
|
if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
|
|
if (speed_l < 0) speed_l = 0;
|
|
if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
|
|
if (speed_r < 0) speed_r = 0;
|
|
|
|
motor_move_custom(speed_l, speed_r);
|
|
|
|
// E. 경로 기록 (Downsampling)
|
|
loop_cnt++;
|
|
if (loop_cnt >= REC_INTERVAL) {
|
|
if (history_idx < MAX_REC_SIZE) {
|
|
// 현재 적용된 PID 보정값을 저장
|
|
pid_history[history_idx++] = (int)pid_out;
|
|
}
|
|
loop_cnt = 0;
|
|
}
|
|
|
|
delay_ms_simple(2); // 루프 주기
|
|
}
|
|
|
|
// ------------------------------------------------
|
|
// 3. [PHASE 2] U턴 (180도 회전)
|
|
// ------------------------------------------------
|
|
// T자 라인을 벗어나기 위해 살짝 후진 (선택 사항)
|
|
motor_move_backward(10);
|
|
delay_ms_simple(200);
|
|
|
|
// 제자리 회전 (Pivot Turn)
|
|
// 왼쪽: 20, 오른쪽: -20 (motor_rotate_clockwise 함수 활용)
|
|
motor_rotate_clockwise(15);
|
|
|
|
// 회전 시간 제어: 라인을 찾을 때까지 돔
|
|
// (처음 0.5초는 무조건 돌아서 기존 라인을 벗어남)
|
|
delay_ms_simple(500);
|
|
|
|
while(1) {
|
|
uint8_t raw_turn;
|
|
ir_read_ready();
|
|
ir_read_value(&raw_turn);
|
|
ir_read_off();
|
|
|
|
// 중앙 센서(00011000)가 감지되면 정지
|
|
if (raw_turn & 0b00011000) {
|
|
motor_move_stop();
|
|
delay_ms_simple(500); // 안정화
|
|
break;
|
|
}
|
|
}
|
|
|
|
// ------------------------------------------------
|
|
// 4. [PHASE 3] 되돌아가기 (Replay)
|
|
// ------------------------------------------------
|
|
// 기록된 인덱스의 끝부터 역순으로 재생
|
|
loop_cnt = 0;
|
|
|
|
while (history_idx >= 0) {
|
|
// 타이밍 맞추기 (기록할 때와 같은 속도로 읽기)
|
|
loop_cnt++;
|
|
if (loop_cnt >= REC_INTERVAL) {
|
|
history_idx--; // 인덱스 감소 (Stack Pop)
|
|
loop_cnt = 0;
|
|
}
|
|
|
|
// 데이터가 끝났으면 종료
|
|
if (history_idx < 0) break;
|
|
|
|
// ★ 핵심 로직: 부호 반전
|
|
// 갈 때: 왼쪽으로 5만큼 꺾음 (PID = +5)
|
|
// 올 때: 로봇이 뒤집혔으므로 오른쪽으로 5만큼 꺾어야 함 (Replay PID = -5)
|
|
int replay_pid = pid_history[history_idx];
|
|
|
|
speed_l = BASE_SPEED + replay_pid;
|
|
speed_r = BASE_SPEED - replay_pid;
|
|
|
|
// 속도 제한
|
|
if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
|
|
if (speed_l < 0) speed_l = 0;
|
|
if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
|
|
if (speed_r < 0) speed_r = 0;
|
|
|
|
motor_move_custom(speed_l, speed_r);
|
|
|
|
delay_ms_simple(2); // 갈 때와 동일한 루프 딜레이
|
|
}
|
|
|
|
// ------------------------------------------------
|
|
// 5. 종료
|
|
// ------------------------------------------------
|
|
motor_move_stop();
|
|
while(1); // 무한 대기
|
|
}
|
|
|
|
#elif PRACTICE == 2
|
|
|
|
#define LOOP_PERIOD_MS 20
|
|
#define REC_INTERVAL 5
|
|
#define MAX_REC_SIZE 1000
|
|
|
|
const float Kp = 0.005f; // proportional gain
|
|
const float Kd = 0.02f; // derivative gain
|
|
#define BASE_SPEED 10 // nominal forward speed (0~20)
|
|
#define MAX_DRV 20 // absolute max speed
|
|
|
|
static int pid_history[MAX_REC_SIZE];
|
|
static int history_idx = 0;
|
|
|
|
void LineTracerPrac2(void) {
|
|
Clock_Init48MHz();
|
|
printf("Init Phase Begin\n");
|
|
|
|
ir_init();
|
|
motor_init();
|
|
|
|
SensorBuffer sb;
|
|
senbuf_init(&sb);
|
|
|
|
printf("Init Phase End\n");
|
|
|
|
uint8_t ir_value;
|
|
|
|
int error = 888481557;
|
|
int prev_error = 0;
|
|
|
|
int output_pid = 0;
|
|
|
|
int speed_left;
|
|
int speed_right;
|
|
|
|
while (1) {
|
|
ir_read_ready();
|
|
ir_read_value(ir_value);
|
|
ir_read_off();
|
|
int i, j;
|
|
|
|
senbuf_push(&sb, ir_value);
|
|
if (is_crossroad_robust(&sb)) {
|
|
// 교차로 처리 로직
|
|
motor_move_stop();
|
|
while(1) {} // succ
|
|
}
|
|
|
|
|
|
|
|
error = get_error(ir_value);
|
|
output_pid = (int)(Kp * error + Kd * (error - prev_error));
|
|
prev_error = error;
|
|
|
|
|
|
// Differential drive speeds based on PID correction
|
|
// Left slows down when error positive (line to right), right speeds up
|
|
speed_left = BASE_SPEED - output_pid;
|
|
speed_right = BASE_SPEED + output_pid;
|
|
|
|
// Clamp to allowed range [-20, 20]
|
|
if (speed_left > MAX_DRV) speed_left = MAX_DRV;
|
|
if (speed_left < -MAX_DRV) speed_left = -MAX_DRV;
|
|
if (speed_right > MAX_DRV) speed_right = MAX_DRV;
|
|
if (speed_right < -MAX_DRV) speed_right = -MAX_DRV;
|
|
|
|
// Move motors with custom differential speeds
|
|
motor_move_custom(speed_left, speed_right);
|
|
|
|
Clock_Delay1ms(LOOP_PERIOD_MS);
|
|
|
|
}
|
|
}
|
|
|
|
#elif PRACTICE == 3
|
|
|
|
#define TEST_IR TRUE
|
|
#define PRINT_DEBUG FALSE
|
|
#define DELAY_FOR_PUT_DOWN_s 5
|
|
#define POSITION_INIT_MOTOR TRUE
|
|
#define POSITION_INIT_LED TRUE
|
|
#define TRACK_NUMBER 1
|
|
// 1: Half Circle
|
|
// 2: Worm-Like
|
|
// 3: Dots Line
|
|
// 4: Long Term Line
|
|
|
|
#if TRACK_NUMBER == 1 // Half Circle
|
|
#define STRAIGHT_SPEED 5
|
|
#define TURN_SPEED 5
|
|
#define ROTATE_SPEED 3
|
|
|
|
#elif TRACK_NUMBER == 2 // Worm-Like
|
|
#define STRAIGHT_SPEED_BEFORE_ROTATE 1
|
|
#define STRAIGHT_SPEED 5
|
|
#define TURN_SPEED 5
|
|
#define ROTATE_SPEED 3
|
|
#define ROTATE_SPEED_90DEG 5
|
|
|
|
#elif TRACK_NUMBER == 3 // Dots Line
|
|
#define STRAIGHT_SPEED 5
|
|
#define TURN_SPEED 5
|
|
#define ROTATE_SPEED 3
|
|
|
|
#elif TRACK_NUMBER == 4 // Long Term Line
|
|
#define STRAIGHT_SPEED 5
|
|
#define TURN_SPEED 5
|
|
#define ROTATE_SPEED 3
|
|
#endif
|
|
|
|
|
|
void position_init_motor(void);
|
|
void position_init_led(void);
|
|
|
|
|
|
// global var
|
|
HistoryBuffer hisbuf;
|
|
int buf_lookahead;
|
|
int use_buf = 0;
|
|
//int set_pre = 1;
|
|
|
|
int LineTracerPrac3(void) {
|
|
// init
|
|
Clock_Init48MHz();
|
|
ir_init();
|
|
hisbuf_init(&hisbuf);
|
|
|
|
#if TEST_IR
|
|
test_ir();
|
|
#endif
|
|
|
|
// position init
|
|
#if POSITION_INIT_MOTOR
|
|
Clock_Delay1ms(1000 * DELAY_FOR_PUT_DOWN_s); // delay for init
|
|
motor_init();
|
|
position_init_motor();
|
|
#endif
|
|
#if POSITION_INIT_LED
|
|
position_init_led();
|
|
#endif
|
|
|
|
|
|
// local var for running
|
|
uint8_t ir_value;
|
|
uint8_t target_ir_value;
|
|
// uint8_t pre_target_ir_value = 0b00011000;
|
|
while (1) {
|
|
ir_read_ready();
|
|
ir_read_value(&ir_value);
|
|
ir_read_off();
|
|
target_ir_value = extract_ir_value(0b00111100, ir_value);
|
|
|
|
/* when using buffer */
|
|
if (use_buf) {
|
|
if (target_ir_value != 0b00000000) {
|
|
while (hisbuf_pop(&hisbuf)); // make buf empty
|
|
buf_lookahead = 0;
|
|
use_buf = 0;
|
|
} else {
|
|
hisbuf_get(&hisbuf, &target_ir_value, buf_lookahead);
|
|
// set_pre = target_ir_value & 1;
|
|
target_ir_value = extract_ir_value(0b00111100, target_ir_value);
|
|
buf_lookahead = (buf_lookahead + 1) % hisbuf.size;
|
|
}
|
|
}
|
|
/* end when using buffer */
|
|
|
|
#if TRACK_NUMBER == 1 // Half Circle
|
|
if (use_buf && ir_value) return 0;
|
|
switch (target_ir_value) {
|
|
case 0b00011000:
|
|
motor_move_forward(STRAIGHT_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00110000: // left-biased a lot
|
|
motor_rotate_cclockwise(ROTATE_SPEED);
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001100: // right-biased a lot
|
|
motor_rotate_clockwise(ROTATE_SPEED);
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00010000: // left-biased
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001000: // right-biased
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00000000: // no line to trace
|
|
buf_lookahead = 0;
|
|
use_buf = 1;
|
|
break;
|
|
}
|
|
|
|
#elif TRACK_NUMBER == 2 // Worm-Like
|
|
switch (target_ir_value) {
|
|
case 0b00011000:
|
|
motor_move_forward(STRAIGHT_SPEED);
|
|
break;
|
|
case 0b00111000: // left-right angle
|
|
motor_move_forward(STRAIGHT_SPEED_BEFORE_ROTATE);
|
|
motor_rotate_cclockwise(ROTATE_SPEED_90DEG);
|
|
break;
|
|
case 0b00011100: // right-right angle
|
|
motor_move_forward(STRAIGHT_SPEED_BEFORE_ROTATE);
|
|
motor_rotate_clockwise(ROTATE_SPEED_90DEG);
|
|
break;
|
|
case 0b00110000: // left-biased a lot
|
|
motor_rotate_cclockwise(ROTATE_SPEED);
|
|
motor_turn_left(TURN_SPEED);
|
|
break;
|
|
case 0b00001100: // right-biased a lot
|
|
motor_rotate_clockwise(ROTATE_SPEED);
|
|
motor_turn_right(TURN_SPEED);
|
|
break;
|
|
case 0b00010000: // left-biased
|
|
motor_turn_left(TURN_SPEED);
|
|
break;
|
|
case 0b00001000: // right-biased
|
|
motor_turn_right(TURN_SPEED);
|
|
break;
|
|
case 0b00000000: // no line to trace
|
|
buf_lookahead = 0;
|
|
break;
|
|
}
|
|
|
|
#elif TRACK_NUMBER == 3 // Dots Line
|
|
switch (target_ir_value) {
|
|
case 0b00111100:
|
|
return 0;
|
|
case 0b00011000:
|
|
motor_move_forward(STRAIGHT_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00110000: // left-biased a lot
|
|
motor_rotate_cclockwise(ROTATE_SPEED);
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001100: // right-biased a lot
|
|
motor_rotate_clockwise(ROTATE_SPEED);
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00010000: // left-biased
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001000: // right-biased
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00000000: // no line to trace
|
|
buf_lookahead = 0;
|
|
use_buf = 1;
|
|
break;
|
|
}
|
|
|
|
#elif TRACK_NUMBER == 4 // Long Term Line
|
|
switch (target_ir_value) {
|
|
case 0b00111100:
|
|
return 0;
|
|
case 0b00011000:
|
|
motor_move_forward(STRAIGHT_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00110000: // left-biased a lot
|
|
motor_rotate_cclockwise(ROTATE_SPEED);
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001100: // right-biased a lot
|
|
motor_rotate_clockwise(ROTATE_SPEED);
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00010000: // left-biased
|
|
motor_turn_left(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00001000: // right-biased
|
|
motor_turn_right(TURN_SPEED);
|
|
use_buf = 0;
|
|
break;
|
|
case 0b00000000: // no line to trace
|
|
buf_lookahead = 0;
|
|
use_buf = 1;
|
|
break;
|
|
}
|
|
#endif
|
|
|
|
// if (set_pre) pre_target_ir_value = target_ir_value;
|
|
if (!use_buf) hisbuf_push(&hisbuf, target_ir_value); // when use_buf, record track
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void position_init_motor(void) {
|
|
uint8_t ir_value, target_ir_value;
|
|
|
|
while (1) {
|
|
ir_read_ready();
|
|
ir_read_value(&ir_value);
|
|
ir_read_off();
|
|
target_ir_value = extract_ir_value(0b00111100, ir_value);
|
|
|
|
if (target_ir_value == 0b00011000) break;
|
|
}
|
|
}
|
|
void position_init_led(void) {
|
|
uint8_t ir_value, target_ir_value;
|
|
|
|
// led init
|
|
P2->SEL0 &= ~0x07;
|
|
P2->SEL1 &= ~0x07;
|
|
P2->DIR |= 0x07;
|
|
P2->OUT &= ~0x07;
|
|
|
|
int cnt = 0;
|
|
while (1) {
|
|
ir_read_ready();
|
|
ir_read_value(&ir_value);
|
|
ir_read_off();
|
|
target_ir_value = extract_ir_value(0b00111100, ir_value);
|
|
|
|
if (target_ir_value == 0b00011000) {
|
|
P2->OUT &= ~0b111;
|
|
P2->OUT |= 0b001;
|
|
cnt++;
|
|
} else {
|
|
P2->OUT &= ~0b111;
|
|
cnt = 0;
|
|
}
|
|
|
|
// Hard Coded
|
|
if (cnt == DELAY_FOR_PUT_DOWN_s * 50) break;
|
|
Clock_Delay1ms(20);
|
|
}
|
|
P2->OUT &= ~0b111;
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
int main(void) {
|
|
#if PRACTICE == 1
|
|
LineTracerPrac1();
|
|
#elif PRACTICE == 2
|
|
LineTracerPrac2();
|
|
#elif PRACTICE == 3
|
|
LineTracerPrac3();
|
|
#else
|
|
printf("No practice selected.\n");
|
|
#endif
|
|
return 0;
|
|
} |