346 lines
8.8 KiB
C++
346 lines
8.8 KiB
C++
#include <Arduino.h>
|
||
#include <BluetoothSerial.h>
|
||
|
||
#define STATUS_LED 2
|
||
|
||
#define HC_SR04_TRIG 23
|
||
#define HC_SR04_ECHO 22
|
||
|
||
// 使用 2 片 TB6612 控制 4 个电机
|
||
// 使用差速控制转向
|
||
|
||
// TB6612 #1 控制 A B 电机
|
||
// A 电机 左前轮
|
||
#define MOTOR_A_PWMA 21 // 电机 A PWM
|
||
#define MOTOR_A_AIN1 18 // 电机 A 方向控制1
|
||
#define MOTOR_A_AIN2 19 // 电机 A 方向控制2
|
||
|
||
// B 电机 左后轮
|
||
#define MOTOR_B_PWMB 5 // 电机 B PWM
|
||
#define MOTOR_B_BIN1 16 // 电机 B 方向控制1
|
||
#define MOTOR_B_BIN2 17 // 电机 B 方向控制2
|
||
|
||
// TB6612 #2 控制 C D 电机
|
||
// C 电机 右后轮
|
||
#define MOTOR_C_PWMA 4 // 电机 C PWM
|
||
#define MOTOR_C_AIN1 15 // 电机 C 方向控制1
|
||
#define MOTOR_C_AIN2 2 // 电机 C 方向控制2
|
||
|
||
// D 电机 右前轮
|
||
#define MOTOR_D_PWMB 32 // 电机 D PWM
|
||
#define MOTOR_D_BIN1 25 // 电机 D 方向控制1
|
||
#define MOTOR_D_BIN2 33 // 电机 D 方向控制2
|
||
|
||
// 运动控制参数
|
||
#define MAX_SPEED 255 // 最大速度
|
||
#define BASE_SPEED 200 // 基础速度
|
||
#define MIN_SPEED 50 // 最小速度
|
||
#define TURN_RATIO 0.6 // 转向时的速度比例
|
||
#define SPEED_INCREMENT 10 // 速度增量
|
||
|
||
// 全局变量
|
||
int currentSpeed = 0; // 当前速度
|
||
int turnOffset = 0; // 转向偏移量 (-100 到 100)
|
||
bool isMoving = false; // 运动状态
|
||
bool isTurning = false; // 转向状态
|
||
|
||
BluetoothSerial SerialBT;
|
||
|
||
// 在全局变量定义区域添加以下内容
|
||
#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
|
||
|
||
// 数据包缓冲区
|
||
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 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;
|
||
}
|
||
|
||
u_char getSerialBTStatus()
|
||
{
|
||
return SerialBT.connected() ? 0x01 : 0x00;
|
||
}
|
||
|
||
void setup()
|
||
{
|
||
// 初始化串口
|
||
Serial.begin(115200);
|
||
|
||
// 初始化蓝牙
|
||
SerialBT.begin("WhiteTiger");
|
||
|
||
// 设置引脚模式
|
||
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, Stream &serial)
|
||
{
|
||
if (length < 4)
|
||
return; // 最小包长度：包头(1) + 长度(1) + 指令(1) + 包尾(1)
|
||
|
||
uint8_t packetLength = packet[1]; // 获取长度
|
||
uint8_t cmd = packet[2]; // 获取指令
|
||
uint8_t direction, speed, time;
|
||
float distance;
|
||
|
||
uint8_t transmitBuffer[PACKET_MAX_LENGTH];
|
||
|
||
switch (cmd)
|
||
{
|
||
case CMD_GET_BT_STATUS:
|
||
Serial.println("CMD_GET_BT_STATUS");
|
||
// 回复状态
|
||
serial.write(PACKET_T_HEAD);
|
||
serial.write(0x05); // 包体长度
|
||
serial.write(CMD_GET_BT_STATUS);
|
||
serial.write(getSerialBTStatus());
|
||
serial.write(PACKET_T_TAIL);
|
||
break;
|
||
|
||
case CMD_GET_DISTANCE:
|
||
Serial.println("CMD_GET_DISTANCE");
|
||
distance = getDistance();
|
||
Serial.printf("distance = %f\n", distance);
|
||
floatToBytes(distance, transmitBuffer);
|
||
// 回复距离
|
||
serial.write(PACKET_T_HEAD);
|
||
serial.write(0x08); // 包体长度
|
||
serial.write(CMD_GET_DISTANCE);
|
||
serial.write(transmitBuffer, 4);
|
||
serial.write(PACKET_T_TAIL);
|
||
break;
|
||
|
||
case CMD_MOTOR_MOVE_CONTROL:
|
||
// 获取方向和速度
|
||
direction = packet[3];
|
||
speed = packet[4];
|
||
|
||
Serial.printf("CMD_MOTOR_MOVE_CONTROL: direction = 0x%02X, speed = %d\n", direction, 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);
|
||
}
|
||
|
||
// 前进
|
||
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);
|
||
}
|
||
|
||
// 后退
|
||
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.printf("CMD_MOTOR_ROTATE_CONTROL: direction = 0x%02X, time = %d\n", direction, 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); // 右前
|
||
}
|
||
|
||
// 逆时针
|
||
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, Stream &serial)
|
||
{
|
||
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, serial);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
isReceivingPacket = false;
|
||
}
|
||
}
|
||
}
|
||
|
||
void updateStatusLED()
|
||
{
|
||
if (SerialBT.connected())
|
||
{
|
||
// 蓝牙已连接，LED常亮
|
||
digitalWrite(STATUS_LED, HIGH);
|
||
}
|
||
else
|
||
{
|
||
// 蓝牙未连接，LED慢闪
|
||
unsigned long currentMillis = millis();
|
||
if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL)
|
||
{
|
||
lastLedToggle = currentMillis;
|
||
ledState = !ledState;
|
||
digitalWrite(STATUS_LED, ledState);
|
||
}
|
||
}
|
||
}
|
||
|
||
void loop()
|
||
{
|
||
if (SerialBT.available())
|
||
{
|
||
uint8_t incomingByte = SerialBT.read();
|
||
processSerialIncomingByte(incomingByte, SerialBT);
|
||
Serial.printf("BT Received: 0x%02X\n", incomingByte);
|
||
}
|
||
|
||
if (Serial.available())
|
||
{
|
||
uint8_t incomingByte = Serial.read();
|
||
processSerialIncomingByte(incomingByte, Serial);
|
||
Serial.printf("UART Received: 0x%02X\n", incomingByte);
|
||
}
|
||
|
||
updateStatusLED(); // 更新LED状态
|
||
}
|