asd/Infantry_new
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加入PID

Browse Source
This commit is contained in:
Wei Zitong
2026-07-15 17:09:24 +08:00
parent 050ae25e9c
commit 8b24a3da19
10 changed files with 139 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Core/Src/can.c
View File

@@ -83,6 +83,9 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
GPIO_InitStruct.Alternate = GPIO_AF9_CAN1;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
/* USER CODE BEGIN CAN1_MspInit 1 */
/* USER CODE END CAN1_MspInit 1 */
@@ -106,6 +109,8 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
*/
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
/* CAN1 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
/* USER CODE BEGIN CAN1_MspDeInit 1 */
/* USER CODE END CAN1_MspDeInit 1 */

54
Core/Src/chassis_control.c Normal file
View File

@@ -0,0 +1,54 @@
#include "chassis_control.h"
#include "CAN_receive.h"
static pid_t pid_motor1;
static pid_t pid_motor2;
static pid_t pid_motor3;
static pid_t pid_motor4;
static int16_t target_speed_m1;
static int16_t target_speed_m2;
static int16_t target_speed_m3;
static int16_t target_speed_m4;
/**
* @brief 初始化底盘电机的PID
* 从左到右依次是：PID作用模块，KP,KI,KD,电机最大输出，电机最大限制输出
*/
void chassis_control_init(void)
{
pid_init(&pid_motor1, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机1
pid_init(&pid_motor2, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机2
pid_init(&pid_motor3, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机3
pid_init(&pid_motor4, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机4
}
/**
* @brief 控制底盘电机的转速
*/
void chassis_control_task(void)
{
const motor_measure_t *m1 = get_chassis_motor_measure_point(0);
const motor_measure_t *m2 = get_chassis_motor_measure_point(1);
const motor_measure_t *m3 = get_chassis_motor_measure_point(2);
const motor_measure_t *m4 = get_chassis_motor_measure_point(3);
//pid运算
int16_t cur1 = (int16_t)pid_calc(&pid_motor1, target_speed_m1, m1->speed_rpm);
int16_t cur2 = (int16_t)pid_calc(&pid_motor2, target_speed_m2, m2->speed_rpm);
int16_t cur3 = (int16_t)pid_calc(&pid_motor3, target_speed_m3, m3->speed_rpm);
int16_t cur4 = (int16_t)pid_calc(&pid_motor4, target_speed_m4, m4->speed_rpm);
CAN_cmd_chassis(cur1, cur2, cur3, cur4);
}
/**
* @brief 设置电机转速的电流值，范围（0~16384）
*/
void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4)
{
target_speed_m1 = m1;
target_speed_m2 = m2;
target_speed_m3 = m3;
target_speed_m4 = m4;
}

9
Core/Src/freertos.c
View File

@@ -27,6 +27,7 @@
/* USER CODE BEGIN Includes */
#include "bsp_can.h"
#include "CAN_receive.h"
#include "chassis_control.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@@ -126,10 +127,10 @@ void MX_FREERTOS_Init(void) {
void StartCAN_Receive_Task(void *argument)
{
/* USER CODE BEGIN StartCAN_Receive_Task */
/* Infinite loop */
for(;;)
{
CAN_cmd_chassis(1000,0,0,0);
osDelay(1);
}
/* USER CODE END StartCAN_Receive_Task */
@@ -145,9 +146,12 @@ void StartCAN_Receive_Task(void *argument)
void StartChassis_Control_Task(void *argument)
{
/* USER CODE BEGIN StartChassis_Control_Task */
chassis_control_init();
chassis_set_speed(100, 0, 0, 0);
/* Infinite loop */
for(;;)
{
chassis_control_task();//电机控制
osDelay(1);
}
/* USER CODE END StartChassis_Control_Task */
@@ -156,4 +160,5 @@ void StartChassis_Control_Task(void *argument)
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE END Application */
/* USER CODE END Application */

26
Core/Src/pid.c Normal file
View File

@@ -0,0 +1,26 @@
#include "pid.h"
void pid_init(pid_t *pid, float kp, float ki, float kd, float max_out, float max_i)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->integral = 0.0f;
pid->last_error = 0.0f;
pid->max_out = max_out;
pid->max_integral = max_i;
}
float pid_calc(pid_t *pid, float target, float actual)
{
float error = target - actual;
pid->integral += error;
if (pid->integral > pid->max_integral) pid->integral = pid->max_integral;
if (pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
float output = pid->kp * error + pid->ki * pid->integral
+ pid->kd * (error - pid->last_error);
pid->last_error = error;
if (output > pid->max_out) output = pid->max_out;
if (output < -pid->max_out) output = -pid->max_out;
return output;
}

17
Core/Src/stm32f4xx_it.c
View File

@@ -55,6 +55,7 @@
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;
extern TIM_HandleTypeDef htim6;
/* USER CODE BEGIN EV */
@@ -159,6 +160,20 @@ void DebugMon_Handler(void)
/* please refer to the startup file (startup_stm32f4xx.s). */
/******************************************************************************/
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */
/* USER CODE END CAN1_RX0_IRQn 1 */
}
/**
* @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts.
*/
@@ -175,4 +190,4 @@ void TIM6_DAC_IRQHandler(void)
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* USER CODE END 1 */