334 lines
8.6 KiB
C++
334 lines
8.6 KiB
C++
#include <Arduino.h>
|
||
#include <BLEDevice.h>
|
||
#include <BLEServer.h>
|
||
#include <BLEUtils.h>
|
||
#include <BLE2902.h>
|
||
#include "consts.h"
|
||
|
||
// 使用 2 片 TB6612 控制 4 个电机
|
||
// 使用差速控制转向
|
||
|
||
#define PACKET_R_HEAD 0x00
|
||
#define PACKET_R_TAIL 0xFF
|
||
#define PACKET_T_HEAD 0x01
|
||
#define PACKET_T_TAIL 0xFE
|
||
#define PACKET_MAX_LENGTH 32 // 数据包最大长度
|
||
|
||
// 指令定义
|
||
#define CMD_GET_BT_STATUS 0x10
|
||
#define CMD_GET_DISTANCE 0x11
|
||
#define CMD_MOTOR_MOVE_CONTROL 0x20
|
||
#define CMD_MOTOR_ROTATE_CONTROL 0x21
|
||
|
||
// 全局变量
|
||
int currentSpeed = 0; // 当前速度
|
||
int turnOffset = 0; // 转向偏移量 (-100 到 100)
|
||
bool isMoving = false; // 运动状态
|
||
bool isTurning = false; // 转向状态
|
||
|
||
// BLE 相关
|
||
BLEServer *pServer = nullptr;
|
||
BLECharacteristic *pTxCharacteristic = nullptr;
|
||
bool deviceConnected = false;
|
||
|
||
// 数据包缓冲区
|
||
uint8_t packetBuffer[PACKET_MAX_LENGTH];
|
||
int packetIndex = 0;
|
||
bool isReceivingPacket = false;
|
||
|
||
// 在全局变量定义区域添加
|
||
#define LED_FLASH_INTERVAL 1000 // LED闪烁间隔(ms)
|
||
unsigned long lastLedToggle = 0; // 上次LED切换状态的时间
|
||
bool ledState = false; // LED当前状态
|
||
|
||
void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic);
|
||
|
||
class CarBLEServerCallbacks : public BLEServerCallbacks
|
||
{
|
||
void onConnect(BLEServer *pServer)
|
||
{
|
||
deviceConnected = true;
|
||
};
|
||
|
||
void onDisconnect(BLEServer *pServer)
|
||
{
|
||
deviceConnected = false;
|
||
// 重新开始广播
|
||
pServer->getAdvertising()->start();
|
||
}
|
||
};
|
||
|
||
class CarBLECharacteristicCallbacks : public BLECharacteristicCallbacks
|
||
{
|
||
void onWrite(BLECharacteristic *pCharacteristic)
|
||
{
|
||
std::string rxValue = pCharacteristic->getValue();
|
||
if (rxValue.length() > 0)
|
||
{
|
||
for (int i = 0; i < rxValue.length(); i++)
|
||
{
|
||
processSerialIncomingByte((uint8_t)rxValue[i], *pTxCharacteristic);
|
||
}
|
||
}
|
||
}
|
||
};
|
||
|
||
void floatToBytes(float val, uint8_t *bytes)
|
||
{
|
||
union
|
||
{
|
||
float f;
|
||
uint8_t bytes[4];
|
||
} u;
|
||
u.f = val;
|
||
|
||
// 考虑大小端问题
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
bytes[i] = u.bytes[i];
|
||
}
|
||
}
|
||
|
||
// 电机控制函数
|
||
void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed)
|
||
{
|
||
if (speed > 0)
|
||
{
|
||
digitalWrite(in1Pin, HIGH);
|
||
digitalWrite(in2Pin, LOW);
|
||
}
|
||
else if (speed < 0)
|
||
{
|
||
digitalWrite(in1Pin, LOW);
|
||
digitalWrite(in2Pin, HIGH);
|
||
speed = -speed;
|
||
}
|
||
else
|
||
{
|
||
digitalWrite(in1Pin, LOW);
|
||
digitalWrite(in2Pin, LOW);
|
||
}
|
||
analogWrite(pwmPin, speed);
|
||
}
|
||
|
||
float getDistance()
|
||
{
|
||
digitalWrite(HC_SR04_TRIG, HIGH);
|
||
delayMicroseconds(1);
|
||
digitalWrite(HC_SR04_TRIG, LOW);
|
||
float distance = pulseIn(HC_SR04_ECHO, HIGH); // 计数接收高电平时间
|
||
distance = distance * 340 / 2 / 10000; // 计算距离 1：声速：340M/S 2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度）
|
||
return distance;
|
||
}
|
||
|
||
void setup()
|
||
{
|
||
// 初始化串口
|
||
Serial.begin(115200);
|
||
|
||
// 初始化 BLE
|
||
BLEDevice::init(DEVICE_NAME);
|
||
pServer = BLEDevice::createServer();
|
||
pServer->setCallbacks(new CarBLEServerCallbacks());
|
||
|
||
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||
|
||
// 创建 RX 特征值 (用于接收数据)
|
||
BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
|
||
CHARACTERISTIC_UUID_RX,
|
||
BLECharacteristic::PROPERTY_WRITE);
|
||
pRxCharacteristic->setCallbacks(new CarBLECharacteristicCallbacks());
|
||
|
||
// 创建 TX 特征值 (用于发送数据)
|
||
pTxCharacteristic = pService->createCharacteristic(
|
||
CHARACTERISTIC_UUID_TX,
|
||
BLECharacteristic::PROPERTY_NOTIFY);
|
||
pTxCharacteristic->addDescriptor(new BLE2902());
|
||
|
||
pService->start();
|
||
pServer->getAdvertising()->start();
|
||
|
||
// 设置引脚模式
|
||
pinMode(STATUS_LED, OUTPUT);
|
||
digitalWrite(STATUS_LED, HIGH);
|
||
|
||
pinMode(MOTOR_A_PWMA, OUTPUT);
|
||
pinMode(MOTOR_A_AIN1, OUTPUT);
|
||
pinMode(MOTOR_A_AIN2, OUTPUT);
|
||
|
||
pinMode(MOTOR_B_PWMB, OUTPUT);
|
||
pinMode(MOTOR_B_BIN1, OUTPUT);
|
||
pinMode(MOTOR_B_BIN2, OUTPUT);
|
||
|
||
pinMode(MOTOR_C_PWMA, OUTPUT);
|
||
pinMode(MOTOR_C_AIN1, OUTPUT);
|
||
pinMode(MOTOR_C_AIN2, OUTPUT);
|
||
|
||
pinMode(MOTOR_D_PWMB, OUTPUT);
|
||
pinMode(MOTOR_D_BIN1, OUTPUT);
|
||
pinMode(MOTOR_D_BIN2, OUTPUT);
|
||
|
||
pinMode(HC_SR04_TRIG, OUTPUT);
|
||
pinMode(HC_SR04_ECHO, INPUT);
|
||
}
|
||
|
||
void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic)
|
||
{
|
||
if (!deviceConnected || length < 4)
|
||
return;
|
||
|
||
uint8_t packetLength = packet[1]; // 获取长度
|
||
uint8_t cmd = packet[2]; // 获取指令
|
||
uint8_t direction, speed, time;
|
||
float distance;
|
||
uint8_t buffer[PACKET_MAX_LENGTH]; // 用于存储要发送的数据
|
||
int bufferIndex = 0;
|
||
|
||
switch (cmd)
|
||
{
|
||
case CMD_GET_BT_STATUS:
|
||
Serial.println("CMD_GET_BT_STATUS");
|
||
// 构建响应数据
|
||
buffer[0] = PACKET_T_HEAD;
|
||
buffer[1] = 0x05;
|
||
buffer[2] = CMD_GET_BT_STATUS;
|
||
buffer[3] = (uint8_t)(deviceConnected ? 0x01 : 0x00);
|
||
buffer[4] = PACKET_T_TAIL;
|
||
characteristic.setValue(buffer, 5);
|
||
characteristic.notify();
|
||
break;
|
||
|
||
case CMD_GET_DISTANCE:
|
||
distance = getDistance();
|
||
Serial.println("CMD_GET_DISTANCE, distance: " + String(distance));
|
||
// 构建响应数据
|
||
buffer[0] = PACKET_T_HEAD;
|
||
buffer[1] = 0x08;
|
||
buffer[2] = CMD_GET_DISTANCE;
|
||
floatToBytes(distance, &buffer[3]);
|
||
buffer[7] = PACKET_T_TAIL;
|
||
characteristic.setValue(buffer, 8);
|
||
characteristic.notify();
|
||
break;
|
||
|
||
case CMD_MOTOR_MOVE_CONTROL:
|
||
direction = packet[3];
|
||
speed = packet[4];
|
||
Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed));
|
||
// 移动
|
||
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);
|
||
}
|
||
// 前进
|
||
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);
|
||
}
|
||
// 后退
|
||
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);
|
||
}
|
||
break;
|
||
|
||
case CMD_MOTOR_ROTATE_CONTROL:
|
||
direction = packet[3];
|
||
time = packet[4];
|
||
Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time));
|
||
// 顺时针
|
||
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);
|
||
}
|
||
// 逆时针
|
||
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);
|
||
}
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
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, *pTxCharacteristic);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
isReceivingPacket = false;
|
||
}
|
||
}
|
||
}
|
||
|
||
void updateStatusLED()
|
||
{
|
||
if (deviceConnected)
|
||
{
|
||
digitalWrite(STATUS_LED, HIGH);
|
||
}
|
||
else
|
||
{
|
||
unsigned long currentMillis = millis();
|
||
if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL)
|
||
{
|
||
lastLedToggle = currentMillis;
|
||
ledState = !ledState;
|
||
digitalWrite(STATUS_LED, ledState);
|
||
}
|
||
}
|
||
}
|
||
|
||
void loop()
|
||
{
|
||
updateStatusLED();
|
||
}
|