compiling main.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/main.c(48): error: #256: invalid redeclaration of type name "Cylinder" (declared at line 10 of "../Core/Inc/path_planner.h")
} Cylinder;
../Core/Src/main.c(73): error: #101: "TASK_SIMPLE_EXIT" has already been declared in the current scope
TASK_SIMPLE_EXIT,
../Core/Src/main.c(74): error: #101: "TASK_SNAKE_PATH" has already been declared in the current scope
TASK_SNAKE_PATH,
../Core/Src/main.c(75): error: #101: "TASK_CIRCLE_TWO_COLORS" has already been declared in the current scope
TASK_CIRCLE_TWO_COLORS,
../Core/Src/main.c(76): error: #101: "TASK_COLUMN_SWAP" has already been declared in the current scope
TASK_COLUMN_SWAP,
../Core/Src/main.c(77): error: #101: "TASK_RANDOM_DETECT" has already been declared in the current scope
TASK_RANDOM_DETECT,
../Core/Src/main.c(78): error: #101: "TASK_DETECT_AND_PATH" has already been declared in the current scope
TASK_DETECT_AND_PATH,
../Core/Src/main.c(79): error: #101: "TASK_NONE" has already been declared in the current scope
TASK_NONE
../Core/Src/main.c(80): error: #256: invalid redeclaration of type name "TaskMode" (declared at line 20 of "../Core/Inc/path_planner.h")
} TaskMode;
../Core/Src/main.c(398): error: #20: identifier "M_PI" is undefined
current_x += encoder_dist * cosf(fused_yaw * M_PI / 180.0f);
../Core/Src/main.c: 2 warnings, 10 errors
compiling pid.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\pid.c: 2 warnings, 0 errors
compiling usart.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/usart.c: 2 warnings, 0 errors
compiling ultrasonic.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\ultrasonic.c: 2 warnings, 0 errors
compiling encoder.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
..\Core\Src\encoder.c: 3 warnings, 0 errors
compiling kalman_filter.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\kalman_filter.c: 2 warnings, 0 errors
compiling motor.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\motor.c: 2 warnings, 0 errors
compiling jy61p.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\jy61p.c(18): warning: #1-D: last line of file ends without a newline
}
..\Core\Src\jy61p.c: 3 warnings, 0 errors
compiling i2c.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/i2c.c: 2 warnings, 0 errors
compiling tim.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/tim.c: 2 warnings, 0 errors
compiling gpio.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/gpio.c: 2 warnings, 0 errors
compiling motion.c...
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\motion.c(13): error: #20: identifier "Cylinder" is undefined
extern Cylinder cylinders[MAX_CYLINDERS];
..\Core\Src\motion.c(13): error: #20: identifier "MAX_CYLINDERS" is undefined
extern Cylinder cylinders[MAX_CYLINDERS];
..\Core\Src\motion.c: 3 warnings, 2 errors
compiling stm32f1xx_it.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/stm32f1xx_it.c: 2 warnings, 0 errors
compiling stm32f1xx_hal_msp.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
../Core/Src/stm32f1xx_hal_msp.c: 2 warnings, 0 errors
compiling path_planner.c...
../Core/Inc/encoder.h(11): warning: #1-D: last line of file ends without a newline
#endif /* __ENCODER_H */
../Core/Inc/motion.h(24): warning: #1-D: last line of file ends without a newline
#endif /* __MOTION_H */
..\Core\Src\path_planner.c(51): warning: #1-D: last line of file ends without a newline
}
..\Core\Src\path_planner.c: 3 warnings, 0 errors
"carL\carL.axf" - 12 Error(s), 34 Warning(s).
Target not created.
以下是现有的代码
main.c
/* 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"
#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"
#include "path_planner.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
// ???????
#define MAX_CYLINDERS 10
typedef struct {
int x, y, r;
char color;
uint8_t visited;
} Cylinder;
Cylinder cylinders[MAX_CYLINDERS];
int cylinder_count = 0;
uint8_t mission_running = 0;
uint32_t start_time = 0;
#define TASK_TIMEOUT_MS 10000
#define DETECT_TIMEOUT_MS 30000
uint32_t timeout = TASK_TIMEOUT_MS;
char openmv_buffer[128];
uint8_t openmv_index = 0;
uint8_t rx_byte;
uint8_t openmv_data_ready = 0;
uint8_t task_selected = 0;
PID_Controller speed_pid;
PID_Controller angle_pid;
KalmanFilter kf;
// ????
typedef enum {
TASK_SIMPLE_EXIT,
TASK_SNAKE_PATH,
TASK_CIRCLE_TWO_COLORS,
TASK_COLUMN_SWAP,
TASK_RANDOM_DETECT,
TASK_DETECT_AND_PATH,
TASK_NONE
} TaskMode;
TaskMode current_task = TASK_SIMPLE_EXIT;
// ?????
typedef enum {
TASK_IDLE,
TASK_DETECTING,
TASK_PATH_PLANNING,
TASK_EXECUTING,
TASK_CIRCLE_COLUMN,
TASK_COMPLETED,
TASK_FAILED
} TaskState;
TaskState current_state = TASK_IDLE;
// ?????
typedef enum {
MOVE_IDLE,
MOVE_FORWARD,
MOVE_TURNING,
MOVE_CIRCLE
} MoveState;
MoveState move_state = MOVE_IDLE;
// ??????
float current_x = 0.0f;
float current_y = 0.0f;
float current_angle = 0.0f;
// ?????
PathPlanner planner;
// ????
void Detect_Cylinders(void);
void Plan_Path(TaskMode task);
void ExecuteStep(void);
void Reset_Cylinders(void);
float Calculate_Distance(int x1, int y1, int x2, int y2);
void Parse_OpenMV_Data(void);
/* 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 StartMission(void);
void ExecuteMission(TaskMode task);
void SimpleExitPath(void);
void SnakePath(void);
void CircleTwoColors(void);
void ColumnSwapPath(void);
void RandomDetectPath(void);
void AddCylinder(int x, int y, int r, char color);
void BuildPath(char color);
void AvoidObstacle(void);
void Send_Debug_Info(void);
void SensorFusion_Init(void);
float SensorFusion_Update(float imu_yaw, float encoder_angle);
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart);
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
if (huart == &huart1) {
if (rx_byte == '\n' || openmv_index >= sizeof(openmv_buffer) - 1) {
openmv_buffer[openmv_index] = '\0';
openmv_index = 0;
openmv_data_ready = 1;
} else {
openmv_buffer[openmv_index++] = rx_byte;
}
HAL_UART_Receive_IT(&huart1, &rx_byte, 1);
}
}
void StartMission() {
mission_running = 1;
start_time = HAL_GetTick();
current_state = TASK_DETECTING;
Reset_Cylinders();
}
uint8_t IsMissionTimeout() {
return (HAL_GetTick() - start_time) > timeout;
}
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;
cylinders[cylinder_count].visited = 0;
cylinder_count++;
}
}
void Reset_Cylinders() {
for (int i = 0; i < cylinder_count; i++) {
cylinders[i].visited = 0;
}
}
float Calculate_Distance(int x1, int y1, int x2, int y2) {
return sqrtf(powf(x2 - x1, 2) + powf(y2 - y1, 2));
}
void Parse_OpenMV_Data(void) {
char *token = strtok(openmv_buffer, ";");
cylinder_count = 0;
while (token != NULL && cylinder_count < MAX_CYLINDERS) {
int x, y, r;
char color;
if (sscanf(token, "%d,%d,%d,%c", &x, &y, &r, &color) == 4) {
AddCylinder(x, y, r, color);
}
token = strtok(NULL, ";");
}
}
void Detect_Cylinders(void) {
// ????
HAL_Delay(2000);
AddCylinder(100, 200, 30, 'W');
AddCylinder(300, 150, 35, 'B');
printf("Detected %d cylinders.\r\n", cylinder_count);
}
void Plan_Path(TaskMode task) {
// ??????
Path_Init(&planner);
switch (task) {
case TASK_SIMPLE_EXIT:
Path_AddPoint(&planner, 0, 200, 0);
Path_AddPoint(&planner, 300, 200, 1);
break;
case TASK_SNAKE_PATH:
for (int i = 0; i < cylinder_count; i++) {
Path_AddPoint(&planner, cylinders[i].x, cylinders[i].y, 1);
}
break;
case TASK_CIRCLE_TWO_COLORS:
Path_AddPoint(&planner, 100, 200, 2);
Path_AddPoint(&planner, 300, 150, 2);
break;
}
}
void ExecuteStep(void) {
static uint8_t step_index = 0;
if (step_index < planner.count) {
PathPoint *point = &planner.points[step_index];
if (point->type == 0) {
Move_Forward(100); // ????
} else if (point->type == 1) {
Turn_Right(90); // ??
} else {
CircleColumn(step_index); // ??
}
step_index++;
} else {
current_state = TASK_COMPLETED;
}
}
void TaskMachine_Run() {
switch (current_state) {
case TASK_IDLE:
if (mission_running) {
current_state = TASK_DETECTING;
}
break;
case TASK_DETECTING:
if (openmv_data_ready) {
Parse_OpenMV_Data();
openmv_data_ready = 0;
current_state = TASK_PATH_PLANNING;
} else if (IsMissionTimeout()) {
current_state = TASK_FAILED;
}
break;
case TASK_PATH_PLANNING:
Plan_Path(current_task);
current_state = TASK_EXECUTING;
break;
case TASK_EXECUTING:
ExecuteStep();
AvoidObstacle(); // ????
break;
case TASK_CIRCLE_COLUMN:
CircleTwoColors();
current_state = TASK_COMPLETED;
break;
case TASK_COMPLETED:
Motor_Stop();
mission_running = 0;
current_state = TASK_IDLE;
break;
case TASK_FAILED:
Motor_Stop();
printf("Mission Failed!\r\n");
mission_running = 0;
current_state = TASK_IDLE;
break;
}
if (IsMissionTimeout() && current_state != TASK_DETECTING) {
current_state = TASK_FAILED;
}
}
/* 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);
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
JY61P_Init(&hi2c1);
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);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
AddCylinder(100, 200, 30, 'W');
AddCylinder(300, 150, 35, 'B');
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET) {
HAL_Delay(20);
if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET) {
StartMission();
while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_11) == GPIO_PIN_RESET);
}
}
if (mission_running) {
TaskMachine_Run();
}
// ???????
float encoder_dist = Get_Total_Distance();
JY61P_Data imu_data;
JY61P_Read_Attitude(&imu_data);
float fused_yaw = KalmanFilter_Update(&kf, imu_data.yaw, imu_data.gz, 0.01f);
// ??????
current_x += encoder_dist * cosf(fused_yaw * M_PI / 180.0f);
current_y += encoder_dist * sinf(fused_yaw * M_PI / 180.0f);
current_angle = fused_yaw;
HAL_Delay(10);
}
/* USER CODE END WHILE */
/* 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_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 */
gpio.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file gpio.c
* @brief This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* @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 "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_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/* Configure TB6612 Control Pins */
GPIO_InitStruct.Pin = 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_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Configure PWM Pins (PB0, PB1) */
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(GPIOB, &GPIO_InitStruct);
/* Configure Ultrasonic TRIG (PB2, PB4) */
GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_4;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Configure Ultrasonic ECHO (PB3, PB5) */
GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_5;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Configure Start Button (PB11) */
GPIO_InitStruct.Pin = GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
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 (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 "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 (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 "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 (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 "usart.h"
#include "stdio.h"
#include "string.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 */
motion.c
#include "motion.h"
#include "motor.h"
#include "encoder.h"
#include "jy61p.h"
#include "pid.h"
#include "stdio.h"
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
extern Cylinder cylinders[MAX_CYLINDERS];
extern int cylinder_count;
extern PID_Controller angle_pid;
float current_x = 0.0f;
float current_y = 0.0f;
float current_angle = 0.0f;
void Move_Forward(float distance) {
float start_ticks = Get_Total_Distance();
float target_ticks = start_ticks + distance * 10.0f;
Motor_Forward(80);
while (Get_Total_Distance() < target_ticks) {
HAL_Delay(10);
}
Motor_Stop();
}
void Move_Backward(float distance) {
float start_ticks = Get_Total_Distance();
float target_ticks = start_ticks + distance * 10.0f;
Motor_Backward(80);
while (Get_Total_Distance() < target_ticks) {
HAL_Delay(10);
}
Motor_Stop();
}
void Turn_Left(float angle) {
JY61P_Data imu;
JY61P_Read_Attitude(&imu);
float target_yaw = imu.yaw - angle;
if (target_yaw < -180.0f) target_yaw += 360.0f;
Motor_Turn_Left(40);
do {
JY61P_Read_Attitude(&imu);
HAL_Delay(10);
} while (fabsf(imu.yaw - target_yaw) > 5.0f);
Motor_Stop();
current_angle = target_yaw;
}
void Turn_Right(float angle) {
JY61P_Data imu;
JY61P_Read_Attitude(&imu);
float target_yaw = imu.yaw + angle;
if (target_yaw > 180.0f) target_yaw -= 360.0f;
Motor_Turn_Right(40);
do {
JY61P_Read_Attitude(&imu);
HAL_Delay(10);
} while (fabsf(imu.yaw - target_yaw) > 5.0f);
Motor_Stop();
current_angle = target_yaw;
}
void CircleColumn(int index) {
float center_x = cylinders[index].x;
float center_y = cylinders[index].y;
float radius = cylinders[index].r + 15.0f;
for (int i = 0; i < 36; i++) {
float angle = current_angle + i * 10 * M_PI / 180.0f;
float target_x = center_x + radius * cosf(angle);
float target_y = center_y + radius * sinf(angle);
float dist = sqrtf(powf(target_x - current_x, 2) + powf(target_y - current_y, 2));
float target_angle = atan2f(target_y - current_y, target_x - current_x) * 180.0f / M_PI;
Turn_Right(target_angle - current_angle);
Move_Forward(dist);
}
cylinders[index].visited = 1;
}
void Follow_Wall(float distance) {
float wall_dist = Get_Left_Distance();
float start_x = current_x;
float start_y = current_y;
Motor_Forward(70);
while (sqrtf(powf(current_x - start_x, 2) + powf(current_y - start_y, 2)) < distance) {
float error = Get_Left_Distance() - wall_dist;
float steer = PID_Update(&angle_pid, error, 0.01f);
int left_speed = 70 + steer;
int right_speed = 70 - steer;
Motor_SetSpeed(left_speed, right_speed);
HAL_Delay(10);
}
Motor_Stop();
}
void AvoidObstacle() {
float left = Get_Left_Distance();
float right = Get_Right_Distance();
if (left < 15 || right < 15) {
Motor_Backward(10);
if (left < right) {
Turn_Right(60);
} else {
Turn_Left(60);
}
}
}
path_planner.c
#include "path_planner.h"
void Path_Init(PathPlanner *planner) {
planner->count = 0;
}
void Path_AddPoint(PathPlanner *planner, int x, int y, uint8_t type) {
if (planner->count < MAX_PATH_POINTS) {
planner->points[planner->count].x = x;
planner->points[planner->count].y = y;
planner->points[planner->count].type = type;
planner->count++;
}
}
void Path_Generate(PathPlanner *planner, Cylinder *cylinders, uint8_t count, TaskMode task) {
switch (task) {
case TASK_SIMPLE_EXIT:
Path_AddPoint(planner, 0, 200, 0);
Path_AddPoint(planner, 300, 200, 1);
break;
case TASK_SNAKE_PATH:
for (int i = 0; i < count; i++) {
Path_AddPoint(planner, cylinders[i].x, cylinders[i].y, 1);
}
break;
case TASK_CIRCLE_TWO_COLORS:
Path_AddPoint(planner, 100, 200, 2);
Path_AddPoint(planner, 300, 150, 2);
break;
}
}
uint8_t Path_Execute(PathPlanner *planner) {
static uint8_t step_index = 0;
if (step_index < planner->count) {
PathPoint *point = &planner->points[step_index];
if (point->type == 0) {
Move_Forward(100);
} else if (point->type == 1) {
Turn_Right(90);
} else {
CircleColumn(step_index);
}
step_index++;
return 0;
} else {
step_index = 0;
return 1;
}
}
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;
}
ultrasonic.c
#include "ultrasonic.h"
float Get_Left_Distance(void) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_SET);
HAL_Delay(10);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, GPIO_PIN_RESET);
uint32_t start = 0, end = 0;
while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_RESET);
start = htim3.Instance->CNT;
while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET);
end = htim3.Instance->CNT;
return (end - start) * 0.034 / 2;
}
float Get_Right_Distance(void) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);
HAL_Delay(10);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
uint32_t start = 0, end = 0;
while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET);
start = htim3.Instance->CNT;
while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET);
end = htim3.Instance->CNT;
return (end - start) * 0.034 / 2;
}
kalman_filter.c
#include "kalman_filter.h"
void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure) {
kf->Q_angle = Q_angle;
kf->Q_bias = Q_bias;
kf->R_measure = R_measure;
kf->angle = 0.0f;
kf->bias = 0.0f;
kf->P[0][0] = 0.0f;
kf->P[0][1] = 0.0f;
kf->P[1][0] = 0.0f;
kf->P[1][1] = 0.0f;
}
float KalmanFilter_Update(KalmanFilter *kf, float new_angle, float new_rate, float dt) {
kf->rate = new_rate - kf->bias;
kf->angle += dt * kf->rate;
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_bias * dt;
float y = new_angle - kf->angle;
float S = kf->P[0][0] + kf->R_measure;
float K0 = kf->P[0][0] / S;
float K1 = kf->P[1][0] / S;
kf->angle += K0 * y;
kf->bias += K1 * y;
float P00_temp = kf->P[0][0];
float P01_temp = kf->P[0][1];
kf->P[0][0] -= K0 * P00_temp;
kf->P[0][1] -= K0 * P01_temp;
kf->P[1][0] -= K1 * P00_temp;
kf->P[1][1] -= K1 * P01_temp;
return kf->angle;
}
motor.c
#include "motor.h"
void Motor_SetSpeed(int left, int right) {
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, left);
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, right);
}
void Motor_Forward(int speed) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
Motor_SetSpeed(speed, speed);
}
void Motor_Backward(int speed) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
Motor_SetSpeed(speed, speed);
}
void Motor_Stop() {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
Motor_SetSpeed(0, 0);
}
void Motor_Turn_Left(int speed) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
Motor_SetSpeed(speed, speed);
}
void Motor_Turn_Right(int speed) {
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
Motor_SetSpeed(speed, speed);
}
encoder.c
#include "encoder.h"
void Reset_Encoders(void) {
htim3.Instance->CNT = 0;
htim4.Instance->CNT = 0;
}
float Get_Total_Distance(void) {
float left = htim3.Instance->CNT * 0.1f;
float right = htim4.Instance->CNT * 0.1f;
return (left + right) / 2.0f;
}
float Get_Left_Encoder(void) {
return htim3.Instance->CNT * 0.1f;
}
float Get_Right_Encoder(void) {
return htim4.Instance->CNT * 0.1f;
}
jy61p.c
#include "jy61p.h"
#include "main.h"
extern I2C_HandleTypeDef hi2c1;
#define JY61P_ADDR 0x50 << 1
void JY61P_Init(I2C_HandleTypeDef *hi2c) {
// ³õʼ»¯
}
void JY61P_Read_Attitude(JY61P_Data *data) {
uint8_t buf[6];
HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x56, 1, buf, 6, HAL_MAX_DELAY);
data->pitch = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 180;
data->roll = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 180;
data->yaw = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 180;
}
最新发布
本文介绍了Memcached在作为root用户启动时需要注意的问题。如果直接使用root权限运行Memcached，需要添加-u参数指定运行用户，否则将无法启动。该文通过实例展示了如何正确地配置和启动Memcached服务。
扫一扫
1317
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
