From c1112b65c590da5b66a83fc18be141a082588cf2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= Date: Fri, 27 Dec 2024 10:34:06 +0800 Subject: [PATCH] feat: set pid config --- include/consts.example.h | 5 +++++ include/handlers.h | 4 +++- include/pid.h | 30 ++++++++++++++++++++++++++++++ include/utils.h | 5 ++++- packet.md | 8 ++++++++ src/handlers.cpp | 8 ++++++++ src/pid.cpp | 22 ++++++++++++++++++++++ src/transmit.cpp | 8 ++++++++ src/utils.cpp | 14 ++++++++++++++ 9 files changed, 102 insertions(+), 2 deletions(-) create mode 100644 include/pid.h create mode 100644 src/pid.cpp diff --git a/include/consts.example.h b/include/consts.example.h index 1378e50..7bed484 100644 --- a/include/consts.example.h +++ b/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" diff --git a/include/handlers.h b/include/handlers.h index f95b924..2956088 100644 --- a/include/handlers.h +++ b/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); diff --git a/include/pid.h b/include/pid.h new file mode 100644 index 0000000..766e6a1 --- /dev/null +++ b/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 diff --git a/include/utils.h b/include/utils.h index f6a5556..5286b36 100644 --- a/include/utils.h +++ b/include/utils.h @@ -3,6 +3,9 @@ #include +#define FIRMWARE_VERSION 1 + void floatToBytes(float val, uint8_t *bytes); +float bytesToFloat(uint8_t *bytes); void updateStatusLED(bool deviceConnected, int ledPin); -#endif \ No newline at end of file +#endif diff --git a/packet.md b/packet.md index 5308cab..b19185e 100644 --- a/packet.md +++ b/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` diff --git a/src/handlers.cpp b/src/handlers.cpp index 1ea56d3..da76975 100644 --- a/src/handlers.cpp +++ b/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]; diff --git a/src/pid.cpp b/src/pid.cpp new file mode 100644 index 0000000..47ae91c --- /dev/null +++ b/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; +} diff --git a/src/transmit.cpp b/src/transmit.cpp index 4554324..bae7cf2 100644 --- a/src/transmit.cpp +++ b/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; diff --git a/src/utils.cpp b/src/utils.cpp index 4ddc65b..40146c8 100644 --- a/src/utils.cpp +++ b/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)