加入PID

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