changed to start using UDP for communication

robot/encoder.cpp

@@ -42,8 +42,9 @@ void initEncoder(){
 }
 
 void updateEncoder(){
-  #define WHEEL_CIRCUM (2*PI*4.0625)
+  #define WHEEL_CIRCUM (PI*4.256)
-  #define WHEEL_DISTANCE (7 + 12.0/16.0)
+//  #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 dispx = desiredPos.x-odom.x;
-  float dispy = odom.y-desiredPos.y;
+  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);

robot/headers.h

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

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;
+  posSetpoint = 0;
-  posPID.SetOutputLimits(-1, 1);
+  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);

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();
 

robot/webserver.cpp

@@ -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 initWebServer(){
-void initServer(){
   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());
+}