changed things

robot/distance.cpp

@@ -1,10 +1,12 @@
+#include "headers.h"
 #include "Adafruit_VL53L0X.h"
+
 Adafruit_VL53L0X lox = Adafruit_VL53L0X();
 float distanceReading = 0;
 
 void initDistance(){
 
-  if (!lox.begin()) {
+  if (!lox.begin(0x29, false, &Wire1, Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE)) {
     Serial.println(F("Failed to boot VL53L0X"));
     while(1);
   }

robot/encoder.cpp

@@ -0,0 +1,82 @@
+#include "headers.h"
+#include "AS5600.h"
+
+AS5600 as5600_0(&Wire1);
+AS5600 as5600_1(&Wire2);
+
+EncoderOdom odom;
+
+void zeroOdom(){
+ 
+    odom.left = 0.0;
+    odom.right = 0.0;
+    odom.x = 0.0;
+    odom.y = 0.0;
+    odom.angle = 0.0; 
+    
+    wire2();
+    as5600_0.resetCumulativePosition(0);
+    wire1();
+    as5600_1.resetCumulativePosition(0);
+}
+
+
+void initEncoder(){
+    odom.left = 0.0;
+    odom.right = 0.0;
+    odom.x = 0.0;
+    odom.y = 0.0;
+    odom.angle = 0.0;
+
+    wire2();
+    as5600_0.begin();
+    Serial.print("Connect device 0: ");
+    Serial.println(as5600_0.isConnected() ? "true" : "false");
+    as5600_0.resetCumulativePosition(0);
+  
+    wire1();
+    as5600_1.begin();
+    Serial.print("Connect device 1: ");
+    Serial.println(as5600_1.isConnected() ? "true" : "false");
+    as5600_1.resetCumulativePosition(0);
+}
+
+void updateEncoder(){
+  #define WHEEL_CIRCUM (2*PI*4.0625)
+  #define WHEEL_DISTANCE (7 + 12.0/16.0)
+
+  wire2();
+  int rawL = as5600_0.getCumulativePosition();
+  wire1();
+  int rawR = as5600_1.getCumulativePosition();
+  
+  float left = rawL/4096.0 * -WHEEL_CIRCUM;
+  float right = rawR/4096.0 * WHEEL_CIRCUM;
+
+  float d_left = left-odom.left;
+  float d_right= right-odom.right;
+  
+  odom.left = left;
+  odom.right = right;
+
+  float displacement = (d_left + d_right)/2;
+  float oldAng = odom.angle;
+  odom.angle += (d_left-d_right)/(2*WHEEL_DISTANCE);
+  float ang = (odom.angle+oldAng)/2;
+
+  float dispx = odom.x-desiredPos.x;
+  float dispy = odom.y-desiredPos.y;
+  posInput = sqrt(dispx*dispx+dispy*dispy);
+
+  desiredYaw = atan2(dispy, dispx)*180/PI;
+  if(abs(fmod(desiredYaw-currentYaw, 180.0))<90){
+    desiredYaw = fmod(desiredYaw+180.0, 360.0);
+    if(desiredYaw>180)desiredYaw-=180.0;
+
+    posInput = -posInput;
+  }
+//  Serial.println(desiredYaw);
+  
+  odom.x += (float)(cos(ang)*displacement);
+  odom.y += (float)(sin(ang)*displacement);
+}

robot/encoder.h

@@ -1,50 +0,0 @@
-
-
-struct Encoder{
-  int rotations;
-  int currentAngle;  
-  int estAngle;
-  bool threashhold;
-
-  float position(){
-    return rotations + (estAngle / 360.0);
-  }
-};
-
-Encoder encoder;
-
-
-void initEncoder(){
-    pinMode(A0, INPUT);
-    encoder.currentAngle = 0;
-    encoder.rotations = 0;
-    encoder.estAngle=0;
-    encoder.threashhold=false;
-}
-
-void updateEncoder(){
-  int rawIn = analogRead(A0);
-//  static long lastRan = 0;
-//  if(50>millis()-lastRan) return;
-//  lastRan = millis();
-
-  
-  int rotation = map(rawIn, 4, 348, 0, 360);
-  int diff = rotation-encoder.currentAngle;
-  if(!encoder.threashhold && (abs(diff)>=50)){
-    encoder.threashhold=true; 
-    if(diff<0){
-      encoder.rotations++;
-      encoder.estAngle = 0;
-    }
-    if(diff>0){
-      encoder.rotations--;
-      encoder.estAngle = 360;
-    }
-  }
-  if(abs(diff)<30){
-    encoder.threashhold=false; 
-    encoder.estAngle = rotation;
-  }
-  encoder.currentAngle = rotation;
-}

robot/gyro.cpp

@@ -1,12 +1,8 @@
+#include "headers.h"
 #include "MPU6050_6Axis_MotionApps612.h"
 //#include "MPU6050.h" // not necessary if using MotionApps include file
 
-
+MPU6050 mpu(MPU6050_ADDRESS_AD0_LOW, &Wire1);
-#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-#include "Wire.h"
-#endif
-
-MPU6050 mpu;
 
 // MPU control/status vars
 bool dmpReady = false;  // set true if DMP init was successful
@@ -54,6 +50,8 @@ void initGyro(){
   Serial.println(F("Initializing DMP..."));
   devStatus = mpu.dmpInitialize();
 
+//  mpu.setRate(0);
+
   // supply your own gyro offsets here, scaled for min sensitivity
 
   mpu.setXAccelOffset(-6009);
@@ -102,17 +100,15 @@ void initGyro(){
 bool updateGyro(){
   if (!dmpReady) return false;
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
-//    Xf = 1/4*(aaRealLast.x + aaReal.x)t^2 + Vot + Xo
-//    aaRealLast = aaReal;
     
     mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetAccel(&aa, fifoBuffer);
+//    mpu.dmpGetAccel(&aa, fifoBuffer);
-    mpu.dmpGetGyro(&gy, fifoBuffer);
+//    mpu.dmpGetGyro(&gy, fifoBuffer);
     mpu.dmpGetGravity(&gravity, &q);
     mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-    mpu.dmpGetEuler(euler, &q);
+//    mpu.dmpGetEuler(euler, &q);
-    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+//    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-    mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+//    mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
 
     return true;
   }

robot/headers.h

@@ -0,0 +1,114 @@
+
+#define LOOP_INTERVAL_MS 20
+
+struct FVec2{
+  float x,y;
+};
+struct FVec3{
+  float x,y,z;
+};
+struct FVec4{
+  float x,y,z;
+};
+
+extern FVec2 desiredPos;
+
+struct Everything{
+  float motorTargetAngle;
+  float position;
+  FVec3 anglePID;
+  FVec3 posPID;
+  FVec3 ypr;
+  FVec3 euler;
+  FVec3 gravity;
+  FVec4 q;
+  FVec3 aa;
+  FVec3 gy;
+  FVec3 aaReal;
+  FVec3 aaWorld;
+};
+
+extern float angleOffset;
+extern float desiredYaw;
+extern float currentYaw;
+
+struct DebugState{
+  int motorTargetAngle;
+};
+
+extern DebugState dbgState;
+
+//-------- wire
+#include <Wire.h>
+#define Wire1 Wire
+#define Wire2 Wire
+
+void wire1();
+void wire2();
+
+//-------- gyro
+#include "MPU6050_6Axis_MotionApps612.h"
+
+extern Quaternion q;           // [w, x, y, z]         quaternion container
+extern VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+extern VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
+extern VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+extern VectorInt16 aaRealLast; // [x, y, z]            gravity-free accel sensor measurements
+extern VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+extern VectorFloat gravity;    // [x, y, z]            gravity vector
+extern float euler[3];         // [psi, theta, phi]    Euler angle container
+extern float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+void initGyro();
+bool updateGyro();
+
+//-------- distance
+extern float distanceReading;
+
+void initDistance();
+void updateDistance();
+
+//-------- encoder
+
+struct EncoderOdom{
+  float x;
+  float y;
+  float angle;  
+  
+  float left;
+  float right;
+};
+
+
+extern EncoderOdom odom;
+
+void zeroOdom();
+void initEncoder();
+void updateEncoder();
+
+//--------- pid
+#include <PID_v1.h>
+#define PID_ARR_COUNT 3
+extern PID* pids[];
+
+extern double angleInput, angleOutput, angleSetpoint;
+extern PID anglePID;
+
+extern double posInput, posOutput, posSetpoint;
+extern PID posPID;
+
+extern double turnInput, turnOutput, turnSetpoint;
+extern PID turnPID;
+
+struct Speeds{
+  float left;
+  float right;
+};
+  
+Speeds updatePID();
+void initPID();
+
+//--------- webserver
+
+void initWifi(bool host);
+void initServer();

robot/pid.cpp

@@ -1,3 +1,4 @@
+#include "headers.h"
 #include <PID_v1.h>
 
 //double angKp=3.5, angKi=80, angKd=0.042;
@@ -17,9 +18,9 @@ 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, REVERSE);
+PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT);
 
-PID* pids[] = {&anglePID, &posPID, &turnPID};
+PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
 
 void initPID(){
   angleSetpoint = 0;
@@ -39,11 +40,7 @@ void initPID(){
   turnPID.SetSampleTime(5);
 }
 
-struct Speeds{
+
-  float left;
-  float right;
-};
-  
 Speeds updatePID(){
   posPID.Compute();
   angleSetpoint = posOutput;
@@ -51,8 +48,8 @@ Speeds updatePID(){
 
   float maxTurn = max(0.0f, 15.0f-abs((float)angleOutput));
   turnPID.SetOutputLimits(-maxTurn, maxTurn);
-  turnSetpoint = desiredYaw;
+  turnSetpoint = 0;
-  turnInput = currentYaw;
+  turnInput = fmod(currentYaw-desiredYaw, 180);
   turnPID.Compute();
 
   Speeds speeds;

robot/robot.ino

@@ -1,5 +1,16 @@
 
-#include "I2Cdev.h"
+
+#include "headers.h"
+
+void wire1(){
+  Wire1.begin(SDA, SCL);
+  Wire1.setClock(400000);
+}
+
+void wire2(){
+  Wire1.begin(D7, D5);
+  Wire2.setClock(400000);
+}
 
 
 #include <Servo.h>
@@ -10,19 +21,10 @@ float angleOffset = -2.4241745;
 float desiredYaw = 0.0;
 float currentYaw = 0.0;
 
-struct DebugState{
+FVec2 desiredPos;
-  int motorTargetAngle;
-};
 
 DebugState dbgState;
 
-#include "distance.h"
-#include "encoder.h"
-#include "gyro.h"
-#include "pid.h"
-#include "webserver.h"
-
-
 void initSerial(){
   Serial.begin(115200);
   while (!Serial);
@@ -36,11 +38,10 @@ void initMotors(){
 }
 
 void initI2C(){
-  digitalWrite(D2, LOW);
+  delay(100);  
-  digitalWrite(D1, LOW);
+
-  delay(100);
+  wire2();
-  Wire.begin();
+  wire1();
-  Wire.setClock(400000);
 }
 
 void setup() {
@@ -50,23 +51,24 @@ void setup() {
   initMotors();
   initPID();
   initI2C();
-  initEncoder();
   initDistance();
-  initGyro();
+  initGyro();  
+  initEncoder();
 }
 
 void loop() {
+  long start = millis();
+  
   if (updateGyro()) { //gyro data
-    currentYaw=ypr[0]*180/M_PI;
     double angle = ypr[1] * 180 / M_PI;
     if(angle>180) 
       angle -= 180;
     angleInput = angle + angleOffset;
   }
-  {// encoder data
+  
-    updateEncoder();
+  updateEncoder();
-    posInput = encoder.position();
+  currentYaw=odom.angle*180/M_PI;
-  }
+  
   updateDistance();
 
   
@@ -76,6 +78,13 @@ void loop() {
   speeds.right = min(90.0f-10, max(-90.0f+10, speeds.right));
   left.write(90+(int)speeds.left);
   right.write(90+(int)speeds.right);
+
+  long end = millis();
   
-  delay(5);
+  if(end-start>LOOP_INTERVAL_MS){
+    Serial.print("Overran ");
+    Serial.println(end-start);
+  }else{
+    delay(LOOP_INTERVAL_MS-(end-start)); 
+  }
 }

robot/webserver.cpp

@@ -1,3 +1,5 @@
+#include "headers.h"
+
 #include <ESP8266WiFi.h>
 #include <ESPAsyncTCP.h>
 #include <ESPAsyncWebServer.h>
@@ -31,48 +33,31 @@ void initWifi(bool host){
   Serial.println(WiFi.localIP());
 }
 
-struct FVec3{
-  float x,y,z;
-};
-struct FVec4{
-  float x,y,z;
-};
-
-struct Everything{
-  float motorTargetAngle;
-  float position;
-  FVec3 anglePID;
-  FVec3 posPID;
-  FVec3 ypr;
-  FVec3 euler;
-  FVec3 gravity;
-  FVec4 q;
-  FVec3 aa;
-  FVec3 gy;
-  FVec3 aaReal;
-  FVec3 aaWorld;
-};
 
 void initServer(){
   server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
     request->send_P(200, "text/html", index_html);
   });
   server.on("/get_stuff_bin", HTTP_GET, [](AsyncWebServerRequest *request){
+    static float arr[4];
+    arr[0] = odom.angle;
+    arr[1] = distanceReading;
+    arr[2] = odom.x;
+    arr[3] = odom.y;
+    request->send(200, "application/octet-stream", (uint8_t*)(const char*)arr, sizeof(arr));
+  });
+  server.on("/fuckyou", HTTP_GET, [](AsyncWebServerRequest *request){
     float arr[] = {
-      dbgState.motorTargetAngle, 
+      odom.angle,
-      encoder.position(),
+      distanceReading,
-      angleSetpoint, angleInput, angleOutput,
+      odom.x,
-      posSetpoint, posInput, posOutput,
+      odom.y
-      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
     };
-    request->send(200, "application/text", (const uint8_t*)arr, sizeof(arr)*4);
+    request->send(200, "application/text", "hello");
+  });
+  server.on("/zero", HTTP_GET, [](AsyncWebServerRequest *request){
+    zeroOdom();
+    request->send(200);
   });
   server.on("/get_stuff", HTTP_GET, [](AsyncWebServerRequest *request){
     char buff[1024];
@@ -84,6 +69,7 @@ void initServer(){
         "anglePID": {"setpoint": %lf, "input": %lf, "output": %lf},
         "posPID": {"setpoint": %lf, "input": %lf, "output": %lf},
         "turnPID": {"setpoint": %lf, "input": %lf, "output": %lf},
+        "odom": {"left": %f, "right": %f, "x": %f, "y": %f, "angle": %f},
         "ypr": {"yaw": %f, "pitch": %f, "roll": %f},
         "euler": {"psi": %f, "theta": %f, "phi": %f},
         "gravity": {"x": %f, "y": %f, "z": %f},
@@ -95,10 +81,11 @@ void initServer(){
       })",
       (float)dbgState.motorTargetAngle, 
       (float)distanceReading,
-      encoder.position(),
+      0.0, //encoder.position(),
       angleSetpoint, angleInput, angleOutput,
       posSetpoint, posInput, posOutput,
       turnSetpoint, turnInput, turnOutput,
+      odom.left, odom.right, odom.x, odom.y, odom.angle,
       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,