Done

.vscode/settings.json

@@ -1,6 +1,6 @@
 {
   "C_Cpp.intelliSenseEngine": "default",
-  "idf.port": "/dev/ttyUSB0",
+  "idf.port": "/dev/ttyUSB1",
   "idf.flashType": "UART",
   "files.associations": {
     "array": "cpp",

main/CMakeLists.txt

@@ -6,5 +6,6 @@ idf_component_register(SRCS "main.cpp"
     "vec/src/vector2f.cpp"
     "vec/src/matrix2f.cpp"
     "src/catchPoint.cpp"
+    "src/wifiServer.cpp"
     INCLUDE_DIRS "include"
         "vec/include")

main/include/catchPoint.h

@@ -7,27 +7,42 @@
 #include "moveInfo.h"
 #include "ps2_controller.h"
 #include <string>
+#include <math.h>
+#include "wifiServer.h"
 class CatchPoint
 {
 public:
-    CatchPoint(const Vector3f &p=Vector3f(0,0,ARM_LENGTH_1+ARM_LENGTH_2)) : target3(p), o(Vector3f::ZERO)
+    CatchPoint() : o(Vector3f::ZERO)
     {
+        lastTry=true;
+        angleBase=PI/2;
+        angle=Vector2f(PI/2,0);
+        target3=Vector3f(0,ARM_LENGTH_1*std::cos(angle[0])+ARM_LENGTH_2*std::cos(angle[1]+angle[0]-PI/2),
+        ARM_LENGTH_1*std::sin(angle[0])+ARM_LENGTH_2*std::sin(angle[1]+angle[0]-PI/2));
+        target2 = Vector2f((target3.xy().length()), target3.z());
     };
     ~CatchPoint() = default;
-    void move(const PS2Controller &ps2);
+    bool move(const PS2Controller &ps2);
-    bool solution(double Eps);
+    bool move(WifiServer &wifi);
-    void setSteerInfo();
+    bool solution(Vector3f &tar,double Eps);
+    void setSteerInfo(Vector2f &sol,int Speed=CATCHER_TIME);
     std::string getSteerInfo();
+    void reset();
+    void getBall();
 private:
     void calculateJacobi(Matrix2f &jacobi,Vector2f &f,Vector2f &sol);
     Vector3f target3;
     Vector2f target2;
-    double angle_base;
+    double angleBase;
+    double pawState;//0~1
     Vector2f angle;
     Vector3f o;
     
-    MoveInfo steer_base;
+    MoveInfo steerBase;
     MoveInfo steer_1;
     MoveInfo steer_2;
+    MoveInfo steerPaw;
+
+    bool lastTry;//上次检测是否为无解
 };
 #endif // CATCHPOINT_H

main/include/parameter.h

@@ -1,9 +1,17 @@
 #ifndef PARAMETER_H
 #define PARAMETER_H
 #include <string>
+
+#define UART_PORT UART_NUM_2
+#define TXD GPIO_NUM_17
+#define RXD GPIO_NUM_16
+
+static const int MAX_WHEEL_STEER_SPEED = 100;
 static const int MAX_WHEEL_SPEED = 1000;
 static const int MAX_STEER_PWM = 700;
-static const double CATCHER_SPEED = 0.01;
+static const double CATCHER_SPEED = 0.1;
+static const double WIFI_CATCHER_SPEED = 0.15;
+static const double PAW_SPEED=0.01;
 
 static const std::string WHEEL_FL="001";
 static const std::string WHEEL_FR="002";
@@ -20,13 +28,13 @@ static const std::string CATCHER_2="022";
 static const std::string CATCHER_3="023";
 static const std::string CATCHER_4="024";
 
-static const int CATCHER_TIME= 1000;
+static const int CATCHER_TIME= 3;
 static const int FRAME_TIME= 100;
-static const double EPS=1e-4;
+static const double EPSSILON=1e-4;
 
 static const double H=0;
-static const double ARM_LENGTH_1=5;
+static const double ARM_LENGTH_1=15;
-static const double ARM_LENGTH_2=5;
+static const double ARM_LENGTH_2=20;
 
 static const double PI=3.1415926535;
 

main/include/ps2_controller.h

@@ -44,7 +44,6 @@ public:
 
     bool ButtonPressed(uint16_t button) const;
     bool Button(uint16_t button) const;
-    bool updateStatic();
     void launch();
     
 private:

main/include/wifiServer.h

@@ -0,0 +1,129 @@
+#ifndef WIFI_SERVER_H
+#define WIFI_SERVER_H
+
+#include <string>
+#include <vector>
+#include "parameter.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_system.h"
+#include "esp_wifi.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "nvs_flash.h"
+#include "lwip/err.h"
+#include "lwip/sys.h"
+#include "lwip/sockets.h"
+#include "driver/gpio.h"
+#include <driver/uart.h>
+
+// 常量定义
+#define READ_DELAY_MS 10
+#define SHORT_DELAY_MS 1
+// 按钮常量定义
+#define BUTTON_SELECT 0x0001
+#define BUTTON_L3 0x0002
+#define BUTTON_R3 0x0004
+#define BUTTON_START 0x0008
+#define BUTTON_PAD_UP 0x0010
+#define BUTTON_PAD_RIGHT 0x0020
+#define BUTTON_PAD_DOWN 0x0040
+#define BUTTON_PAD_LEFT 0x0080
+#define BUTTON_L2 0x0100
+#define BUTTON_R2 0x0200
+#define BUTTON_L1 0x0400
+#define BUTTON_R1 0x0800
+#define BUTTON_TRIANGLE 0x1000
+#define BUTTON_CIRCLE 0x2000
+#define BUTTON_CROSS 0x4000
+#define BUTTON_SQUARE 0x8000
+
+#define W_LX 0
+#define W_LY 1
+#define W_RX 2
+#define W_RY 3
+#define W_UP 0
+#define W_DOWN 1
+#define W_FORWARD 2
+#define W_BACKWARD 3
+#define W_RIGHT 4
+#define W_LEFT 5
+class WifiServer
+{
+public:
+    // 初始化配置
+    struct Config
+    {
+        const char *wifi_ssid;
+        const char *wifi_pass;
+        uart_port_t uart_port;
+        gpio_num_t txd_pin;
+        gpio_num_t rxd_pin;
+        int web_port = 80;
+        int max_clients = 5;
+    };
+
+    WifiServer(const Config &config);
+
+    int get_reset(){return reset;}
+    int get_ball(){return getBall;}
+    double* get_catcherData(){return catcherData;}
+    double* get_carData(){return carData;}
+
+    double getUP(){return catcherData[W_UP];}
+    double getDOWN(){return catcherData[W_DOWN];}
+    double getLEFT(){return catcherData[W_LEFT];}
+    double getRIGHT(){return catcherData[W_RIGHT];}
+    double getFORWARD(){return catcherData[W_FORWARD];}
+    double getBACKWARD(){return catcherData[W_BACKWARD];}
+
+    double getLx(){return carData[W_LX];};
+    double getLy(){return carData[W_LY];};
+    double getRx(){return carData[W_RX];};
+    double getRy(){return carData[W_RY];};
+    double getPaw(){return pawData;}
+    // 启动服务
+    void start();
+    void normlized();
+    // 停止服务
+    void stop();
+    void print();
+private:
+    double carData[4];
+    double catcherData[6];
+    int reset;
+    int getBall;
+    double pawData;
+    // 解析 GET 参数
+    std::pair<std::string, std::vector<std::pair<std::string, std::string>>>
+    parse_get_params(const std::string &request);
+
+    // WiFi 初始化
+    void wifi_init_sta();
+
+    // HTTP 服务器任务
+    static void udp_server_task(void *arg);
+
+    // 任务句柄
+    TaskHandle_t server_task_handle = nullptr;
+
+    // 配置
+    Config config;
+
+    // HTML 响应内容
+    const char *html_response =
+        "HTTP/1.1 200 OK\r\n"
+        "Content-Type: text/html\r\n\r\n"
+        "<!DOCTYPE html>"
+        "<html>"
+        "<head>"
+        "    <title>MarsCar Web Server</title>"
+        "</head>"
+        "<body>"
+        "    <h1>Hello from ESP32!</h1>"
+        "    <p>这是一个运行在 ESP32 上的基本 Web 服务器。</p>"
+        "</body>"
+        "</html>";
+};
+
+#endif // WIFI_SERVER_H

main/main.cpp

@@ -7,32 +7,39 @@
 #include "esp_timer.h"
 #include "uartTool.h"
 #include "catchPoint.h"
+#include "wifiServer.h"
 #include <iostream>
 extern "C" void app_main()
 {
-    PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23);
+    //PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23);
-    ps2.launch();
+    //ps2.launch();
     CatchPoint catcher;
     UartTool uartTool(UART_NUM_2, GPIO_NUM_17, GPIO_NUM_16);
+
+    WifiServer::Config config;
+    config.wifi_ssid = "ahxbxa";
+    config.wifi_pass = "12345678";
+    config.uart_port = UART_PORT;
+    config.txd_pin = TXD;
+    config.rxd_pin = RXD;
+    WifiServer server(config);
+    server.start();
+    
     while (1)
     {
         // ps2.updateStatic();
-        WheelMoveInfo wheel(WHEEL_FL, WHEEL_FR, WHEEL_BL, WHEEL_BR, ps2.getLx(), ps2.getLy());
+        WheelMoveInfo wheel(WHEEL_FL, WHEEL_FR, WHEEL_BL, WHEEL_BR,server.getLx(), server.getLy());
         std::string cmd1 = wheel.getWheelInfo();
 
-        SteerMoveInfo steer(STEER_FL, STEER_FR, STEER_BL, STEER_BR, ps2.getRx(), ps2.getRy());
+        SteerMoveInfo steer(STEER_FL, STEER_FR, STEER_BL, STEER_BR, server.getRx(), server.getRy());
         std::string cmd2 = steer.getSteerInfo();
 
-        catcher.move(ps2);
         std::string cmd3 = "";
-        if (catcher.solution(1e-2))
+        catcher.move(server);
-        {
+            cmd3 = catcher.getSteerInfo();
-            std::string cmd3 = catcher.getSteerInfo();
+
-            std::cout<<cmd3<<std::endl;
-       }
-        //printf("update!\n");
         uartTool.sendData("Move", cmd1+cmd2+cmd3);
-        // uartTool.sendData("SteerMove","#013P1500T0100!");
+       // std::cout<<" "<<cmd3<<std::endl;
         
         vTaskDelay(10 / portTICK_PERIOD_MS); // 延
     }

main/src/catchPoint.cpp

@@ -4,48 +4,141 @@
 #include "matrix2f.h"
 #include "vector2f.h"
 #include "ps2_controller.h"
-void CatchPoint::move(const PS2Controller &ps2)
+
+void CatchPoint::reset()
 {
+    lastTry = true;
+    pawState = 0;
+    angleBase = PI / 2;
+    angle = Vector2f(PI / 2 + PI / 12, 0);
+    target3 = Vector3f(0, ARM_LENGTH_1 * std::cos(angle[0]) + ARM_LENGTH_2 * std::cos(angle[1] + angle[0] - PI / 2),
+                       ARM_LENGTH_1 * std::sin(angle[0]) + ARM_LENGTH_2 * std::sin(angle[1] + angle[0] - PI / 2));
+    target2 = Vector2f((target3.xy().length()), target3.z());
+    setSteerInfo(angle, 1000);
+    vTaskDelay(100 / portTICK_PERIOD_MS);
+}
+
+void CatchPoint::getBall()
+{
+    lastTry = true;
+    pawState = 1; // TODO
+    angleBase = PI*1.4;
+    angle = Vector2f(1.7, -0.37);
+    double y = ARM_LENGTH_1 * std::cos(angle[0]) + ARM_LENGTH_2 * std::cos(angle[1] + angle[0] - PI / 2);
+    target3 = Vector3f(std::cos(angleBase) * y, std::sin(angleBase) * y,
+                       ARM_LENGTH_1 * std::sin(angle[0]) + ARM_LENGTH_2 * std::sin(angle[1] + angle[0] - PI / 2));
+    target2 = Vector2f((target3.xy().length()), target3.z());
+
+    setSteerInfo(angle, 1000);
+    vTaskDelay(100 / portTICK_PERIOD_MS);
+}
+bool CatchPoint::move(const PS2Controller &ps2)
+{
+    if (ps2.Button(BUTTON_START))
+    {
+        reset();
+        return false;
+    }
+    if (ps2.Button(BUTTON_CIRCLE) && pawState < 1)
+        pawState += PAW_SPEED;
+    else if (ps2.Button(BUTTON_SQUARE) && pawState > 0)
+        pawState -= PAW_SPEED;
+
     Vector3f speed = Vector3f::ZERO;
+    double r = 1;
+    if (!lastTry)
+        r = 5;
     if (ps2.Button(BUTTON_PAD_UP))
         speed[1] += 1;
     if (ps2.Button(BUTTON_PAD_DOWN))
         speed[1] -= 1;
     if (ps2.Button(BUTTON_PAD_RIGHT))
-        speed[0] -= 1;
-    if (ps2.Button(BUTTON_PAD_LEFT))
         speed[0] += 1;
+    if (ps2.Button(BUTTON_PAD_LEFT))
+        speed[0] -= 1;
     if (ps2.Button(BUTTON_TRIANGLE))
         speed[2] += 1;
     if (ps2.Button(BUTTON_CROSS))
         speed[2] -= 1;
-    if (speed.length() > EPS)
+    if (speed.length() > EPSSILON)
-        speed = speed.normalized() * CATCHER_SPEED;
+        speed = speed.normalized() * CATCHER_SPEED * r;
+
+    Vector3f temp = target3 + speed;
+    if (solution(temp, 1e-2))
+    {
         target3 += speed;
         target2 = Vector2f((target3.xy().length()), target3.z());
+        return true;
+    }
     // target3.print();
+    return false;
 }
 
-bool CatchPoint::solution(double Eps = 1e-2)
+bool CatchPoint::move(WifiServer &wifi)
+{
+    if (wifi.get_reset())
+    {
+        reset();
+        return false;
+    }
+    if (wifi.get_ball())
+    {
+        getBall();
+        return false;
+    }
+    if (wifi.getPaw() > 0.5 && pawState < 1)
+        pawState += (wifi.getPaw()-0.5)*PAW_SPEED;
+    else if (wifi.getPaw() < 0.5 && pawState > 0)
+        pawState += ((wifi.getPaw()-0.5))*PAW_SPEED;
+
+    Vector3f speed = Vector3f::ZERO;
+    speed[1] = wifi.getFORWARD() + wifi.getBACKWARD();
+    speed[0] = wifi.getRIGHT() + wifi.getLEFT();
+    speed[2] = wifi.getUP() + wifi.getDOWN();
+
+    // if (speed.length() > EPSSILON)
+    //  speed = speed.normalized() * WIFI_CATCHER_SPEED;
+
+    Vector3f temp = target3 + speed;
+    if (temp.length() > ARM_LENGTH_1 + ARM_LENGTH_2 - 0.1)
+        temp = temp.normalized() * (ARM_LENGTH_1 + ARM_LENGTH_2 - 0.1);
+    // speed.print();
+    // target3.print();
+    if (solution(temp, 1e-2))
+    {
+        target3 = temp;
+        target2 = Vector2f((target3.xy().length()), target3.z());
+
+        return true;
+    }
+    return false;
+}
+
+bool CatchPoint::solution(Vector3f &tar, double Eps = 1e-2)
 {
     bool success = false;
+    bool success2 = false;
+    angleBase = tar.xy().angleWithX();
 
-    angle_base = target3.xy().angleWithX();
+    Vector2f sol;
-
+    // 牛顿迭代50次,尝试10次不同初值
-    // 牛顿迭代20次,尝试10次不同初值
     for (int now = 0; now < 10; now++)
     {
+        // 在此时值扰动
         uint32_t random_integer = esp_random();
-        double random_double = (double)random_integer / UINT32_MAX;
+        double random_double = (double)random_integer * 2.0 / UINT32_MAX - 1;
-        Vector2f sol;
+
-        sol[0] = random_double * 2 * PI;
+        sol[0] = random_double * PI * 0.01;
         random_integer = esp_random();
         random_double = (double)random_integer / UINT32_MAX;
-        sol[1] = random_double * 2 * PI;
+        sol[1] = random_double * PI * 0.01;
+        sol += angle;
 
         Matrix2f jacobi;
         Vector2f f;
         double eps;
+        // 找到一个解
+        success = false;
         for (int i = 0; i < 50; i++)
         {
             int zero;
@@ -53,15 +146,16 @@ bool CatchPoint::solution(double Eps = 1e-2)
             Vector2f last_sol = sol;
             sol = sol - jacobi.inverse(zero) * f;
             eps = (sol - last_sol).length();
-            if (zero)
+
-                break;
             if (eps < Eps)
             {
                 success = true;
                 break;
             }
+            if (zero)
+                break;
         }
-
+        // 判断解可行性
         if (success)
         {
             while (sol[0] > PI)
@@ -73,19 +167,40 @@ bool CatchPoint::solution(double Eps = 1e-2)
             while (sol[1] < -PI)
                 sol[1] += 2 * PI;
 
+            Vector2f theta = target2.angleWithX();
+            Vector2f sol_2 = 2 * theta - sol;
+
             sol[1] = sol[1] - sol[0] + PI / 2;
+            sol_2[1] = sol_2[1] - sol_2[0] + PI / 2;
+
+            if ((angle - sol).squaredLength() > (angle - sol_2).squaredLength())
+                sol = sol_2;
+            // printf("%.2f ", (angle - sol).length());
+            if ((angle - sol).length() < 1)
+            {
+                success2 = true;
                 angle = sol;
-            setSteerInfo();
+                setSteerInfo(angle);
                 break;
             }
         }
+    }
 
-    if (success)
+    // printf("sol: ");
-        printf("有解! ");
+    // sol.print();
-    else
-        printf("当前钩爪无解！\n");
 
-    return success;
+    // printf("target: ");
+    // tar.print();
+
+    if (!success2)
+    {
+        printf("target: ");
+        tar.print();
+        printf("无解！ ");
+    }
+    // printf("\n");
+
+    return success2;
 }
 void CatchPoint::calculateJacobi(Matrix2f &jacobi, Vector2f &f, Vector2f &sol)
 {
@@ -94,18 +209,20 @@ void CatchPoint::calculateJacobi(Matrix2f &jacobi, Vector2f &f, Vector2f &sol)
     f = Vector2f(ARM_LENGTH_1 * std::cos(sol[0]) + ARM_LENGTH_2 * std::cos(sol[1]) - target2.x(),
                  ARM_LENGTH_1 * std::sin(sol[0]) + ARM_LENGTH_2 * std::sin(sol[1]) - target2.y());
 }
-void CatchPoint::setSteerInfo()
+void CatchPoint::setSteerInfo(Vector2f &sol, int Speed)
 {
-    double pwm = 1020 + (angle_base * 1.0f / PI) * 980;
+    double pwm = 1020 + (angleBase * 1.0f / PI) * 1000;
-    steer_base.setInfo(CATCHER_1, (int)pwm, CATCHER_TIME);
+    steerBase.setInfo(CATCHER_1, (int)pwm, Speed);
-    pwm = 850 + (angle[0] * 1.0f / PI) / 1300;
+    pwm = 850 + (sol[0] * 1.0f / PI) * 1300;
-    steer_1.setInfo(CATCHER_2, (int)pwm, CATCHER_TIME);
+    steer_1.setInfo(CATCHER_2, (int)pwm, Speed);
-    // TODO:
+    pwm = 850 + (sol[1] * 1.0f / PI) * 1300;
-    pwm = 0;
+    steer_2.setInfo(CATCHER_3, (int)pwm, Speed);
-    steer_2.setInfo(CATCHER_3, (int)pwm, CATCHER_TIME);
+
+    pwm = 1500 + (pawState - 0.5) * 900;
+    steerPaw.setInfo(CATCHER_4, (int)pwm, Speed);
 }
 std::string CatchPoint::getSteerInfo()
 {
-    std::string result = steer_base.getInfo() + steer_1.getInfo() + steer_2.getInfo();
+    std::string result = steerBase.getInfo() + steer_1.getInfo() + steer_2.getInfo() + steerPaw.getInfo();
     return result;
 }

main/src/moveInfo.cpp

@@ -27,9 +27,11 @@ WheelMoveInfo::WheelMoveInfo(std::string fl, std::string fr, std::string bl, std
     { return x > 0 ? x : -x; };
     auto max = [](auto x, auto y)
     { return x > y ? x : y; };
+    auto test = [](auto x)
+    {if(x>1.0||x<-1.0)return 0.0;return x; };
 
-    double x = (lx * 1.0 - 255.0 / 2) / 127.5;
+    double x = test((lx * 1.0 - 255.0 / 2) / 127.5);
-    double y = ((255.0 - ly) * 1.0 - 255.0 / 2) / 127.5;
+    double y = test(((255.0 - ly) * 1.0 - 255.0 / 2) / 127.5);
     double leftRatio, rightRatio;
     if (x < 0 && y > 0)
     {
@@ -82,13 +84,14 @@ SteerMoveInfo::SteerMoveInfo(std::string fl, std::string fr, std::string bl, std
     {if(x<-1)return -1.0;else if(x>1)return 1.0;else return x; };
     double x = (rx * 1.0 - 255.0 / 2) / 127.5;
     double y = ((255.0 - ry) * 1.0 - 255.0 / 2) / 127.5;
-    double frontRatio = clamp(x*1.0 - y*1.0);
-    double backRatio = -y;
 
-    front_left.setInfo(fl, 1500 + frontRatio * MAX_STEER_PWM, FRAME_TIME);
+    double frontRatio = clamp(x * 1.0 - y * 1.0);
-    back_left.setInfo(bl, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME);
+    double backRatio = clamp(-x * 1.0 - y * 1.0);
-    front_right.setInfo(fr, 1500 + frontRatio * MAX_STEER_PWM, FRAME_TIME);
+
-    back_right.setInfo(br, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME);
+    front_left.setInfo(fl, 1450 + frontRatio * MAX_STEER_PWM, MAX_WHEEL_STEER_SPEED);
+    back_left.setInfo(bl, 1500 + backRatio * MAX_STEER_PWM, MAX_WHEEL_STEER_SPEED);
+    front_right.setInfo(fr, 1500 + frontRatio * MAX_STEER_PWM, MAX_WHEEL_STEER_SPEED);
+    back_right.setInfo(br, 1600 + backRatio * MAX_STEER_PWM, MAX_WHEEL_STEER_SPEED);
 }
 std::string SteerMoveInfo::getSteerInfo()
 {

main/src/wifiServer.cpp

@@ -0,0 +1,217 @@
+#include "wifiServer.h"
+#include <algorithm>
+#include <cstdlib> // 包含 atoi 和 strtok_r 函数
+#include <cstring> // 包含 strtok_r 函数
+#include <string>
+static const char *TAG = "WIFI_UDP_SERVER";
+
+WifiServer::WifiServer(const Config &config) : config(config)
+{
+    // 初始化数据
+    for (int i = 0; i < 4; i++)
+        carData[i] = 127.5;
+    for (int i = 0; i < 6; i++)
+        catcherData[i] = 0;
+    reset = 0;
+    pawData = 0;
+    getBall=0;
+    // 初始化 NVS
+    esp_err_t ret = nvs_flash_init();
+    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
+    {
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        ret = nvs_flash_init();
+    }
+    ESP_ERROR_CHECK(ret);
+}
+
+void WifiServer::start()
+{
+    // 初始化 WiFi
+    wifi_init_sta();
+
+    // 启动 UDP 服务器任务
+    xTaskCreate(udp_server_task, "udp_server", 8192, this, 5, &server_task_handle);
+}
+
+void WifiServer::stop()
+{
+    if (server_task_handle)
+    {
+        vTaskDelete(server_task_handle);
+        server_task_handle = nullptr;
+    }
+}
+
+void WifiServer::wifi_init_sta()
+{
+    ESP_ERROR_CHECK(esp_netif_init());
+    ESP_ERROR_CHECK(esp_event_loop_create_default());
+    esp_netif_t *sta_netif = esp_netif_create_default_wifi_sta();
+    assert(sta_netif);
+
+    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
+
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+
+    wifi_config_t wifi_config = {};
+    strcpy((char *)wifi_config.sta.ssid, config.wifi_ssid);
+    strcpy((char *)wifi_config.sta.password, config.wifi_pass);
+    // wifi_config.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK; // 此处配置项已不推荐使用
+
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
+    ESP_ERROR_CHECK(esp_wifi_start());
+    ESP_LOGI(TAG, "Disabling WiFi power save mode for low latency.");
+    ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
+    ESP_ERROR_CHECK(esp_wifi_connect());
+
+    ESP_LOGI(TAG, "正在连接到 WiFi...");
+
+    // 等待连接
+    int retry = 0;
+    while (true)
+    {
+        // 使用 event loop 来等待连接成功事件，而不是轮询
+        // 这里为了简单，暂时保留轮询，但在生产环境中建议使用事件处理
+        wifi_ap_record_t ap;
+        if (esp_wifi_sta_get_ap_info(&ap) == ESP_OK)
+        {
+            ESP_LOGI(TAG, "WiFi 连接成功!");
+            esp_netif_ip_info_t ip_info;
+            esp_netif_get_ip_info(sta_netif, &ip_info);
+            ESP_LOGI(TAG, "设备 IP 地址: " IPSTR, IP2STR(&ip_info.ip));
+            break;
+        }
+        if (retry++ > 20)
+        {
+            ESP_LOGE(TAG, "连接 WiFi 失败!");
+            esp_restart();
+        }
+        vTaskDelay(1000 / portTICK_PERIOD_MS);
+        ESP_LOGI(TAG, ".");
+    }
+}
+
+void WifiServer::udp_server_task(void *arg)
+{
+    WifiServer *server = static_cast<WifiServer *>(arg);
+    const Config &config = server->config;
+
+    int server_fd;
+    struct sockaddr_in server_addr, client_addr;
+    socklen_t socklen = sizeof(client_addr);
+    char rx_buffer[1024];
+
+    // 创建 UDP socket
+    if ((server_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
+    {
+        ESP_LOGE(TAG, "创建 socket 失败");
+        vTaskDelete(NULL);
+    }
+
+    // 绑定地址
+    memset(&server_addr, 0, sizeof(server_addr));
+    server_addr.sin_family = AF_INET;
+    server_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    server_addr.sin_port = htons(config.web_port); // 使用相同的端口号
+
+    if (bind(server_fd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)
+    {
+        ESP_LOGE(TAG, "绑定端口失败");
+        close(server_fd);
+        vTaskDelete(NULL);
+    }
+
+    ESP_LOGI(TAG, "UDP 服务器正在监听端口 %d...", config.web_port);
+
+    while (true)
+    {
+        // 接收 UDP 数据
+        int len = recvfrom(server_fd, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&client_addr, &socklen);
+
+        if (len > 0)
+        {
+            rx_buffer[len] = '\0';
+
+            char client_ip[16];
+            inet_ntoa_r(client_addr.sin_addr, client_ip, sizeof(client_ip));
+
+            for (int i = 0; i < 4; i++)
+                server->carData[i] = 127.5;
+            for (int i = 0; i < 6; i++)
+                server->catcherData[i] = 0;
+            server->reset = 0;
+            server->pawData = 0;
+            server->getBall=0;
+
+            std::string testTimeValue;
+            char *saveptr;
+            char *pair = strtok_r(rx_buffer, "&", &saveptr);
+            while (pair != NULL)
+            {
+                char *eq_pos = strchr(pair, '=');
+                if (eq_pos != NULL)
+                {
+                    *eq_pos = '\0';
+                    const char *key = pair;
+                    const char *value = eq_pos + 1;
+
+                    if (strcmp(key, "TESTTIME") == 0)
+                    {
+                        testTimeValue = value;
+                    }
+                    else if (strcmp(key, "LX") == 0)
+                        server->carData[W_LX] = std::atof(value);
+                    else if (strcmp(key, "LY") == 0)
+                        server->carData[W_LY] = std::atof(value);
+                    else if (strcmp(key, "RX") == 0)
+                        server->carData[W_RX] = std::atof(value);
+                    else if (strcmp(key, "RY") == 0)
+                        server->carData[W_RY] = std::atof(value);
+                    else if (strcmp(key, "UP") == 0)
+                        server->catcherData[W_UP] = std::atof(value);
+                    else if (strcmp(key, "DOWN") == 0)
+                        server->catcherData[W_DOWN] = std::atof(value);
+                    else if (strcmp(key, "FORWARD") == 0)
+                        server->catcherData[W_FORWARD] = std::atof(value);
+                    else if (strcmp(key, "BACKWARD") == 0)
+                        server->catcherData[W_BACKWARD] = std::atof(value);
+                    else if (strcmp(key, "RIGHT") == 0)
+                        server->catcherData[W_RIGHT] = std::atof(value);
+                    else if (strcmp(key, "LEFT") == 0)
+                        server->catcherData[W_LEFT] = std::atof(value);
+                    else if (strcmp(key, "RESET") == 0)
+                        server->reset = std::atof(value);
+                    else if (strcmp(key, "PAW") == 0)
+                        server->pawData = std::atof(value);
+                    else if (strcmp(key, "GETBALL") == 0)
+                        server->getBall = std::atoi(value);
+                }
+                pair = strtok_r(NULL, "&", &saveptr);
+            }
+            //server->print();
+            if (!testTimeValue.empty())
+            {
+                std::string responseStr = "TESTTIME=" + testTimeValue;
+                sendto(server_fd, responseStr.c_str(), responseStr.length(), 0, (struct sockaddr *)&client_addr, socklen);
+            }
+        }
+    }
+
+    close(server_fd);
+    vTaskDelete(NULL);
+}
+void WifiServer::print()
+{
+    printf("LX=%.2f, LY=%.2f, RX=%.2f, RY=%.2f, FORWARD=%.2f, BACKWARD=%.2f, LEFT=%.2f, RIGHT=%.2f, UP=%.2f, DOWN=%.2f\n, RESET=%d\n", carData[W_LX], carData[W_LY], carData[W_RX], carData[W_RY], catcherData[W_FORWARD], catcherData[W_BACKWARD], catcherData[W_LEFT], catcherData[W_RIGHT], catcherData[W_UP], catcherData[W_DOWN], reset);
+}
+void WifiServer::normlized()
+{
+
+    for (int i = 0; i < 4; i++)
+        carData[i] = 127.5;
+    for (int i = 0; i < 6; i++)
+        catcherData[i] = 0;
+    reset = 0;
+}

main/vec/include/matrix2f.h

@@ -13,7 +13,7 @@ public:
     float &operator()(int i, int j);
 
     float determinant();
-    Matrix2f inverse(int &error,float epsilon = EPS);
+    Matrix2f inverse(int &error,float epsilon = EPSSILON);
 
     void print();
 

main/vec/include/vector2f.h

@@ -38,7 +38,7 @@ public:
     double angleWithX()
     {
         double angle = std::atan2(e[1], e[0]);
-        if (angle < 0)
+        if (angle < -PI/2)
             angle += 2 * PI;
         return angle;
     }

main/vec/src/vector2f.cpp

@@ -91,7 +91,7 @@ Vector2f Vector2f::normalized() const
 }
 void Vector2f::print() const
 {
-    printf("< %.4f, %.4f >\n",
+    printf("< %.4f, %.4f > ",
            e[0], e[1]);
 }
 
@@ -169,7 +169,7 @@ Vector2f operator/(const Vector2f &v, float f)
 
 bool operator==(const Vector2f &v0, const Vector2f &v1)
 {
-	return (v0-v1).length()<EPS;
+	return (v0-v1).length()<EPSSILON;
 }
 
 bool operator!=(const Vector2f &v0, const Vector2f &v1)

main/vec/src/vector3f.cpp

@@ -115,7 +115,7 @@ Vector3f Vector3f::normalized() const
 }
 void Vector3f::print() const
 {
-    printf("< %.4f, %.4f, %.4f >\n",
+    printf("< %.4f, %.4f, %.4f > ",
            e[0], e[1], e[2]);
 }
 
@@ -196,7 +196,7 @@ Vector3f operator/(const Vector3f &v, float f)
 
 bool operator==(const Vector3f &v0, const Vector3f &v1)
 {
-    return (v0 - v1).length() < EPS;
+    return (v0 - v1).length() < EPSSILON;
 }
 
 bool operator!=(const Vector3f &v0, const Vector3f &v1)