first commit

.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,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"
+    ]
+}

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,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

sketch_oct8a.code-workspace

@@ -0,0 +1,12 @@
+{
+	"folders": [
+		{
+			"path": "../../../Arduino/sketch_oct8a"
+		},
+		{
+			"name": "TwoFootBot-arduion",
+			"path": "."
+		}
+	],
+	"settings": {}
+}

src/imu/imu.cpp

@@ -0,0 +1,81 @@
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "Wire.h"
+#include <MPU6050.h>
+#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;
+}

src/imu/imu.h

@@ -0,0 +1,27 @@
+#ifndef IMU_DATA_H
+#define IMU_DATA_H
+
+#include <Arduino.h>
+
+// 返回姿态角（单位：度）
+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

src/main.cpp

@@ -0,0 +1,25 @@
+
+#include "imu/imu.h"
+#include "wifi/wifi-sg90.h"
+#include <esp32-hal.h>
+
+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);
+  }
+}

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

src/wifi/wifi.cpp

@@ -0,0 +1,381 @@
+#include <WiFi.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
+#include <math.h>
+#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<char>(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<uint8_t>(crc >> 8);
+    packet[21] = static_cast<uint8_t>(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<int16_t>(scaled);
+}
+
+void writeInt16BE(uint8_t* buffer, size_t offset, int16_t value) {
+    uint16_t encoded = static_cast<uint16_t>(value);
+    buffer[offset] = static_cast<uint8_t>(encoded >> 8);
+    buffer[offset + 1] = static_cast<uint8_t>(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<uint16_t>(data[i]) << 8;
+        for (uint8_t bit = 0; bit < 8; bit++) {
+            crc = (crc & 0x8000)
+                ? static_cast<uint16_t>((crc << 1) ^ 0x1021)
+                : static_cast<uint16_t>(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
+}

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