From f5969120576093ee574cb4ab522563c4a499e664 Mon Sep 17 00:00:00 2001 From: zhenai Date: Mon, 12 Oct 2026 22:00:30 +0800 Subject: [PATCH] first commit --- .gitignore | 5 + .vscode/extensions.json | 11 ++ include/README | 37 ++++ lib/README | 46 +++++ platformio.ini | 21 ++ sketch_oct8a.code-workspace | 12 ++ src/imu/imu.cpp | 81 ++++++++ src/imu/imu.h | 27 +++ src/main.cpp | 25 +++ src/wifi/wifi-sg90.h | 15 ++ src/wifi/wifi.cpp | 381 ++++++++++++++++++++++++++++++++++++ test/README | 11 ++ 12 files changed, 672 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 sketch_oct8a.code-workspace create mode 100644 src/imu/imu.cpp create mode 100644 src/imu/imu.h create mode 100644 src/main.cpp create mode 100644 src/wifi/wifi-sg90.h create mode 100644 src/wifi/wifi.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..a91ca8f --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,11 @@ +{ + // 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", + "pioarduino.pioarduino-ide" + ] +} 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..0dda995 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,21 @@ +; 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:esp32dev] +platform = espressif32 +board = esp32dev +framework = arduino +monitor_speed = 115200 +lib_compat_mode = soft +lib_deps = + jrowberg/I2Cdevlib-Core + jrowberg/I2Cdevlib-MPU6050 + Adafruit_MPU6050 + Adafruit_Sensor diff --git a/sketch_oct8a.code-workspace b/sketch_oct8a.code-workspace new file mode 100644 index 0000000..4f860fc --- /dev/null +++ b/sketch_oct8a.code-workspace @@ -0,0 +1,12 @@ +{ + "folders": [ + { + "path": "../../../Arduino/sketch_oct8a" + }, + { + "name": "TwoFootBot-arduion", + "path": "." + } + ], + "settings": {} +} \ No newline at end of file diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp new file mode 100644 index 0000000..5c4b615 --- /dev/null +++ b/src/imu/imu.cpp @@ -0,0 +1,81 @@ +#include "I2Cdev.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "Wire.h" +#include +#include "imu.h" + +MPU6050 mpu; +bool dmpReady = false; +uint8_t fifoBuffer[64]; + +Quaternion q; +VectorFloat gravity; + +// ===== 初始化 ===== +void imu_setup() { + Serial.begin(115200); + Wire.begin(21, 22); + + mpu.initialize(); + if (!mpu.testConnection()) { + Serial.println("MPU6050 连接失败"); + while (1); + } + + uint8_t devStatus = mpu.dmpInitialize(); + if (devStatus == 0) { + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + mpu.setDMPEnabled(true); + dmpReady = true; + Serial.println("DMP 就绪"); + } else { + Serial.print("DMP 初始化失败，错误码: "); + Serial.println(devStatus); + } +} + +// ===== DMP 读取，返回 YPR（角度制）===== +YPR dmp() { + YPR out = {0, 0, 0}; + if (!dmpReady) return out; + + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + float ypr[3]; + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + + out.yaw = ypr[0] * 180.0f / M_PI; + out.pitch = ypr[1] * 180.0f / M_PI; + out.roll = ypr[2] * 180.0f / M_PI; + } + // Serial.print("Yaw: "); Serial.print(a.yaw); + // Serial.print(" Pitch: "); Serial.print(a.pitch); + // Serial.print(" Roll: "); Serial.println(a.roll); + return out; +} + +// ===== 原始数据读取，返回 RawData ===== +RawData raw() { + RawData r; + mpu.getMotion6(&r.ax, &r.ay, &r.az, &r.gx, &r.gy, &r.gz); + + // ±2g 量程下 1g = 16384 LSB + // r.ax_g = r.ax / 16384.0f; + // r.ay_g = r.ay / 16384.0f; + // r.az_g = r.az / 16384.0f; + + // // ±250°/s 量程下 1°/s = 131 LSB + // r.gx_dps = r.gx / 131.0f; + // r.gy_dps = r.gy / 131.0f; + // r.gz_dps = r.gz / 131.0f; + // Serial.print("Accel(g): X="); Serial.print(r.ax_g, 3); + // Serial.print(" Y="); Serial.print(r.ay_g, 3); + // Serial.print(" Z="); Serial.print(r.az_g, 3); + // Serial.print(" | Gyro(°/s): X="); Serial.print(r.gx_dps, 2); + // Serial.print(" Y="); Serial.print(r.gy_dps, 2); + // Serial.print(" Z="); Serial.println(r.gz_dps, 2); + + return r; +} \ No newline at end of file diff --git a/src/imu/imu.h b/src/imu/imu.h new file mode 100644 index 0000000..85d280d --- /dev/null +++ b/src/imu/imu.h @@ -0,0 +1,27 @@ +#ifndef IMU_DATA_H +#define IMU_DATA_H + +#include + +// 返回姿态角（单位：度） +struct YPR { + float yaw; + float pitch; + float roll; +}; + +// 返回原始数据及换算值 +struct RawData { + int16_t ax, ay, az; + int16_t gx, gy, gz; +// float ax_g, ay_g, az_g; +// float gx_dps, gy_dps, gz_dps; +// +}; + +// 函数声明 +YPR dmp(); +RawData raw(); +void imu_setup(); + +#endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..5c912ed --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,25 @@ + +#include "imu/imu.h" +#include "wifi/wifi-sg90.h" +#include + +void setup() { + // 串口初始化等公共配置 + Serial.begin(115200); + + // 调用子模块的初始化函数 + imu_setup(); + wifi_setup(); +} +void loop() { + static unsigned long lastTelemetry = 0; + unsigned long now = millis(); + //unsigned long now = micros(); // 改用微秒级计时 + + if (now - lastTelemetry >= 300) { + lastTelemetry = now; + YPR a = dmp(); + RawData r = raw(); + sendLineData(a, r); + } +} \ No newline at end of file diff --git a/src/wifi/wifi-sg90.h b/src/wifi/wifi-sg90.h new file mode 100644 index 0000000..0b3aa51 --- /dev/null +++ b/src/wifi/wifi-sg90.h @@ -0,0 +1,15 @@ +#pragma once +#ifndef WIFI_SG90_H +#define WIFI_SG90_H + +#include "../imu/imu.h" + +void sg90(); +void wifi_setup(); + +// TCP IMU telemetry: 22-byte frame: A5 5A, then 9 signed big-endian int16 +// values (yaw/pitch/roll in centidegrees, then ax/ay/az/gx/gy/gz), then +// CRC-16/CCITT-FALSE over the first 20 bytes. Text command replies remain lines. +void sendLineData(YPR a, RawData r); + +#endif diff --git a/src/wifi/wifi.cpp b/src/wifi/wifi.cpp new file mode 100644 index 0000000..32d2ba5 --- /dev/null +++ b/src/wifi/wifi.cpp @@ -0,0 +1,381 @@ +#include +#include +#include +#include +#include "wifi/wifi-sg90.h" + +// ==================== WiFi / TCP 配置 ==================== +const char* AP_SSID = "ESP32_AP"; +const char* AP_PASS = "12345678"; +const int TCP_PORT = 3333; + +// ==================== 舵机配置 ==================== +const uint8_t NUM_SERVOS = 8; +// 注意：避开 GPIO 6~11（Flash）、34~39（仅输入）、2/15（启动脚） +const uint8_t SERVO_PINS[NUM_SERVOS] = {13, 14, 25, 26, 27, 32, 33, 4}; + +const uint16_t PULSE_MIN_US = 500; // 0 rad +const uint16_t PULSE_MAX_US = 2400; // π rad +const uint16_t PERIOD_US = 20000; // 50Hz = 20ms +const float RAD_MIN = 0.0f; +const float RAD_MAX = PI; + +// ==================== 状态 ==================== +struct ServoState { + uint8_t pin; + float targetRad; + unsigned long lastCycle; +}; + +ServoState g_servos[NUM_SERVOS]; + +WiFiServer tcpServer(TCP_PORT); +WiFiClient client; + +struct TelemetryData { + YPR attitude; + RawData raw; +}; + +QueueHandle_t telemetryQueue = nullptr; +char commandBuffer[128]; +size_t commandLength = 0; +bool commandOverflow = false; + +// ==================== 前向声明 ==================== +bool isNumeric(const String& s); +uint8_t parseFloatList(const String& s, float* out, uint8_t maxCount); +void sendLine(const String& msg); +void sg90Task(void* parameter); +void sendQueuedTelemetry(); +int16_t encodeAngle(float degrees); +void writeInt16BE(uint8_t* buffer, size_t offset, int16_t value); +uint16_t calculateCrc16(const uint8_t* data, size_t length); + +// ==================== 舵机初始化 / 更新 ==================== + +void servoInit() { + for (uint8_t i = 0; i < NUM_SERVOS; i++) { + g_servos[i].pin = SERVO_PINS[i]; + g_servos[i].targetRad = PI / 2.0f; // 默认 90° + // 相位错开：第 i 个舵机在 20ms 周期内的 (i * 2.5ms) 处触发 + g_servos[i].lastCycle = micros() - + (unsigned long)i * (PERIOD_US / NUM_SERVOS); + + pinMode(SERVO_PINS[i], OUTPUT); + digitalWrite(SERVO_PINS[i], LOW); + } +} + +/** + * @brief 更新 8 个舵机的 PWM，由 SG90 通讯任务周期调用 + */ +void servoUpdateAll() { + unsigned long now = micros(); + for (uint8_t i = 0; i < NUM_SERVOS; i++) { + ServoState& s = g_servos[i]; + + if ((unsigned long)(now - s.lastCycle) >= PERIOD_US) { + s.lastCycle += PERIOD_US; + + // 若长时间滞后（例如遇到耗时长的指令），重新对齐到当前时刻 + if ((unsigned long)(now - s.lastCycle) >= PERIOD_US) { + s.lastCycle = now; + } + + // 弧度 -> 脉宽（µs） + float rad = s.targetRad; + if (rad < RAD_MIN) rad = RAD_MIN; + if (rad > RAD_MAX) rad = RAD_MAX; + + uint16_t pulse = PULSE_MIN_US + + (uint16_t)((PULSE_MAX_US - PULSE_MIN_US) * + (rad / PI) + 0.5f); + + digitalWrite(s.pin, HIGH); + delayMicroseconds(pulse); + digitalWrite(s.pin, LOW); + } + } +} + +// ==================== WiFi / TCP ==================== + +void initWiFiAP() { + WiFi.mode(WIFI_AP); + WiFi.softAP(AP_SSID, AP_PASS); + Serial.println("==== WiFi AP 已启动 ===="); + Serial.print("SSID : "); Serial.println(AP_SSID); + Serial.print("Password: "); Serial.println(AP_PASS); + Serial.print("AP IP : "); Serial.println(WiFi.softAPIP()); +} + +void initTcpServer() { + tcpServer.begin(); + tcpServer.setNoDelay(true); + Serial.printf("TCP 服务器已启动，端口: %d\n", TCP_PORT); +} + +void sendLine(const String& msg) { + if (client && client.connected()) { + client.println(msg); + } +} + +void acceptClientIfNeeded() { + if (!client || !client.connected()) { + client = tcpServer.available(); + if (client) { + commandLength = 0; + commandOverflow = false; + Serial.print("上位机已连接: "); + Serial.println(client.remoteIP()); + sendLine("ESP32: 已连接。请发送 8 个角度，例如 " + "RAD:1.57,1.57,1.57,1.57,1.57,1.57,1.57,1.57"); + } else { + commandLength = 0; + commandOverflow = false; + } + } +} + +// ==================== 解析工具 ==================== + +bool isNumeric(const String& s) { + if (s.length() == 0) return false; + bool dotSeen = false; + for (size_t i = 0; i < s.length(); i++) { + char c = s.charAt(i); + if (c == '-' || c == '+') { + if (i != 0) return false; + } else if (c == '.') { + if (dotSeen) return false; + dotSeen = true; + } else if (!isDigit(c)) { + return false; + } + } + return true; +} + +/** + * @brief 解析逗号分隔的浮点列表 + * @return 成功解析出的元素个数（遇到非法 token 立即停止） + */ +uint8_t parseFloatList(const String& s, float* out, uint8_t maxCount) { + uint8_t count = 0; + int start = 0; + int len = s.length(); + + while (start < len && count < maxCount) { + int comma = s.indexOf(',', start); + String token; + if (comma < 0) { + token = s.substring(start); + start = len; + } else { + token = s.substring(start, comma); + start = comma + 1; + } + token.trim(); + if (token.length() == 0) continue; + + if (!isNumeric(token)) break; // 非法 token -> 放弃后续 + out[count++] = token.toFloat(); + } + return count; +} + +// ==================== 指令处理 ==================== + +void sendCurrentState() { + String msg = "ESP32:"; + for (uint8_t i = 0; i < NUM_SERVOS; i++) { + msg += " S"; + msg += String(i); + msg += "="; + msg += String(g_servos[i].targetRad, 4); + } + sendLine(msg); +} + +/** + * @brief 支持指令： + * RAD:r0,r1,...,r7 弧度值列表 + * DEG:d0,d1,...,d7 角度值列表（内部转弧度） + * r0,r1,...,r7 纯数字按弧度处理 + * GET 查询当前 8 路角度 + * PING 心跳 + */ +void processCommand(const String& raw) { + String cmd = raw; + cmd.trim(); + if (cmd.length() == 0) return; + + if (cmd == "PING") { sendLine("ESP32: PONG"); return; } + if (cmd == "GET") { sendCurrentState(); return; } + + float values[NUM_SERVOS]; + uint8_t n = 0; + bool inDeg = false; + + if (cmd.startsWith("RAD:")) { + n = parseFloatList(cmd.substring(4), values, NUM_SERVOS); + } else if (cmd.startsWith("DEG:")) { + n = parseFloatList(cmd.substring(4), values, NUM_SERVOS); + inDeg = true; + } else if (cmd.startsWith("SET:")) { + n = parseFloatList(cmd.substring(4), values, NUM_SERVOS); + } else { + n = parseFloatList(cmd, values, NUM_SERVOS); + } + + if (n == 0) { + sendLine("ESP32: 无法解析 -> " + cmd); + return; + } + + // 写入目标角度 + for (uint8_t i = 0; i < n && i < NUM_SERVOS; i++) { + float rad = inDeg ? (values[i] * PI / 180.0f) : values[i]; + if (rad < RAD_MIN) rad = RAD_MIN; + if (rad > RAD_MAX) rad = RAD_MAX; + g_servos[i].targetRad = rad; + } + + // 简单日志（不要频繁打印，避免影响时序） + Serial.printf("SET %u servo(s)\n", n); + + sendLine("ESP32: OK 设置 " + String(n) + " 个舵机"); +} + +void handleClientCommand() { + if (!client || !client.connected()) return; + + uint16_t bytesRead = 0; + while (client.available() && bytesRead < sizeof(commandBuffer)) { + int incoming = client.read(); + if (incoming < 0) break; + bytesRead++; + + if (incoming == '\n') { + if (commandOverflow) { + sendLine("ESP32: 指令过长"); + } else { + commandBuffer[commandLength] = '\0'; + processCommand(String(commandBuffer)); + } + commandLength = 0; + commandOverflow = false; + } else if (!commandOverflow) { + if (commandLength < sizeof(commandBuffer) - 1) { + commandBuffer[commandLength++] = static_cast(incoming); + } else { + commandOverflow = true; + } + } + } +} + +void sendQueuedTelemetry() { + if (!client || !client.connected()) { + return; + } + + TelemetryData data; + if (telemetryQueue == nullptr || + xQueueReceive(telemetryQueue, &data, 0) != pdTRUE) { + return; + } + + // Frame: A5 5A, 3 angles (centidegrees), 6 raw int16 values, CRC16-CCITT. + uint8_t packet[22] = {0xA5, 0x5A}; + writeInt16BE(packet, 2, encodeAngle(data.attitude.yaw)); + writeInt16BE(packet, 4, encodeAngle(data.attitude.pitch)); + writeInt16BE(packet, 6, encodeAngle(data.attitude.roll)); + writeInt16BE(packet, 8, data.raw.ax); + writeInt16BE(packet, 10, data.raw.ay); + writeInt16BE(packet, 12, data.raw.az); + writeInt16BE(packet, 14, data.raw.gx); + writeInt16BE(packet, 16, data.raw.gy); + writeInt16BE(packet, 18, data.raw.gz); + uint16_t crc = calculateCrc16(packet, 20); + packet[20] = static_cast(crc >> 8); + packet[21] = static_cast(crc); + + if (client.write(packet, sizeof(packet)) != sizeof(packet)) { + Serial.println("警告：IMU 二进制数据发送不完整"); + } +} + +int16_t encodeAngle(float degrees) { + long scaled = lroundf(degrees * 100.0f); + if (scaled > 32767L) scaled = 32767L; + if (scaled < -32768L) scaled = -32768L; + return static_cast(scaled); +} + +void writeInt16BE(uint8_t* buffer, size_t offset, int16_t value) { + uint16_t encoded = static_cast(value); + buffer[offset] = static_cast(encoded >> 8); + buffer[offset + 1] = static_cast(encoded); +} + +uint16_t calculateCrc16(const uint8_t* data, size_t length) { + uint16_t crc = 0xFFFF; + for (size_t i = 0; i < length; i++) { + crc ^= static_cast(data[i]) << 8; + for (uint8_t bit = 0; bit < 8; bit++) { + crc = (crc & 0x8000) + ? static_cast((crc << 1) ^ 0x1021) + : static_cast(crc << 1); + } + } + return crc; +} + +void sg90Task(void* parameter) { + (void)parameter; + for (;;) { + sg90(); + sendQueuedTelemetry(); + vTaskDelay(1); + } +} +void sendLineData(YPR a, RawData r) { + if (telemetryQueue == nullptr) { + return; + } + + TelemetryData data = {a, r}; + xQueueOverwrite(telemetryQueue, &data); +} + +// ==================== Arduino 主入口 ==================== + +void wifi_setup() { + Serial.begin(115200); + servoInit(); + initWiFiAP(); + initTcpServer(); + + telemetryQueue = xQueueCreate(1, sizeof(TelemetryData)); + if (telemetryQueue == nullptr) { + Serial.println("错误：无法创建 IMU 遥测队列"); + return; + } + + BaseType_t taskCreated = xTaskCreatePinnedToCore( + sg90Task, "sg90", 4096, nullptr, 2, nullptr, 1); + if (taskCreated != pdPASS) { + Serial.println("错误：无法创建 SG90 通讯任务"); + vQueueDelete(telemetryQueue); + telemetryQueue = nullptr; + } +} + +void sg90() { + acceptClientIfNeeded(); // 1) 接入客户端 + handleClientCommand(); // 2) 解析上位机指令 + servoUpdateAll(); // 3) 非阻塞更新 8 路 PWM +} \ 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