diff --git a/ccs/.ccsproject b/ccs/.ccsproject old mode 100644 new mode 100755 diff --git a/ccs/.cproject b/ccs/.cproject old mode 100644 new mode 100755 diff --git a/ccs/.project b/ccs/.project old mode 100644 new mode 100755 diff --git a/ccs/.vscode/c_cpp_properties.json b/ccs/.vscode/c_cpp_properties.json old mode 100644 new mode 100755 index de1110d..76a83c5 --- a/ccs/.vscode/c_cpp_properties.json +++ b/ccs/.vscode/c_cpp_properties.json @@ -5,7 +5,7 @@ "includePath": [ "${workspaceFolder}/**", "/opt/ti/ccs/tools/compiler/ti-cgt-armllvm_3.2.2.LTS/include/**", - "/opt/ti/ccs/ccs_base/arm/include/**", + "/opt/ti/ccs/ccs_base/arm/include/**" ], "defines": ["__MSP432P401R__"], "compilerPath": "/opt/ti/ccs/tools/compiler/ti-cgt-armllvm_3.2.2.LTS/bin/tiarmclang", diff --git a/ccs/.vscode/settings.json b/ccs/.vscode/settings.json old mode 100644 new mode 100755 diff --git a/ccs/Clock.c b/ccs/Clock.c old mode 100644 new mode 100755 diff --git a/ccs/Clock.h b/ccs/Clock.h old mode 100644 new mode 100755 diff --git a/ccs/ir.c b/ccs/ir.c old mode 100644 new mode 100755 index 7c72e12..e84dca4 --- a/ccs/ir.c +++ b/ccs/ir.c @@ -27,13 +27,22 @@ void ir_read_ready() { void ir_read_value(uint8_t *value) { int i; - uint8_t port_value = P7->IN; - for (i = 0; i < 8; i++) { - value[i] = !!(port_value & (0x01 << i)); - } + *value = P7->IN; } void ir_read_off() { P5->OUT &= ~0x08; P9->OUT &= ~0x04; } + +void test_ir(void) { + uint8_t ir_value; + + while (1) { + ir_ready_ready(); + ir_read_value(&ir_value); + print_binary(ir_value); + printf("\n";) + ir_read_off(); + } +} diff --git a/ccs/ir.h b/ccs/ir.h old mode 100644 new mode 100755 index ded8d1c..bb6898a --- a/ccs/ir.h +++ b/ccs/ir.h @@ -1,6 +1,7 @@ #pragma once #include "msp.h" +#include "util.h" #include void ir_init(void); @@ -9,4 +10,6 @@ void ir_read_ready(void); void ir_read_value(uint8_t *value); -void ir_read_off(void); \ No newline at end of file +void ir_read_off(void); + +void test_ir(void); diff --git a/ccs/main.c b/ccs/main.c old mode 100644 new mode 100755 index f20d2b6..f3bcb00 --- a/ccs/main.c +++ b/ccs/main.c @@ -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; } \ No newline at end of file diff --git a/ccs/motor.c b/ccs/motor.c old mode 100644 new mode 100755 index 0045d45..40db44e --- a/ccs/motor.c +++ b/ccs/motor.c @@ -1,5 +1,6 @@ #include "motor.h" #include "Clock.h" +#include // Period: 20ms // MAX speed: 20 @@ -172,7 +173,6 @@ void motor_move_custom(int left_speed, int right_speed) { // positive will be fo 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); diff --git a/ccs/motor.h b/ccs/motor.h old mode 100644 new mode 100755 diff --git a/ccs/msp432p401r.cmd b/ccs/msp432p401r.cmd old mode 100644 new mode 100755 diff --git a/ccs/startup_msp432p401r_ccs.c b/ccs/startup_msp432p401r_ccs.c old mode 100644 new mode 100755 diff --git a/ccs/state.c b/ccs/state.c old mode 100644 new mode 100755 index 01ef5e0..9aa683d --- a/ccs/state.c +++ b/ccs/state.c @@ -35,4 +35,45 @@ void senbuf_get(SensorBuffer *sb, uint8_t *out, int lookahead) { int index = (sb->rear - 1 - lookahead + STATE_SENSOR_BUFFER_SIZE) % STATE_SENSOR_BUFFER_SIZE; *out = sb->buffer[index]; } -} \ No newline at end of file +} + + + +// 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 old mode 100644 new mode 100755 index aa5b835..3a5e441 --- a/ccs/state.h +++ b/ccs/state.h @@ -20,3 +20,26 @@ 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/system_msp432p401r.c b/ccs/system_msp432p401r.c old mode 100644 new mode 100755 diff --git a/ccs/targetConfigs/MSP432P401R.ccxml b/ccs/targetConfigs/MSP432P401R.ccxml old mode 100644 new mode 100755 diff --git a/ccs/targetConfigs/readme.txt b/ccs/targetConfigs/readme.txt old mode 100644 new mode 100755 diff --git a/ccs/util.c b/ccs/util.c old mode 100644 new mode 100755 index c2bce2a..1503e20 --- a/ccs/util.c +++ b/ccs/util.c @@ -95,4 +95,13 @@ int is_crossroad_robust(SensorBuffer *sb) { return 1; fail: return 0; -} \ No newline at end of file +} + +void print_binary(const uint8_t n) { + int i; + for (i = 7; i >= 0; i--) printf("%d", (n >> i) & 0b1); +} + +uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value) { + return mask & ir_value; +} diff --git a/ccs/util.h b/ccs/util.h old mode 100644 new mode 100755 index 45059a2..1cc235d --- a/ccs/util.h +++ b/ccs/util.h @@ -8,4 +8,8 @@ int get_error(uint8_t raw); int is_crossroad(uint8_t raw); -int is_crossroad_robust(SensorBuffer *sb); \ No newline at end of file +int is_crossroad_robust(SensorBuffer *sb); + +void print_binary(const uint8_t n); + +uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value); diff --git a/docker-compose.yml b/docker-compose.yml index 512459c..e5ebed1 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -6,10 +6,11 @@ services: network_mode: host # GUI 디스플레이 호환성을 위해 호스트 네트워크 사용 user: "1000:1000" environment: - - TZ=Asia/Seoul + - TZ=Asia/Seoul + - HOME=/workspaces volumes: - /dev/bus/usb:/dev/bus/usb - ./ccs:/ccs_projects # 프로젝트 디렉토리 (Entrypoint가 여기를 참조) - ./res:/workspaces # 워크스페이스 및 빌드 결과 저장소 entrypoint: ["/bin/bash", "-c", "sleep infinity"] - #entrypoint: ["/entrypoint.sh", ".ccsproject", "DEBUG"] # 컨테이너가 계속 실행되도록 설정 \ No newline at end of file + #entrypoint: ["/entrypoint.sh", ".ccsproject", "DEBUG"] # 컨테이너가 계속 실행되도록 설정