tri11paragon
/
4p78-final-project
mirror of https://github.com/tri11paragon/4p78-final-project.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

changed to start using UDP for communication

main
ParkerTenBroeck 2025-04-16 23:33:37 -04:00
parent 2bbd5254a9
commit 4a92234cc8
5 changed files with 246 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
robot/encoder.cpp
View File

@ -42,8 +42,9 @@ void initEncoder(){
}
void updateEncoder(){
#define WHEEL_CIRCUM (2*PI*4.0625)
#define WHEEL_DISTANCE (7 + 12.0/16.0)
#define WHEEL_CIRCUM (PI*4.256)
// #define WHEEL_CIRCUM (1.0)
#define WHEEL_DISTANCE (8-0.125)
wire2();
int rawL = as5600_0.getCumulativePosition();
@ -61,18 +62,16 @@ void updateEncoder(){
float displacement = (d_left + d_right)/2;
float oldAng = odom.angle;
odom.angle += (d_left-d_right)/(2*WHEEL_DISTANCE);
odom.angle += (d_left-d_right)/(WHEEL_DISTANCE);
float ang = (odom.angle+oldAng)/2;
float dispx = odom.x-desiredPos.x;
float dispy = odom.y-desiredPos.y;
float dispx = desiredPos.x-odom.x;
float dispy = desiredPos.y-odom.y;
posInput = sqrt(dispx*dispx+dispy*dispy);
desiredYaw = atan2(dispy, dispx)*180/PI;
if(abs(fmod(desiredYaw-currentYaw, 180.0))<90){
if(abs(fmod(currentYaw-desiredYaw, 180.0))>90){
desiredYaw = fmod(desiredYaw+180.0, 360.0);
if(desiredYaw>180)desiredYaw-=180.0;
posInput = -posInput;
}
// Serial.println(desiredYaw);

1
robot/headers.h
View File

@ -112,3 +112,4 @@ void initPID();
void initWifi(bool host);
void initServer();
void updateServer();

10
robot/pid.cpp
View File

@ -7,7 +7,7 @@
double angKp=4.0, angKi=106.75, angKd=0.0472;
double posKp=0.96, posKi=1.28, posKd=0.5;
double posKp=0.96, posKi=1.28, posKd=1.0;
double turnKp=1, turnKi=5.0, turnKd=0.17;
@ -18,7 +18,7 @@ double posInput, posOutput, posSetpoint;
PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, P_ON_E, DIRECT);
double turnInput, turnOutput, turnSetpoint;
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT);
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_M, DIRECT);
PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
@ -28,8 +28,8 @@ void initPID(){
anglePID.SetMode(AUTOMATIC);
anglePID.SetSampleTime(5);
posSetpoint = 0.5;
posPID.SetOutputLimits(-1, 1);
posSetpoint = 0;
posPID.SetOutputLimits(-2, 2);
posPID.SetMode(AUTOMATIC);
posPID.SetSampleTime(5);
posPID.SetControllerDirection(DIRECT);
@ -46,7 +46,7 @@ Speeds updatePID(){
angleSetpoint = posOutput;
anglePID.Compute();
float maxTurn = max(0.0f, 15.0f-abs((float)angleOutput));
float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput));
turnPID.SetOutputLimits(-maxTurn, maxTurn);
turnSetpoint = 0;
turnInput = fmod(currentYaw-desiredYaw, 180);

6
robot/robot.ino
View File

@ -17,7 +17,7 @@ void wire2(){
Servo left;
Servo right;
float angleOffset = -2.4241745;
float angleOffset = 4.620817395210266;
float desiredYaw = 0.0;
float currentYaw = 0.0;
@ -63,14 +63,14 @@ void loop() {
double angle = ypr[1] * 180 / M_PI;
if(angle>180)
angle -= 180;
angleInput = angle + angleOffset;
angleInput = angle - angleOffset;
}
updateEncoder();
currentYaw=odom.angle*180/M_PI;
updateDistance();
updateServer();
Speeds speeds = updatePID();

254
robot/webserver.cpp
View File

@ -1,6 +1,14 @@
#include "headers.h"
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
WiFiUDP Udp;
unsigned int localUdpPort = 42069;
#ifdef DO_WEB_SERVER
#include <ESPAsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <ArduinoJson.h>
@ -11,30 +19,8 @@
AsyncWebServer server(80);
void initWifi(bool host){
if(host){
WiFi.softAP("MEOW");
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);
}else{
Serial.print("Connecting to ");
Serial.println("Michael Loves CP");
WiFi.enableInsecureWEP(false);
WiFi.mode(WIFI_STA);
WiFi.setPhyMode(WIFI_PHY_MODE_11G);
WiFi.begin("Michael Loves CP", "cockandpavly20");
while(WiFi.status() != WL_CONNECTED){
delay(500);
Serial.print('.');
}
Serial.println();
}
Serial.println(WiFi.localIP());
}
void initServer(){
void initWebServer(){
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
request->send_P(200, "text/html", index_html);
});
@ -85,7 +71,7 @@ void initServer(){
angleSetpoint, angleInput, angleOutput,
posSetpoint, posInput, posOutput,
turnSetpoint, turnInput, turnOutput,
odom.left, odom.right, odom.x, odom.y, odom.angle,
odom.left, odom.right, odom.x, odom.y, odom.angle*180/M_PI,
ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI,
euler[0]*180/M_PI, euler[1]*180/M_PI, euler[2]*180/M_PI,
gravity.x, gravity.y, gravity.z,
@ -115,6 +101,16 @@ void initServer(){
request->send(200, "application/json", buff);
});
server.on("/set_desired_pos", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
deserializeJson(json, &bodyData[index], bodyLen);
desiredPos.x = json["x"];
desiredPos.y = json["y"];
request->send(200);
});
server.on("/set_pid", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
@ -144,3 +140,213 @@ void initServer(){
});
server.begin();
}
#endif
void initServer(){
#ifdef DO_WEB_SERVER
initWebServer()
#endif
Udp.begin(localUdpPort);
}
struct ZeroPacket{
static constexpr uint32_t ID = 0;
};
struct GetDataPacket{
static constexpr uint32_t ID = 1;
};
struct SetTargetPacket{
static constexpr uint32_t ID = 2;
FVec2 pos;
};
struct EverythingPacket{
static constexpr uint32_t ID = 3;
};
struct GetDataPacketPlus{
static constexpr uint32_t ID = 4;
};
struct GetPIDPacket{
static constexpr uint32_t ID = 5;
uint32_t index;
};
struct SetPIDPacket{
static constexpr uint32_t ID = 6;
uint32_t index;
float kp,ki,kd;
uint32_t direction;
};
struct Packet{
uint32_t id;
union{
ZeroPacket zero;
GetDataPacket get_data;
SetTargetPacket set_target;
EverythingPacket everything;
GetDataPacketPlus get_data_plus;
GetPIDPacket get_pid_packet;
SetPIDPacket set_pid_packet;
} data;
};
struct DataPacket{
float yaw;
float distance;
FVec2 position;
};
struct DataPacketPlus{
float yaw;
float desiredYaw;
float distance;
FVec2 position;
FVec2 targetPosition;
};
void respond_data_packet(){
DataPacket dp;
dp.yaw = currentYaw;
dp.distance = distanceReading;
dp.position.x = odom.x;
dp.position.y = odom.y;
Udp.write((const char*)&dp, sizeof(dp));
}
void respond_data_packet_plus(){
DataPacketPlus dp;
dp.yaw = currentYaw;
dp.desiredYaw = desiredYaw;
dp.distance = distanceReading;
dp.position.x = odom.x;
dp.position.y = odom.y;
dp.targetPosition.x = desiredPos.x;
dp.targetPosition.y = desiredPos.y;
Udp.write((const char*)&dp, sizeof(dp));
}
void respond_everything_packet(){
float everything[] = {
(float)dbgState.motorTargetAngle,
(float)distanceReading,
0.0, //encoder.position(),
angleSetpoint, angleInput, angleOutput,
posSetpoint, posInput, posOutput,
turnSetpoint, turnInput, turnOutput,
odom.left, odom.right, odom.x, odom.y, odom.angle*180/M_PI,
ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI,
euler[0]*180/M_PI, euler[1]*180/M_PI, euler[2]*180/M_PI,
gravity.x, gravity.y, gravity.z,
q.w, q.x, q.y, q.z,
aa.x, aa.y, aa.z,
gy.x, gy.y, gy.z,
aaReal.x, aaReal.y, aaReal.z,
aaWorld.x, aaWorld.y, aaWorld.z
};
Udp.write((const char*)everything, sizeof(everything));
}
void set_pid(SetPIDPacket spid){
if(spid.index>PID_ARR_COUNT){
return;
}
PID& pid = *pids[spid.index];
pid.SetTunings(spid.kp, spid.ki, spid.kd);
pid.SetControllerDirection(spid.direction);
}
void get_pid(GetPIDPacket gpid){
if(gpid.index>PID_ARR_COUNT){
return;
}
PID& pid = *pids[gpid.index];
struct {float kp,ki,kd; uint32_t direction;} pidData = {
.kp=pid.GetKp(),
.ki=pid.GetKi(),
.kd=pid.GetKd(),
.direction=pid.GetDirection()
};
Udp.write((const char*)&pidData, sizeof(pidData));
}
bool handleUDP(){
int size = Udp.parsePacket();
constexpr size_t buffer_size = 256;
alignas(alignof(float)) char buffer[buffer_size];
if (size>=4){
int len = Udp.read(buffer, buffer_size);
Packet* packet = (Packet*)buffer;
static uint32_t sequence = 0;
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write((const char*)&packet->id, sizeof(packet->id));
Udp.write((const char*)&sequence, sizeof(sequence));
sequence++;
switch(packet->id){
case ZeroPacket::ID:
zeroOdom();
desiredPos.x = 0;
desiredPos.y = 0;
break;
case GetDataPacket::ID:
respond_data_packet();
break;
case SetTargetPacket::ID:
desiredPos.x = packet->data.set_target.pos.x;
desiredPos.y = packet->data.set_target.pos.y;
break;
case EverythingPacket::ID:
respond_everything_packet();
break;
case GetDataPacketPlus::ID:
respond_data_packet_plus();
break;
case SetPIDPacket::ID:
set_pid(packet->data.set_pid_packet);
break;
case GetPIDPacket::ID:
get_pid(packet->data.get_pid_packet);
break;
}
Udp.endPacket();
return true;
}
return false;
}
void updateServer(){
while(handleUDP());
}
void initWifi(bool host){
if(host){
WiFi.softAP("MEOW");
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);
}else{
Serial.print("Connecting to ");
Serial.println("Michael Loves CP");
WiFi.enableInsecureWEP(false);
WiFi.mode(WIFI_STA);
WiFi.setPhyMode(WIFI_PHY_MODE_11G);
WiFi.begin("Michael Loves CP", "cockandpavly20");
while(WiFi.status() != WL_CONNECTED){
delay(500);
Serial.print('.');
}
Serial.println();
}
Serial.println(WiFi.localIP());
}