mirror of
https://github.com/MeowLynxSea/MarsCar.git
synced 2025-07-09 10:44:35 +00:00
Done
This commit is contained in:
sushi rui
parent
6261959cd0
commit
979f33bece
2
.vscode/settings.json
vendored
2
.vscode/settings.json
vendored
@ -1,6 +1,6 @@
|
||||
{
|
||||
"C_Cpp.intelliSenseEngine": "default",
|
||||
"idf.port": "/dev/ttyUSB0",
|
||||
"idf.port": "/dev/ttyUSB1",
|
||||
"idf.flashType": "UART",
|
||||
"files.associations": {
|
||||
"array": "cpp",
|
||||
|
||||
@ -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")
|
||||
|
||||
@ -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 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
|
||||
@ -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_2=5;
|
||||
static const double ARM_LENGTH_1=15;
|
||||
static const double ARM_LENGTH_2=20;
|
||||
|
||||
static const double PI=3.1415926535;
|
||||
|
||||
|
||||
@ -44,7 +44,6 @@ public:
|
||||
|
||||
bool ButtonPressed(uint16_t button) const;
|
||||
bool Button(uint16_t button) const;
|
||||
bool updateStatic();
|
||||
void launch();
|
||||
|
||||
private:
|
||||
|
||||
129
main/include/wifiServer.h
Normal file
129
main/include/wifiServer.h
Normal file
@ -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
|
||||
@ -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);
|
||||
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<<cmd3<<std::endl;
|
||||
}
|
||||
//printf("update!\n");
|
||||
uartTool.sendData("Move",cmd1+cmd2+cmd3);
|
||||
// uartTool.sendData("SteerMove","#013P1500T0100!");
|
||||
std::string cmd3 = "";
|
||||
catcher.move(server);
|
||||
cmd3 = catcher.getSteerInfo();
|
||||
|
||||
uartTool.sendData("Move", cmd1+cmd2+cmd3);
|
||||
// std::cout<<" "<<cmd3<<std::endl;
|
||||
|
||||
vTaskDelay(10 / portTICK_PERIOD_MS); // 延
|
||||
}
|
||||
|
||||
@ -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)
|
||||
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;
|
||||
}
|
||||
@ -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()
|
||||
{
|
||||
|
||||
217
main/src/wifiServer.cpp
Normal file
217
main/src/wifiServer.cpp
Normal file
@ -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;
|
||||
}
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user