refactor: 规整格式化
This commit is contained in:
玖叁
parent
879f9e790d
commit
4ac26311f9
|
|
@ -3,6 +3,8 @@
|
|||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
src/consts.h
|
||||
.DS_Store
|
||||
src/consts.h
|
||||
src/consts-*.h
|
||||
include/consts.h
|
||||
include/consts-*.h
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ git clone https://github.com/colour93/esp32-car.git
|
|||
|
||||
### 打开项目并修改初始化设置
|
||||
|
||||
找到 `src/consts.example.h` 文件，复制一份并命名为 `src/consts.h`，根据实际情况，修改其中的内容。
|
||||
找到 `include/consts.example.h` 文件，复制一份并命名为 `include/consts.h`，根据实际情况，修改其中的内容。
|
||||
|
||||
### 上传
|
||||
|
||||
|
|
@ -33,7 +33,7 @@ git clone https://github.com/colour93/esp32-car.git
|
|||
|
||||
## BLE 串口通信协议
|
||||
|
||||
其他设备与 ESP32 通信采用 BLE 的 GATT 协议，其服务默认为 Nordic 串口服务，可以在 `src/consts.h` 中修改。
|
||||
其他设备与 ESP32 通信采用 BLE 的 GATT 协议，其服务默认为 Nordic 串口服务，可以在 `include/consts.h` 中修改。
|
||||
|
||||
```cpp
|
||||
# 服务与特征 UUID
|
||||
|
|
|
|||
|
|
@ -0,0 +1,22 @@
|
|||
#ifndef BLE_H
|
||||
#define BLE_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
#include <transmit.h>
|
||||
#include "consts.h"
|
||||
|
||||
class BLEManager
|
||||
{
|
||||
public:
|
||||
static bool deviceConnected;
|
||||
static BLEServer *pServer;
|
||||
static BLECharacteristic *pTxCharacteristic;
|
||||
|
||||
static void init(String deviceName);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#ifndef HANDLERS_H
|
||||
#define HANDLERS_H
|
||||
|
||||
#include "motor.h"
|
||||
#include "storage.h"
|
||||
#include "ble.h"
|
||||
#include "ultrasound.h"
|
||||
#include "transmit.h"
|
||||
#include "utils.h"
|
||||
|
||||
// 指令定义
|
||||
#define CMD_GET_BT_STATUS 0x10
|
||||
#define CMD_GET_SPIFFS_STATUS 0x11
|
||||
#define CMD_GET_DISTANCE 0x12
|
||||
#define CMD_MOTOR_MOVE_CONTROL 0x20
|
||||
#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_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令
|
||||
|
||||
#define CMD_STATUS_MOTOR 0xE0
|
||||
|
||||
class Handlers
|
||||
{
|
||||
public:
|
||||
static void getBTStatus(BLECharacteristic &characteristic);
|
||||
static void getSPIFFSStatus(BLECharacteristic &characteristic);
|
||||
static void getDistance(BLECharacteristic &characteristic);
|
||||
static void motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
static void motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
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 demoPID(BLECharacteristic &characteristic, uint8_t *packet);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
#ifndef MOTOR_H
|
||||
#define MOTOR_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
struct MotorPin
|
||||
{
|
||||
int pwm;
|
||||
int in1;
|
||||
int in2;
|
||||
};
|
||||
|
||||
// 电机状态结构体
|
||||
struct MotorStatus
|
||||
{
|
||||
MotorPin pin;
|
||||
bool in1;
|
||||
bool in2;
|
||||
unsigned char pwm;
|
||||
};
|
||||
|
||||
class MotorController
|
||||
{
|
||||
public:
|
||||
static void init(MotorPin aPin, MotorPin bPin, MotorPin cPin, MotorPin dPin);
|
||||
static void pwmControl(char motor, unsigned char pwm);
|
||||
static void motorControl(char motor, int speed);
|
||||
static void motorXYRControl(int8_t x, int8_t y, int8_t r);
|
||||
|
||||
// 获取电机状态
|
||||
static MotorStatus getMotorStatus(char motor);
|
||||
|
||||
private:
|
||||
static MotorStatus motorA;
|
||||
static MotorStatus motorB;
|
||||
static MotorStatus motorC;
|
||||
static MotorStatus motorD;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#ifndef STORAGE_H
|
||||
#define STORAGE_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SPIFFS.h>
|
||||
|
||||
class Storage
|
||||
{
|
||||
public:
|
||||
static bool isMounted;
|
||||
static void init();
|
||||
static void setPID(float kp, float ki, float kd);
|
||||
static void getPID(float &kp, float &ki, float &kd);
|
||||
static void setSensitivity(unsigned int sensitivity);
|
||||
static unsigned int getSensitivity();
|
||||
static void setName(String name);
|
||||
static String getName();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#ifndef TRANSMIT_H
|
||||
#define TRANSMIT_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLECharacteristic.h>
|
||||
#include "handlers.h"
|
||||
|
||||
#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 // 数据包最大长度
|
||||
|
||||
void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic);
|
||||
void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic);
|
||||
void sendStatus(BLECharacteristic &characteristic);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#ifndef ULTRASOUND_H
|
||||
#define ULTRASOUND_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
struct UltrasonicPin
|
||||
{
|
||||
int trig;
|
||||
int echo;
|
||||
};
|
||||
|
||||
struct Ultrasonic
|
||||
{
|
||||
static UltrasonicPin pin;
|
||||
static void init(UltrasonicPin pin);
|
||||
static float getDistance();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#ifndef UTILS_H
|
||||
#define UTILS_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
void floatToBytes(float val, uint8_t *bytes);
|
||||
void updateStatusLED(bool deviceConnected, int ledPin);
|
||||
#endif
|
||||
|
|
@ -8,7 +8,12 @@
|
|||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[env:esp32dev]
|
||||
[env:esp32]
|
||||
platform = espressif32
|
||||
board = esp32dev
|
||||
framework = arduino
|
||||
|
||||
[env:esp32s3]
|
||||
platform = espressif32
|
||||
board = adafruit_feather_esp32s3_tft
|
||||
framework = arduino
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
#include "ble.h"
|
||||
|
||||
bool BLEManager::deviceConnected = false;
|
||||
BLEServer* BLEManager::pServer = nullptr;
|
||||
BLECharacteristic* BLEManager::pTxCharacteristic = nullptr;
|
||||
|
||||
class BLEManagerServerCallbacks : public BLEServerCallbacks
|
||||
{
|
||||
void onConnect(BLEServer *pServer)
|
||||
{
|
||||
BLEManager::deviceConnected = true;
|
||||
};
|
||||
|
||||
void onDisconnect(BLEServer *pServer)
|
||||
{
|
||||
BLEManager::deviceConnected = false;
|
||||
// 重新开始广播
|
||||
pServer->getAdvertising()->start();
|
||||
}
|
||||
};
|
||||
|
||||
class BLEManagerCharacteristicCallbacks : 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], *BLEManager::pTxCharacteristic);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void BLEManager::init(String deviceName)
|
||||
{
|
||||
BLEDevice::init(deviceName.c_str());
|
||||
pServer = BLEDevice::createServer();
|
||||
pServer->setCallbacks(new BLEManagerServerCallbacks());
|
||||
|
||||
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||||
|
||||
// 创建 RX 特征值 (用于接收数据)
|
||||
BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID_RX,
|
||||
BLECharacteristic::PROPERTY_WRITE);
|
||||
pRxCharacteristic->setCallbacks(new BLEManagerCharacteristicCallbacks());
|
||||
|
||||
// 创建 TX 特征值 (用于发送数据)
|
||||
pTxCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID_TX,
|
||||
BLECharacteristic::PROPERTY_NOTIFY);
|
||||
pTxCharacteristic->addDescriptor(new BLE2902());
|
||||
|
||||
pService->start();
|
||||
pServer->getAdvertising()->start();
|
||||
}
|
||||
|
|
@ -0,0 +1,219 @@
|
|||
#include "handlers.h"
|
||||
|
||||
uint8_t buffer[PACKET_MAX_LENGTH];
|
||||
|
||||
void Handlers::getBTStatus(BLECharacteristic &characteristic)
|
||||
{
|
||||
Serial.println("CMD_GET_BT_STATUS");
|
||||
// 构建响应数据
|
||||
buffer[0] = PACKET_T_HEAD;
|
||||
buffer[1] = 0x05;
|
||||
buffer[2] = CMD_GET_BT_STATUS;
|
||||
buffer[3] = (uint8_t)(BLEManager::deviceConnected ? 0x01 : 0x00);
|
||||
buffer[4] = PACKET_T_TAIL;
|
||||
characteristic.setValue(buffer, 5);
|
||||
characteristic.notify();
|
||||
}
|
||||
|
||||
void Handlers::getSPIFFSStatus(BLECharacteristic &characteristic)
|
||||
{
|
||||
Serial.println("CMD_GET_SPIFFS_STATUS");
|
||||
// 构建响应数据
|
||||
buffer[0] = PACKET_T_HEAD;
|
||||
buffer[1] = 0x05;
|
||||
buffer[2] = CMD_GET_SPIFFS_STATUS;
|
||||
buffer[3] = (uint8_t)(Storage::isMounted ? 0x01 : 0x00);
|
||||
buffer[4] = PACKET_T_TAIL;
|
||||
characteristic.setValue(buffer, 5);
|
||||
characteristic.notify();
|
||||
}
|
||||
|
||||
void Handlers::getDistance(BLECharacteristic &characteristic)
|
||||
{
|
||||
float distance = Ultrasonic::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();
|
||||
}
|
||||
|
||||
void Handlers::motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t direction = packet[3];
|
||||
uint8_t speed = packet[4];
|
||||
Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed));
|
||||
// 移动
|
||||
if (direction == 0x00)
|
||||
{
|
||||
MotorController::motorControl('A', 0);
|
||||
MotorController::motorControl('B', 0);
|
||||
MotorController::motorControl('C', 0);
|
||||
MotorController::motorControl('D', 0);
|
||||
}
|
||||
// 前进
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
MotorController::motorControl('A', speed);
|
||||
MotorController::motorControl('B', speed);
|
||||
MotorController::motorControl('C', speed);
|
||||
MotorController::motorControl('D', speed);
|
||||
}
|
||||
// 后退
|
||||
else if (direction == 0x02)
|
||||
{
|
||||
MotorController::motorControl('A', -speed);
|
||||
MotorController::motorControl('B', -speed);
|
||||
MotorController::motorControl('C', -speed);
|
||||
MotorController::motorControl('D', -speed);
|
||||
}
|
||||
}
|
||||
|
||||
void Handlers::motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t direction = packet[3];
|
||||
uint8_t diffSpeed = packet[4];
|
||||
Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed));
|
||||
// 左转
|
||||
if (direction == 0x00)
|
||||
{
|
||||
MotorController::motorControl('A', MotorController::getMotorStatus('A').pwm - diffSpeed);
|
||||
MotorController::motorControl('B', MotorController::getMotorStatus('B').pwm - diffSpeed);
|
||||
}
|
||||
// 右转
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
MotorController::motorControl('C', MotorController::getMotorStatus('C').pwm - diffSpeed);
|
||||
MotorController::motorControl('D', MotorController::getMotorStatus('D').pwm - diffSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
void Handlers::motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t direction = packet[3];
|
||||
uint8_t time = packet[4];
|
||||
Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time));
|
||||
// 顺时针
|
||||
if (direction == 0x00)
|
||||
{
|
||||
MotorController::motorControl('A', 255);
|
||||
MotorController::motorControl('B', 255);
|
||||
MotorController::motorControl('C', -255);
|
||||
MotorController::motorControl('D', -255);
|
||||
}
|
||||
// 逆时针
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
MotorController::motorControl('A', -255);
|
||||
MotorController::motorControl('B', -255);
|
||||
MotorController::motorControl('C', 255);
|
||||
MotorController::motorControl('D', 255);
|
||||
}
|
||||
}
|
||||
|
||||
void Handlers::motorSingleControl(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
uint8_t wheel = packet[3];
|
||||
uint8_t direction = packet[4];
|
||||
uint8_t speed = packet[5];
|
||||
Serial.println("CMD_MOTOR_SINGLE_CONTROL, wheel: " + String(wheel) + ", direction: " + String(direction) + ", speed: " + String(speed));
|
||||
// 单轮控制
|
||||
switch (wheel)
|
||||
{
|
||||
case 0x00:
|
||||
MotorController::motorControl('A', speed);
|
||||
break;
|
||||
case 0x01:
|
||||
MotorController::motorControl('B', speed);
|
||||
break;
|
||||
case 0x02:
|
||||
MotorController::motorControl('C', speed);
|
||||
break;
|
||||
case 0x03:
|
||||
MotorController::motorControl('D', speed);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Handlers::motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
int8_t x = (int8_t)packet[3]; // X轴向速度
|
||||
int8_t y = (int8_t)packet[4]; // Y轴向速度
|
||||
int8_t r = (int8_t)packet[5]; // 旋转速度
|
||||
|
||||
Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r);
|
||||
MotorController::motorXYRControl(x, y, r);
|
||||
}
|
||||
|
||||
void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet)
|
||||
{
|
||||
Serial.println("CMD_DEMO_PID");
|
||||
|
||||
// PID 参数
|
||||
float Kp = 2.0;
|
||||
float Ki = 0.5;
|
||||
float Kd = 0.1;
|
||||
|
||||
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;
|
||||
|
||||
// 应用到所有电机
|
||||
MotorController::motorControl('A', output);
|
||||
MotorController::motorControl('B', output);
|
||||
MotorController::motorControl('C', output);
|
||||
MotorController::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;
|
||||
|
||||
// 应用到所有电机
|
||||
MotorController::motorControl('A', output);
|
||||
MotorController::motorControl('B', output);
|
||||
MotorController::motorControl('C', output);
|
||||
MotorController::motorControl('D', output);
|
||||
|
||||
lastError = error;
|
||||
delay(10);
|
||||
}
|
||||
|
||||
// 确保完全停止
|
||||
MotorController::motorControl('A', 0);
|
||||
MotorController::motorControl('B', 0);
|
||||
MotorController::motorControl('C', 0);
|
||||
MotorController::motorControl('D', 0);
|
||||
}
|
||||
724
src/main.cpp
724
src/main.cpp
|
|
@ -1,736 +1,44 @@
|
|||
#include <Arduino.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
#include <SPIFFS.h>
|
||||
#include <motor.h>
|
||||
#include <utils.h>
|
||||
#include <storage.h>
|
||||
#include <ble.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_SPIFFS_STATUS 0x11
|
||||
#define CMD_GET_DISTANCE 0x12
|
||||
#define CMD_MOTOR_MOVE_CONTROL 0x20
|
||||
#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_MOTOR_XYR_CONTROL 0x24 // XYR轴向控制指令
|
||||
|
||||
#define CMD_STATUS_MOTOR 0xE0
|
||||
|
||||
// 全局变量
|
||||
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当前状态
|
||||
|
||||
#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
|
||||
{
|
||||
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 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;
|
||||
}
|
||||
pwmControl(motor, speed);
|
||||
}
|
||||
|
||||
void motorXYRControl(int8_t x, int8_t y, int8_t r) {
|
||||
// 将输入范围限制在 -100 到 100
|
||||
x = constrain(x, -100, 100);
|
||||
y = constrain(y, -100, 100);
|
||||
r = constrain(r, -100, 100);
|
||||
|
||||
// 将 -100~100 映射到 -255~255
|
||||
int16_t mappedX = map(x, -100, 100, -255, 255);
|
||||
int16_t mappedY = map(y, -100, 100, -255, 255);
|
||||
int16_t mappedR = map(r, -100, 100, -255, 255);
|
||||
|
||||
// 麦克纳姆轮运动学方程
|
||||
// 左前轮 = Y + X + R
|
||||
// 右前轮 = Y - X - R
|
||||
// 右后轮 = Y + X - R
|
||||
// 左后轮 = Y - X + R
|
||||
int16_t speedA = mappedY + mappedX + mappedR; // 左前轮
|
||||
int16_t speedB = mappedY - mappedX - mappedR; // 右前轮
|
||||
int16_t speedC = mappedY + mappedX - mappedR; // 右后轮
|
||||
int16_t speedD = mappedY - mappedX + mappedR; // 左后轮
|
||||
|
||||
// 找出最大速度的绝对值
|
||||
int16_t maxSpeed = max(max(abs(speedA), abs(speedB)),
|
||||
max(abs(speedC), abs(speedD)));
|
||||
|
||||
// 如果最大速度超过255，等比例缩放所有速度
|
||||
if (maxSpeed > 255) {
|
||||
float scale = 255.0f / maxSpeed;
|
||||
speedA *= scale;
|
||||
speedB *= scale;
|
||||
speedC *= scale;
|
||||
speedD *= scale;
|
||||
}
|
||||
|
||||
// 控制各个电机
|
||||
motorControl('A', speedA);
|
||||
motorControl('B', speedB);
|
||||
motorControl('C', speedC);
|
||||
motorControl('D', speedD);
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
class CarStorage
|
||||
{
|
||||
public:
|
||||
static bool isMounted;
|
||||
|
||||
static void init()
|
||||
{
|
||||
if (!SPIFFS.begin(true))
|
||||
{
|
||||
Serial.println("SPIFFS Mount Failed");
|
||||
isMounted = false;
|
||||
}
|
||||
else
|
||||
isMounted = true;
|
||||
}
|
||||
|
||||
static unsigned int getSensitivity()
|
||||
{
|
||||
if (!isMounted)
|
||||
return 0xFF;
|
||||
|
||||
File file = SPIFFS.open("/sensitivity.txt", "r");
|
||||
if (!file)
|
||||
return 0xFF;
|
||||
|
||||
String sensitivity = file.readStringUntil('\n');
|
||||
file.close();
|
||||
|
||||
return sensitivity.toInt();
|
||||
}
|
||||
|
||||
static void setSensitivity(unsigned int sensitivity)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/sensitivity.txt", "w");
|
||||
file.println(sensitivity);
|
||||
file.close();
|
||||
}
|
||||
};
|
||||
bool CarStorage::isMounted = false;
|
||||
|
||||
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();
|
||||
BLEManager::init(DEVICE_NAME);
|
||||
|
||||
// 初始化 EEPROM
|
||||
CarStorage::init();
|
||||
Storage::init();
|
||||
|
||||
// 初始化电机
|
||||
MotorController::init(
|
||||
{MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2},
|
||||
{MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2},
|
||||
{MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2},
|
||||
{MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2});
|
||||
|
||||
// 设置引脚模式
|
||||
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, wheel, diffSpeed;
|
||||
float distance;
|
||||
uint8_t buffer[PACKET_MAX_LENGTH];
|
||||
int bufferIndex = 0;
|
||||
|
||||
// PID 相关变量声明移到这里
|
||||
float Kp, Ki, Kd;
|
||||
float targetSpeed, currentSpeed, lastError, integral;
|
||||
unsigned long lastTime;
|
||||
unsigned long startTime;
|
||||
|
||||
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_SPIFFS_STATUS:
|
||||
Serial.println("CMD_GET_SPIFFS_STATUS");
|
||||
// 构建响应数据
|
||||
buffer[0] = PACKET_T_HEAD;
|
||||
buffer[1] = 0x05;
|
||||
buffer[2] = CMD_GET_SPIFFS_STATUS;
|
||||
buffer[3] = (uint8_t)(CarStorage::isMounted ? 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('A', 0);
|
||||
motorControl('B', 0);
|
||||
motorControl('C', 0);
|
||||
motorControl('D', 0);
|
||||
}
|
||||
// 前进
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
motorControl('A', speed);
|
||||
motorControl('B', speed);
|
||||
motorControl('C', speed);
|
||||
motorControl('D', speed);
|
||||
}
|
||||
// 后退
|
||||
else if (direction == 0x02)
|
||||
{
|
||||
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;
|
||||
|
||||
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('A', 255);
|
||||
motorControl('B', 255);
|
||||
motorControl('C', -255);
|
||||
motorControl('D', -255);
|
||||
}
|
||||
// 逆时针
|
||||
else if (direction == 0x01)
|
||||
{
|
||||
motorControl('A', -255);
|
||||
motorControl('B', -255);
|
||||
motorControl('C', 255);
|
||||
motorControl('D', 255);
|
||||
}
|
||||
break;
|
||||
|
||||
case CMD_MOTOR_SINGLE_CONTROL:
|
||||
wheel = packet[3];
|
||||
direction = packet[4];
|
||||
speed = packet[5];
|
||||
Serial.println("CMD_MOTOR_SINGLE_CONTROL, wheel: " + String(wheel) + ", direction: " + String(direction) + ", speed: " + String(speed));
|
||||
// 单轮控制
|
||||
switch (wheel)
|
||||
{
|
||||
case 0x00:
|
||||
motorControl('A', speed);
|
||||
break;
|
||||
case 0x01:
|
||||
motorControl('B', speed);
|
||||
break;
|
||||
case 0x02:
|
||||
motorControl('C', speed);
|
||||
break;
|
||||
case 0x03:
|
||||
motorControl('D', speed);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case CMD_DEMO_PID:
|
||||
Serial.println("CMD_DEMO_PID");
|
||||
|
||||
// PID 参数
|
||||
Kp = 2.0;
|
||||
Ki = 0.5;
|
||||
Kd = 0.1;
|
||||
|
||||
targetSpeed = 255;
|
||||
currentSpeed = 0;
|
||||
lastError = 0;
|
||||
integral = 0;
|
||||
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;
|
||||
startTime = millis();
|
||||
// 第一段：全速前进1秒
|
||||
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;
|
||||
|
||||
case CMD_MOTOR_XYR_CONTROL:
|
||||
if (length >= 7) { // 确保数据包长度正确
|
||||
int8_t x = (int8_t)packet[3]; // X轴向速度
|
||||
int8_t y = (int8_t)packet[4]; // Y轴向速度
|
||||
int8_t r = (int8_t)packet[5]; // 旋转速度
|
||||
|
||||
Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r);
|
||||
motorXYRControl(x, y, r);
|
||||
}
|
||||
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 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)
|
||||
updateStatusLED(BLEManager::deviceConnected, STATUS_LED);
|
||||
if (BLEManager::deviceConnected)
|
||||
{
|
||||
sendMotorStatus();
|
||||
sendStatus(*BLEManager::pTxCharacteristic);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
#include "motor.h"
|
||||
|
||||
|
||||
// 静态成员初始化
|
||||
MotorStatus MotorController::motorA = {{0, 0, 0}, false, false, 0};
|
||||
MotorStatus MotorController::motorB = {{0, 0, 0}, false, false, 0};
|
||||
MotorStatus MotorController::motorC = {{0, 0, 0}, false, false, 0};
|
||||
MotorStatus MotorController::motorD = {{0, 0, 0}, false, false, 0};
|
||||
|
||||
void MotorController::init(
|
||||
MotorPin aPin,
|
||||
MotorPin bPin,
|
||||
MotorPin cPin,
|
||||
MotorPin dPin)
|
||||
{
|
||||
motorA.pin = aPin;
|
||||
motorB.pin = bPin;
|
||||
motorC.pin = cPin;
|
||||
motorD.pin = dPin;
|
||||
|
||||
pinMode(aPin.pwm, OUTPUT);
|
||||
pinMode(aPin.in1, OUTPUT);
|
||||
pinMode(aPin.in2, OUTPUT);
|
||||
|
||||
pinMode(bPin.pwm, OUTPUT);
|
||||
pinMode(bPin.in1, OUTPUT);
|
||||
pinMode(bPin.in2, OUTPUT);
|
||||
|
||||
pinMode(cPin.pwm, OUTPUT);
|
||||
pinMode(cPin.in1, OUTPUT);
|
||||
pinMode(cPin.in2, OUTPUT);
|
||||
|
||||
pinMode(dPin.pwm, OUTPUT);
|
||||
pinMode(dPin.in1, OUTPUT);
|
||||
pinMode(dPin.in2, OUTPUT);
|
||||
}
|
||||
|
||||
void MotorController::pwmControl(char motor, unsigned char pwm)
|
||||
{
|
||||
int pwmPin;
|
||||
switch (motor)
|
||||
{
|
||||
case 'A':
|
||||
pwmPin = motorA.pin.pwm;
|
||||
motorA.pwm = pwm;
|
||||
break;
|
||||
case 'B':
|
||||
pwmPin = motorB.pin.pwm;
|
||||
motorB.pwm = pwm;
|
||||
break;
|
||||
case 'C':
|
||||
pwmPin = motorC.pin.pwm;
|
||||
motorC.pwm = pwm;
|
||||
break;
|
||||
case 'D':
|
||||
pwmPin = motorD.pin.pwm;
|
||||
motorD.pwm = pwm;
|
||||
break;
|
||||
}
|
||||
analogWrite(pwmPin, pwm);
|
||||
}
|
||||
|
||||
// 电机控制函数
|
||||
void MotorController::motorControl(char motor, int speed)
|
||||
{
|
||||
int in1Pin, in2Pin;
|
||||
MotorStatus *status;
|
||||
|
||||
switch (motor)
|
||||
{
|
||||
case 'A':
|
||||
in1Pin = motorA.pin.in1;
|
||||
in2Pin = motorA.pin.in2;
|
||||
status = &motorA;
|
||||
break;
|
||||
case 'B':
|
||||
in1Pin = motorB.pin.in2;
|
||||
in2Pin = motorB.pin.in1;
|
||||
status = &motorB;
|
||||
break;
|
||||
case 'C':
|
||||
in1Pin = motorC.pin.in2;
|
||||
in2Pin = motorC.pin.in1;
|
||||
status = &motorC;
|
||||
break;
|
||||
case 'D':
|
||||
in1Pin = motorD.pin.in1;
|
||||
in2Pin = motorD.pin.in2;
|
||||
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;
|
||||
}
|
||||
pwmControl(motor, speed);
|
||||
}
|
||||
|
||||
void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r)
|
||||
{
|
||||
// 将输入范围限制在 -100 到 100
|
||||
x = constrain(x, -100, 100);
|
||||
y = constrain(y, -100, 100);
|
||||
r = constrain(r, -100, 100);
|
||||
|
||||
// 将 -100~100 映射到 -255~255
|
||||
int16_t mappedX = map(x, -100, 100, -255, 255);
|
||||
int16_t mappedY = map(y, -100, 100, -255, 255);
|
||||
int16_t mappedR = map(r, -100, 100, -255, 255);
|
||||
|
||||
// 麦克纳姆轮运动学方程
|
||||
// 左前轮 = Y + X + R
|
||||
// 右前轮 = Y - X - R
|
||||
// 右后轮 = Y + X - R
|
||||
// 左后轮 = Y - X + R
|
||||
int16_t speedA = mappedY + mappedX + mappedR; // 左前轮
|
||||
int16_t speedB = mappedY - mappedX - mappedR; // 右前轮
|
||||
int16_t speedC = mappedY + mappedX - mappedR; // 右后轮
|
||||
int16_t speedD = mappedY - mappedX + mappedR; // 左后轮
|
||||
|
||||
// 找出最大速度的绝对值
|
||||
int16_t maxSpeed = max(max(abs(speedA), abs(speedB)),
|
||||
max(abs(speedC), abs(speedD)));
|
||||
|
||||
// 如果最大速度超过255，等比例缩放所有速度
|
||||
if (maxSpeed > 255)
|
||||
{
|
||||
float scale = 255.0f / maxSpeed;
|
||||
speedA *= scale;
|
||||
speedB *= scale;
|
||||
speedC *= scale;
|
||||
speedD *= scale;
|
||||
}
|
||||
|
||||
// 控制各个电机
|
||||
motorControl('A', speedA);
|
||||
motorControl('B', speedB);
|
||||
motorControl('C', speedC);
|
||||
motorControl('D', speedD);
|
||||
}
|
||||
|
||||
MotorStatus MotorController::getMotorStatus(char motor)
|
||||
{
|
||||
switch (motor)
|
||||
{
|
||||
case 'A':
|
||||
return motorA;
|
||||
case 'B':
|
||||
return motorB;
|
||||
case 'C':
|
||||
return motorC;
|
||||
case 'D':
|
||||
return motorD;
|
||||
default:
|
||||
return {false, false, 0};
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
#include "storage.h"
|
||||
|
||||
bool Storage::isMounted;
|
||||
|
||||
void Storage::init()
|
||||
{
|
||||
if (!SPIFFS.begin(true))
|
||||
{
|
||||
Serial.println("SPIFFS Mount Failed");
|
||||
isMounted = false;
|
||||
}
|
||||
else
|
||||
isMounted = true;
|
||||
}
|
||||
|
||||
unsigned int Storage::getSensitivity()
|
||||
{
|
||||
if (!isMounted)
|
||||
return 0xFF;
|
||||
|
||||
File file = SPIFFS.open("/sensitivity.txt", "r");
|
||||
if (!file)
|
||||
return 0xFF;
|
||||
|
||||
String sensitivity = file.readStringUntil('\n');
|
||||
file.close();
|
||||
|
||||
return sensitivity.toInt();
|
||||
}
|
||||
|
||||
void Storage::setSensitivity(unsigned int sensitivity)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/sensitivity.txt", "w");
|
||||
file.println(sensitivity);
|
||||
file.close();
|
||||
}
|
||||
|
||||
void Storage::setPID(float kp, float ki, float kd)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/pid.txt", "w");
|
||||
file.println(kp);
|
||||
file.println(ki);
|
||||
file.println(kd);
|
||||
file.close();
|
||||
}
|
||||
|
||||
void Storage::getPID(float &kp, float &ki, float &kd)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/pid.txt", "r");
|
||||
kp = file.readStringUntil('\n').toFloat();
|
||||
ki = file.readStringUntil('\n').toFloat();
|
||||
kd = file.readStringUntil('\n').toFloat();
|
||||
file.close();
|
||||
}
|
||||
|
||||
void Storage::setName(String name)
|
||||
{
|
||||
if (!isMounted)
|
||||
return;
|
||||
|
||||
File file = SPIFFS.open("/name.txt", "w");
|
||||
file.println(name);
|
||||
file.close();
|
||||
}
|
||||
|
||||
String Storage::getName()
|
||||
{
|
||||
if (!isMounted)
|
||||
return "";
|
||||
|
||||
File file = SPIFFS.open("/name.txt", "r");
|
||||
String name = file.readStringUntil('\n');
|
||||
file.close();
|
||||
|
||||
return name;
|
||||
}
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
#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_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_DEMO_PID:
|
||||
Handlers::demoPID(characteristic, packet);
|
||||
break;
|
||||
|
||||
case CMD_MOTOR_XYR_CONTROL:
|
||||
Handlers::motorXYRControl(characteristic, packet);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void sendStatus(BLECharacteristic &characteristic)
|
||||
{
|
||||
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] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1;
|
||||
buffer[4] = MotorController::getMotorStatus('A').pwm;
|
||||
buffer[5] = (MotorController::getMotorStatus('B').in2 << 1) | MotorController::getMotorStatus('B').in1;
|
||||
buffer[6] = MotorController::getMotorStatus('B').pwm;
|
||||
buffer[7] = (MotorController::getMotorStatus('C').in2 << 1) | MotorController::getMotorStatus('C').in1;
|
||||
buffer[8] = MotorController::getMotorStatus('C').pwm;
|
||||
buffer[9] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1;
|
||||
buffer[10] = MotorController::getMotorStatus('D').pwm;
|
||||
buffer[11] = PACKET_T_TAIL;
|
||||
characteristic.setValue(buffer, 12);
|
||||
characteristic.notify();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#include "ultrasound.h"
|
||||
|
||||
UltrasonicPin Ultrasonic::pin = {0, 0};
|
||||
|
||||
void Ultrasonic::init(UltrasonicPin pin)
|
||||
{
|
||||
Ultrasonic::pin = pin;
|
||||
pinMode(pin.trig, OUTPUT);
|
||||
pinMode(pin.echo, INPUT);
|
||||
}
|
||||
|
||||
float Ultrasonic::getDistance()
|
||||
{
|
||||
digitalWrite(pin.trig, HIGH);
|
||||
delayMicroseconds(1);
|
||||
digitalWrite(pin.trig, LOW);
|
||||
float distance = pulseIn(pin.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;
|
||||
}
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#include "utils.h"
|
||||
|
||||
#define LED_FLASH_INTERVAL 1000 // LED闪烁间隔(ms)
|
||||
unsigned long lastLedToggle = 0; // 上次LED切换状态的时间
|
||||
bool ledState = false; // LED当前状态
|
||||
|
||||
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 updateStatusLED(bool deviceConnected, int ledPin)
|
||||
{
|
||||
if (deviceConnected)
|
||||
{
|
||||
digitalWrite(ledPin, HIGH);
|
||||
}
|
||||
else
|
||||
{
|
||||
unsigned long currentMillis = millis();
|
||||
if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL)
|
||||
{
|
||||
lastLedToggle = currentMillis;
|
||||
ledState = !ledState;
|
||||
digitalWrite(ledPin, ledState);
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue