F - Count the Colors

本文介绍线段树的基本概念及其实现细节,通过具体示例讲解如何利用线段树解决区间更新和查询问题,并针对特定场景提出优化方案。

http://acm.hust.edu.cn/vjudge/contest/view.action?cid=66989#problem/F

线段树入门,起初,设置数组sum=-1表示无颜色,sum=-2表示下方无统一颜色;sum>=0表示颜色为sum的占据了整个子树

li ri数组==-1,-1表示左右无统一颜色其他表示区间rt左边颜色为li右边为ri,这样既可实现本题

看代码

#include <cstdio>
#include <cstring>
#include <algorithm>
#include <queue>
#include <map>
#include <set>
#include <stack>
#include <string>
#define SI(T)int T;scanf("%d",&T)
#define lson l,m,rt<<1
#define rson m+1,r,rt<<1|1
#define LL long long
using namespace std;
const int SIZE=5;
const int maxn=1<<30;
int sum[SIZE<<2],li[SIZE<<2],ri[SIZE<<2];
int cnt[SIZE];
void pushup(int rt){
    if(sum[rt<<1]!=-1||sum[rt<<1|1]!=-1){
        li[rt]=ri[rt]=-1;
        sum[rt]=-2;
        return;
    }
    if(ri[rt<<1]==li[rt<<1|1]){
        sum[rt]=li[rt]=ri[rt]=ri[rt<<1];
    }
}
void pushdown(int rt){
    if(sum[rt]>0){
        sum[rt<<1]=sum[rt<<1|1]=sum[rt];
        sum[rt]=-2;
    }
}
void build(int l,int r,int rt){
    memset(sum,-1,sizeof(sum));
    memset(li,-1,sizeof(li));
    memset(ri,-1,sizeof(ri));
}
void update(int L,int R,int c,int l,int r,int rt){
    if(L<=l&&r<=R){
        sum[rt]=li[rt]=ri[rt]=c;
        return;
    }
    pushdown(rt);
    int m=(l+r)>>1;
    if(L<=m)update(L,R,c,lson);
    if(R>m)update(L,R,c,rson);
    pushup(rt);
}
void query(int l,int r,int rt){
    if(sum[rt]>0){
        cnt[sum[rt]]++;
        return ;
    }
    if(sum[rt]==-1)return;
    int m=(l+r)>>1;
    if(l<=m)query(lson);
    if(r>m)query(rson);
}
int main()
{
    int n,x1,x2,c;
    while(scanf("%d",&n)!=EOF){
        memset(cnt,0,sizeof(cnt));
        build(1,SIZE,1);
        for(int i=0;i<n;i++){
            scanf("%d%d%d",&x1,&x2,&c);
            update(x1+1,x2,c,1,SIZE,1);
        }
        query(1,SIZE,1);
        for(int i=0;i<SIZE<<1;i++)
            printf("%d %d\n",sum[i],i);
        for(int i=0;i<SIZE;i++){
            if(cnt[i]){
                printf("%d %d\n",i,cnt[i]);
            }
        }
    }
    return 0;
}
但是要是更新的是3跟4区间从二叉树上看3跟4区间分隔在了两颗子树,虽然他们在线上是链接的,但是在线段树上却是断开的,导致了错误。

吃个饭回来好像明白了些什么了,rili数组干嘛的。。。明显上面的代码没有用,然后根据后序遍历树个特点(神奇的地方)设置一个前驱temp,刚刚好解决了上面的问题,4的前驱刚好是3。代码贴上

#include <cstdio>
#include <cstring>
#include <algorithm>
#include <queue>
#include <map>
#include <set>
#include <stack>
#include <string>
#define SI(T)int T;scanf("%d",&T)
#define lson l,m,rt<<1
#define rson m+1,r,rt<<1|1
#define LL long long
using namespace std;
const int SIZE=8080;
const int maxn=1<<30;
int sum[SIZE<<2];
int cnt[SIZE];
int temp;
void pushup(int rt){
    if(sum[rt<<1]==-2||sum[rt<<1|1]==-2){
        sum[rt]=-2;
        return;
    }
    if(sum[rt<<1]==sum[rt<<1|1])sum[rt]=sum[rt<<1];
    else sum[rt]=-2;
}
void pushdown(int rt){
    if(sum[rt]>=0){
        sum[rt<<1]=sum[rt<<1|1]=sum[rt];
        sum[rt]=-2;
    }
}
void build(int l,int r,int rt){
    sum[rt]=-1;
    if(l==r)return;
    int m=(l+r)>>1;
    build(lson);
    build(rson);
}
void update(int L,int R,int c,int l,int r,int rt){
    if(L<=l&&r<=R){
        sum[rt]=c;
        return;
    }
    pushdown(rt);
    int m=(l+r)>>1;
    if(L<=m)update(L,R,c,lson);
    if(R>m)update(L,R,c,rson);
    pushup(rt);
}
void query(int l,int r,int rt){
    if(sum[rt]>=0){
        if(temp==sum[rt])return;
        temp=sum[rt];
        cnt[sum[rt]]++;
        return ;
    }
    if(sum[rt]==-1){temp=-1;return;}
    int m=(l+r)>>1;
    if(l<=m)query(lson);
    if(r>m)query(rson);
}
int main()
{
    int n,x1,x2,c;
    while(scanf("%d",&n)!=EOF){
        memset(cnt,0,sizeof(cnt));
        build(1,SIZE,1);
        for(int i=0;i<n;i++){
            scanf("%d%d%d",&x1,&x2,&c);
            update(x1+1,x2,c,1,SIZE,1);
        }
        temp=-1;
        query(1,SIZE,1);
        for(int i=0;i<SIZE;i++){
            if(cnt[i]){
                printf("%d %d\n",i,cnt[i]);
            }
        }
        printf("\n");
    }
    return 0;
}


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; }
08-01
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” #include “stdio.h” #include “string.h” / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes */ // Ô²ÖùÐÅÏ¢½á¹¹Ìå #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; } Cylinder; Cylinder cylinders[MAX_CYLINDERS]; int cylinder_count = 0; uint8_t mission_running = 0; uint32_t start_time = 0; uint32_t timeout = 10000; // 10s uint8_t rx_byte; char openmv_buffer[128]; uint8_t openmv_index = 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 } TaskMode; TaskMode current_task = TASK_SIMPLE_EXIT; // ״̬»ú¶¨Òå typedef enum { TASK_IDLE, TASK_MOVING_FORWARD, TASK_TURNING, TASK_AVOIDING, TASK_COMPLETED } TaskState; TaskState current_state = TASK_IDLE; /* 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 >= ‘1’ && rx_byte <= ‘5’) { current_task = rx_byte - ‘1’; // 1~5 Ó³É䵽ö¾Ù printf(“Selected Task: %d\r\n”, current_task + 1); } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //¼ÌÐø½ÓÊÕ } } // Æô¶¯ÈÎÎñ void StartMission() { mission_running = 1; start_time = HAL_GetTick(); current_state = TASK_IDLE; } // ÊÇ·ñ³¬Ê± 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; cylinder_count++; } } // ¹¹½¨ÈÆÏß·¾¶ void BuildPath(char color) { if (color == ‘W’) { Turn_Left(90); } else if (color == ‘B’) { Turn_Right(90); } } //±ÜÕÏ void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 10 && right < 10) { Motor_Stop(); printf(“Obstacle detected in both directions. Stopping.\r\n”); } else if (left < 10) { Turn_Right(90); } else if (right < 10) { Turn_Left(90); } } //¼òµ¥Â·¾¶ void SimpleExitPath() { Move_Forward(100); Turn_Right(90); Move_Forward(50); } //ÉßÐη¾¶ void SnakePath() { for (int i = 0; i < cylinder_count; i++) { if (i % 2 == 0) { Turn_Left(90); } else { Turn_Right(90); } Move_Forward(20); } } //ÈÆÁ½¸öÑÕɫԲÖù void CircleTwoColors() { char colors_seen[2] = {0}; int count = 0; for (int i = 0; i < cylinder_count && count < 2; i++) { if (colors_seen[0] == 0 && cylinders[i].color == ‘W’) { BuildPath(‘W’); colors_seen[0] = ‘W’; count++; } else if (colors_seen[1] == 0 && cylinders[i].color == ‘B’) { BuildPath(‘B’); colors_seen[1] = ‘B’; count++; } } } //ºÚ°×Öù½»»»ºó·¾¶ void ColumnSwapPath() { //¼ÙÉèÒѽ»»»Öù×ÓÖØÐ¹滮·¾¶ BuildPath(‘W’); BuildPath(‘B’); } // Ëæ»ú̽²â·¾¶ void RandomDetectPath() { // ½ÓÊÕOpenMV Êý¾Ý HAL_UART_Receive_IT(&huart1, &rx_byte, 1); HAL_Delay(30000); // 30s BuildPath(‘W’); // ¼ÙÉè̽²âµ½°×Öù Move_Forward(100); } void ExecuteMission(TaskMode task); void ExecuteMission(TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; default: printf(“Unknown task mode: %d\r\n”, task); break; } } // ״̬»ú¿ØÖÆÈÎÎñ void TaskMachine_Run() { switch (current_state) { case TASK_IDLE: if (mission_running) { current_state = TASK_MOVING_FORWARD; } break; case TASK_MOVING_FORWARD: switch (current_task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; } current_state = TASK_COMPLETED; break; case TASK_COMPLETED: Motor_Stop(); mission_running = 0; current_state = TASK_IDLE; break; } } /* 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); //Ìí¼Ó²âÊÔÔ²Öù 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(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { HAL_Delay(20); if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { StartMission(); while (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET); // µÈ´ýÊÍ·Å } } if (mission_running) { ExecuteMission(current_task); //Ö´Ðе±Ç°ÈÎÎñ if (IsMissionTimeout()) { mission_running = 0; Motor_Stop(); printf(“Mission Timeout!\r\n”); } /* 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 © 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 / ic2.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” #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 / 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 “encoder.h” #include “motor.h” #include “jy61p.h” #include “math.h” #include “stddef.h” void Move_Forward(int distance) { int target_ticks = distance * 10; int current_ticks = 0; Reset_Encoders(); Motor_Forward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); current_ticks = (left + right) / 2; if (current_ticks >= target_ticks) { Motor_Stop(); break; } } } void Move_Backward(int distance) { int target_ticks = distance * 10; int current_ticks = 0; Reset_Encoders(); Motor_Backward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); current_ticks = (left + right) / 2; if (current_ticks >= target_ticks) { Motor_Stop(); break; } } } void Turn_Left(int angle) { float target_angle = angle; float current_yaw = 0.0f; JY61P_Data imu_data; while (fabsf(current_yaw) < target_angle) { Motor_Turn_Left(50); JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; } Motor_Stop(); } void Turn_Right(int angle) { float target_angle = angle; float current_yaw = 0.0f; JY61P_Data imu_data; while (fabsf(current_yaw) < target_angle) { Motor_Turn_Right(50); JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; } Motor_Stop(); } 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” #include “tim.h” #include “gpio.h” float Get_Left_Distance() { 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 = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } float Get_Right_Distance() { 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 = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } motor.c #include “motor.h” #include “tim.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); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 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); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 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); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 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); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 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); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed); } 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 K[2]; K[0] = kf->P[0][0] / S; K[1] = kf->P[1][0] / S; kf->angle += K[0] * y; kf->bias += K[1] * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K[0] * P00_temp; kf->P[0][1] -= K[0] * P01_temp; kf->P[1][0] -= K[1] * P00_temp; kf->P[1][1] -= K[1] * P01_temp; return kf->angle; } encoder.c #include “encoder.h” #include “tim.h” void Reset_Encoders(void) { htim3.Instance->CNT = 0; htim4.Instance->CNT = 0; } float Get_Total_Distance(void) { int32_t left_ticks = htim3.Instance->CNT; int32_t right_ticks = htim4.Instance->CNT; float distance = (left_ticks + right_ticks) / 2.0f * 0.1f; return distance; } float Get_Left_Encoder(void) { return (float)htim3.Instance->CNT; } float Get_Right_Encoder(void) { return (float)htim4.Instance->CNT; } jy61p.c #include “jy61p.h” #include <string.h> #include “i2c.h” #define JY61P_ADDR 0x50 << 1 void JY61P_Init(I2C_HandleTypeDef *hi2c) { } void JY61P_Read_Acc(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x34, 1, buf, 6, HAL_MAX_DELAY); data->ax = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 16; data->ay = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 16; data->az = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 16; } void JY61P_Read_Gyro(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x38, 1, buf, 6, HAL_MAX_DELAY); data->gx = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 2000; data->gy = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 2000; data->gz = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 2000; } 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; } 在这里的代码上进行修改硬件为一个jy61p陀螺仪,两个带电机MG513编码器,一个f103c8t6,两个HC-SR04超声波,两个红外测距,一个TB6612FNG,一个openMV,i2c1_SDA和SCL接在PB8和9,HC-SR04左TRIG和ECHO接在PB2和3右TRIG和ECHO接在PB4和5,TB6612FNG的ADC接在PA6,PWMB接在PB0,BIN1-2接PB13-12,STBY接在3.3v,AIN1-2接在PB14-15,PWMA接在PB1,修改为可以更好的实现一、 任务 制作一辆自动避障小车,从测试场地边墙入口 A 驶入,按具体任务要求绕 过场地内的圆柱障碍,在规定时间内从出口 C 驶出。测试场地如图 1 所示,为 边长 2 米的正方形,四周有边墙围挡,两侧有入口 A 和出口 C,4 个白色、5 个 黑色圆柱用圆柱座固定在场地上作为障碍物。二、 要求 小车在场地中行驶不应触碰圆柱,可触碰但不可跨越场地边墙。 基本要求 圆柱排列如图 1 所示。小车放置在准备区。 (1)一键启动小车并开始计时,小车从 A 口进入,任选路径,10s 内车身 完全从 C 口驶出。 (2)一键启动小车并开始计时,小车从 A 口进入,左右变向蛇行绕过第 2 行各圆柱,行驶轨迹参见说明(6),10s 内车身完全从 C 口驶出。 (3)一键启动小车并开始计时,小车从 A 口进入,分别绕任意两个不同颜 色的圆柱各转行 1 圈(方向不限),10s 内车身完全从 C 口驶出。 发挥部分 小车每次从入口到出口穿越过程中还必须满足:不从两个黑柱间穿过,沿边 墙墙角转弯不超过 1 次。 (1)小车放置在准备区,在图 1 的基础上按指令将第 1 行第 3 列处的黑柱 与任一白柱互换位置,一键启动小车并开始计时,从 A 口进入,计时至车身完 全从 C 口驶出,用时越少越好。 (2)小车放置在准备区,按指令随机排列圆柱,一键启动探测圆柱,在 30s 内完成探测、穿过出发线进入 A 口,10s 内车身应完全从 C 口驶出.三、 说明 (1)测试时场地、边墙及圆柱座由赛区提供。场地用长 300cm 宽 210cm 的 哑光喷绘布制作,背景色为灰色(R:170 G:170 B:170),喷绘出图 1 中圆 柱位置(直径 2.5cm 黑色圆圈)、黄色边墙位置、黑色出发线和黑色虚线,其线 宽分别为 0.2cm、1cm、2cm 和 0.2cm。障碍圆柱用直径 2cm 白色 PVC 电工穿线 管制作,长度 20±1cm。白柱采用 PVC 原色,黑柱表面涂哑光黑色。圆柱座采 用白色“直接套管”(PVC 管标准配件,长度 4~5cm)。在喷绘布上圆柱位置开 孔,将圆柱座用热熔胶粘接固定在地面上。场地边墙高 3±0.5cm,用木板制作, 表面黄色,用热熔胶固定。测试时参赛队必须自带圆柱,插入赛区准备好的圆柱 座。圆柱和圆柱座、场地及周边不得有任何其他标志和传感器。 (2)所有传感器及控制电路均安装在小车上,小车尺寸在任何状态均必须 满足(含所有传感器和控制电路):长≤35cm、宽≤25cm、高≤35cm。测试中小车 与外部不得有任何通信交互。 (3)小车上只有 1 个“启动按键”,设置好测试项目后可用其一键启动小车。 (4)发挥部分(2),小车一键启动后在 30s 探测时间内可在准备区中任意 移动。
08-01
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” #include “stdio.h” #include “string.h” / Private includes ----------------------------------------------------------/ / USER CODE BEGIN Includes */ // Ô²ÖùÐÅÏ¢½á¹¹Ìå #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; } 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 WAIT_TASK_SELECT_TIMEOUT_MS 5000 uint32_t timeout = 10000; // 10s char openmv_buffer[128]; uint8_t openmv_index = 0; uint8_t rx_byte; 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_NONE } TaskMode; TaskMode current_task = TASK_NONE; // ״̬»ú¶¨Òå typedef enum { TASK_IDLE, TASK_MOVING_FORWARD, TASK_TURNING, TASK_AVOIDING, TASK_COMPLETED } TaskState; TaskState current_state = TASK_IDLE; /* 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); 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); void UpdateCarState(void); uint8_t WaitForTaskSelection(uint32_t timeout_ms); / USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------/ / USER CODE BEGIN 0 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == &huart1) { if (rx_byte >= ‘1’ && rx_byte <= ‘5’) { current_task = (TaskMode)(rx_byte - ‘1’); // 1~5 Ó³É䵽ö¾Ù task_selected = 1; printf(“Selected Task: %d\r\n”, current_task + 1); StartMission(); } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //¼ÌÐø½ÓÊÕ } } // Æô¶¯ÈÎÎñ void StartMission(void) { mission_running = 1; current_state = TASK_MOVING_FORWARD; start_time = HAL_GetTick(); } // ÊÇ·ñ³¬Ê± uint8_t IsMissionTimeout() { return (HAL_GetTick() - start_time) > TASK_TIMEOUT_MS; } uint8_t WaitForTaskSelection(uint32_t timeout_ms) { uint32_t start = HAL_GetTick(); while (!task_selected && (HAL_GetTick() - start < timeout_ms)) { // ¿É¼ÓÈëÆäËû״̬¸üлò¿´ÃŹ· } return task_selected; } // Ìí¼ÓÔ²Öùµ½µØÍ¼ 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 BuildPath(char color) { if (color == ‘W’) { Turn_Left(90); } else if (color == ‘B’) { Turn_Right(90); } } //±ÜÕÏ void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 10 && right < 10) { Motor_Stop(); printf("Obstacle detected in both directions. Stopping.\r\n"); } else if (left < 10) { Turn_Right(90); } else if (right < 10) { Turn_Left(90); } } //¼òµ¥Â·¾¶ void SimpleExitPath() { Move_Forward(100); Turn_Right(90); Move_Forward(50); } //ÉßÐη¾¶ void SnakePath() { for (int i = 0; i < cylinder_count; i++) { if (i % 2 == 0) { Turn_Left(90); } else { Turn_Right(90); } Move_Forward(20); } } //ÈÆÁ½¸öÑÕɫԲÖù void CircleTwoColors() { char colors_seen[2] = {0}; int count = 0; for (int i = 0; i < cylinder_count && count < 2; i++) { if (colors_seen[0] == 0 && cylinders[i].color == ‘W’) { BuildPath(‘W’); colors_seen[0] = ‘W’; count++; } else if (colors_seen[1] == 0 && cylinders[i].color == ‘B’) { BuildPath(‘B’); colors_seen[1] = ‘B’; count++; } } } //ºÚ°×Öù½»»»ºó·¾¶ void ColumnSwapPath() { //¼ÙÉèÒѽ»»»Öù×ÓÖØÐ¹滮·¾¶ BuildPath(‘W’); BuildPath(‘B’); } // Ëæ»ú̽²â·¾¶ static uint32_t detect_start_time = 0; static uint8_t detect_state = 0; void RandomDetectPath() {//½ÓÊÕOpenMVÊý¾Ý if (detect_state == 0) { HAL_UART_Receive_IT(&huart1, &rx_byte, 1); detect_start_time = HAL_GetTick(); detect_state = 1; } else if (detect_state == 1 && (HAL_GetTick() - detect_start_time > 30000)) { BuildPath(‘W’); Move_Forward(100); detect_state = 0; } } //Ö´ÐÐÈÎÎñ void ExecuteMission(TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; default: printf(“Unknown task mode: %d\r\n”, task); break; } } //¸üÐÂС³µ×´Ì¬ void UpdateCarState() { if (!mission_running) return; switch (current_state) { case TASK_IDLE: //¿ÕÏÐ״̬ break; case TASK_MOVING_FORWARD: //Ö´ÐÐÈÎÎñ switch (current_task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; } current_state = TASK_COMPLETED; break; case TASK_COMPLETED: Motor_Stop(); mission_running = 0; current_state = TASK_IDLE; break; } } /* 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 */ //³õʼ»¯OpenMV½ÓÊÕ HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //Æô¶¯±àÂëÆ÷¼ÆÊý 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); //Ìí¼Ó²âÊÔÔ²Öù AddCylinder(100, 200, 30, ‘W’); AddCylinder(300, 150, 35, ‘B’); // µÈ´ýÈÎÎñ if (!WaitForTaskSelection(WAIT_TASK_SELECT_TIMEOUT_MS)) { current_task = TASK_SIMPLE_EXIT; } /* USER CODE END 2 */ /* Infinite loop / / USER CODE BEGIN WHILE */ while (1) { } //¸üÐÂ״̬»ú UpdateCarState(); //³¬Ê±¼ì²â if (IsMissionTimeout()) { mission_running = 0; Motor_Stop(); printf("Mission Timeout!\r\n"); } /* 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 GPIO ³õʼ»¯ @attention Ó²¼þ:TB6612FNG£¬HC-SR04£¬I2C£¬USART1 / / USER CODE END Header */ #include “gpio.h” /----------------------------------------------------------------------------/ /* Configure GPIO / /----------------------------------------------------------------------------*/ void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; /* GPIO Ports Clock Enable */ __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /-------------------------- TB6612FNG ¿ØÖÆÒý½Å ------------------------------/ /** PWMA - PB1 (TIM2_CH2) AIN1 - PB14 AIN2 - PB15 PWMB - PB0 (TIM2_CH1) BIN1 - PB12 BIN2 - PB13 STBY - 3.3V */ GPIO_InitStruct.Pin = GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_12 | GPIO_PIN_13; 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); /-------------------------- PWM Êä³ö PB0, PB1 ---------------------------/ /** ??? PWM ??(TIM2_CH1 ? CH2) */ GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- HC-SR04 ×ó TRIG & ECHO ---------------------------/ /** TRIG - PB2 ECHO - PB3 */ GPIO_InitStruct.Pin = GPIO_PIN_2; 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); GPIO_InitStruct.Pin = GPIO_PIN_3; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- HC-SR04 ÓÒ TRIG & ECHO ---------------------------/ /** TRIG - PB4 ECHO - PB5 */ GPIO_InitStruct.Pin = GPIO_PIN_4; 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); GPIO_InitStruct.Pin = GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /-------------------------- I2C1 SCL & SDA ----------------------------------/ /** SCL - PB8 SDA - PB9 */ 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); /-------------------------- USART1 TX & RX ----------------------------------/ /** TX - PA9 RX - PA10 */ GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); } motion.c #include “motion.h” #include “encoder.h” #include “motor.h” #include “jy61p.h” #include “math.h” #include “stddef.h” #include “pid.h” void Move_Forward(int distance_cm) { int target_ticks = distance_cm * 10; Reset_Encoders(); Motor_Forward(100); while (1) { int left = Get_Left_Encoder(); int right = Get_Right_Encoder(); int avg = (left + right) / 2; if (avg >= target_ticks) { Motor_Stop(); break; } } } void Turn_Left(int angle_deg) { float target_yaw = angle_deg; float current_yaw = 0.0f; JY61P_Data imu_data; PID_Controller yaw_pid; PID_Init(&yaw_pid, 2.0f, 0.0f, 0.1f); while (fabsf(current_yaw) < target_yaw) { JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; float error = target_yaw - current_yaw; float output = PID_Update(&yaw_pid, error, 0.01f); Motor_Turn_Left((int)(output)); } Motor_Stop(); } void Turn_Right(int angle_deg) { float target_yaw = angle_deg; float current_yaw = 0.0f; JY61P_Data imu_data; PID_Controller yaw_pid; PID_Init(&yaw_pid, 2.0f, 0.0f, 0.1f); while (fabsf(current_yaw) < target_yaw) { JY61P_Read_Attitude(&imu_data); current_yaw = imu_data.yaw; float error = target_yaw - current_yaw; float output = PID_Update(&yaw_pid, error, 0.01f); Motor_Turn_Right((int)(output)); } Motor_Stop(); } 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” #include “tim.h” #include “gpio.h” float Get_Left_Distance() { 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 = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } float Get_Right_Distance() { 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 = TIM3->CNT; while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET); end = TIM3->CNT; float distance = (end - start) * 0.034 / 2; return distance; } 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 K[2]; K[0] = kf->P[0][0] / S; K[1] = kf->P[1][0] / S; kf->angle += K[0] * y; kf->bias += K[1] * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K[0] * P00_temp; kf->P[0][1] -= K[0] * P01_temp; kf->P[1][0] -= K[1] * P00_temp; kf->P[1][1] -= K[1] * P01_temp; return kf->angle; } motor.c #include “motor.h” #include “tim.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_14, GPIO_PIN_SET); // AIN1 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); // AIN2 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); // BIN1 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); // BIN2 Motor_SetSpeed(speed, speed); } void Motor_Backward(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } void Motor_Stop() { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); Motor_SetSpeed(0, 0); } void Motor_Turn_Left(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET); Motor_SetSpeed(speed, speed); } void Motor_Turn_Right(int speed) { HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); Motor_SetSpeed(speed, speed); } encoder.c #include “encoder.h” #include “tim.h” void Reset_Encoders(void) { htim3.Instance->CNT = 0; htim4.Instance->CNT = 0; } float Get_Total_Distance(void) { int32_t left_ticks = htim3.Instance->CNT; int32_t right_ticks = htim4.Instance->CNT; float distance = (left_ticks + right_ticks) / 2.0f * 0.1f; return distance; } float Get_Left_Encoder(void) { return (float)htim3.Instance->CNT; } float Get_Right_Encoder(void) { return (float)htim4.Instance->CNT; } jy61p.c #include “jy61p.h” #include <string.h> #include “i2c.h” #define JY61P_ADDR 0x50 << 1 void JY61P_Init(I2C_HandleTypeDef *hi2c) { } void JY61P_Read_Acc(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x34, 1, buf, 6, HAL_MAX_DELAY); data->ax = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 16; data->ay = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 16; data->az = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 16; } void JY61P_Read_Gyro(JY61P_Data *data) { uint8_t buf[6]; HAL_I2C_Mem_Read(&hi2c1, JY61P_ADDR, 0x38, 1, buf, 6, HAL_MAX_DELAY); data->gx = (int16_t)(buf[0] << 8 | buf[1]) / 32768.0f * 2000; data->gy = (int16_t)(buf[2] << 8 | buf[3]) / 32768.0f * 2000; data->gz = (int16_t)(buf[4] << 8 | buf[5]) / 32768.0f * 2000; } 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; } 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 */ 这个代码可以实现的功能,接线
最新发布
08-01
/* 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 "stdio.h" #include "string.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ // Ô²ÖùÐÅÏ¢½á¹¹Ìå #define MAX_CYLINDERS 10 typedef struct { int x, y, r; char color; } Cylinder; Cylinder cylinders[MAX_CYLINDERS]; int cylinder_count = 0; uint8_t mission_running = 0; uint32_t start_time = 0; uint32_t timeout = 10000; // 10s uint8_t rx_byte; char openmv_buffer[128]; uint8_t openmv_index = 0; // ÈÎÎñģʽö¾Ù typedef enum { TASK_SIMPLE_EXIT, TASK_SNAKE_PATH, TASK_CIRCLE_TWO_COLORS, TASK_COLUMN_SWAP, TASK_RANDOM_DETECT } TaskMode; TaskMode current_task = TASK_SIMPLE_EXIT; /* 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); /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == &huart1) { if (rx_byte >= '1' && rx_byte <= '5') { current_task = rx_byte - '1'; // 1~5 Ó³É䵽ö¾Ù printf("Selected Task: %d\r\n", current_task + 1); } HAL_UART_Receive_IT(&huart1, &rx_byte, 1); //¼ÌÐø½ÓÊÕ } } // Æô¶¯ÈÎÎñ void StartMission() { mission_running = 1; start_time = HAL_GetTick(); } // ÊÇ·ñ³¬Ê± uint8_t IsMissionTimeout() { return (HAL_GetTick() - start_time) > timeout; } // Ä£¿éÊý¾Ý½á¹¹Ìå 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 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 BuildPath(char color) { if (color == 'W') { SetTargetYaw(-180.0f); // °××ó } else if (color == 'B') { SetTargetYaw(180.0f); // ºÚÓÒ } } //±ÜÕÏ void AvoidObstacle() { float left = Get_Left_Distance(); float right = Get_Right_Distance(); if (left < 10 && right < 10) { Motor_Stop(); printf("Obstacle detected in both directions. Stopping.\r\n"); } else if (left < 10) { Turn_Right(90); } else if (right < 10) { Turn_Left(90); } } //¼òµ¥Â·¾¶ void SimpleExitPath() { Move_Forward(100); Turn_Right(90); Move_Forward(50); } //ÉßÐη¾¶ void SnakePath() { for (int i = 0; i < cylinder_count; i++) { if (i % 2 == 0) { Turn_Left(90); } else { Turn_Right(90); } Move_Forward(20); } } //ÈÆÁ½¸öÑÕɫԲÖù void CircleTwoColors() { char colors_seen[2] = {0}; int count = 0; for (int i = 0; i < cylinder_count && count < 2; i++) { if (colors_seen[0] == 0 && cylinders[i].color == 'W') { BuildPath('W'); colors_seen[0] = 'W'; count++; } else if (colors_seen[1] == 0 && cylinders[i].color == 'B') { BuildPath('B'); colors_seen[1] = 'B'; count++; } } } //ºÚ°×Öù½»»»ºó·¾¶ void ColumnSwapPath() { //¼ÙÉèÒѽ»»»Öù×ÓÖØÐ¹滮·¾¶ BuildPath('W'); BuildPath('B'); } // Ëæ»ú̽²â·¾¶ void RandomDetectPath() { // ½ÓÊÕOpenMV Êý¾Ý HAL_UART_Receive_IT(&huart1, &rx_byte, 1); HAL_Delay(30000); // 30s BuildPath('W'); // ¼ÙÉè̽²âµ½°×Öù Move_Forward(100); } // ÈÎÎñÖ´Ðк¯Êý void ExecuteMission(TaskMode task) { switch (task) { case TASK_SIMPLE_EXIT: SimpleExitPath(); break; case TASK_SNAKE_PATH: SnakePath(); break; case TASK_CIRCLE_TWO_COLORS: CircleTwoColors(); break; case TASK_COLUMN_SWAP: ColumnSwapPath(); break; case TASK_RANDOM_DETECT: RandomDetectPath(); break; } } /* 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); //Ìí¼Ó²âÊÔÔ²Öù 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(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { HAL_Delay(20); if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { StartMission(); while (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET); // µÈ´ýÊÍ·Å } } if (mission_running) { ExecuteMission(current_task); //Ö´Ðе±Ç°ÈÎÎñ if (IsMissionTimeout()) { mission_running = 0; Motor_Stop(); printf("Mission Timeout!\r\n"); } /* 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 */
08-01
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值