/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
float distance1 = 0;
float distance2 = 0;
float distance3 = 0;
uint16_t pwm_dc1t = 0;
uint16_t pwm_dc1n = 0;
uint16_t pwm_dc2t = 0;
uint16_t pwm_dc2n = 0;
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
UART_HandleTypeDef huart1;
/* USER CODE BEGIN PV */
// Sensor 1
#define TRIG1_PIN GPIO_PIN_8
#define TRIG1_PORT GPIOA
#define ECHO1_PIN GPIO_PIN_9
#define ECHO1_PORT GPIOA
// Sensor 2
#define TRIG2_PIN GPIO_PIN_11
#define TRIG2_PORT GPIOA
#define ECHO2_PIN GPIO_PIN_10
#define ECHO2_PORT GPIOA
// Sensor 3
#define TRIG3_PIN GPIO_PIN_8
#define TRIG3_PORT GPIOB
#define ECHO3_PIN GPIO_PIN_9
#define ECHO3_PORT GPIOB
void delay_us_TIM1(uint16_t us) {
__HAL_TIM_SET_COUNTER(&htim1, 0);
while (__HAL_TIM_GET_COUNTER(&htim1) < us);
void delay_us_TIM2(uint16_t us) {
__HAL_TIM_SET_COUNTER(&htim2, 0);
while (__HAL_TIM_GET_COUNTER(&htim2) < us);
void delay_us_TIM4(uint16_t us) {
__HAL_TIM_SET_COUNTER(&htim4, 0);
while (__HAL_TIM_GET_COUNTER(&htim4) < us);
void HCSR05_Trigger1(void) {
HAL_GPIO_WritePin(TRIG1_PORT, TRIG1_PIN, GPIO_PIN_RESET);
delay_us_TIM1(2);
HAL_GPIO_WritePin(TRIG1_PORT, TRIG1_PIN, GPIO_PIN_SET);
delay_us_TIM1(10);
HAL_GPIO_WritePin(TRIG1_PORT, TRIG1_PIN, GPIO_PIN_RESET);
void HCSR05_Trigger2(void) {
HAL_GPIO_WritePin(TRIG2_PORT, TRIG2_PIN, GPIO_PIN_RESET);
delay_us_TIM2(2);
HAL_GPIO_WritePin(TRIG2_PORT, TRIG2_PIN, GPIO_PIN_SET);
delay_us_TIM2(10);
HAL_GPIO_WritePin(TRIG2_PORT, TRIG2_PIN, GPIO_PIN_RESET);
void HCSR05_Trigger3(void) {
HAL_GPIO_WritePin(TRIG3_PORT, TRIG3_PIN, GPIO_PIN_RESET);
delay_us_TIM4(2);
HAL_GPIO_WritePin(TRIG3_PORT, TRIG3_PIN, GPIO_PIN_SET);
delay_us_TIM4(10);
HAL_GPIO_WritePin(TRIG3_PORT, TRIG3_PIN, GPIO_PIN_RESET);
uint32_t HCSR05_Read1(void) {
uint32_t time = 0;
while (HAL_GPIO_ReadPin(ECHO1_PORT, ECHO1_PIN) == GPIO_PIN_RESET);
while (HAL_GPIO_ReadPin(ECHO1_PORT, ECHO1_PIN) == GPIO_PIN_SET) {
time++;
delay_us_TIM1(1);
if (time > 30000) break;
return time;
uint32_t HCSR05_Read2(void) {
uint32_t time = 0;
while (HAL_GPIO_ReadPin(ECHO2_PORT, ECHO2_PIN) == GPIO_PIN_RESET);
while (HAL_GPIO_ReadPin(ECHO2_PORT, ECHO2_PIN) == GPIO_PIN_SET) {
time++;
delay_us_TIM2(1);
if (time > 30000) break;
return time;
uint32_t HCSR05_Read3(void) {
uint32_t time = 0;
while (HAL_GPIO_ReadPin(ECHO3_PORT, ECHO3_PIN) == GPIO_PIN_RESET);
while (HAL_GPIO_ReadPin(ECHO3_PORT, ECHO3_PIN) == GPIO_PIN_SET) {
time++;
delay_us_TIM4(1);
if (time > 30000) break;
return time;
typedef enum {
STATE_FORWARD,
STATE_TURN_LEFT,
STATE_TURN_RIGHT,
STATE_STOP,
} RobotState;
void Motor_Control(float d_left, float d_right, float d_front)
{
//uint16_t pwm_dc1t = 0;
//uint16_t pwm_dc1n = 0;
//uint16_t pwm_dc2t = 0;
//uint16_t pwm_dc2n = 0;
RobotState state;
// --- Xác định trạng thái ---
if (d_front >= 2.0) // Phía trước trống, đi thẳng
state = STATE_FORWARD;
else // Có vật cản phía trước
if (d_left < 2.0 && d_right >= 2.0)
state = STATE_TURN_RIGHT; // Trái bị chặn → rẽ phải
else if (d_right < 2.0 && d_left >= 2.0)
state = STATE_TURN_LEFT; // Phải bị chặn → rẽ trái
else if (d_left < 2.0 && d_right < 2.0)
state = STATE_STOP; // Cả hai bên đều chặn → dừng
else
state = STATE_STOP;
// --- Hành động tương ứng ---
switch (state)
{
case STATE_FORWARD:
HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);
pwm_dc1t = 250;
pwm_dc1n = 0;
pwm_dc2t = 250;
pwm_dc2n = 0;
break;
case STATE_TURN_LEFT:
pwm_dc1t = 0;
pwm_dc1n = 250;
pwm_dc2t = 250;
pwm_dc2n = 0;
break;
case STATE_TURN_RIGHT:
pwm_dc1t = 250;
pwm_dc1n = 0;
pwm_dc2t = 0;
pwm_dc2n = 250;
break;
case STATE_STOP:
default:
pwm_dc1t = 0;
pwm_dc1n = 0;
pwm_dc2t = 0;
pwm_dc2n = 0;
HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_RESET);
break;
// --- Cập nhật PWM ---
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, pwm_dc1t);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, pwm_dc2t);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, pwm_dc1n);
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, pwm_dc2n);
// --- LED báo trạng thái ---
if (state == STATE_STOP)
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET); // LED sáng khi dừng
else
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET); // LED tắt khi đang di chuyển
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM4_Init(void);
static void MX_TIM2_Init(void);
static void MX_USART1_UART_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
- HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM1_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM2_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
// HAL_UART_Receive_IT(&huart1, &rx_data, 1);
HAL_TIM_Base_Start(&htim1);
HAL_TIM_Base_Start(&htim2);
HAL_TIM_Base_Start(&htim4);
HAL_GPIO_WritePin(GPIOA, EN1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA, EN2_Pin, GPIO_PIN_SET);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
/* USER CODE END WHILE */
// Cảm biến 1
HCSR05_Trigger1();
uint32_t t1 = HCSR05_Read1();
distance1 = t1 / 58.0;
HAL_Delay(70);
// Cảm biến 2
HCSR05_Trigger2();
uint32_t t2 = HCSR05_Read2();
distance2 = t2 / 58.0;
HAL_Delay(70);
// Cảm biến 3
HCSR05_Trigger3();
uint32_t t3 = HCSR05_Read3();
distance3 = t3 / 58.0;
HAL_Delay(70);
//Logic điều khiển động cơ
Motor_Control(distance3, distance1, distance2);
HAL_Delay(70);
/* USER CODE BEGIN 3 */
/* USER CODE END 3 */
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.[Link] = RCC_PLL_NONE;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
Error_Handler();
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
Error_Handler();
}
/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
/* USER CODE BEGIN TIM1_Init 0 */
/* USER CODE END TIM1_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM1_Init 1 */
/* USER CODE END TIM1_Init 1 */
[Link] = TIM1;
[Link] = 72-1;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 65535;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = 0;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
{
Error_Handler();
[Link] = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
Error_Handler();
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
Error_Handler();
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_Init 2 */
/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
[Link] = TIM2;
[Link] = 72-1;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 65535;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
Error_Handler();
[Link] = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
Error_Handler();
}
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
Error_Handler();
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
/**
* @brief TIM3 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM3_Init(void)
/* USER CODE BEGIN TIM3_Init 0 */
/* USER CODE END TIM3_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM3_Init 1 */
/* USER CODE END TIM3_Init 1 */
[Link] = TIM3;
[Link] = 0;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 1053;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
Error_Handler();
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
Error_Handler();
[Link] = TIM_OCMODE_PWM1;
[Link] = 0;
[Link] = TIM_OCPOLARITY_HIGH;
[Link] = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
Error_Handler();
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
Error_Handler();
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
Error_Handler();
/* USER CODE BEGIN TIM3_Init 2 */
/* USER CODE END TIM3_Init 2 */
HAL_TIM_MspPostInit(&htim3);
/**
* @brief TIM4 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM4_Init(void)
/* USER CODE BEGIN TIM4_Init 0 */
/* USER CODE END TIM4_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM4_Init 1 */
/* USER CODE END TIM4_Init 1 */
[Link] = TIM4;
[Link] = 72-1;
[Link] = TIM_COUNTERMODE_UP;
[Link] = 65535;
[Link] = TIM_CLOCKDIVISION_DIV1;
[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
Error_Handler();
[Link] = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
[Link] = TIM_TRGO_RESET;
[Link] = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
Error_Handler();
/* USER CODE BEGIN TIM4_Init 2 */
/* USER CODE END TIM4_Init 2 */
/**
* @brief USART1 Initialization Function
* @param None
* @retval None
*/
static void MX_USART1_UART_Init(void)
/* USER CODE BEGIN USART1_Init 0 */
/* USER CODE END USART1_Init 0 */
/* USER CODE BEGIN USART1_Init 1 */
/* USER CODE END USART1_Init 1 */
[Link] = USART1;
[Link] = 115200;
[Link] = UART_WORDLENGTH_8B;
[Link] = UART_STOPBITS_1;
[Link] = UART_PARITY_NONE;
[Link] = UART_MODE_TX_RX;
[Link] = UART_HWCONTROL_NONE;
[Link] = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
Error_Handler();
/* USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_13;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, EN1_Pin|EN2_Pin|GPIO_PIN_8|GPIO_PIN_11, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_RESET);
/*Configure GPIO pins : EN1_Pin EN2_Pin PA8 PA11 */
GPIO_InitStruct.Pin = EN1_Pin|EN2_Pin|GPIO_PIN_8|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PA9 PA10 */
GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_10;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pin : PB8 */
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/*Configure GPIO pin : PB9 */
GPIO_InitStruct.Pin = GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
/* USER CODE END Error_Handler_Debug */
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
#endif /* USE_FULL_ASSERT */