main

.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

include/README

@@ -0,0 +1,37 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the convention is to give header files names that end with `.h'.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into the executable file.
+
+The source code of each library should be placed in a separate directory
+("lib/your_library_name/[Code]").
+
+For example, see the structure of the following example libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+Example contents of `src/main.c` using Foo and Bar:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+The PlatformIO Library Dependency Finder will find automatically dependent
+libraries by scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

platformio.ini

@@ -0,0 +1,18 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32-s3-devkitc-1]
+platform = espressif32 @ ^4.4.0
+board = esp32-s3-devkitc-1
+framework = arduino
+platform_packages = toolchain-riscv32-esp @ 8.4.0
+board_upload.flash_size = 4MB
+board_upload.psram_size = 2MB
+lib_deps = robtillaart/INA226@^0.6.0

src/BLE.cpp

@@ -0,0 +1,65 @@
+// BLE.cpp
+#include "BLE.h"
+#include <Arduino.h>
+
+BLEController::BLEController(const char* deviceName, const char* serviceUUID, const char* characteristicUUID)
+    : _deviceName(deviceName), _serviceUUID(serviceUUID), _characteristicUUID(characteristicUUID),
+      _pServer(nullptr), _pCharacteristic(nullptr), _buttonPressed(false) {}
+
+void BLEController::setup() {
+    BLEDevice::init(_deviceName);
+    _pServer = BLEDevice::createServer();
+    _pServer->setCallbacks(new MyServerCallbacks(this));
+    BLEService* pService = _pServer->createService(_serviceUUID);
+    _pCharacteristic = pService->createCharacteristic(
+        _characteristicUUID,
+        BLECharacteristic::PROPERTY_READ |
+        BLECharacteristic::PROPERTY_WRITE
+    );
+    _pCharacteristic->setCallbacks(new MyCharacteristicCallbacks(this));
+    pService->start();
+    BLEAdvertising* pAdvertising = BLEDevice::getAdvertising();
+    pAdvertising->addServiceUUID(_serviceUUID);
+    pAdvertising->setScanResponse(false);
+    pAdvertising->setMinPreferred(0x06);  // iPHone
+    pAdvertising->setMaxPreferred(0x12);
+    BLEDevice::startAdvertising();
+}
+
+void BLEController::loop() {
+    // 连接状态检查
+    bool deviceConnected = _pServer->getConnectedCount();
+    if (deviceConnected) {
+        // Do nothing, device is connected
+    } else {
+        delay(500); // 等待500ms
+        _pServer->startAdvertising(); // 重新开始广播
+    }
+}
+
+bool BLEController::isButtonPressed() {
+    bool pressed = _buttonPressed;
+    _buttonPressed = false; // Reset the button pressed flag
+    return pressed;
+}
+
+void BLEController::MyServerCallbacks::onConnect(BLEServer* pServer) {
+    Serial.println("Bluetooth connected");
+    _controller->_buttonPressed = false; // Reset the button pressed flag on connect
+}
+
+void BLEController::MyServerCallbacks::onDisconnect(BLEServer* pServer) {
+    Serial.println("Bluetooth disconnected");
+}
+
+void BLEController::MyCharacteristicCallbacks::onWrite(BLECharacteristic *pCharacteristic) {
+    std::string value = pCharacteristic->getValue();
+    if (value.length() > 0) {
+        Serial.print("Received: ");
+        Serial.println(value.c_str());
+
+        if (value == "1") { // "1"--蓝牙控制命令
+            _controller->_buttonPressed = true;
+        }
+    }
+}

src/BLE.h

@@ -0,0 +1,44 @@
+// BLE.h
+#ifndef BLE_H
+#define BLE_H
+
+#include <BLEDevice.h>
+#include <BLEUtils.h>
+#include <BLEServer.h>
+
+class BLEController {
+public:
+    BLEController(const char* deviceName, const char* serviceUUID, const char* characteristicUUID);
+    void setup();
+    void loop();
+    bool isButtonPressed();
+
+private:
+    const char* _deviceName;
+    const char* _serviceUUID;
+    const char* _characteristicUUID;
+    BLEServer* _pServer;
+    BLECharacteristic* _pCharacteristic;
+    bool _buttonPressed;
+
+    class MyServerCallbacks : public BLEServerCallbacks {
+    public:
+        MyServerCallbacks(BLEController* controller) : _controller(controller) {}
+        void onConnect(BLEServer* pServer);
+        void onDisconnect(BLEServer* pServer);
+
+    private:
+        BLEController* _controller;
+    };
+
+    class MyCharacteristicCallbacks : public BLECharacteristicCallbacks {
+    public:
+        MyCharacteristicCallbacks(BLEController* controller) : _controller(controller) {}
+        void onWrite(BLECharacteristic *pCharacteristic);
+
+    private:
+        BLEController* _controller;
+    };
+};
+
+#endif // BLE_H

src/DRV8870.cpp

@@ -0,0 +1,113 @@
+#include "DRV8870.h"
+#include <Arduino.h>
+DRV8870::DRV8870(int motor_pin_1, int motor_pin_2)
+{
+    this->_motor_count = 1;
+    this->_motor_pin_1 = motor_pin_1;
+    this->_motor_pin_2 = motor_pin_2;
+    pinMode(this->_motor_pin_1, OUTPUT);
+    pinMode(this->_motor_pin_2, OUTPUT);
+}
+DRV8870::DRV8870(int motor_pin_1, int motor_pin_2, int motor_pin_3, int motor_pin_4)
+{
+    this->_motor_count = 2;
+    this->_motor_pin_1 = motor_pin_1;
+    this->_motor_pin_2 = motor_pin_2;
+    this->_motor_pin_3 = motor_pin_3;
+    this->_motor_pin_4 = motor_pin_4;
+    pinMode(this->_motor_pin_1, OUTPUT);
+    pinMode(this->_motor_pin_2, OUTPUT);
+    pinMode(this->_motor_pin_3, OUTPUT);
+    pinMode(this->_motor_pin_4, OUTPUT);
+}
+void DRV8870::setMaxSpeed(int max_speed)
+{
+    this->max_speed = max_speed;
+}
+void DRV8870::setSpeed(int motor_speed, int direction)
+{
+    switch (this->_motor_count)
+    {
+    case 1:
+        switch (direction)
+        {
+        case HighLow:
+            analogWrite(this->_motor_pin_1, motor_speed);
+            analogWrite(this->_motor_pin_2, 0);
+            break;
+        case LowHigh:
+            analogWrite(this->_motor_pin_1, 0);
+            analogWrite(this->_motor_pin_2, motor_speed);
+            break;
+        case HighHigh:
+            analogWrite(this->_motor_pin_1, 0);
+            analogWrite(this->_motor_pin_2, 0);
+            break;
+        default:
+            break;
+        }
+        break;
+    case 2:
+        switch (direction)
+        {
+        case HighLow:
+            analogWrite(this->_motor_pin_1, motor_speed);
+            analogWrite(this->_motor_pin_2, 0);
+            analogWrite(this->_motor_pin_3, motor_speed);
+            analogWrite(this->_motor_pin_4, 0);
+            break;
+        case LowHigh:
+            analogWrite(this->_motor_pin_1, 0);
+            analogWrite(this->_motor_pin_2, motor_speed);
+            analogWrite(this->_motor_pin_3, 0);
+            analogWrite(this->_motor_pin_4, motor_speed);
+            break;
+        default:
+            break;
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+//brake 有误，不在代码中使用
+void DRV8870::brake(int mode)  //LL休眠，HH刹车
+{
+    if (mode == COAST)
+    {
+        switch (this->_motor_count)
+        {
+        case 1:
+            digitalWrite(this->_motor_pin_1, LOW);
+            digitalWrite(this->_motor_pin_2, LOW);
+            break;
+        case 2:
+            digitalWrite(this->_motor_pin_1, LOW);
+            digitalWrite(this->_motor_pin_2, LOW);
+            digitalWrite(this->_motor_pin_3, LOW);
+            digitalWrite(this->_motor_pin_4, LOW);
+            break;
+        default:
+            break;
+        }
+    }
+    else if (mode == BRAKE)
+    {
+        switch (this->_motor_count)
+        {
+        case 1:
+            digitalWrite(this->_motor_pin_1, HIGH);
+            digitalWrite(this->_motor_pin_2, HIGH);
+            break;
+        case 2:
+            digitalWrite(this->_motor_pin_1, HIGH);
+            digitalWrite(this->_motor_pin_2, HIGH);
+            digitalWrite(this->_motor_pin_3, HIGH);
+            digitalWrite(this->_motor_pin_4, HIGH);
+            break;
+        default:
+            break;
+        }
+    }
+}

src/DRV8870.h

@@ -0,0 +1,87 @@
+/*
+ *
+ *    x_PWM1 x_PWM2    Mode
+ *      0      0       Coast/Fast decay
+ *      0      1       Reverse
+ *      1      0       Forward
+ *      1      1       Brake/slow decay
+ */
+
+#ifndef DRV8870_H
+#define DRV8870_H
+
+#define HighLow 1
+#define LowHigh 0
+#define HighHigh 2
+#define LowLow 3
+
+#define COAST 1
+#define BRAKE 0
+
+
+
+class DRV8870
+{
+private:
+    // The library version number
+    int _version = 1;
+
+    // The maximum value for an analogWrite(pin, value). This varies from board to board
+    int max_speed = 255;
+
+    // motor pin numbers. It can driver up to a maximum of 4 motors
+    int _motor_pin_1;
+    int _motor_pin_2;
+    int _motor_pin_3;
+    int _motor_pin_4;
+
+    // Motor count
+    int _motor_count;
+
+public:
+    /** Creates a DRV8870(H-bridge motor controller) control interface to drive 1 motor
+     *
+     * @param motor_pin_1 A PWM enabled pin, tied to the IN1 Logic input and controls state of OUT1
+     * @param motor_pin_2 A PWM enabled pin, tied to the IN2 Logic input and controls state of OUT2
+     *
+     */
+    DRV8870(int motor_pin_1, int motor_pin_2);
+
+    /** Creates a DRV8870(H-bridge motor controller) control interface to drive 2 motors simultaneously
+     *
+     * @param motor_pin_1 A PWM enabled pin, tied to the IN1 Logic input and controls state of OUT1
+     * @param motor_pin_2 A PWM enabled pin, tied to the IN2 Logic input and controls state of OUT2
+     * @param motor_pin_3 A PWM enabled pin, tied to the IN3 Logic input and controls state of OUT3
+     * @param motor_pin_4 A PWM enabled pin, tied to the IN4 Logic input and controls state of OUT4
+     *
+     */
+    DRV8870(int motor_pin_1, int motor_pin_2, int motor_pin_3, int motor_pin_4);
+
+    /** Set the maximum speed the motor can run
+     *
+     * @param max_speed The maximum speed value for an analogWrite(value);
+     */
+    void setMaxSpeed(int max_speed);
+
+    /** Set the speed of the motor
+     *
+     * @param motor_speed The speed of the motor as a normalised value between 0 and max_speed
+     */
+    void setSpeed(int motor_speed, int direction);
+
+    /** Brake the H-bridge coast or brake.
+     *
+     * Defaults to coast.
+     * @param mode - Braking mode.COAST(default)or BRAKE.
+     *
+     */
+    void brake(int mode);
+
+    /** Returns the version of the library
+     *
+     *
+     */
+    int version(void);
+};
+
+#endif // DRV8870_H

src/main.cpp

@@ -0,0 +1,418 @@
+#include <Arduino.h>
+#include "DRV8870.h"
+#include "BLE.h"
+#include "INA226.h"
+
+BLEController bleController("ASD-Backpack", "4fafc201-1fb5-459e-8fcc-c5c9c331914b", "beb5483e-36e1-4688-b7f5-ea07361b26a8");
+
+
+DRV8870 mymotor(12, 13);    // 电机
+DRV8870 mylock(10, 11);     // 电磁锁
+
+// INA226 电流监控器
+INA226 INA(0x40);
+
+// 限位传感器
+const int LEFT_LIMIT_HIGH_PIN = 17;
+const int LEFT_LIMIT_LOW_PIN = 18;
+const int RIGHT_LIMIT_HIGH_PIN = 19;
+const int RIGHT_LIMIT_LOW_PIN = 20;
+
+// 电磁锁推杆微动开关限位
+const int LOCK_POSITION_SWITCH_PIN = 9;
+
+// 控制电机正反转
+const int CONTROL_PIN = 14;
+
+// 解锁尝试参数
+const int MAX_UNLOCK_ATTEMPTS = 3;        // 最大解锁尝试次数
+const unsigned long UNLOCK_WAIT_TIME = 300;  // 每次解锁后等待时间(ms)
+const unsigned long UNLOCK_RETRY_DELAY = 500; // 重试间隔时间(ms)
+
+// 电机堵转检测参数
+const float STALL_CURRENT_THRESHOLD = 2.0;  // 堵转电流阈值(A)
+const unsigned long STALL_CHECK_INTERVAL = 500;  // 检测间隔(ms)
+const unsigned long STALL_TIME_THRESHOLD = 1000;  // 堵转时间阈值(ms)
+
+// 堵转检测变量
+float motorCurrent = 0.0;
+unsigned long lastCurrentCheck = 0;
+unsigned long stallStartTime = 0;
+bool isStalled = false;
+bool stallDetected = false;
+
+// 电机状态
+enum MotorState {
+    STOPPED,
+    LEFT_TURNING,
+    RIGHT_TURNING
+};
+
+MotorState motorState = STOPPED;
+
+// 标志变量，用于控制打印信息
+bool leftTurnPrinted = false;
+bool rightTurnPrinted = false;
+
+// 检查限位传感器状态
+bool checkLimit(int highPin, int lowPin) {
+    bool highState = digitalRead(highPin);
+    bool lowState = digitalRead(lowPin);
+
+    // 触发时返回true
+    return highState && !lowState;
+}
+
+// 检查电磁锁是否已解锁（微动开关被触发）
+bool isLockUnlocked() {
+    // 微动开关被触发时返回LOW，表示推杆已推出（解锁状态）
+    return digitalRead(LOCK_POSITION_SWITCH_PIN) == LOW;
+}
+
+// 上锁
+void lock_sleep() {
+    mylock.setSpeed(0, HighHigh); 
+}
+
+// 解锁
+void unlock() {
+    mylock.setSpeed(255, HighLow);
+}
+
+// 多次尝试解锁函数
+bool attemptUnlock() {
+    Serial.println("Starting unlock...");
+    
+    for (int attempt = 1; attempt <= MAX_UNLOCK_ATTEMPTS; attempt++) {
+        Serial.print("Unlock attempt ");
+        Serial.print(attempt);
+        Serial.print("/");
+        Serial.println(MAX_UNLOCK_ATTEMPTS);
+        
+        // 执行解锁操作
+        unlock();
+        
+        // 等待解锁完成
+        delay(UNLOCK_WAIT_TIME);
+        
+        // 检查是否成功解锁
+        if (isLockUnlocked()) {
+            Serial.println("Unlock successful!");
+            return true;
+        }
+        
+        // 如果不是最后一次尝试，等待一段时间后重试
+        if (attempt < MAX_UNLOCK_ATTEMPTS) {
+            Serial.println("Unlock failed, retrying...");
+            
+            // 先停止解锁操作
+            lock_sleep();
+            delay(UNLOCK_RETRY_DELAY);
+        }
+    }
+    
+    Serial.println("All unlock attempts failed!");
+    return false;
+}
+
+// 电机堵转检测函数
+bool checkMotorStall() {
+    unsigned long currentTime = millis();
+    
+    // 按设定间隔检测电流
+    if (currentTime - lastCurrentCheck >= STALL_CHECK_INTERVAL) {
+        lastCurrentCheck = currentTime;
+        
+        // 读取电机电流
+        motorCurrent = INA.getCurrent_mA() / 1000.0;  // 转换为安培
+        
+        // 检测是否超过堵转电流阈值
+        if (motorCurrent > STALL_CURRENT_THRESHOLD) {
+            if (!isStalled) {
+                // 开始记录堵转时间
+                stallStartTime = currentTime;
+                isStalled = true;
+                Serial.print("Motor current high: ");
+                Serial.print(motorCurrent);
+                Serial.println("A");
+            } else {
+                // 检查堵转持续时间
+                if (currentTime - stallStartTime >= STALL_TIME_THRESHOLD) {
+                    if (!stallDetected) {
+                        stallDetected = true;
+                        Serial.println("MOTOR STALL DETECTED!");
+                        return true;
+                    }
+                }
+            }
+        } else {
+            // 电流正常，重置堵转检测
+            if (isStalled) {
+                isStalled = false;
+                stallDetected = false;
+                Serial.println("Motor current normal");
+            }
+        }
+    }
+    
+    return false;
+}
+
+// 处理电机堵转
+void handleMotorStall() {
+    Serial.println("Handling motor stall...");
+    
+    // 立即停止电机
+    mymotor.setSpeed(0, HighHigh);
+    
+    // 短暂延时
+    delay(500);
+    
+    // 尝试反向转动以解除堵转
+    if (motorState == LEFT_TURNING) {
+        Serial.println("Attempting reverse rotation (Right)");
+        mymotor.setSpeed(200, LowHigh);  // 降低速度反向转动
+        delay(1000);
+        mymotor.setSpeed(0, HighHigh);   // 停止
+        delay(500);
+        
+        // 如果没有触发限位，继续原方向
+        if (!checkLimit(LEFT_LIMIT_HIGH_PIN, LEFT_LIMIT_LOW_PIN)) {
+            Serial.println("Resuming left turn");
+            mymotor.setSpeed(200, HighLow);  // 降低速度继续左转
+        } else {
+            motorState = STOPPED;
+        }
+    } else if (motorState == RIGHT_TURNING) {
+        Serial.println("Attempting reverse rotation (Left)");
+        mymotor.setSpeed(200, HighLow);  // 降低速度反向转动
+        delay(1000);
+        mymotor.setSpeed(0, HighHigh);   // 停止
+        delay(500);
+        
+        // 如果没有触发限位，继续原方向
+        if (!checkLimit(RIGHT_LIMIT_HIGH_PIN, RIGHT_LIMIT_LOW_PIN)) {
+            Serial.println("Resuming right turn");
+            mymotor.setSpeed(200, LowHigh);  // 降低速度继续右转
+        } else {
+            motorState = STOPPED;
+        }
+    }
+    
+    // 重置堵转检测标志
+    isStalled = false;
+    stallDetected = false;
+}
+
+// 左转（仅在解锁状态下执行）
+void turnLeft() {
+    if (!isLockUnlocked()) {
+        Serial.println("Cannot turn left: Lock not unlocked!");
+        return;
+    }
+    
+    if (!leftTurnPrinted) {
+        Serial.println("Turn Left...");
+        leftTurnPrinted = true;
+        rightTurnPrinted = false;
+    }
+    mymotor.setSpeed(255, HighLow);
+    motorState = LEFT_TURNING;
+    
+    // 重置堵转检测
+    isStalled = false;
+    stallDetected = false;
+}
+
+// 右转（仅在解锁状态下执行）
+void turnRight() {
+    if (!isLockUnlocked()) {
+        Serial.println("Cannot turn right: Lock not unlocked!");
+        return;
+    }
+    
+    if (!rightTurnPrinted) {
+        Serial.println("Turn Right...");
+        rightTurnPrinted = true;
+        leftTurnPrinted = false;
+    }
+    mymotor.setSpeed(255, LowHigh);
+    motorState = RIGHT_TURNING;
+    
+    // 重置堵转检测
+    isStalled = false;
+    stallDetected = false;
+}
+
+// 设置蓝牙模块的日志级别为NONE,避免断开蓝牙后串口打印错误信息
+void esp_log_level_set(){
+    esp_log_level_set("BT_HCI", ESP_LOG_NONE);
+    esp_log_level_set("BTDM_INIT", ESP_LOG_NONE);
+    esp_log_level_set("BT_L2CAP", ESP_LOG_NONE);
+    esp_log_level_set("BT_SMP", ESP_LOG_NONE);
+    esp_log_level_set("BT_GATT", ESP_LOG_NONE);
+    esp_log_level_set("BT_GATTS", ESP_LOG_NONE);
+    esp_log_level_set("BT_BREDR", ESP_LOG_NONE);
+    esp_log_level_set("BT_AVDT", ESP_LOG_NONE);
+    esp_log_level_set("BT_AVCT", ESP_LOG_NONE);
+    esp_log_level_set("BT_AVDTP", ESP_LOG_NONE);
+    esp_log_level_set("BT_AVRC", ESP_LOG_NONE);
+    esp_log_level_set("BT_AVRCP", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTC", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTA", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTIF", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTC", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTA", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTIF", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTA_HCI", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_HCI", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_BTC", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_BTA", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_BTIF", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_BTM", ESP_LOG_NONE);
+    esp_log_level_set("BT_BTM_BTM_BTM_HCI", ESP_LOG_NONE);
+}
+
+
+void setup()
+{
+    Serial.begin(115200);
+    mymotor.setMaxSpeed(255);
+    mylock.setMaxSpeed(255);
+
+    // 初始化 I2C 总线
+    Wire.begin(5, 4); // SDA SCL
+
+    // INA226 初始化
+    if (!INA.begin()) {
+        Serial.println("INA226 initialization failed!");
+        while (1);
+    }
+    
+    INA.setMaxCurrentShunt(10.0, 0.005); // 设置最大电流和分流电阻
+    INA.setAverage(4);   // 设置平均次数以减少噪声
+    
+    Serial.println("INA226 initialized successfully");
+    
+    // 限位传感器，输入模式
+    pinMode(LEFT_LIMIT_HIGH_PIN, INPUT);
+    pinMode(LEFT_LIMIT_LOW_PIN, INPUT);
+    pinMode(RIGHT_LIMIT_HIGH_PIN, INPUT);
+    pinMode(RIGHT_LIMIT_LOW_PIN, INPUT);
+    
+    // 电磁锁推杆微动开关，输入模式，内部上拉电阻
+    pinMode(LOCK_POSITION_SWITCH_PIN, INPUT_PULLUP);
+
+    // 开关按钮，输入模式，内部上拉电阻
+    pinMode(CONTROL_PIN, INPUT_PULLUP);
+
+    // 初始化电磁锁为上锁状态
+    //lock();
+
+    // 初始化蓝牙
+    bleController.setup();
+
+    esp_log_level_set();
+
+    Serial.println("Welcome - ASD Backpack");
+}
+
+void loop()
+{
+    // 检测限位状态
+    bool leftLimitTriggered = checkLimit(LEFT_LIMIT_HIGH_PIN, LEFT_LIMIT_LOW_PIN);
+    bool rightLimitTriggered = checkLimit(RIGHT_LIMIT_HIGH_PIN, RIGHT_LIMIT_LOW_PIN);
+
+    // 检测按钮按下状态
+    if (digitalRead(CONTROL_PIN) == LOW || bleController.isButtonPressed()) {
+        // 尝试多次解锁
+        if (attemptUnlock()) {
+            // 解锁成功，执行电机操作
+            if (motorState == LEFT_TURNING) {
+                // 如果正在左转，停止并右转
+                mymotor.setSpeed(0, HighHigh);
+                Serial.println("STOP, Turn Right...");
+                turnRight();
+            } else if (motorState == RIGHT_TURNING) {
+                // 如果正在右转，停止并左转
+                mymotor.setSpeed(0, HighHigh);
+                Serial.println("STOP, Turn Left...");
+                turnLeft();
+            } else if (!leftLimitTriggered && !rightLimitTriggered) {
+                turnLeft();
+            } else if (leftLimitTriggered) {
+                turnRight();
+            } else if (rightLimitTriggered) {
+                turnLeft();
+            }
+        } else {
+            // 解锁失败
+            Serial.println("Unlock failed - motor operation cancelled");
+            motorState = STOPPED;
+        }
+    }
+
+    // 电机堵转检测（仅在电机运行时）
+    if (motorState != STOPPED) {
+        if (checkMotorStall()) {
+            handleMotorStall();
+        }
+    }
+
+    // 根据当前电机状态执行相应的操作
+    switch (motorState) {
+        case LEFT_TURNING:
+            if (checkLimit(LEFT_LIMIT_HIGH_PIN, LEFT_LIMIT_LOW_PIN)) {
+                mymotor.setSpeed(0, HighHigh);
+                Serial.println("STOP LOCKED");
+                motorState = STOPPED;
+                leftTurnPrinted = false;
+            }
+            break;
+        case RIGHT_TURNING:
+            if (checkLimit(RIGHT_LIMIT_HIGH_PIN, RIGHT_LIMIT_LOW_PIN)) {
+                mymotor.setSpeed(0, HighHigh);
+                Serial.println("STOP LOCKED");
+                motorState = STOPPED; 
+                rightTurnPrinted = false;
+            }
+            break;
+        case STOPPED:
+            // 电机停止状态，保持上锁
+            lock_sleep();
+            break;
+    }
+
+    // 蓝牙循环处理
+    bleController.loop();
+}
+
+
+// #include <Arduino.h>
+// void setup() {
+//   // put your setup code here, to run once:
+//   Serial.begin(115200);                                      
+//   Serial.println("Hello World !");     
+//   pinMode(9, INPUT_PULLUP);  //weidong
+//   pinMode(14, INPUT_PULLUP);//key
+// }
+
+// void loop(){
+// if(digitalRead(9) == LOW){
+//   Serial.println("Button 9 Pressed");
+// }
+// else{
+//   Serial.println("Button 9 Released");
+// }
+// if(digitalRead(14) == LOW){
+//   Serial.println("Button 14 Pressed");
+// }
+// else{
+//   Serial.println("Button 14 Released");
+// }
+
+
+// delay(200);
+// }

test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html