diff --git a/.vscode/settings.json b/.vscode/settings.json index 6c8638f..88844b9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,5 +4,10 @@ "**/*.rpa": true, "**/*.rpymc": true, "**/cache/": true + }, + "files.associations": { + "*.toml": "toml", + "Comfyfile": "raw", + "*.tcc": "cpp" } } \ No newline at end of file diff --git a/packet.md b/packet.md new file mode 100644 index 0000000..0ed53dd --- /dev/null +++ b/packet.md @@ -0,0 +1,56 @@ +# 数据包格式 + +## 基础 + +| 含义 | 数据 | +| ---- | ---- | +| 发送包头 | 0x00 | +| 发送包尾 | 0xff | +| 返回包头 | 0x01 | +| 返回包尾 | 0xfe | + +`数据包头` + `包体长度` + `指令` + `数据包体` + `数据包尾` + +如： + +`00 06 20 AA BB FF` + +## 信息查询 + +### 查询蓝牙连接状态 `0x10` + +查询示例 `00 04 10 FF` + +| 返回数据 | 含义 | +| -------- | ---- | +| 01 | 已连接 | +| 00 | 未连接 | + +返回示例 `01 05 10 01 FE` + +## 控制 + +### 行进控制 `0x20` + +| 方向数据 | 含义 | +| -------- | ---- | +| 0x00 | 停止 | +| 0x01 | 前进 | +| 0x02 | 后退 | +| 0x03 | 左转 | +| 0x04 | 右转 | + +包体 `00 06 20 方向 速度 FF` + +控制示例 `00 06 20 01 FF FF` + +### 原地控制 `0x21` + +| 方向数据 | 含义 | +| -------- | ---- | +| 0x00 | 顺时针 | +| 0x01 | 逆时针 | + +包体 `00 06 21 方向 时间 FF` + +控制示例 `00 06 21 01 01 FF` \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 85cb83a..7636622 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,231 +1,301 @@ #include #include +#define STATUS_LED 2 + +#define HC_SR04_TRIG 34 +#define HC_SR04_ECHO 35 + // 使用 2 片 TB6612 控制 4 个电机 // 使用差速控制转向 // TB6612 #1 控制 A B 电机 -#define MOTOR_AB_STBY 14 // TB6612 #1 待机控制 - // A 电机 左前轮 -#define MOTOR_A_PWMA 25 // 电机 A PWM -#define MOTOR_A_AIN1 26 // 电机 A 方向控制1 -#define MOTOR_A_AIN2 27 // 电机 A 方向控制2 +#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 32 // 电机 B PWM -#define MOTOR_B_BIN1 33 // 电机 B 方向控制1 -#define MOTOR_B_BIN2 34 // 电机 B 方向控制2 +#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 电机 -#define MOTOR_CD_STBY 19 // TB6612 #2 待机控制 +// C 电机 右后轮 +#define MOTOR_C_PWMA 4 // 电机 C PWM +#define MOTOR_C_AIN1 15 // 电机 C 方向控制1 +#define MOTOR_C_AIN2 2 // 电机 C 方向控制2 -// C 电机 右前轮 -#define MOTOR_C_PWMA 16 // 电机 C PWM -#define MOTOR_C_AIN1 17 // 电机 C 方向控制1 -#define MOTOR_C_AIN2 18 // 电机 C 方向控制2 - -// D 电机 右后轮 -#define MOTOR_D_PWMB 21 // 电机 D PWM -#define MOTOR_D_BIN1 22 // 电机 D 方向控制1 -#define MOTOR_D_BIN2 23 // 电机 D 方向控制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 // 速度增量 +#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; // 转向状态 +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_CHECK_BT_STATUS 0x10 +#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 motorControl(int pwmPin, int in1Pin, int in2Pin, int speed) { - if (speed > 0) { +void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed) +{ + if (speed > 0) + { digitalWrite(in1Pin, HIGH); digitalWrite(in2Pin, LOW); - } else if (speed < 0) { + } + else if (speed < 0) + { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, HIGH); speed = -speed; - } else { + } + else + { digitalWrite(in1Pin, LOW); digitalWrite(in2Pin, LOW); } analogWrite(pwmPin, speed); } -// 小车运动控制函数 -void carControl(int leftSpeed, int rightSpeed) { - motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, leftSpeed); // 左前 - motorControl(MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2, leftSpeed); // 左后 - motorControl(MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2, rightSpeed); // 右前 - motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, rightSpeed); // 右后 -} - -// 计算左右轮速度 -void calculateWheelSpeeds(int baseSpeed, int turnOffset, int &leftSpeed, int &rightSpeed) { - // turnOffset 范围：-100 到 100 - // 负值表示向左转，正值表示向右转 - float turnMultiplier = 1.0 - (abs(turnOffset) / 100.0) * (1.0 - TURN_RATIO); - - if (turnOffset < 0) { // 向左转 - leftSpeed = baseSpeed * turnMultiplier; - rightSpeed = baseSpeed; - } else if (turnOffset > 0) { // 向右转 - leftSpeed = baseSpeed; - rightSpeed = baseSpeed * turnMultiplier; - } else { // 直行 - leftSpeed = baseSpeed; - rightSpeed = baseSpeed; - } -} - -// 更新小车运动 -void updateCarMovement() { - int leftSpeed = 0, rightSpeed = 0; - - if (isMoving) { - calculateWheelSpeeds(currentSpeed, turnOffset, leftSpeed, rightSpeed); - if (!isTurning) { - // 逐渐加速 - currentSpeed = min(currentSpeed + SPEED_INCREMENT, BASE_SPEED); - } - } else { - // 逐渐减速 - currentSpeed = max(currentSpeed - SPEED_INCREMENT, 0); - if (currentSpeed > 0) { - calculateWheelSpeeds(currentSpeed, turnOffset, leftSpeed, rightSpeed); - } - } - - carControl(leftSpeed, rightSpeed); -} - -// CCD 巡线相关函数 -#define CCD_PIN 35 // CCD 数据引脚 -#define CCD_CLK_PIN 36 // CCD 时钟引脚 -#define CCD_SI_PIN 39 // CCD 启动引脚 -#define CCD_PIXELS 128 // CCD 像素数 - -int ccdData[CCD_PIXELS]; // CCD 数据数组 - -// 读取 CCD 数据 -void readCCD() { - // 启动信号 - digitalWrite(CCD_SI_PIN, HIGH); - digitalWrite(CCD_CLK_PIN, HIGH); - digitalWrite(CCD_SI_PIN, LOW); - digitalWrite(CCD_CLK_PIN, LOW); - - // 读取像素数据 - for(int i = 0; i < CCD_PIXELS; i++) { - digitalWrite(CCD_CLK_PIN, HIGH); - ccdData[i] = analogRead(CCD_PIN); - digitalWrite(CCD_CLK_PIN, LOW); - } -} - -// 计算巡线偏移量 -int calculateLineOffset() { - // 这里添加线位置检测算法 - // 返回值范围：-100 到 100 - // 0 表示线在中间，负值表示线在左边，正值表示线在右边 - return 0; // 临时返回值 +u_char getSerialBTStatus() +{ + return SerialBT.connected() ? 0x01 : 0x00; } void setup() { // 初始化串口 Serial.begin(115200); - + // 初始化蓝牙 - SerialBT.begin("ESP32_Car"); // 蓝牙设备名称 - + SerialBT.begin("WhiteTiger"); + // 设置引脚模式 - pinMode(MOTOR_AB_STBY, OUTPUT); - pinMode(MOTOR_CD_STBY, OUTPUT); - + 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); - - // 启用电机驱动 - digitalWrite(MOTOR_AB_STBY, HIGH); - digitalWrite(MOTOR_CD_STBY, HIGH); - - // 添加 CCD 引脚初始化 - // pinMode(CCD_CLK_PIN, OUTPUT); - // pinMode(CCD_SI_PIN, OUTPUT); - // pinMode(CCD_PIN, INPUT); + + pinMode(HC_SR04_TRIG, INPUT); + 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; + + switch (cmd) + { + case CMD_CHECK_BT_STATUS: + Serial.println("CMD_CHECK_BT_STATUS"); + // 回复状态 + serial.write(PACKET_T_HEAD); + serial.write(0x05); // 包体长度 + serial.write(CMD_CHECK_BT_STATUS); + serial.write(getSerialBTStatus()); + 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()) { - char cmd = SerialBT.read(); - - switch(cmd) { - case 'F': // 前进 - isMoving = true; - turnOffset = 0; - break; - case 'B': // 后退 - isMoving = true; - currentSpeed = -BASE_SPEED; - turnOffset = 0; - break; - case 'L': // 左转 - isTurning = true; - turnOffset = -100; - break; - case 'R': // 右转 - isTurning = true; - turnOffset = 100; - break; - case 'S': // 停止 - isMoving = false; - isTurning = false; - turnOffset = 0; - break; - case 'l': // 微左转 - turnOffset = max(turnOffset - 20, -100); - break; - case 'r': // 微右转 - turnOffset = min(turnOffset + 20, 100); - break; - } + if (SerialBT.available()) + { + uint8_t incomingByte = SerialBT.read(); + processSerialIncomingByte(incomingByte, SerialBT); + Serial.printf("BT Received: 0x%02X\n", incomingByte); } - - // 更新小车运动 - updateCarMovement(); - - // 巡线模式代码（后续启用） - /* - readCCD(); - int lineOffset = calculateLineOffset(); - turnOffset = lineOffset; - isMoving = true; - updateCarMovement(); - */ - - delay(20); // 控制更新频率 + + if (Serial.available()) + { + uint8_t incomingByte = Serial.read(); + processSerialIncomingByte(incomingByte, Serial); + Serial.printf("UART Received: 0x%02X\n", incomingByte); + } + + updateStatusLED(); // 更新LED状态 }