146 lines
3.9 KiB
C++
146 lines
3.9 KiB
C++
#include "transmit.h"
|
|
|
|
#define MOTOR_STATUS_INTERVAL 200 // 发送电机状态的时间间隔(ms)
|
|
unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间
|
|
|
|
// 数据包缓冲区
|
|
uint8_t packetBuffer[PACKET_MAX_LENGTH];
|
|
int packetIndex = 0;
|
|
bool isReceivingPacket = false;
|
|
|
|
void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic)
|
|
{
|
|
static uint8_t expectedLength = 0;
|
|
|
|
packetIndex++;
|
|
|
|
if (incomingByte == PACKET_R_HEAD && !isReceivingPacket)
|
|
{
|
|
isReceivingPacket = true;
|
|
packetIndex = 0;
|
|
packetBuffer[packetIndex] = incomingByte;
|
|
}
|
|
else if (isReceivingPacket)
|
|
{
|
|
if (packetIndex < PACKET_MAX_LENGTH)
|
|
{
|
|
packetBuffer[packetIndex] = incomingByte;
|
|
|
|
// 第二个字节是包体长度
|
|
if (packetIndex == 1)
|
|
{
|
|
expectedLength = incomingByte;
|
|
}
|
|
|
|
// 到预期长度时检查包尾
|
|
if (packetIndex == expectedLength - 1)
|
|
{
|
|
isReceivingPacket = false;
|
|
if (incomingByte == PACKET_R_TAIL)
|
|
{
|
|
handleSerialPacket(packetBuffer, packetIndex + 1, characteristic);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
isReceivingPacket = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic)
|
|
{
|
|
if (length < 4)
|
|
return;
|
|
|
|
uint8_t cmd = packet[2];
|
|
|
|
switch (cmd)
|
|
{
|
|
case CMD_GET_BT_STATUS:
|
|
Handlers::getBTStatus(characteristic);
|
|
break;
|
|
|
|
case CMD_GET_SPIFFS_STATUS:
|
|
Handlers::getSPIFFSStatus(characteristic);
|
|
break;
|
|
|
|
case CMD_GET_DISTANCE:
|
|
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;
|
|
|
|
case CMD_MOTOR_STEER_CONTROL:
|
|
Handlers::motorSteerControl(characteristic, packet);
|
|
break;
|
|
|
|
case CMD_MOTOR_ROTATE_CONTROL:
|
|
Handlers::motorRotateControl(characteristic, packet);
|
|
break;
|
|
|
|
case CMD_MOTOR_SINGLE_CONTROL:
|
|
Handlers::motorSingleControl(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()
|
|
{
|
|
unsigned long currentMillis = millis();
|
|
if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
|
|
{
|
|
lastMotorStatusUpdate = currentMillis;
|
|
|
|
uint8_t buffer[PACKET_MAX_LENGTH];
|
|
uint8_t bufferIndex = 0;
|
|
buffer[bufferIndex++] = PACKET_T_HEAD;
|
|
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;
|
|
buffer[bufferIndex++] = MotorController::getMotorStatus('B').pwm;
|
|
buffer[bufferIndex++] = (MotorController::getMotorStatus('C').in2 << 1) | MotorController::getMotorStatus('C').in1;
|
|
buffer[bufferIndex++] = MotorController::getMotorStatus('C').pwm;
|
|
buffer[bufferIndex++] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1;
|
|
buffer[bufferIndex++] = MotorController::getMotorStatus('D').pwm;
|
|
buffer[bufferIndex++] = IR::data.length;
|
|
uint16_t irData = 0;
|
|
for (uint8_t i = 0; i < IR::data.length; i++)
|
|
{
|
|
irData |= (IR::data.data[i] << i);
|
|
}
|
|
buffer[bufferIndex++] = irData;
|
|
buffer[bufferIndex++] = PACKET_T_TAIL;
|
|
BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex);
|
|
BLEManager::pTxCharacteristic->notify();
|
|
}
|
|
} |