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

약간의 수정

Browse Source
This commit is contained in:
yenru0
2025-12-09 02:34:41 +09:00
parent 0697b5ab31
commit d3a2802333
22 changed files with 406 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

317
ccs/main.c Normal file → Executable file
View File

@@ -10,28 +10,26 @@
#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
// PID gains tuned for speed range [-20, 20]
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
#define Practice1
#ifdef Practice1
static int pid_history[MAX_REC_SIZE];
static int history_idx = 0;
void LineTracerPrac1(void) {
// ------------------------------------------------
// 1. 초기화
// ------------------------------------------------
motor_init();
ir_init();
@@ -168,10 +166,21 @@ void LineTracerPrac1(void) {
while(1); // 무한 대기
}
#endif
#elif PRACTICE == 2
#ifdef Practice2
void LineTracerPrac(void) {
#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");
@@ -232,8 +241,290 @@ void LineTracerPrac(void) {
}
}
#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) {
LineTracerPrac();
#if PRACTICE == 1
LineTracerPrac1();
#elif PRACTICE == 2
LineTracerPrac2();
#elif PRACTICE == 3
LineTracerPrac3();
#else
printf("No practice selected.\n");
#endif
return 0;
}