From 979f33becefb45504f5706dfb15daa39fc480306 Mon Sep 17 00:00:00 2001 From: sushi rui Date: Thu, 3 Jul 2025 09:31:39 +0800 Subject: [PATCH] Done --- .vscode/settings.json | 2 +- main/CMakeLists.txt | 1 + main/include/catchPoint.h | 29 +++-- main/include/parameter.h | 18 ++- main/include/ps2_controller.h | 3 +- main/include/wifiServer.h | 129 ++++++++++++++++++++ main/main.cpp | 37 +++--- main/src/catchPoint.cpp | 187 +++++++++++++++++++++++------ main/src/moveInfo.cpp | 19 +-- main/src/wifiServer.cpp | 217 ++++++++++++++++++++++++++++++++++ main/vec/include/matrix2f.h | 2 +- main/vec/include/vector2f.h | 2 +- main/vec/src/vector2f.cpp | 4 +- main/vec/src/vector3f.cpp | 4 +- 14 files changed, 575 insertions(+), 79 deletions(-) create mode 100644 main/include/wifiServer.h create mode 100644 main/src/wifiServer.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index a5adebc..6a95eea 100644 --- a/.vscode/settings.json +++ b/.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", diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 0291028..8feed73 100644 --- a/main/CMakeLists.txt +++ b/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") diff --git a/main/include/catchPoint.h b/main/include/catchPoint.h index 1ed1a7a..8bc94b1 100644 --- a/main/include/catchPoint.h +++ b/main/include/catchPoint.h @@ -7,27 +7,42 @@ #include "moveInfo.h" #include "ps2_controller.h" #include +#include +#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 solution(double Eps); - void setSteerInfo(); + bool move(const PS2Controller &ps2); + bool move(WifiServer &wifi); + 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 \ No newline at end of file diff --git a/main/include/parameter.h b/main/include/parameter.h index e6bc617..c826bec 100644 --- a/main/include/parameter.h +++ b/main/include/parameter.h @@ -1,9 +1,17 @@ #ifndef PARAMETER_H #define PARAMETER_H #include + +#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_2=5; +static const double ARM_LENGTH_1=15; +static const double ARM_LENGTH_2=20; static const double PI=3.1415926535; diff --git a/main/include/ps2_controller.h b/main/include/ps2_controller.h index 48630d4..f0e46c4 100644 --- a/main/include/ps2_controller.h +++ b/main/include/ps2_controller.h @@ -44,9 +44,8 @@ public: bool ButtonPressed(uint16_t button) const; bool Button(uint16_t button) const; - bool updateStatic(); void launch(); - + private: uint8_t sendAndReceive(uint8_t const &byte) const; bool readState(bool motor1, uint8_t motor2); diff --git a/main/include/wifiServer.h b/main/include/wifiServer.h new file mode 100644 index 0000000..4c377b7 --- /dev/null +++ b/main/include/wifiServer.h @@ -0,0 +1,129 @@ +#ifndef WIFI_SERVER_H +#define WIFI_SERVER_H + +#include +#include +#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 + +// 常量定义 +#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>> + 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" + "" + "" + "" + " MarsCar Web Server" + "" + "" + "

Hello from ESP32!

" + "

这是一个运行在 ESP32 上的基本 Web 服务器。

" + "" + ""; +}; + +#endif // WIFI_SERVER_H \ No newline at end of file diff --git a/main/main.cpp b/main/main.cpp index 0cf1fb9..47a498f 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -7,32 +7,39 @@ #include "esp_timer.h" #include "uartTool.h" #include "catchPoint.h" +#include "wifiServer.h" #include extern "C" void app_main() { - PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23); - ps2.launch(); + //PS2Controller ps2(GPIO_NUM_19, GPIO_NUM_18, GPIO_NUM_15, GPIO_NUM_23); + //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()); + // ps2.updateStatic(); + 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)) - { - std::string cmd3 = catcher.getSteerInfo(); - std::cout< 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) - speed = speed.normalized() * CATCHER_SPEED; - target3 += speed; - target2 = Vector2f((target3.xy().length()), target3.z()); + if (speed.length() > EPSSILON) + 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(); - - // 牛顿迭代20次,尝试10次不同初值 + Vector2f sol; + // 牛顿迭代50次,尝试10次不同初值 for (int now = 0; now < 10; now++) { + // 在此时值扰动 uint32_t random_integer = esp_random(); - double random_double = (double)random_integer / UINT32_MAX; - Vector2f sol; - sol[0] = random_double * 2 * PI; + double random_double = (double)random_integer * 2.0 / UINT32_MAX - 1; + + 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; - angle = sol; - setSteerInfo(); - break; + 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(angle); + break; + } } } - if (success) - printf("有解! "); - else - printf("当前钩爪无解！\n"); + // printf("sol: "); + // sol.print(); - 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; - steer_base.setInfo(CATCHER_1, (int)pwm, CATCHER_TIME); - pwm = 850 + (angle[0] * 1.0f / PI) / 1300; - steer_1.setInfo(CATCHER_2, (int)pwm, CATCHER_TIME); - // TODO: - pwm = 0; - steer_2.setInfo(CATCHER_3, (int)pwm, CATCHER_TIME); + double pwm = 1020 + (angleBase * 1.0f / PI) * 1000; + steerBase.setInfo(CATCHER_1, (int)pwm, Speed); + pwm = 850 + (sol[0] * 1.0f / PI) * 1300; + steer_1.setInfo(CATCHER_2, (int)pwm, Speed); + pwm = 850 + (sol[1] * 1.0f / PI) * 1300; + steer_2.setInfo(CATCHER_3, (int)pwm, Speed); + + 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; } \ No newline at end of file diff --git a/main/src/moveInfo.cpp b/main/src/moveInfo.cpp index 9cfce03..ff60757 100644 --- a/main/src/moveInfo.cpp +++ b/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 y = ((255.0 - ly) * 1.0 - 255.0 / 2) / 127.5; + double x = test((lx * 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); - back_left.setInfo(bl, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME); - front_right.setInfo(fr, 1500 + frontRatio * MAX_STEER_PWM, FRAME_TIME); - back_right.setInfo(br, 1500 + backRatio * MAX_STEER_PWM, FRAME_TIME); + double frontRatio = clamp(x * 1.0 - y * 1.0); + double backRatio = clamp(-x * 1.0 - y * 1.0); + + 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() { diff --git a/main/src/wifiServer.cpp b/main/src/wifiServer.cpp new file mode 100644 index 0000000..5e90ea6 --- /dev/null +++ b/main/src/wifiServer.cpp @@ -0,0 +1,217 @@ +#include "wifiServer.h" +#include +#include // 包含 atoi 和 strtok_r 函数 +#include // 包含 strtok_r 函数 +#include +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(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; +} \ No newline at end of file diff --git a/main/vec/include/matrix2f.h b/main/vec/include/matrix2f.h index a9d2c83..679f27c 100644 --- a/main/vec/include/matrix2f.h +++ b/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(); diff --git a/main/vec/include/vector2f.h b/main/vec/include/vector2f.h index a49a2d2..050da1d 100644 --- a/main/vec/include/vector2f.h +++ b/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; } diff --git a/main/vec/src/vector2f.cpp b/main/vec/src/vector2f.cpp index 215905a..545f26f 100644 --- a/main/vec/src/vector2f.cpp +++ b/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()\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)