加入PID

CMakeLists.txt

@@ -47,6 +47,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
     Core/Src/CAN_receive.c
     Core/Src/bsp_can.c
+    Core/Src/chassis_control.c
+    Core/Src/pid.c
 )
 
 # Add include paths

Core/Inc/chassis_control.h

@@ -0,0 +1,12 @@
+#ifndef CHASSIS_CONTROL_H
+#define CHASSIS_CONTROL_H
+
+#include "pid.h"
+#include "main.h"
+
+void chassis_control_init(void);
+void chassis_control_task(void);
+
+void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4);
+
+#endif

Core/Inc/pid.h

@@ -0,0 +1,15 @@
+#ifndef PID_H
+#define PID_H
+
+typedef struct {
+    float kp, ki, kd;
+    float integral;
+    float last_error;
+    float max_out;
+    float max_integral;
+} pid_t;
+
+void  pid_init(pid_t *pid, float kp, float ki, float kd, float max_out, float max_i);
+float pid_calc(pid_t *pid, float target, float actual);
+
+#endif

Core/Inc/stm32f4xx_it.h

@@ -52,6 +52,7 @@ void MemManage_Handler(void);
 void BusFault_Handler(void);
 void UsageFault_Handler(void);
 void DebugMon_Handler(void);
+void CAN1_RX0_IRQHandler(void);
 void TIM6_DAC_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 

Core/Src/can.c

@@ -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 */

Core/Src/chassis_control.c

@@ -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;
+}

Core/Src/freertos.c

@@ -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 */
@@ -157,3 +161,4 @@ void StartChassis_Control_Task(void *argument)
 /* USER CODE BEGIN Application */
 
 /* USER CODE END Application */
+

Core/Src/pid.c

@@ -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;
+}

Core/Src/stm32f4xx_it.c

@@ -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.
   */

Infantry_chassis.ioc

@@ -40,6 +40,7 @@ Mcu.UserName=STM32F407IGHx
 MxCube.Version=6.15.0
 MxDb.Version=DB.6.0.150
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false