From 1bdd51db002e063f5ad07e73305b69da293c8e14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=8E=96=E5=8F=81?= Date: Sat, 28 Dec 2024 00:38:42 +0800 Subject: [PATCH] feat: tracking --- include/handlers.h | 7 +++- include/mode.h | 19 ++++++++++ include/pid.h | 19 +++++++--- include/tracking.h | 28 +++++++++++++++ include/transmit.h | 5 ++- packet.md | 16 +++++++-- src/handlers.cpp | 16 +++++++++ src/main.cpp | 15 +++++++- src/mode.cpp | 11 ++++++ src/pid.cpp | 66 ++++++++++++++++++++++++++++++++--- src/tracking.cpp | 86 ++++++++++++++++++++++++++++++++++++++++++++++ src/transmit.cpp | 21 ++++++----- 12 files changed, 285 insertions(+), 24 deletions(-) create mode 100644 include/mode.h create mode 100644 include/tracking.h create mode 100644 src/mode.cpp create mode 100644 src/tracking.cpp diff --git a/include/handlers.h b/include/handlers.h index 7289ded..512aba0 100644 --- a/include/handlers.h +++ b/include/handlers.h @@ -8,6 +8,8 @@ #include "transmit.h" #include "pid.h" #include "ir.h" +#include "tracking.h" +#include "mode.h" #include "utils.h" // 指令定义 @@ -23,7 +25,9 @@ #define CMD_MOTOR_ROTATE_CONTROL 0x23 #define CMD_DEMO_PID 0xf0 #define CMD_DEMO_PATH 0xf1 -#define CMD_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令 +#define CMD_MOTOR_XYR_CONTROL 0x24 + +#define CMD_MODE_CONTROL 0x30 #define CMD_STATUS_MOTOR 0xE0 @@ -40,6 +44,7 @@ public: static void motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet); static void motorSingleControl(BLECharacteristic &characteristic, uint8_t *packet); static void motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet); + static void modeControl(BLECharacteristic &characteristic, uint8_t *packet); static void demoPID(BLECharacteristic &characteristic, uint8_t *packet); }; diff --git a/include/mode.h b/include/mode.h new file mode 100644 index 0000000..f1329bd --- /dev/null +++ b/include/mode.h @@ -0,0 +1,19 @@ +#ifndef MODE_H +#define MODE_H + +#include "Arduino.h" +#include "tracking.h" +enum ModeType +{ + MODE_MANUAL_CONTROL, + MODE_TRACK, +}; + +class Mode +{ +public: + static ModeType mode; + static void processDeviceMode(); +}; + +#endif \ No newline at end of file diff --git a/include/pid.h b/include/pid.h index 766e6a1..2306204 100644 --- a/include/pid.h +++ b/include/pid.h @@ -1,6 +1,7 @@ -#ifndef __PID_H__ -#define __PID_H__ +#ifndef _PID_H_ +#define _PID_H_ +#include #include "consts.h" struct PIDConfig @@ -8,22 +9,30 @@ struct PIDConfig float Kp; float Ki; float Kd; + float outputMax; // 输出限幅 + float outputMin; // 输出限幅 + float integralMax; // 积分限幅 }; struct PID { float error; + float last_error; float integral; float derivative; - float last_error; + unsigned long lastTime; // 上次更新时间 }; class PIDController { public: - static PIDConfig pidConfig; static PID pid; - static void setConfig(float Kp, float Ki, float Kd); + + static PIDConfig pidConfig; + static void reset(); // 重置PID状态 + static void setConfig(float Kp, float Ki, float Kd, + float outputMax = 255, float outputMin = -255, + float integralMax = 1000); static float update(float target, float current); }; diff --git a/include/tracking.h b/include/tracking.h new file mode 100644 index 0000000..78ac8c3 --- /dev/null +++ b/include/tracking.h @@ -0,0 +1,28 @@ +#ifndef _TRACKING_H_ +#define _TRACKING_H_ + +#include "ir.h" +#include "motor.h" +#include "pid.h" +class TrackingController { +public: + static uint8_t baseSpeed; // 基础前进速度 + static uint8_t turnSpeed; // 转弯速度 + + // 初始化循迹控制器 + static void init(); + + // 更新循迹状态并控制电机 + static void update(); + + // 设置循迹速度 + static void setSpeed(uint8_t baseSpeed, uint8_t turnSpeed); + + // 计算偏移量 + static float calculateOffset(const IRData& irData); + +private: + static constexpr float LOST_LINE = 999.0f; // 使用一个特殊值表示丢线状态 +}; + +#endif \ No newline at end of file diff --git a/include/transmit.h b/include/transmit.h index 6872e57..c4eb0c9 100644 --- a/include/transmit.h +++ b/include/transmit.h @@ -2,8 +2,7 @@ #define TRANSMIT_H #include -#include -#include +#include "ble.h" #include "handlers.h" #define PACKET_R_HEAD 0x00 @@ -14,6 +13,6 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic); void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic); -void sendStatus(BLECharacteristic &characteristic); +void sendStatus(); #endif \ No newline at end of file diff --git a/packet.md b/packet.md index d8be855..446c3bd 100644 --- a/packet.md +++ b/packet.md @@ -116,6 +116,17 @@ 控制示例 `00 07 24 01 01 01 FF` +## 模式切换 `0x30` + +| 模式数据 | 含义 | +| -------- | ---- | +| 0x00 | 手动模式 | +| 0x01 | 巡线模式 | + +包体 `00 05 30 模式 FF` + +控制示例 `00 05 30 01 FE` + ## 设置 ### 设置设备名称 `0xA1` @@ -140,13 +151,14 @@ ### 设备状态回报 `0xE0` -包体 `01 0E E0 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 FE` +包体 `01 0F E0 运行模式 电机A_IN_1_2 电机A_PWM 电机B_IN_1_2 电机B_PWM 电机C_IN_1_2 电机C_PWM 电机D_IN_1_2 电机D_PWM 红外引脚数量 红外数据 FE` | 数据 | 类型 | 表示 | | -------- | ---- | ---- | +| 运行模式 | 无符号整数 | 0x00 手动模式，0x01 巡线模式 | | IN_1_2 | 无符号整数 | 按位表示 IN1 IN2 的值， 0000 00 IN2 IN1 | | PWM | 无符号整数 | - | | 红外引脚数量 | 无符号整数 | - | | 红外数据 | 无符号整数 | 按位表示红外循迹模块数据， 0001 1111 | -示例 `01 0E E0 01 FF 02 FF 02 FF 01 FF 05 1F FE` +示例 `01 0F E0 00 01 FF 02 FF 02 FF 01 FF 05 1F FE` diff --git a/src/handlers.cpp b/src/handlers.cpp index 502e267..bbfa685 100644 --- a/src/handlers.cpp +++ b/src/handlers.cpp @@ -178,6 +178,22 @@ void Handlers::motorXYRControl(BLECharacteristic &characteristic, uint8_t *packe MotorController::motorXYRControl(x, y, r); } +void Handlers::modeControl(BLECharacteristic &characteristic, uint8_t *packet) +{ + uint8_t mode = packet[3]; + switch (mode) + { + case 0x01: + Mode::mode = ModeType::MODE_TRACK; + break; + + case 0x00: + default: + Mode::mode = ModeType::MODE_MANUAL_CONTROL; + break; + } +} + void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet) { Serial.println("CMD_DEMO_PID"); diff --git a/src/main.cpp b/src/main.cpp index ba52b7e..ab49411 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,8 @@ #include "ble.h" #include "led.h" #include "ir.h" +#include "tracking.h" +#include "mode.h" #include "consts.h" void setup() @@ -33,17 +35,28 @@ void setup() { LED::init(STATUS_LED); } + + // 初始化循迹控制器 + TrackingController::init(); } void loop() { + // 循环 1 部分，获取传感器数据 IR::update(); + + // 循环 2 部分，更新状态灯 if (STATUS_LED_ENABLE) { LED::updateStatusLED(BLEManager::deviceConnected); } + + // 循环 3 部分，处理设备模式 + Mode::processDeviceMode(); + + // 循环 4 部分，反馈状态 if (BLEManager::deviceConnected) { - sendStatus(*BLEManager::pTxCharacteristic); + sendStatus(); } } \ No newline at end of file diff --git a/src/mode.cpp b/src/mode.cpp new file mode 100644 index 0000000..8be4bae --- /dev/null +++ b/src/mode.cpp @@ -0,0 +1,11 @@ +#include "mode.h" + +ModeType Mode::mode = ModeType::MODE_MANUAL_CONTROL; + +void Mode::processDeviceMode() +{ + if (mode == ModeType::MODE_TRACK) + { + TrackingController::update(); + } +} \ No newline at end of file diff --git a/src/pid.cpp b/src/pid.cpp index 47ae91c..ca2df22 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -1,22 +1,80 @@ #include "pid.h" +PID PIDController::pid = {0, 0, 0, 0, 0}; + PIDConfig PIDController::pidConfig = { PID_KP, PID_KI, PID_KD}; -void PIDController::setConfig(float Kp, float Ki, float Kd) +void PIDController::reset() +{ + pid.error = 0; + pid.last_error = 0; + pid.integral = 0; + pid.derivative = 0; + pid.lastTime = millis(); +} + +void PIDController::setConfig(float Kp, float Ki, float Kd, + float outputMax, float outputMin, + float integralMax) { pidConfig.Kp = Kp; pidConfig.Ki = Ki; pidConfig.Kd = Kd; + pidConfig.outputMax = outputMax; + pidConfig.outputMin = outputMin; + pidConfig.integralMax = integralMax; } float PIDController::update(float target, float current) { + // 计算时间间隔 + unsigned long now = millis(); + float dt = (now - pid.lastTime) / 1000.0f; // 转换为秒 + pid.lastTime = now; + + // 如果时间间隔过大，说明可能是第一次运行或长时间未运行 + if (dt > 1.0f) + { + dt = 0.01f; // 使用默认值 + } + + // 计算误差 pid.error = target - current; - pid.integral += pid.error; - pid.derivative = pid.error - pid.last_error; + + // 计算积分项 + pid.integral += pid.error * dt; + + // 积分限幅 + if (pid.integral > pidConfig.integralMax) + { + pid.integral = pidConfig.integralMax; + } + else if (pid.integral < -pidConfig.integralMax) + { + pid.integral = -pidConfig.integralMax; + } + + // 计算微分项（使用错误微分而不是输出微分，避免微分突跳） + pid.derivative = (pid.error - pid.last_error) / dt; pid.last_error = pid.error; - return pidConfig.Kp * pid.error + pidConfig.Ki * pid.integral + pidConfig.Kd * pid.derivative; + + // 计算PID输出 + float output = pidConfig.Kp * pid.error + + pidConfig.Ki * pid.integral + + pidConfig.Kd * pid.derivative; + + // 输出限幅 + if (output > pidConfig.outputMax) + { + output = pidConfig.outputMax; + } + else if (output < pidConfig.outputMin) + { + output = pidConfig.outputMin; + } + + return output; } diff --git a/src/tracking.cpp b/src/tracking.cpp new file mode 100644 index 0000000..73d147d --- /dev/null +++ b/src/tracking.cpp @@ -0,0 +1,86 @@ +#include "tracking.h" + +uint8_t TrackingController::baseSpeed = 10; // 默认速度50% +uint8_t TrackingController::turnSpeed = 10; // 默认转弯速度30% + +void TrackingController::init() +{ + Serial.println("Tracking Init"); +} + +void TrackingController::setSpeed(uint8_t base, uint8_t turn) +{ + baseSpeed = base; + turnSpeed = turn; +} + +void TrackingController::update() +{ + IR::update(); + + const IRData &irData = IR::data; + float offset = calculateOffset(irData); + + Serial.print("Offset: "); + Serial.println(offset); + + // 检查是否丢线 + if (offset == LOST_LINE) + { // 需要定义 LOST_LINE 常量 + // 丢线时停止移动 + Serial.println("Lost line!"); + MotorController::motorXYRControl(0, 0, 0); + return; + } + + // 正常巡线控制 + float correction = PIDController::update(0, offset); + MotorController::motorXYRControl(0, baseSpeed, -correction); +} + +// 计算偏移量，返回范围 [-2, 2] +float TrackingController::calculateOffset(const IRData &irData) +{ + float offset = 0; + int activeCount = 0; + bool allHigh = true; + bool allLow = true; + + Serial.print("IR Data: "); + for (int i = 0; i < irData.length; i++) { + Serial.print(irData.data[i]); + Serial.print(" "); + } + Serial.println(); + + // 检查是否所有传感器都是高电平或低电平 + for (int i = 0; i < irData.length; i++) { + if (irData.data[i]) { + allLow = false; + } else { + allHigh = false; + } + } + + // 如果所有传感器都是高电平或低电平，说明不在线上 + if (allHigh || allLow) { + return LOST_LINE; + } + + // 计算加权平均偏移 + for (int i = 0; i < irData.length; i++) + { + if (irData.data[i]) + { + // 将传感器位置映射到 [-2, 2] 范围 + offset += (i - (irData.length - 1) / 2.0f) * (4.0f / (irData.length - 1)); + activeCount++; + } + } + + // 如果有传感器检测到线，返回平均偏移 + if (activeCount > 0) + return offset / activeCount; + + return LOST_LINE; +} \ No newline at end of file diff --git a/src/transmit.cpp b/src/transmit.cpp index 66153f6..814f1f7 100644 --- a/src/transmit.cpp +++ b/src/transmit.cpp @@ -94,20 +94,24 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte Handlers::motorSingleControl(characteristic, packet); break; - case CMD_DEMO_PID: - Handlers::demoPID(characteristic, packet); - break; - case CMD_MOTOR_XYR_CONTROL: Handlers::motorXYRControl(characteristic, packet); break; + case CMD_MODE_CONTROL: + Handlers::modeControl(characteristic, packet); + break; + + case CMD_DEMO_PID: + Handlers::demoPID(characteristic, packet); + break; + default: break; } } -void sendStatus(BLECharacteristic &characteristic) +void sendStatus() { unsigned long currentMillis = millis(); if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL) @@ -117,8 +121,9 @@ void sendStatus(BLECharacteristic &characteristic) uint8_t buffer[PACKET_MAX_LENGTH]; uint8_t bufferIndex = 0; buffer[bufferIndex++] = PACKET_T_HEAD; - buffer[bufferIndex++] = 0x0E; + buffer[bufferIndex++] = 0x0F; buffer[bufferIndex++] = CMD_STATUS_MOTOR; + buffer[bufferIndex++] = Mode::mode; buffer[bufferIndex++] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1; buffer[bufferIndex++] = MotorController::getMotorStatus('A').pwm; buffer[bufferIndex++] = (MotorController::getMotorStatus('B').in2 << 1) | MotorController::getMotorStatus('B').in1; @@ -135,7 +140,7 @@ void sendStatus(BLECharacteristic &characteristic) } buffer[bufferIndex++] = irData; buffer[bufferIndex++] = PACKET_T_TAIL; - characteristic.setValue(buffer, bufferIndex); - characteristic.notify(); + BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex); + BLEManager::pTxCharacteristic->notify(); } } \ No newline at end of file