feat: set pid config

include/consts.example.h

@@ -44,6 +44,11 @@
 #define TURN_RATIO 0.6     // 转向时的速度比例
 #define SPEED_INCREMENT 10 // 速度增量
 
+// 默认 PID 参数
+#define PID_KP 0.1
+#define PID_KI 0.01
+#define PID_KD 0.001
+
 // Nordic UART Service UUID
 #define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
 #define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"

include/handlers.h

@@ -6,6 +6,7 @@
 #include "ble.h"
 #include "ultrasound.h"
 #include "transmit.h"
+#include "pid.h"
 #include "utils.h"
 
 // 指令定义
@@ -14,7 +15,7 @@
 #define CMD_GET_DISTANCE 0x12
 
 #define CMD_SET_NAME 0xA1
-
+#define CMD_SET_PID 0xA2
 #define CMD_MOTOR_MOVE_CONTROL 0x20
 #define CMD_MOTOR_STEER_CONTROL 0x21
 #define CMD_MOTOR_SINGLE_CONTROL 0x22
@@ -32,6 +33,7 @@ public:
   static void getSPIFFSStatus(BLECharacteristic &characteristic);
   static void getDistance(BLECharacteristic &characteristic);
   static void setName(BLECharacteristic &characteristic, uint8_t *packet);
+  static void setPID(BLECharacteristic &characteristic, uint8_t *packet);
   static void motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet);
   static void motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet);

include/pid.h

@@ -0,0 +1,30 @@
+#ifndef __PID_H__
+#define __PID_H__
+
+#include "consts.h"
+
+struct PIDConfig
+{
+  float Kp;
+  float Ki;
+  float Kd;
+};
+
+struct PID
+{
+  float error;
+  float integral;
+  float derivative;
+  float last_error;
+};
+
+class PIDController
+{
+public:
+  static PIDConfig pidConfig;
+  static PID pid;
+  static void setConfig(float Kp, float Ki, float Kd);
+  static float update(float target, float current);
+};
+
+#endif

include/utils.h

@@ -3,6 +3,9 @@
 
 #include <Arduino.h>
 
+#define FIRMWARE_VERSION 1
+
 void floatToBytes(float val, uint8_t *bytes);
+float bytesToFloat(uint8_t *bytes);
 void updateStatusLED(bool deviceConnected, int ledPin);
 #endif

packet.md

@@ -122,6 +122,14 @@
 
 设置示例 `00 0E A1 57 68 69 74 65 54 69 67 65 72 FF`
 
+### 设置 PID 参数 `0xA2`
+
+> ⚠️ 注意： Kp Ki Kd 类型为 float，每个参数 4 个字节
+
+包体 `00 07 A2 Kp1 Kp2 Kp3 Kp4 Ki1 Ki2 Ki3 Ki4 Kd1 Kd2 Kd3 Kd4 FF`
+
+设置示例 `00 11 A2 01 01 01 01 01 01 01 01 01 01 01 01 01 FF`
+
 ## 状态回报
 
 ### 电机状态汇报 `0xE0`

src/handlers.cpp

@@ -61,6 +61,14 @@ void Handlers::setName(BLECharacteristic &characteristic, uint8_t *packet)
   BLEManager::updateDeviceName(name);
 }
 
+void Handlers::setPID(BLECharacteristic &characteristic, uint8_t *packet)
+{
+  float Kp = bytesToFloat(&packet[3]);
+  float Ki = bytesToFloat(&packet[7]);
+  float Kd = bytesToFloat(&packet[11]);
+  PIDController::setConfig(Kp, Ki, Kd);
+}
+
 void Handlers::motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet)
 {
   uint8_t direction = packet[3];

src/pid.cpp

@@ -0,0 +1,22 @@
+#include "pid.h"
+
+PIDConfig PIDController::pidConfig = {
+    PID_KP,
+    PID_KI,
+    PID_KD};
+
+void PIDController::setConfig(float Kp, float Ki, float Kd)
+{
+  pidConfig.Kp = Kp;
+  pidConfig.Ki = Ki;
+  pidConfig.Kd = Kd;
+}
+
+float PIDController::update(float target, float current)
+{
+  pid.error = target - current;
+  pid.integral += pid.error;
+  pid.derivative = pid.error - pid.last_error;
+  pid.last_error = pid.error;
+  return pidConfig.Kp * pid.error + pidConfig.Ki * pid.integral + pidConfig.Kd * pid.derivative;
+}

src/transmit.cpp

@@ -70,6 +70,14 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte
     Handlers::getDistance(characteristic);
     break;
 
+  case CMD_SET_NAME:
+    Handlers::setName(characteristic, packet);
+    break;
+
+  case CMD_SET_PID:
+    Handlers::setPID(characteristic, packet);
+    break;
+
   case CMD_MOTOR_MOVE_CONTROL:
     Handlers::motorMoveControl(characteristic, packet);
     break;

src/utils.cpp

@@ -20,6 +20,20 @@ void floatToBytes(float val, uint8_t *bytes)
   }
 }
 
+float bytesToFloat(uint8_t *bytes)
+{
+  union
+  {
+    float f;
+    uint8_t bytes[4];
+  } u;
+  for (int i = 0; i < 4; i++)
+  {
+    u.bytes[i] = bytes[i];
+  }
+  return u.f;
+}
+
 void updateStatusLED(bool deviceConnected, int ledPin)
 {
   if (deviceConnected)