main.c
/* USER CODE BEGIN Header /
/*
@file : main.c
@brief : Main program body
@attention
Copyright © 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”
#include “i2c.h”
#include “tim.h”
#include “usart.h”
#include “gpio.h”
#include “jy61p.h”
#include “pid.h”
#include “kalman_filter.h”
#include “motor.h”
#include “encoder.h”
#include “ultrasonic.h”
#include “motion.h”
/* Private includes ----------------------------------------------------------/
/ USER CODE BEGIN Includes */
#include “stdio.h”
#include “string.h”
// Ô²ÖùÐÅÏ¢½á¹¹Ìå
#define MAX_CYLINDERS 10
typedef struct {
int x, y, r;
char color;
} Cylinder;
Cylinder cylinders[MAX_CYLINDERS];
int cylinder_count = 0;
uint8_t rx_byte;
char openmv_buffer[128];
uint8_t openmv_index = 0;
uint32_t start_time = 0;
uint32_t timeout = 10000; // 10s
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------/
/ USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------/
/ USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------/
/ USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------/
void SystemClock_Config(void);
/ USER CODE BEGIN PFP */
void SetTargetYaw(float angle);
void AddCylinder(int x, int y, int r, char color);
void Path_AroundCylinder(char color);
void BuildPath(void);
void StartMission(void);
uint8_t IsMissionTimeout(void);
int CheckColumnSwap(void);
void HandleColumnSwap(void);
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------/
/ USER CODE BEGIN 0 */
// Ä£¿éÊý¾Ý½á¹¹Ìå
JY61P_Data imu_data;
PID_Controller speed_pid, angle_pid;
KalmanFilter kf;
// ¿ØÖƲÎÊý
float target_speed = 100;
float base_pwm = 70;
float dt = 0.01; // ¿ØÖÆÖÜÆÚ/Ãë
void Control_Straight(void);
void Move_Forward(float cm);
void Turn_Left(float angle);
void Turn_Right(float angle);
void Avoid_Obstacle(void);
void Send_Debug_Info(void);
// PID ¿ØÖÆÖ±ÐÐ
void Control_Straight(void) {
float left_speed = Get_Left_Encoder();
float right_speed = Get_Right_Encoder();
float error = left_speed - right_speed; float correction = PID_Update(&angle_pid, error, dt); float left_pwm = base_pwm + correction; float right_pwm = base_pwm - correction; Motor_SetSpeed(left_pwm, right_pwm);
}
// PID ¿ØÖÆתÏòº¯Êý
void SetTargetYaw(float angle) {
printf(“Turning %.1f degrees\n”, angle);
HAL_Delay(500);
}
// Ìí¼ÓÔ²Öùµ½µØͼ
void AddCylinder(int x, int y, int r, char color)
{
if (cylinder_count < MAX_CYLINDERS) {
cylinders[cylinder_count].x = x;
cylinders[cylinder_count].y = y;
cylinders[cylinder_count].r = r;
cylinders[cylinder_count].color = color;
cylinder_count++;
}
}
// ÈÆÏß·¾¶
void Path_AroundCylinder(char color)
{
if (color == ‘W’) {
SetTargetYaw(-180.0f); // °××ó
} else if (color == ‘B’) {
SetTargetYaw(180.0f); // ºÚÓÒ
}
}
// ¹¹½¨Â·¾¶
void BuildPath()
{
for (int i = 0; i < cylinder_count; i++) {
Path_AroundCylinder(cylinders[i].color);
}
}
//¿ªÊ¼¼Æʱ
void StartMission()
{
start_time = HAL_GetTick();
}
// ÊÇ·ñ³¬Ê±
uint8_t IsMissionTimeout()
{
return (HAL_GetTick() - start_time) > timeout;
}
// ¼ì²âÔ²ÖùÊÇ·ñ»¥»»
int CheckColumnSwap()
{
for (int i = 0; i < cylinder_count; i++) {
if (cylinders[i].x == 3 && cylinders[i].color == ‘B’) {
return 1;
}
}
return 0;
}
//´¦ÀíÖù×Ó»¥»»
void HandleColumnSwap()
{
if (CheckColumnSwap()) {
printf(“Öù×ÓλÖñ仯£¬ÖØй滮·Ïß…\n”);
BuildPath(); // ÖØÐÂÈÆÖù
}
}
//´®¿Ú½ÓÊջص÷
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if (huart == &huart1)
{
if (rx_byte == ‘\n’)
{
openmv_buffer[openmv_index] = ‘\0’;
openmv_index = 0;
if (strncmp(openmv_buffer, "C ", 2) == 0) { int x, y, r; char color; sscanf(openmv_buffer, “C %d %d %d %c”, &x, &y, &r, &color); AddCylinder(x, y, r, color); Path_AroundCylinder(color); } } else { openmv_buffer[openmv_index++] = rx_byte; } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); }
}
/* 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_I2C1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_USART1_UART_Init();
/ USER CODE BEGIN 2 */
HAL_UART_Receive_IT(&huart1, &rx_byte, 1);
StartMission();
//Æô¶¯±àÂëÆ÷¼ÆÊý
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
// ³õʼ»¯ IMU
JY61P_Init(&hi2c1);
// ³õʼ»¯ PID
PID_Init(&speed_pid, 1.0f, 0.1f, 0.05f);
PID_Init(&angle_pid, 2.0f, 0.0f, 0.1f);
// ³õʼ»¯ ¿¨¶ûÂüÂ˲¨
KalmanFilter_Init(&kf, 0.001f, 0.003f, 0.03f);
// Æô¶¯ PWM Êä³ö
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
/* USER CODE END 2 */
/* Infinite loop /
/ USER CODE BEGIN WHILE */
/* USER CODE END WHILE / / USER CODE BEGIN 3 */ while (1)
{ float left = Get_Left_Distance();
float right = Get_Right_Distance();
if (left > 10.0f && right > 10.0f) { Control_Straight(); // Ö±ÐпØÖÆ } else { Motor_Stop(); // Í£Ö¹ if (left > right) Turn_Left(90); // ×óת else Turn_Right(90); // ÓÒת Move_Forward(20); // Ç°½ø20cm Turn_Right(90); // »ØÕý Move_Forward(20); // ¼ÌÐøÇ°½ø } Send_Debug_Info(); // ´®¿ÚÊä³öµ÷ÊÔÐÅÏ¢ HAL_Delay(10); // 10ms ÑÓʱ
}
/* 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_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
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_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/* 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 */
jy61p.c
#include “jy61p.h”
#include <string.h>
#define JY61P_ADDRESS 0xA0
void JY61P_Init(I2C_HandleTypeDef *hi2c) {}
float JY61P_Read_Float(I2C_HandleTypeDef *hi2c, uint8_t reg)
{
uint8_t tx_data = reg;
uint8_t rx_data[4];
HAL_I2C_Master_Transmit(hi2c, JY61P_ADDRESS, &tx_data, 1, HAL_MAX_DELAY);
HAL_I2C_Master_Receive(hi2c, JY61P_ADDRESS, rx_data, 4, HAL_MAX_DELAY);
float value;
memcpy(&value, rx_data, sizeof(float));
return value;
}
void JY61P_Read_Data(I2C_HandleTypeDef *hi2c, JY61P_Data *data)
{
data->yaw = JY61P_Read_Float(hi2c, 0x34);
data->pitch = JY61P_Read_Float(hi2c, 0x32);
data->roll = JY61P_Read_Float(hi2c, 0x30);
}
encoder.c
#include “encoder.h”
#include “main.h”
#include “tim.h”
int16_t Get_Left_Encoder(void) {
return (int16_t)__HAL_TIM_GET_COUNTER(&htim3);
}
int16_t Get_Right_Encoder(void) {
return (int16_t)__HAL_TIM_GET_COUNTER(&htim4);
}
motor.c
#include “motor.h”
#include “main.h”
#include “tim.h”
void Motor_SetSpeed(float left, float right)
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)left);
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, (uint32_t)right);
}
void Motor_Stop(void) {
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0);
}
kalman_filter.c
#include “kalman_filter.h”
void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_gyro, float R_angle) {
kf->theta = 0.0f;
kf->omega = 0.0f;
kf->P[0][0] = 1.0f;
kf->P[0][1] = 0.0f;
kf->P[1][0] = 0.0f;
kf->P[1][1] = 1.0f;
kf->Q_angle = Q_angle;
kf->Q_gyro = Q_gyro;
kf->R_angle = R_angle;
}
float KalmanFilter_Update(KalmanFilter *kf, float measured_angle, float measured_omega, float dt) {
kf->omega = measured_omega;
kf->theta += dt * kf->omega;
kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= dt * kf->P[1][1]; kf->P[1][0] -= dt * kf->P[1][1]; kf->P[1][1] += kf->Q_gyro * dt; float y = measured_angle - kf->theta; float S = kf->P[0][0] + kf->R_angle; float K0 = kf->P[0][0] / S; float K1 = kf->P[1][0] / S; kf->theta += K0 * y; kf->omega += K1 * y; kf->P[0][0] -= K0 * kf->P[0][0]; kf->P[0][1] -= K0 * kf->P[0][1]; kf->P[1][0] -= K1 * kf->P[0][0]; kf->P[1][1] -= K1 * kf->P[0][1]; return kf->theta;
}
pid.c
#include “pid.h”
void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd) {
pid->Kp = Kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->integral = 0.0f;
pid->last_error = 0.0f;
}
float PID_Update(PID_Controller pid, float error, float dt) {
pid->integral += error * dt;
float derivative = (error - pid->last_error) / dt;
float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
pid->last_error = error;
return output;
}
gpio.c
/ USER CODE BEGIN Header /
/*
@file gpio.c
@brief This file provides code for the configuration
of all used GPIO pins.
@attention
Copyright © 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 “gpio.h”
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/----------------------------------------------------------------------------/
/* Configure GPIO /
/----------------------------------------------------------------------------/
/ USER CODE BEGIN 1 */
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2|GPIO_PIN_4, GPIO_PIN_RESET);
/*Configure GPIO pins : PB2 PB4 */
GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_4 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;;
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 pins : PB3 PB5 */
GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_5;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 /
i2c.c
/ USER CODE BEGIN Header /
/*
@file i2c.c
@brief This file provides code for the configuration
of the I2C instances.
@attention
Copyright © 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 “i2c.h”
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
I2C_HandleTypeDef hi2c1;
/* I2C1 init function */
void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 /
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 400000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/ USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(i2cHandle->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspInit 0 */
/* USER CODE END I2C1_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE(); /**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA */ GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); __HAL_AFIO_REMAP_I2C1_ENABLE(); /* I2C1 clock enable */ __HAL_RCC_I2C1_CLK_ENABLE();
/* USER CODE BEGIN I2C1_MspInit 1 */
/* USER CODE END I2C1_MspInit 1 */
}
}
void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle)
{
if(i2cHandle->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspDeInit 0 */
/* USER CODE END I2C1_MspDeInit 0 /
/ Peripheral clock disable */
__HAL_RCC_I2C1_CLK_DISABLE();
/**I2C1 GPIO Configuration PB8 ------> I2C1_SCL PB9 ------> I2C1_SDA */ HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8); HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9);
/* USER CODE BEGIN I2C1_MspDeInit 1 */
/* USER CODE END I2C1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 /
tim.c
/ USER CODE BEGIN Header /
/*
@file tim.c
@brief This file provides code for the configuration
of the TIM instances.
@attention
Copyright © 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 “tim.h”
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
/* TIM2 init function */
void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 /
htim2.Instance = TIM2;
htim2.Init.Prescaler = 71;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 999;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
/ USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/* TIM3 init function */
void MX_TIM3_Init(void)
{
/* USER CODE BEGIN TIM3_Init 0 */
/* USER CODE END TIM3_Init 0 */
TIM_Encoder_InitTypeDef sConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM3_Init 1 */
/* USER CODE END TIM3_Init 1 /
htim3.Instance = TIM3;
htim3.Init.Prescaler = 0;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 65535;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/ USER CODE BEGIN TIM3_Init 2 */
/* USER CODE END TIM3_Init 2 */
}
/* TIM4 init function */
void MX_TIM4_Init(void)
{
/* USER CODE BEGIN TIM4_Init 0 */
/* USER CODE END TIM4_Init 0 */
TIM_Encoder_InitTypeDef sConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM4_Init 1 */
/* USER CODE END TIM4_Init 1 /
htim4.Instance = TIM4;
htim4.Init.Prescaler = 0;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim4.Init.Period = 65535;
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = 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 */
}
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
{
if(tim_pwmHandle->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspInit 0 */
/* USER CODE END TIM2_MspInit 0 /
/ TIM2 clock enable /
__HAL_RCC_TIM2_CLK_ENABLE();
/ USER CODE BEGIN TIM2_MspInit 1 */
/* USER CODE END TIM2_MspInit 1 */
}
}
void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(tim_encoderHandle->Instance==TIM3)
{
/* USER CODE BEGIN TIM3_MspInit 0 */
/* USER CODE END TIM3_MspInit 0 /
/ TIM3 clock enable */
__HAL_RCC_TIM3_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* TIM3 interrupt Init */ HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn);
/* USER CODE BEGIN TIM3_MspInit 1 */
/* USER CODE END TIM3_MspInit 1 /
}
else if(tim_encoderHandle->Instance==TIM4)
{
/ USER CODE BEGIN TIM4_MspInit 0 */
/* USER CODE END TIM4_MspInit 0 /
/ TIM4 clock enable */
__HAL_RCC_TIM4_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE(); /**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* TIM4 interrupt Init */ HAL_NVIC_SetPriority(TIM4_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM4_IRQn);
/* USER CODE BEGIN TIM4_MspInit 1 */
/* USER CODE END TIM4_MspInit 1 /
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef timHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(timHandle->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspPostInit 0 */
/* USER CODE END TIM2_MspPostInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE(); /**TIM2 GPIO Configuration PA0-WKUP ------> TIM2_CH1 PA1 ------> TIM2_CH2 */ GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN TIM2_MspPostInit 1 */
/* USER CODE END TIM2_MspPostInit 1 */
}
}
void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
{
if(tim_pwmHandle->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspDeInit 0 */
/* USER CODE END TIM2_MspDeInit 0 /
/ Peripheral clock disable /
__HAL_RCC_TIM2_CLK_DISABLE();
/ USER CODE BEGIN TIM2_MspDeInit 1 */
/* USER CODE END TIM2_MspDeInit 1 */
}
}
void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle)
{
if(tim_encoderHandle->Instance==TIM3)
{
/* USER CODE BEGIN TIM3_MspDeInit 0 */
/* USER CODE END TIM3_MspDeInit 0 /
/ Peripheral clock disable */
__HAL_RCC_TIM3_CLK_DISABLE();
/**TIM3 GPIO Configuration PA6 ------> TIM3_CH1 PA7 ------> TIM3_CH2 */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_6|GPIO_PIN_7); /* TIM3 interrupt Deinit */ HAL_NVIC_DisableIRQ(TIM3_IRQn);
/* USER CODE BEGIN TIM3_MspDeInit 1 */
/* USER CODE END TIM3_MspDeInit 1 /
}
else if(tim_encoderHandle->Instance==TIM4)
{
/ USER CODE BEGIN TIM4_MspDeInit 0 */
/* USER CODE END TIM4_MspDeInit 0 /
/ Peripheral clock disable */
__HAL_RCC_TIM4_CLK_DISABLE();
/**TIM4 GPIO Configuration PB6 ------> TIM4_CH1 PB7 ------> TIM4_CH2 */ HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6|GPIO_PIN_7); /* TIM4 interrupt Deinit */ HAL_NVIC_DisableIRQ(TIM4_IRQn);
/* USER CODE BEGIN TIM4_MspDeInit 1 */
/* USER CODE END TIM4_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 /
usart.c
/ USER CODE BEGIN Header /
/*
@file usart.c
@brief This file provides code for the configuration
of the USART instances.
@attention
Copyright © 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 “usart.h”
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
UART_HandleTypeDef huart1;
/* USART1 init function */
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 /
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
/ USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspInit 0 */
/* USER CODE END USART1_MspInit 0 /
/ USART1 clock enable */
__HAL_RCC_USART1_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE(); /**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ GPIO_InitStruct.Pin = GPIO_PIN_9; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* USART1 interrupt Init */ HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); HAL_NVIC_EnableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspInit 1 */
/* USER CODE END USART1_MspInit 1 */
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspDeInit 0 */
/* USER CODE END USART1_MspDeInit 0 /
/ Peripheral clock disable */
__HAL_RCC_USART1_CLK_DISABLE();
/**USART1 GPIO Configuration PA9 ------> USART1_TX PA10 ------> USART1_RX */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10); /* USART1 interrupt Deinit */ HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */
/* USER CODE END USART1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 /
void Send_Debug_Info(void) {
char buffer[100];
sprintf(buffer, “Left: %.2f cm, Right: %.2f cm\r\n”, Get_Left_Distance(), Get_Right_Distance());
HAL_UART_Transmit(&huart1, (uint8_t)buffer, strlen(buffer), HAL_MAX_DELAY);
}
/* USER CODE END 1 /
stm32f1xx_it.c
/ USER CODE BEGIN Header /
/*
@file stm32f1xx_it.c
@brief Interrupt Service Routines.
@attention
Copyright © 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”
#include “stm32f1xx_it.h”
/ Private includes ----------------------------------------------------------/
/ USER CODE BEGIN Includes /
/ USER CODE END Includes */
/* Private typedef -----------------------------------------------------------/
/ USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------/
/ USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------/
/ USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------/
/ USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------/
/ USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------/
/ USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------/
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
extern UART_HandleTypeDef huart1;
/ USER CODE BEGIN EV */
/* USER CODE END EV */
//
/ Cortex-M3 Processor Interruption and Exception Handlers /
//
/**
@brief This function handles Non maskable interrupt.
/
void NMI_Handler(void)
{
/ USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 /
/ USER CODE BEGIN NonMaskableInt_IRQn 1 /
while (1)
{
}
/ USER CODE END NonMaskableInt_IRQn 1 */
}
/**
@brief This function handles Hard fault interrupt.
/
void HardFault_Handler(void)
{
/ USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 /
while (1)
{
/ USER CODE BEGIN W1_HardFault_IRQn 0 /
/ USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
@brief This function handles Memory management fault.
/
void MemManage_Handler(void)
{
/ USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 /
while (1)
{
/ USER CODE BEGIN W1_MemoryManagement_IRQn 0 /
/ USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
@brief This function handles Prefetch fault, memory access fault.
/
void BusFault_Handler(void)
{
/ USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 /
while (1)
{
/ USER CODE BEGIN W1_BusFault_IRQn 0 /
/ USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
@brief This function handles Undefined instruction or illegal state.
/
void UsageFault_Handler(void)
{
/ USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 /
while (1)
{
/ USER CODE BEGIN W1_UsageFault_IRQn 0 /
/ USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
@brief This function handles System service call via SWI instruction.
/
void SVC_Handler(void)
{
/ USER CODE BEGIN SVCall_IRQn 0 */
/* USER CODE END SVCall_IRQn 0 /
/ USER CODE BEGIN SVCall_IRQn 1 */
/* USER CODE END SVCall_IRQn 1 */
}
/**
@brief This function handles Debug monitor.
/
void DebugMon_Handler(void)
{
/ USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 /
/ USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/**
@brief This function handles Pendable request for system service.
/
void PendSV_Handler(void)
{
/ USER CODE BEGIN PendSV_IRQn 0 */
/* USER CODE END PendSV_IRQn 0 /
/ USER CODE BEGIN PendSV_IRQn 1 */
/* USER CODE END PendSV_IRQn 1 */
}
/**
@brief This function handles System tick timer.
/
void SysTick_Handler(void)
{
/ USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 /
HAL_IncTick();
/ USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
//
/ STM32F1xx Peripheral Interrupt Handlers /
/ Add here the Interrupt Handlers for the used peripherals. /
/ For the available peripheral interrupt handler names, /
/ please refer to the startup file (startup_stm32f1xx.s). /
//
/**
@brief This function handles TIM3 global interrupt.
/
void TIM3_IRQHandler(void)
{
/ USER CODE BEGIN TIM3_IRQn 0 */
/* USER CODE END TIM3_IRQn 0 /
HAL_TIM_IRQHandler(&htim3);
/ USER CODE BEGIN TIM3_IRQn 1 */
/* USER CODE END TIM3_IRQn 1 */
}
/**
@brief This function handles TIM4 global interrupt.
/
void TIM4_IRQHandler(void)
{
/ USER CODE BEGIN TIM4_IRQn 0 */
/* USER CODE END TIM4_IRQn 0 /
HAL_TIM_IRQHandler(&htim4);
/ USER CODE BEGIN TIM4_IRQn 1 */
/* USER CODE END TIM4_IRQn 1 */
}
/**
@brief This function handles USART1 global interrupt.
/
void USART1_IRQHandler(void)
{
/ USER CODE BEGIN USART1_IRQn 0 */
/* USER CODE END USART1_IRQn 0 /
HAL_UART_IRQHandler(&huart1);
/ USER CODE BEGIN USART1_IRQn 1 */
/* USER CODE END USART1_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 /
stm32f1xx_hal_msp.c
/ USER CODE BEGIN Header /
/*
@file stm32f1xx_hal_msp.c
@brief This file provides code for the MSP Initialization
and de-Initialization codes.
@attention
Copyright © 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”
/ USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------/
/ USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------/
/ USER CODE BEGIN Define */
/* USER CODE END Define */
/* Private macro -------------------------------------------------------------/
/ USER CODE BEGIN Macro */
/* USER CODE END Macro */
/* Private variables ---------------------------------------------------------/
/ USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------/
/ USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* External functions --------------------------------------------------------/
/ USER CODE BEGIN ExternalFunctions */
/* USER CODE END ExternalFunctions */
/* USER CODE BEGIN 0 */
/* USER CODE END 0 /
/*
Initializes the Global MSP.
/
void HAL_MspInit(void)
{
/ USER CODE BEGIN MspInit 0 */
/* USER CODE END MspInit 0 */
__HAL_RCC_AFIO_CLK_ENABLE();
__HAL_RCC_PWR_CLK_ENABLE();
/* System interrupt init*/
/** NOJTAG: JTAG-DP Disabled and SW-DP Enabled
*/
__HAL_AFIO_REMAP_SWJ_NOJTAG();
/* USER CODE BEGIN MspInit 1 */
/* USER CODE END MspInit 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
motion.c
#include “motion.h”
#include “main.h”
#include “main.h”
#include <stdio.h>
void Motor_SetDirection(int left_forward, int right_forward)
{
// ×óµç»ú·½Ïò¿ØÖÆ(PB12 PB13)
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, left_forward ? GPIO_PIN_SET : GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, left_forward ? GPIO_PIN_RESET : GPIO_PIN_SET);
// ÓÒµç»ú·½Ïò¿ØÖÆ(PB14 PB15) HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, right_forward ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, right_forward ? GPIO_PIN_RESET : GPIO_PIN_SET);
}
void Move_Forward(float cm) {
//ʵÏÖÇ°½øÂß¼
Motor_SetDirection(1, 1); // ×óÓÒµç»úÕýת
printf(“Moving forward %.1f cm\n”, cm);
HAL_Delay(500);
}
void Turn_Left(float angle) {
// ʵÏÖ×óתÂß¼
Motor_SetDirection(0, 1); // ×óµç»ú·´×ª£¬ÓÒµç»úÕýת
printf(“Turning left %.1f degrees\n”, angle);
HAL_Delay(500);
}
void Turn_Right(float angle) {
//ʵÏÖÓÒתÂß¼
Motor_SetDirection(1, 0); // ×óµç»úÕýת£¬ÓÒµç»ú·´×ª
printf(“Turning right %.1f degrees\n”, angle);
HAL_Delay(500);
}在这里完全保留你原始代码结构和变量命名（包括中文注释、拼音命名、// Ä£¿éÊý¾Ý½á¹¹Ìå 等），并完整整合你提供的所有模块（如 JY61P_Data, PID_Controller, Control_Straight, Move_Forward, Turn_Left, Avoid_Obstacle 等函数），同时添加你要求的：
路径规划
任务切换
一键启动
多任务支持（Basic/Advanced）
完整保留 /* USER CODE BEGIN 4 */、/* USER CODE END 4 */ 和 #endif /* USE_FULL_ASSERT */ 等 STM32CubeMX 生成的结构
最新发布
博客介绍了让代码依赖关系更明显的方法，包括组织代码、命名例程、使用例程参数、用注释记录不明确依赖、用断言或错误处理代码检查依赖等，还提及了顺序无关紧要的事项，如让代码从上到下阅读、分组相关语句。
扫一扫
854
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
