From d7fc51018e01bbdfbc986a09f5463ae944b47e72 Mon Sep 17 00:00:00 2001 From: chenxin Date: Thu, 23 Apr 2026 11:17:01 +0800 Subject: [PATCH] main --- .gitignore | 5 + .vscode/extensions.json | 10 + include/README | 37 ++++ lib/README | 46 +++++ platformio.ini | 18 ++ src/BLE.cpp | 65 +++++++ src/BLE.h | 44 +++++ src/DRV8870.cpp | 113 +++++++++++ src/DRV8870.h | 87 +++++++++ src/main.cpp | 418 ++++++++++++++++++++++++++++++++++++++++ test/README | 11 ++ 11 files changed, 854 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/extensions.json create mode 100644 include/README create mode 100644 lib/README create mode 100644 platformio.ini create mode 100644 src/BLE.cpp create mode 100644 src/BLE.h create mode 100644 src/DRV8870.cpp create mode 100644 src/DRV8870.h create mode 100644 src/main.cpp create mode 100644 test/README diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/.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" + ] +} diff --git a/include/README b/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/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 diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..9379397 --- /dev/null +++ b/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 +#include + +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 diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..1c9561a --- /dev/null +++ b/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 diff --git a/src/BLE.cpp b/src/BLE.cpp new file mode 100644 index 0000000..593b938 --- /dev/null +++ b/src/BLE.cpp @@ -0,0 +1,65 @@ +// BLE.cpp +#include "BLE.h" +#include + +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; + } + } +} \ No newline at end of file diff --git a/src/BLE.h b/src/BLE.h new file mode 100644 index 0000000..8b1862a --- /dev/null +++ b/src/BLE.h @@ -0,0 +1,44 @@ +// BLE.h +#ifndef BLE_H +#define BLE_H + +#include +#include +#include + +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 \ No newline at end of file diff --git a/src/DRV8870.cpp b/src/DRV8870.cpp new file mode 100644 index 0000000..ba8b6ec --- /dev/null +++ b/src/DRV8870.cpp @@ -0,0 +1,113 @@ +#include "DRV8870.h" +#include +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; + } + } +} diff --git a/src/DRV8870.h b/src/DRV8870.h new file mode 100644 index 0000000..62127fd --- /dev/null +++ b/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 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..fd03490 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,418 @@ +#include +#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 +// 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); +// } \ No newline at end of file diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/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