diff --git a/ccs/history.c b/ccs/history.c new file mode 100755 index 0000000..3c4a44e --- /dev/null +++ b/ccs/history.c @@ -0,0 +1,48 @@ +#include "history.h" + +void hisbuf_init(HistoryBuffer *sb) { + sb->front = 0; + sb->rear = 0; + sb->size = 0; +} + +int hisbuf_is_full(HistoryBuffer *sb) { + return sb->size == HISTORY_SIZE; +} + +int hisbuf_push(HistoryBuffer *sb, HistoryEntry entry) { + if (sb->size < HISTORY_SIZE) { + sb->data[sb->rear] = entry; + sb->rear = (sb->rear + 1) % HISTORY_SIZE; + sb->size++; + return 1; // Success + } + return 0; // Buffer full +} + +int hisbuf_pop(HistoryBuffer *sb) { + if (sb->size > 0) { + sb->front = (sb->front + 1) % HISTORY_SIZE; + sb->size--; + return 1; // Success + } + return 0; // Buffer empty +} + +int hisbuf_pop_data(HistoryBuffer *sb, HistoryEntry *out) { + if (sb->size > 0) { + *out = sb->data[sb->front]; + sb->front = (sb->front + 1) % HISTORY_SIZE; + sb->size--; + return 1; // Success + } + return 0; // Buffer empty +} + +void hisbuf_get(HistoryBuffer *sb, HistoryEntry *out, int lookahead) { + if (lookahead >= 0 && lookahead < sb->size) { + // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead + int index = (sb->rear - 1 - lookahead + HISTORY_SIZE) % HISTORY_SIZE; + *out = sb->data[index]; + } +} diff --git a/ccs/history.h b/ccs/history.h new file mode 100755 index 0000000..91e948e --- /dev/null +++ b/ccs/history.h @@ -0,0 +1,33 @@ +#ifndef _history_h_ +#define _history_h_ + +#include + +#define HISTORY_SIZE 20000 + +typedef struct HistoryEntry { + uint8_t sensor; + int error; +} HistoryEntry; + +typedef struct { + HistoryEntry data[HISTORY_SIZE]; + int errors[HISTORY_SIZE]; + int front; + int rear; + int size; +} HistoryBuffer; + +void hisbuf_init(HistoryBuffer *sb); + +int hisbuf_is_full(HistoryBuffer *sb); + +int hisbuf_push(HistoryBuffer *sb, HistoryEntry entry); + +int hisbuf_pop(HistoryBuffer *sb); + +int hisbuf_pop_data(HistoryBuffer *sb, HistoryEntry *out); + +void hisbuf_get(HistoryBuffer *sb, HistoryEntry *out, int lookahead); + +#endif diff --git a/ccs/ir.c b/ccs/ir.c index 301ddb5..9c841e0 100755 --- a/ccs/ir.c +++ b/ccs/ir.c @@ -17,12 +17,13 @@ void ir_init(void) { } void ir_read_ready() { + P7->DIR = 0xff; + P7->OUT &= ~0xff; /* initially offed */ P5->OUT |= 0x08; P9->OUT |= 0x04; - P7->DIR = 0xff; + P7->OUT |= 0xff; /* init charge the caps */ Clock_Delay1us(10); - P7->DIR = 0x00; - Clock_Delay1us(1000); + P7->DIR = 0x00; /* set input */ } void ir_read_value(uint8_t *value) { @@ -40,6 +41,7 @@ void test_ir(void) { while (1) { ir_ready_ready(); + Clock_Delay1us(1000); // 1ms ir_read_value(&ir_value); print_binary(ir_value); printf("\n"); diff --git a/ccs/ir.h b/ccs/ir.h index bb6898a..0f427be 100755 --- a/ccs/ir.h +++ b/ccs/ir.h @@ -1,4 +1,5 @@ -#pragma once +#ifndef _ir_h_ +#define _ir_h_ #include "msp.h" #include "util.h" @@ -13,3 +14,5 @@ void ir_read_value(uint8_t *value); void ir_read_off(void); void test_ir(void); + +#endif diff --git a/ccs/main.c b/ccs/main.c index ac682b8..13af0cb 100755 --- a/ccs/main.c +++ b/ccs/main.c @@ -1,574 +1,122 @@ -#include #include +#include #include #include "Clock.h" #include "msp.h" +#include "history.h" #include "ir.h" #include "motor.h" -#include "state.h" #include "util.h" -#define TRUE 1 -#define FALSE 0 +// 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 PRACTICE 4 +#define BASE_SPEED 1500// Slower speed (approx 10% duty of 15000) +#define MAX_DRV 3000 // Max speed limit (20% duty) -#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(); +void main() { + Clock_Init48MHz(); ir_init(); - - SensorBuffer sb; - senbuf_init(&sb); + motor_init(); + + Clock_Delay1ms(1000);// 휴식 + motor_stop(); 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(); + HistoryEntry entry; + HistoryBuffer hisbuf; 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; -} - - -#elif PRACTICE == 4 - -void LineTracerPrac4(void) { - Clock_Init48MHz(); - ir_init(); - motor_init(); - - // PID Constants - const float Kp = 0.005f; - const float Kd = 0.02f; - const int BASE_SPEED = 10; - const int MAX_DRV = 20; - - int error = 0; - int prev_error = 0; - float pid_out = 0; - int speed_l, speed_r; + 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_Delay1ms(1); + Clock_Delay1us(1000);// Wait for IR LED ir_read_value(&raw_val); ir_read_off(); - error = get_error(raw_val); - pid_out = (Kp * error) + (Kd * (error - prev_error)); - prev_error = error; + // 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; - speed_l = BASE_SPEED + (int)pid_out; - speed_r = BASE_SPEED - (int)pid_out; + 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; - if (speed_l > MAX_DRV) speed_l = MAX_DRV; - if (speed_l < 0) speed_l = 0; - if (speed_r > MAX_DRV) speed_r = MAX_DRV; - if (speed_r < 0) speed_r = 0; + int rec_speed_l = BASE_SPEED + (int) rec_pid; + int rec_speed_r = BASE_SPEED - (int) rec_pid; - motor_move_custom(speed_l, speed_r); + // 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); } -} -#endif -int main(void) { - #if PRACTICE == 1 - LineTracerPrac1(); - #elif PRACTICE == 2 - LineTracerPrac2(); - #elif PRACTICE == 3 - LineTracerPrac3(); - #elif PRACTICE == 4 - LineTracerPrac4(); - #else - printf("No practice selected.\n"); - #endif - return 0; +finish: + motor_stop(); + while(1); } \ No newline at end of file diff --git a/ccs/motor.c b/ccs/motor.c index 40db44e..dcfdb32 100755 --- a/ccs/motor.c +++ b/ccs/motor.c @@ -2,198 +2,60 @@ #include "Clock.h" #include -// Period: 20ms -// MAX speed: 20 -static void normalize_speed(int *speed) { - if (*speed <= 0) *speed = 0; - else if (*speed > MAX_SPEED) *speed = MAX_SPEED; -} void motor_init(void) { + P2->SEL0 &= ~0xc0; + P2->SEL1 &= ~0xc0; + P2->DIR |= 0xc0; // output + P2->OUT &= ~0xc0;// clear + P3->SEL0 &= ~0xc0; P3->SEL1 &= ~0xc0; - P3->DIR |= 0xc0; - P3->OUT &= ~0xc0; + P3->DIR |= 0xc0; // output + P3->OUT &= ~0xc0;// clear P5->SEL0 &= ~0x30; P5->SEL1 &= ~0x30; - P5->DIR |= 0x30; - P5->OUT &= ~0x30; + P5->DIR |= 0x30; + P5->OUT &= ~0x30; - P2->SEL0 &= ~0xc0; - P2->SEL1 &= ~0xc0; - P2->DIR |= 0xc0; - P2->OUT &= ~0xc0; + pwm_init_for_motors(MOTOR_PERIOD); } -static void set_dir_l(char dir) { // f: forward, b: backward - if (dir == 'f') P5->OUT &= ~0b00010000; - else if (dir == 'b') P5->OUT |= 0b00010000; -} - -static void set_dir_r(char dir) { // f: forward, b: backward - if (dir == 'f') P5->OUT &= ~0b00100000; - else if (dir == 'b') P5->OUT |= 0b00100000; -} - -static void set_en_l(int enable) { // 1: on, 0: off - if (enable == 1) P2->OUT |= 0b10000000; - else if (enable == 0) P2->OUT &= ~0b10000000; -} - -static void set_en_r(int enable) { // 1: on, 0: off - if (enable == 1) P2->OUT |= 0b01000000; - else if (enable == 0) P2->OUT &= ~0b01000000; -} - -void motor_move_stop() { - // P2->OUT &= ~0xc0; - set_en_l(0); - set_en_r(0); -} - -static void move_forward() { - P5->OUT &= ~0b00110000; - P3->OUT |= 0b11000000; - P2->OUT |= 0b11000000; -} - -static void move_backward() { - P5->OUT |= 0b00110000; - P3->OUT |= 0b11000000; - P2->OUT |= 0b11000000; -} - -static void turn_left() { - P3->OUT |= 0b11000000; - P2->OUT &= ~0b10000000; - P2->OUT |= 0b01000000; -} - -static void turn_right() { - P3->OUT |= 0b11000000; - P2->OUT &= ~0b01000000; - P2->OUT |= 0b10000000; -} - -static void rotate_clockwise() { - P5->OUT &= ~0b00110000; - P5->OUT |= 0b00100000; - P3->OUT |= 0b11000000; - P2->OUT |= 0b11000000; -} - -static void rotate_cclockwise() { - P5->OUT &= ~0b00110000; - P5->OUT |= 0b00010000; - P3->OUT |= 0b11000000; - P2->OUT |= 0b11000000; -} - -void motor_move_forward(int speed) { - normalize_speed(&speed); - - move_forward(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); -} - -void motor_move_backward(int speed) { - normalize_speed(&speed); - - move_backward(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); -} - -void motor_turn_left(int speed) { - normalize_speed(&speed); - - turn_left(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); -} - -void motor_turn_right(int speed) { - normalize_speed(&speed); - - turn_right(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); -} - -void motor_rotate_clockwise(int speed) { - normalize_speed(&speed); - - rotate_clockwise(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); - - set_dir_l('f'); - set_dir_r('f'); - // move_forward(); - // motor_move_stop(); -} - -void motor_rotate_cclockwise(int speed) { - normalize_speed(&speed); - - rotate_cclockwise(); - Clock_Delay1ms(speed); - - motor_move_stop(); - Clock_Delay1ms(PERIOD_ms - speed); - - set_dir_l('f'); - set_dir_r('f'); - // move_forward(); - // motor_move_stop(); -} - -void motor_move_custom(int left_speed, int right_speed) { // positive will be forward, negative will be backward - int l_speed = left_speed > 0 ? left_speed : -left_speed; - int r_speed = right_speed > 0 ? right_speed : -right_speed; - normalize_speed(&l_speed); - normalize_speed(&r_speed); - - if (left_speed >= 0) set_dir_l('f'); - else if (left_speed < 0) set_dir_l('b'); - - if (right_speed >= 0) set_dir_r('f'); - else if (right_speed < 0) set_dir_r('b'); - - if (l_speed >= r_speed) { - set_en_l(1); - set_en_r(1); - Clock_Delay1ms(r_speed); - - set_en_r(0); - Clock_Delay1ms(l_speed - r_speed); - - set_en_l(0); - Clock_Delay1ms(PERIOD_ms - l_speed); - - } else if (l_speed < r_speed) { - set_en_l(1); - set_en_r(1); - Clock_Delay1ms(l_speed); - - set_en_l(0); - Clock_Delay1ms(r_speed - l_speed); - - set_en_r(0); - Clock_Delay1ms(PERIOD_ms - r_speed); - +void motor_move(int32_t left, int32_t right) { + if (left == 0 && right == 0) { + pwm_set_duty_left(0); + pwm_set_duty_right(0); + } else if (left >= 0 && right >= 0) { + // fwd fwd + P3->OUT |= 0xc0; + P5->OUT &= ~0x30; + pwm_set_duty_left((uint16_t) left); + pwm_set_duty_right((uint16_t) right); + } else if (left < 0 && right < 0) { + // back back + P3->OUT |= 0xc0; + P5->OUT = 0x30; + pwm_set_duty_left((uint16_t) (-left)); + pwm_set_duty_right((uint16_t) (-right)); + } else if (left >= 0 && right < 0) { + // fwd back + P3->OUT |= 0xc0; + P5->OUT |= (P5->OUT & (~0x30)) | 0x10; + pwm_set_duty_left((uint16_t) left); + pwm_set_duty_right((uint16_t) (-right)); + } else if (left < 0 && right >= 0) { + // back fwd + P3->OUT |= 0xc0; + P5->OUT |= (P5->OUT & (~0x30)) | 0x20; + pwm_set_duty_left((uint16_t) (-left)); + pwm_set_duty_right((uint16_t) right); } } + +void motor_stop(void) { + pwm_set_duty_left(0); + pwm_set_duty_right(0); + P3->OUT &= ~0xc0; +} \ No newline at end of file diff --git a/ccs/motor.h b/ccs/motor.h index 223cf3f..5d57e6a 100755 --- a/ccs/motor.h +++ b/ccs/motor.h @@ -2,18 +2,26 @@ #define _motor_h_ #include "msp.h" +#include "pwm.h" +#include -#define PERIOD_ms 20 -#define MAX_SPEED PERIOD_ms +#define MOTOR_PERIOD 15000 -void motor_init(); -void motor_move_stop(); -void motor_move_forward(int speed); -void motor_move_backward(int speed); -void motor_turn_left(int speed); -void motor_turn_right(int speed); -void motor_rotate_clockwise(int speed); -void motor_rotate_cclockwise(int speed); -void motor_move_custom(int left_speed, int right_speed); +/** + * @brief 모터 핀과 PWM 초기화 + * P2.6 P2.7 : PWM 출력 + * P5.4 P5.5 : 방향 제어 + * P3.6 P3.7 : 전원 공급 + */ +void motor_init(void); + +/** + * @brief Set motor speeds. + * @param left 왼쪽 모터 PWM + * @param right 오른쪽 모터 PWM (-1000 ~ 1000) + */ +void motor_move(int32_t left, int32_t right); + +void motor_stop(void); #endif diff --git a/ccs/pwm.c b/ccs/pwm.c new file mode 100644 index 0000000..4de267a --- /dev/null +++ b/ccs/pwm.c @@ -0,0 +1,31 @@ + +#include "pwm.h" + +void pwm_init_for_motors(uint16_t period) { + P2->DIR |= 0xc0; + P2->SEL0 |= 0xc0; + P2->SEL1 &= ~0xc0; + + // 전체 주기 설정 + TIMER_A0->CCTL[0] = 0x0080; + TIMER_A0->CCR[0] = period; + TIMER_A0->EX0 = 0x00000; + // 세부 주기 및 출력 설정 + TIMER_A0->CCTL[3] = 0x0040; + TIMER_A0->CCR[3] = 0; // set duty left + TIMER_A0->CCTL[4] = 0x0040; + TIMER_A0->CCR[4] = 0; // set duty right + // 모드 + TIMER_A0->CTL = 0x02f0; + // SMCLK 12MHz, divide by 8, up-down mode +} + +void pwm_set_duty_right(uint16_t duty) { + if (duty > TIMER_A0->CCR[0]) duty = TIMER_A0->CCR[0]; + TIMER_A0->CCR[3] = duty; +} + +void pwm_set_duty_left(uint16_t duty) { + if (duty > TIMER_A0->CCR[0]) duty = TIMER_A0->CCR[0]; + TIMER_A0->CCR[4] = duty; +} \ No newline at end of file diff --git a/ccs/pwm.h b/ccs/pwm.h new file mode 100644 index 0000000..722ef5c --- /dev/null +++ b/ccs/pwm.h @@ -0,0 +1,11 @@ +#pragma once +#include "msp.h" +#include + +/** + * @brief Initialize PWM for motor control with the specified period. + * @note P2.6 과 P2.7을 PWM 출력으로 설정합니다. + * @param period PWM 주기 (타이머 카운트 값) + */ +void pwm_init_for_motors(uint16_t period); + diff --git a/ccs/state.c b/ccs/state.c deleted file mode 100755 index 1dda911..0000000 --- a/ccs/state.c +++ /dev/null @@ -1,77 +0,0 @@ -#include "state.h" - -void senbuf_init(SensorBuffer *sb) { - sb->front = 0; - sb->rear = 0; - sb->size = 0; -} - -int senbuf_is_full(SensorBuffer *sb) { - return sb->size == STATE_SENSOR_BUFFER_SIZE; -} - -int senbuf_push(SensorBuffer *sb, uint8_t value) { - if (sb->size < STATE_SENSOR_BUFFER_SIZE) { - sb->buffer[sb->rear] = value; - sb->rear = (sb->rear + 1) % STATE_SENSOR_BUFFER_SIZE; - sb->size++; - return 1; // Success - } - return 0; // Buffer full -} - -int senbuf_pop(SensorBuffer *sb) { - if (sb->size > 0) { - sb->front = (sb->front + 1) % STATE_SENSOR_BUFFER_SIZE; - sb->size--; - return 1; // Success - } - return 0; // Buffer empty -} - -void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead) { - if (lookahead >= 0 && lookahead < sb->size) { - // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead - int index = (sb->rear - 1 - lookahead + STATE_SENSOR_BUFFER_SIZE) % STATE_SENSOR_BUFFER_SIZE; - *out = sb->buffer[index]; - } -} - -// History Buffer - -void hisbuf_init(HistoryBuffer *sb) { - sb->front = 0; - sb->rear = 0; - sb->size = 0; -} - -int hisbuf_is_full(HistoryBuffer *sb) { - return sb->size == HISTORY_SIZE; -} - -int hisbuf_push(HistoryBuffer *sb, uint8_t value) { - if (sb->size < HISTORY_SIZE) { - sb->buffer[sb->rear] = value; - sb->rear = (sb->rear + 1) % HISTORY_SIZE; - sb->size++; - return 1; // Success - } - return 0; // Buffer full -} - -int hisbuf_pop(HistoryBuffer *sb) { - if (sb->size > 0) { - sb->front = (sb->front + 1) % HISTORY_SIZE; - sb->size--; - return 1; // Success - } - return 0; // Buffer empty -} - -void hisbuf_get(HistoryBuffer *sb, uint8_t *out, int lookahead) { - if (lookahead >= 0 && lookahead < sb->size) { - // 가장 최근에 넣은 것(rear - 1)을 기준으로 lookahead - int index = (sb->rear - 1 - lookahead + HISTORY_SIZE) % HISTORY_SIZE; - *out = sb->buffer[index]; - } -} diff --git a/ccs/state.h b/ccs/state.h deleted file mode 100755 index 3a5e441..0000000 --- a/ccs/state.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include - -#define STATE_SENSOR_BUFFER_SIZE 8 - -typedef struct { - uint8_t buffer[STATE_SENSOR_BUFFER_SIZE]; - int front; - int rear; - int size; -} SensorBuffer; - -void senbuf_init(SensorBuffer *sb); - -int senbuf_is_full(SensorBuffer *sb); - -int senbuf_push(SensorBuffer *sb, uint8_t value); - -int senbuf_pop(SensorBuffer *sb); - -void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead); - - - -// History Buffer - -#define HISTORY_SIZE 20000 - -typedef struct { - uint8_t buffer[HISTORY_SIZE]; - int front; - int rear; - int size; -} HistoryBuffer; - -void hisbuf_init(HistoryBuffer *sb); - -int hisbuf_is_full(HistoryBuffer *sb); - -int hisbuf_push(HistoryBuffer *sb, uint8_t value); - -int hisbuf_pop(HistoryBuffer *sb); - -void hisbuf_get(HistoryBuffer *sb, uint8_t *out, int lookahead); diff --git a/ccs/util.c b/ccs/util.c index 1503e20..5707b78 100755 --- a/ccs/util.c +++ b/ccs/util.c @@ -3,44 +3,77 @@ #include #include -// 가중치 설정 (-4000 ~ +4000) +// 가중치 설정 (-14 ~ +14) // 중앙(3번과 4번 사이)이 0이 되도록 설정 // [0] [1] [2] [3] | [4] [5] [6] [7] -const int SENSOR_WEIGHTS[8] = {-4000, -3000, -2000, -1000, 1000, 2000, 3000, 4000}; +const int SENSOR_WEIGHTS[8] = {-14, -10, -6, -2, 2, 6, 10, 14}; // 선을 완전히 벗어났을 때 반환할 최대 에러 값 -const int MAX_ERROR = 5000; +const int MAX_ERROR = 20; int get_error(uint8_t raw) { // static 변수: 함수가 끝나도 값이 사라지지 않고 기억됨 (로봇의 마지막 위치) static int last_valid_error = 0; - // 1. 선이 아예 안 보이는 경우 (Line Lost) - if (raw == 0x00) { - // 마지막에 왼쪽(-값)에 있었다면 -> 계속 왼쪽으로 돌아라 (-MAX_ERROR) - if (last_valid_error < 0) return -MAX_ERROR; - // 마지막에 오른쪽(+값)에 있었다면 -> 계속 오른쪽으로 돌아라 (+MAX_ERROR) - else - return MAX_ERROR; - } - // 2. 노이즈/갈림길 처리 (Masking) - 사용자가 걱정했던 '1 0 0 0 1 1 0 0' 상황 // 간단한 로직: 마지막 위치가 양수(오른쪽)였다면, 왼쪽 끝(0,1번) 센서는 노이즈일 확률이 높음 -> 무시 - // 반대의 경우도 마찬가지. + // 2. 노이즈 제거 및 보정 - uint8_t filtered_raw = raw; - - // 로봇이 확실히 오른쪽(>1000)에 있었는데, 갑자기 왼쪽 끝(0,1번 비트)이 켜짐? - if (last_valid_error > 1000 && (raw & 0b00000011)) { - filtered_raw &= ~0b00000011;// 0, 1번 비트 강제 삭제 (Masking) - } - // 로봇이 확실히 왼쪽(<-1000)에 있었는데, 갑자기 오른쪽 끝(6,7번 비트)이 켜짐? - else if (last_valid_error < -1000 && (raw & 0b11000000)) { - filtered_raw &= ~0b11000000;// 6, 7번 비트 강제 삭제 + // [Step 1] Gap Filling (구멍 메우기) + // 센서 불량이나 바닥 상태로 인해 라인 중간이 끊겨 보이는 경우(101)를 보정 + // 예: 01011000 -> 01111000 (중간의 0을 1로 채움) + uint8_t filled_raw = raw; + int k; + for (k = 1; k < 7; k++) { + // 현재 비트가 0이고, 좌우 비트가 모두 1인 경우 + if ( !((raw >> k) & 1) && ((raw >> (k+1)) & 1) && ((raw >> (k-1)) & 1) ) { + filled_raw |= (1 << k); + } } - // 만약 필터링했더니 남는 게 없다면? (노이즈만 떴던 경우) -> 원본 사용 or 이전 값 리턴 - if (filtered_raw == 0x00) filtered_raw = raw; + // [Step 2] Largest Segment Selection (가장 큰 덩어리 선택) + // 보정된 데이터(filled_raw)를 기준으로 가장 큰 덩어리만 남김 + + uint8_t filtered_raw = 0; + int max_consecutive = -1; + int best_mask = 0; + + int current_consecutive = 0; + int current_mask = 0; + int i; + + for (i = 0; i < 8; i++) { + // 7번 비트(왼쪽)부터 0번 비트(오른쪽) 순으로 검사 + if ((filled_raw >> (7 - i)) & 1) { + current_consecutive++; + current_mask |= (1 << (7 - i)); + } else { + // 덩어리가 끊긴 경우 + if (current_consecutive > 0) { + if (current_consecutive > max_consecutive) { + max_consecutive = current_consecutive; + best_mask = current_mask; + } else if (current_consecutive == max_consecutive) { + // 덩어리 크기가 같다면, 이전 진행 방향(last_valid_error)과 가까운 쪽 선택 + if (last_valid_error > 0) best_mask = current_mask; + } + current_consecutive = 0; + current_mask = 0; + } + } + } + + // 마지막 비트까지 1이었던 경우 처리 + if (current_consecutive > 0) { + if (current_consecutive > max_consecutive) { + best_mask = current_mask; + } else if (current_consecutive == max_consecutive) { + if (last_valid_error > 0) best_mask = current_mask; + } + } + + filtered_raw = best_mask; + if (filtered_raw == 0x00) filtered_raw = filled_raw; // 3. 가중 평균 계산 (Weighted Average) long sum_weighted = 0; @@ -81,14 +114,17 @@ int is_crossroad(uint8_t raw) { return 0; } -int is_crossroad_robust(SensorBuffer *sb) { +int is_crossroad_robust(HistoryBuffer *sb) { int crossroad_count = 0; int total_count = sb->size; int i; + if (total_count < 3) { + return 0; // 기록이 충분하지 않음 + } for (i = 0; i < 3; i++) { - uint8_t sensor_value; - senbuf_get(sb, &sensor_value, i); - if (!is_crossroad(sensor_value)) { + HistoryEntry entry; + hisbuf_get(sb, &entry, i); + if (!is_crossroad(entry.sensor)) { goto fail; } } diff --git a/ccs/util.h b/ccs/util.h index 1cc235d..2906407 100755 --- a/ccs/util.h +++ b/ccs/util.h @@ -1,6 +1,6 @@ #pragma once -#include "state.h" +#include "history.h" #include @@ -8,7 +8,7 @@ int get_error(uint8_t raw); int is_crossroad(uint8_t raw); -int is_crossroad_robust(SensorBuffer *sb); +int is_crossroad_robust(HistoryBuffer *sb); void print_binary(const uint8_t n); diff --git a/docker-compose.yml b/docker-compose.yml index e5ebed1..09c9150 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -4,7 +4,7 @@ services: container_name: ccstudio privileged: true # USB JTAG 장비(XDS110 등) 직접 접근 권한 network_mode: host # GUI 디스플레이 호환성을 위해 호스트 네트워크 사용 - user: "1000:1000" + user: "0:0" environment: - TZ=Asia/Seoul - HOME=/workspaces