feat: status push
This commit is contained in:
玖叁
parent
e3ea57f10e
commit
acbb0afd13
29
packet.md
29
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`
|
||||
|
|
|
|||
317
src/main.cpp
317
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()
|
||||
{
|
||||
// 初始化串口
|
||||
// 初始化<EFBFBD><EFBFBD><EFBFBD>口
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue