stm32를 이용하여 초음파센서를 활용한 RC카를 만들어 보는게 목표였습니다. 이러한 차를 만들기 위해서는 3개의 초음파, 1개의 I2C_LCD, 1개의 FND, 1개의 모터드라이버, 4개의 DC모터, 블루투스 모듈을 사용하여 구현했습니다.
RTOS란 Real-Time Operating System의 약어로, 실시간 시스템에서 사용되는 운영체제를 말합니다.쓰레드 사용하여 뮤텍스를 이용하였는데 기존의 while문에서는 하나를 실행하고 끝난후 하나를 실행하고 그런식으로 반복하였지만 쓰레드에서는 각각 실행이 되기때문에 한번에 사용이 됩니다.
뮤텍스는 쓰레드가 접근할때에 한 쓰레드가 뮤텍스를 사용할때 다른쓰레드는 접근할수가 없습니다. 그 쓰레드가 동작을 전부다 한 이후 다른 쓰레드가 접근이 가능합니다.
설계 목표는 다음과같습니다.
1. 초음파 센서를 이용한 자율 주행
2. Bluetooth모듈과 UART 통신을 이용한 수동 주행
3. RTOS에서 동작되는 자율 주행 및 수동 주행
설계 알고리즘은 위의 그림과 같습니다. 파워가 보드에 제공되면 Mannual mode가 실행되고 버튼을 누르면 AUTO MODE로 전환됩니다. 이를 RTOS모드를 통해 사용됩니다.
가장먼저 Manual mode의 기능은 F B L R S로 전진,후진,왼쪽으로,오른쪽으로, 멈춤지시가 있습니다.
위의 그림은 RC카의 초음파센서를 통한 DC모터의 작동방법입니다.
void auto_mode_check(void)
{
static uint8_t tmp_bt_byte;
static int bt_auto_mode;
if(tmp_bt_byte != bt_byte)
{
tmp_bt_byte = bt_byte;
if(bt_byte == 'G')
{
bt_auto_mode = 1;
}
}
if ((get_button(BUTTON0_GPIO_Port, BUTTON0_Pin, BUTTON0) == BUTTON_PRESS) || bt_auto_mode == 1)
{
bt_auto_mode = 0;
HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0);
auto_mode_state = !auto_mode_state;
if (osMutexWait(myMutex01Handle, 1000) == osOK)
; // wait for Mutex idle
{
move_cursor(0, 0);
if (auto_mode_state)
{
lcd_string("MANNUAL Mode ");
}
else
{
lcd_string("AUTO Mode ");
}
move_cursor(1, 0);
lcd_string(" ");
osMutexRelease(myMutex01Handle); // Release Mutex
}
}
}뮤텍스를 사용한 방식의 함수입니다. 다음과같이 글씨를 띄워주고 종료 후 다시 Mutex로 릴리즈해주는 것을 볼수있습니다.
void manual_mode_run(void)
{
// printf("%c", bt_byte);
if (auto_mode_state == 0)
{
if (osMutexWait(myMutex01Handle, 1000) == osOK)
; // wait for Mutex idle
{
move_cursor(1, 0);
}
// printf("%c", bt_byte);
switch (bt_byte)
{
case 'F': // Forward
lcd_string("Forward ");
forward(100);
break;
case 'B': // Backward
lcd_string("Backward ");
backward(80);
break;
case 'L': // Left Turn
lcd_string("Left Turn ");
left_turn(80);
break;
case 'R': // Right Turn
lcd_string("Right Turn ");
right_turn(80);
break;
case 'S': // Stop
lcd_string("Stop ");
stop();
break;
#if 1
//case 'G': // Left Turn Forward
//lcd_string("Left Forward ");
// left_turn_forward(100);
// break;
case 'I': // Right Turn Forward
lcd_string("Right Forward ");
right_turn_forward(100);
break;
case 'H': // Left Turn Backward
lcd_string("Left Backward ");
left_turn_backward(100);
break;
case 'J': // Right Turn Backward
lcd_string("Right Backward ");
right_turn_backward(100);
break;
#endif
default:
break;
}
osMutexRelease(myMutex01Handle); // Release Mutex
}
}수동모드로 진입했을떄 뮤택스 모드에 입장했다는 플래그를 날리고 수동모드에 대한 조건들을 사용하였습니다.
void auto_drive_mode_run()
{
static int prev_if_right_fail = 0;
static int scan_count = 1;
if (near_flag_right & !near_flag_left)
{
left_turn(70);
lcd_string("Left Turn ");
prev_if_right_fail = 0;
scan_count = 1;
}
else if (near_flag_left & !near_flag_right)
{
right_turn(70);
lcd_string("right Turn ");
prev_if_right_fail = 0;
scan_count = 1;
}
else if (!near_flag_left & near_flag_center & !near_flag_right)
{
if(distance_left > distance_right)
{
left_turn(80);
}
else
{
right_turn(80);
}
lcd_string("right Turn ");
prev_if_right_fail = 0;
scan_count = 1;
}
else if (near_flag_left & !near_flag_center & near_flag_right)
{
forward(85);
lcd_string("Forward ");
prev_if_right_fail = 0;
scan_count = 1;
}
else if (!near_flag_left & !near_flag_center & !near_flag_right)
{
forward(85);
lcd_string("Forward ");
prev_if_right_fail = 0;
scan_count = 1;
}
else
{
stop();
HAL_Delay(100);
if(scan_count % 5 == 1)
{
backward(60);
lcd_string("Backward ");
HAL_Delay(500);
scan_count = 1;
}
if(prev_if_right_fail)
{
for(int i=0;i<scan_count;i++)
{
left_turn(80);
HAL_Delay(100);
}
prev_if_right_fail = 0;
scan_count++;
}
else
{
for(int i=0;i<scan_count;i++)
{
right_turn(80);
HAL_Delay(100);
}
prev_if_right_fail = 1;
scan_count++;
}
stop();
}
}
void ultrasonic_main(void)
{
if (finish_flag_left) // 초음파 측정이 완료됨
{
finish_flag_left = 0; // 초기화
distance_left = distance_left * 0.034 / 2; // 1us가 0.034cm를 이동
if (distance_left < 0) distance_left = 100;
printf("left : %d cm\n", distance_left);
if (distance_left <= 20)
{
near_flag_left = 1;
}
else
{
near_flag_left = 0;
}
make_trigger();
}
if (finish_flag_center)
{
finish_flag_center = 0; // 초기화
distance_center = distance_center * 0.034 / 2; // 1us가 0.034cm를 이동
if (distance_center < 0) distance_center = 100;
printf("center : %d cm\n", distance_center);
if (distance_center <= 23)
{
near_flag_center = 1;
}
else
{
near_flag_center = 0;
}
make_trigger_center();
}
if (finish_flag_right)
{
finish_flag_right = 0; // 초기화
distance_right = distance_right * 0.034 / 2; // 1us가 0.034cm를 이동
if (distance_right < 0) distance_right = 100;
printf("right : %d cm\n", distance_right);
if (distance_right <= 18)
{
near_flag_right = 1;
}
else
{
near_flag_right = 0;
}
make_trigger_right();
}
}
아래의 ultrasonic_main에서 flag를 extern 하여 auto모드에 사용하였습니다.
동작을 대략적으로 설명하면
왼쪽의거리가 오른쪽의 거리보다 남은공간이 더 클때, 차는 왼쪽으로 간다고 모터에게 값을 줍니다. 그래서 좌로 우로 계속 돌면서 앞으로 갈거리를 찾습니다.
만약 돌 공간이 없을경우 후진을 진행한후에 차가 앞으로 가게됩니다.
완성된 사진입니다.
'나의 전자 공부방 > STM32' 카테고리의 다른 글
| 2. STM32를 사용하여 도어락만들기(LCD, RFID, 서보모터) (0) | 2023.04.12 |
|---|---|
| 1. [stm32] 동기방식, 비동기방식, LED_processing 동영상 첨부 (0) | 2023.04.05 |
