diff --git a/robot/encoder.cpp b/robot/encoder.cpp index a40afb6..9ddc3eb 100644 --- a/robot/encoder.cpp +++ b/robot/encoder.cpp @@ -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); diff --git a/robot/headers.h b/robot/headers.h index 3dd8ba2..338ad9d 100644 --- a/robot/headers.h +++ b/robot/headers.h @@ -112,3 +112,4 @@ void initPID(); void initWifi(bool host); void initServer(); +void updateServer(); diff --git a/robot/pid.cpp b/robot/pid.cpp index 7f9791e..166f577 100644 --- a/robot/pid.cpp +++ b/robot/pid.cpp @@ -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); diff --git a/robot/robot.ino b/robot/robot.ino index f4feb7d..c67cc85 100644 --- a/robot/robot.ino +++ b/robot/robot.ino @@ -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(); diff --git a/robot/webserver.cpp b/robot/webserver.cpp index 31fe4c6..caa91ad 100644 --- a/robot/webserver.cpp +++ b/robot/webserver.cpp @@ -1,6 +1,14 @@ #include "headers.h" #include +#include + +WiFiUDP Udp; +unsigned int localUdpPort = 42069; + + +#ifdef DO_WEB_SERVER + #include #include #include @@ -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, @@ -114,6 +100,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) { @@ -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()); +}