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 (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; #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 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_GPIOD_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2|GPIO_PIN_4, GPIO_PIN_RESET); /*Configure GPIO pins : PB2 PB4 */ GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_4 | GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /*Configure GPIO pins : PB3 PB5 */ GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } /* USER CODE BEGIN 2 */ /* USER CODE END 2 */ i2c.c /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file i2c.c * @brief This file provides code for the configuration * of the I2C instances. ****************************************************************************** * @attention * * Copyright (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 */ 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(); } motion.h #ifndef __MOTION_H #define __MOTION_H void Move_Forward(int distance); void Move_Backward(int distance); void Turn_Left(int angle); void Turn_Right(int angle); #endif 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; } pid.h #ifndef PID_H #define PID_H typedef struct { float Kp, Ki, Kd; float integral; float last_error; } PID_Controller; void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd); float PID_Update(PID_Controller *pid, float error, float dt); #endif main.h /* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.h * @brief : Header for main.c file. * This file contains the common defines of the application. ****************************************************************************** * @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 */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __MAIN_H #define __MAIN_H #ifdef __cplusplus extern "C" { #endif /* Includes ------------------------------------------------------------------*/ #include "stm32f1xx_hal.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 "ultrasonic.h" #include "motion.h" #include "encoder.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ /* Exported types ------------------------------------------------------------*/ /* USER CODE BEGIN ET */ /* USER CODE END ET */ /* Exported constants --------------------------------------------------------*/ /* USER CODE BEGIN EC */ /* USER CODE END EC */ /* Exported macro ------------------------------------------------------------*/ /* USER CODE BEGIN EM */ /* USER CODE END EM */ /* Exported functions prototypes ---------------------------------------------*/ void Error_Handler(void); /* USER CODE BEGIN EFP */ /* USER CODE END EFP */ /* Private defines -----------------------------------------------------------*/ /* USER CODE BEGIN Private defines */ #define TARGET_SPEED 100 #define BASE_PWM 70 #define DT 0.01f /* USER CODE END Private defines */ #ifdef __cplusplus } #endif #endif /* __MAIN_H */ 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); } motor.h #ifndef __MOTOR_H #define __MOTOR_H void Motor_Forward(int speed); void Motor_Backward(int speed); void Motor_Stop(void); void Motor_Turn_Left(int speed); void Motor_Turn_Right(int speed); void Motor_SetSpeed(int left, int right); #endif 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; } kalman_filter.h #ifndef __KALMAN_FILTER_H #define __KALMAN_FILTER_H typedef struct { float Q_angle; float Q_bias; float R_measure; float angle, bias, rate, P[2][2]; } KalmanFilter; void KalmanFilter_Init(KalmanFilter *kf, float Q_angle, float Q_gyro, float R_angle); float KalmanFilter_Update(KalmanFilter *kf, float measured_angle, float measured_omega, float dt); #endif /* __KALMAN_FILTER_H */ 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; } ultrasonic.h #ifndef __ULTRASONIC_H #define __ULTRASONIC_H #include "stm32f1xx_hal.h" float Get_Left_Distance(void); float Get_Right_Distance(void); #endif /* __ULTRASONIC_H */ 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; } encoder.h #ifndef __ENCODER_H #define __ENCODER_H void Reset_Encoders(void); float Get_Total_Distance(void); float Get_Left_Encoder(void); float Get_Right_Encoder(void); #endif 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.h #ifndef __JY61P_H #define __JY61P_H #include "stm32f1xx_hal.h" typedef struct { float ax, ay, az; // ¼ÓËÙ¶È float gx, gy, gz; // ½ÇËÙ¶È float pitch, roll, yaw; //×Ë̬½Ç } JY61P_Data; void JY61P_Init(I2C_HandleTypeDef *hi2c); void JY61P_Read_Acc(JY61P_Data *data); void JY61P_Read_Gyro(JY61P_Data *data); void JY61P_Read_Attitude(JY61P_Data *data); #endif 在上述的代码进行修改硬件为一个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
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值