diff --git a/packet.md b/packet.md index 04d9e29..0475f43 100644 --- a/packet.md +++ b/packet.md @@ -56,21 +56,19 @@ | 0x00 | 停止 | | 0x01 | 前进 | | 0x02 | 后退 | -| 0x03 | 左转 | -| 0x04 | 右转 | 包体 `00 06 20 方向 速度 FF` 控制示例 `00 06 20 01 FF FF` -### 原地控制 `0x21` +### 方向控制 `0x21` | 方向数据 | 含义 | | -------- | ---- | -| 0x00 | 顺时针 | -| 0x01 | 逆时针 | +| 0x00 | 左转 | +| 0x01 | 右转 | -包体 `00 06 21 方向 时间 FF` +包体 `00 06 21 方向 差速 FF` 控制示例 `00 06 21 01 01 FF` @@ -93,12 +91,21 @@ 控制示例 `00 07 22 01 01 01 FF` -## 设置 +### 原地控制 `0x23` -### 设置转向灵敏度 `0x30` +| 方向数据 | 含义 | +| -------- | ---- | +| 0x00 | 顺时针 | +| 0x01 | 逆时针 | -含义为 1 度对应差速 +包体 `00 06 23 方向 时间 FF` -包体 `00 05 30 差速 FF` +控制示例 `00 06 23 01 01 FF` -设置示例 `00 05 30 10 FF` +## 状态回报 + +### 电机状态汇报 `0xE0` + +包体 `01 0C 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 0C E0 01 FF 02 FF 02 FF 01 FF FE` diff --git a/src/main.cpp b/src/main.cpp index 5fc3a18..e2d393f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,9 +20,14 @@ #define CMD_GET_SPIFFS_STATUS 0x11 #define CMD_GET_DISTANCE 0x12 #define CMD_MOTOR_MOVE_CONTROL 0x20 -#define CMD_MOTOR_ROTATE_CONTROL 0x21 +#define CMD_MOTOR_STEER_CONTROL 0x21 #define CMD_MOTOR_SINGLE_CONTROL 0x22 +#define CMD_MOTOR_ROTATE_CONTROL 0x23 #define CMD_DEMO_PID 0xf0 +#define CMD_DEMO_PATH 0xf1 + +#define CMD_STATUS_MOTOR 0xE0 + // 全局变量 int currentSpeed = 0; // 当前速度 int turnOffset = 0; // 转向偏移量 (-100 到 100) @@ -44,6 +49,23 @@ bool isReceivingPacket = false; unsigned long lastLedToggle = 0; // 上次LED切换状态的时间 bool ledState = false; // LED当前状态 +#define MOTOR_STATUS_INTERVAL 200 // 发送电机状态的时间间隔(ms) +unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间 + +// 在全局变量区域添加电机方向状态跟踪 +struct MotorStatus +{ + bool in1; + bool in2; + unsigned char pwm; +}; + +// 定义四个电机的状态 +MotorStatus motorA = {false, false, 0}; +MotorStatus motorB = {false, false, 0}; +MotorStatus motorC = {false, false, 0}; +MotorStatus motorD = {false, false, 0}; + void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic); class CarBLEServerCallbacks : public BLEServerCallbacks @@ -92,26 +114,101 @@ void floatToBytes(float val, uint8_t *bytes) } } -// 电机控制函数 -void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed) +void pwmControl(char motor, unsigned char pwm) { + int pwmPin; + switch (motor) + { + case 'A': + pwmPin = MOTOR_A_PWMA; + motorA.pwm = pwm; + break; + case 'B': + pwmPin = MOTOR_B_PWMB; + motorB.pwm = pwm; + break; + case 'C': + pwmPin = MOTOR_C_PWMA; + motorC.pwm = pwm; + break; + case 'D': + pwmPin = MOTOR_D_PWMB; + motorD.pwm = pwm; + break; + } + analogWrite(pwmPin, pwm); +} + +// 电机控制函数 +void motorControl(char motor, int speed) +{ + int in1Pin, in2Pin; + MotorStatus *status; + + switch (motor) + { + case 'A': + in1Pin = MOTOR_A_AIN1; + in2Pin = MOTOR_A_AIN2; + status = &motorA; + break; + case 'B': + in1Pin = MOTOR_B_BIN2; + in2Pin = MOTOR_B_BIN1; + status = &motorB; + break; + case 'C': + in1Pin = MOTOR_C_AIN2; + in2Pin = MOTOR_C_AIN1; + status = &motorC; + break; + case 'D': + in1Pin = MOTOR_D_BIN1; + in2Pin = MOTOR_D_BIN2; + status = &motorD; + break; + } + if (speed > 0) { digitalWrite(in1Pin, HIGH); digitalWrite(in2Pin, LOW); + if (motor == 'A' || motor == 'D') + { + status->in1 = true; + status->in2 = false; + } + else + { + status->in1 = false; + status->in2 = true; + } } else if (speed < 0) { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, HIGH); + if (motor == 'A' || motor == 'D') + { + status->in1 = false; + status->in2 = true; + } + else + { + status->in1 = true; + status->in2 = false; + } + speed = -speed; } else { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, LOW); + status->in1 = false; + status->in2 = false; } - analogWrite(pwmPin, speed); + pwmControl(motor, speed); } float getDistance() @@ -169,7 +266,7 @@ bool CarStorage::isMounted = false; void setup() { - // 初始化串口 + // 初始化���口 Serial.begin(115200); // 初始化 BLE @@ -228,7 +325,7 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte uint8_t packetLength = packet[1]; // 获取长度 uint8_t cmd = packet[2]; // 获取指令 - uint8_t direction, speed, time, wheel; + uint8_t direction, speed, time, wheel, diffSpeed; float distance; uint8_t buffer[PACKET_MAX_LENGTH]; // 用于存储要发送的数据 int bufferIndex = 0; @@ -279,26 +376,44 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte // 移动 if (direction == 0x00) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 0); - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2, 0); - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2, 0); - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 0); + motorControl('A', 0); + motorControl('B', 0); + motorControl('C', 0); + motorControl('D', 0); } // 前进 else if (direction == 0x01) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, speed); - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, speed); - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, speed); - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, speed); + motorControl('A', speed); + motorControl('B', speed); + motorControl('C', speed); + motorControl('D', speed); } // 后退 else if (direction == 0x02) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -speed); - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -speed); - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -speed); - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -speed); + motorControl('A', -speed); + motorControl('B', -speed); + motorControl('C', -speed); + motorControl('D', -speed); + } + break; + + case CMD_MOTOR_STEER_CONTROL: + direction = packet[3]; + diffSpeed = packet[4]; + Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed)); + // 左转 + if (direction == 0x00) + { + motorControl('A', motorA.pwm - diffSpeed); + motorControl('B', motorB.pwm - diffSpeed); + } + // 右转 + else if (direction == 0x01) + { + motorControl('C', motorC.pwm - diffSpeed); + motorControl('D', motorD.pwm - diffSpeed); } break; @@ -309,18 +424,18 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte // 顺时针 if (direction == 0x00) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 255); - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, 255); - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -255); - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -255); + motorControl('A', 255); + motorControl('B', 255); + motorControl('C', -255); + motorControl('D', -255); } // 逆时针 else if (direction == 0x01) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -255); - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -255); - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, 255); - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 255); + motorControl('A', -255); + motorControl('B', -255); + motorControl('C', 255); + motorControl('D', 255); } break; @@ -333,16 +448,16 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte switch (wheel) { case 0x00: - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, speed); + motorControl('A', speed); break; case 0x01: - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, speed); + motorControl('B', speed); break; case 0x02: - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, speed); + motorControl('C', speed); break; case 0x03: - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, speed); + motorControl('D', speed); break; } break; @@ -350,12 +465,115 @@ void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characte case CMD_DEMO_PID: Serial.println("CMD_DEMO_PID"); - // // 获取 PID 参数 - // unsigned int sensitivity = CarStorage::getSensitivity(); - // Serial.println("CMD_DEMO_PID, sensitivity: " + String(sensitivity)); + // PID 参数 + float Kp = 2.0; + float Ki = 0.5; + float Kd = 0.1; - // // 设置 PID 参数 - // CarStorage::setSensitivity(sensitivity + 1); + // PID 变量 + float targetSpeed = 255; // 目标速度 + float currentSpeed = 0; // 当前速度 + float lastError = 0; // 上一次误差 + float integral = 0; // 积分项 + unsigned long lastTime = millis(); + + // 加速阶段 - PID控制 + while (millis() - lastTime < 2000) + { // 2秒加速过程 + float error = targetSpeed - currentSpeed; + integral += error; + float derivative = error - lastError; + + float output = Kp * error + Ki * integral + Kd * derivative; + output = constrain(output, 0, 255); // 限制输出范围 + + currentSpeed = output; + + // 应用到所有电机 + motorControl('A', output); + motorControl('B', output); + motorControl('C', output); + motorControl('D', output); + + lastError = error; + delay(10); // 控制周期 + } + + // 停止阶段 - PID控制 + lastTime = millis(); + targetSpeed = 0; // 目标速度设为0 + + while (millis() - lastTime < 1000) + { // 1秒减速过程 + float error = targetSpeed - currentSpeed; + integral += error; + float derivative = error - lastError; + + float output = Kp * error + Ki * integral + Kd * derivative; + output = constrain(output, 0, 255); + + currentSpeed = output; + + // 应用到所有电机 + motorControl('A', output); + motorControl('B', output); + motorControl('C', output); + motorControl('D', output); + + lastError = error; + delay(10); + } + + // 确保完全停止 + motorControl('A', 0); + motorControl('B', 0); + motorControl('C', 0); + motorControl('D', 0); + break; + + case CMD_DEMO_PATH: + Serial.println("CMD_DEMO_PATH"); + + // 第一段：全速前进1秒 + currentSpeed = 255; + unsigned long startTime = millis(); + while (millis() - startTime < 1000) + { + motorControl('A', currentSpeed); + motorControl('B', currentSpeed); + motorControl('C', currentSpeed); + motorControl('D', currentSpeed); + delay(10); + } + + // 第二段：左转90度 + startTime = millis(); + while (millis() - startTime < 500) + { // 假设500ms可以转90度 + // 左侧电机反转,右侧电机正转 + motorControl('A', 200); + motorControl('B', 200); + motorControl('C', 200); + motorControl('D', 200); + delay(10); + } + + // 第三段：全速前进1秒 + startTime = millis(); + while (millis() - startTime < 1000) + { + motorControl('A', currentSpeed); + motorControl('B', currentSpeed); + motorControl('C', currentSpeed); + motorControl('D', currentSpeed); + delay(10); + } + + // 最后停止所有电机 + motorControl('A', 0); + motorControl('B', 0); + motorControl('C', 0); + motorControl('D', 0); break; @@ -423,7 +641,36 @@ void updateStatusLED() } } +void sendMotorStatus() +{ + unsigned long currentMillis = millis(); + if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL) + { + lastMotorStatusUpdate = currentMillis; + + uint8_t buffer[PACKET_MAX_LENGTH]; + buffer[0] = PACKET_T_HEAD; + buffer[1] = 0x0C; + buffer[2] = CMD_STATUS_MOTOR; + buffer[3] = (motorA.in2 << 1) | motorA.in1; + buffer[4] = motorA.pwm; + buffer[5] = (motorB.in2 << 1) | motorB.in1; + buffer[6] = motorB.pwm; + buffer[7] = (motorC.in2 << 1) | motorC.in1; + buffer[8] = motorC.pwm; + buffer[9] = (motorD.in2 << 1) | motorD.in1; + buffer[10] = motorD.pwm; + buffer[11] = PACKET_T_TAIL; + pTxCharacteristic->setValue(buffer, 12); + pTxCharacteristic->notify(); + } +} + void loop() { updateStatusLED(); + if (deviceConnected) + { + sendMotorStatus(); + } }