From 002118e9696da76617e859f0545819a006c2c4a6 Mon Sep 17 00:00:00 2001
From: ParkerTenBroeck <51721964+ParkerTenBroeck@users.noreply.github.com>
Date: Wed, 9 Apr 2025 17:58:24 -0400
Subject: [PATCH] meow

---
 robot/distance.h  | 7 +++++--
 robot/pid.h       | 6 +++---
 robot/webserver.h | 6 ++++--
 3 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/robot/distance.h b/robot/distance.h
index bbfd025..f90ca28 100644
--- a/robot/distance.h
+++ b/robot/distance.h
@@ -1,5 +1,6 @@
 #include "Adafruit_VL53L0X.h"
 Adafruit_VL53L0X lox = Adafruit_VL53L0X();
+float distanceReading = 0;
 
 void initDistance(){
 
@@ -13,7 +14,9 @@ void initDistance(){
 
 void updateDistance(){
   if (lox.isRangeComplete()) {
-    Serial.print("Distance in mm: ");
-    Serial.println(lox.readRange());
+    distanceReading = (float)lox.readRange();
+//    if(distanceReading>2000){
+//      distanceReading = (float)NAN;
+//    }
   }
 }
diff --git a/robot/pid.h b/robot/pid.h
index 2b026d0..c7bddb1 100644
--- a/robot/pid.h
+++ b/robot/pid.h
@@ -11,13 +11,13 @@ double turnKp=1, turnKi=5.0, turnKd=0.17;
 
 
 double angleInput, angleOutput, angleSetpoint;
-PID anglePID(&angleInput, &angleOutput, &angleSetpoint, angKp, angKi, angKd, AUTOMATIC);
+PID anglePID(&angleInput, &angleOutput, &angleSetpoint, angKp, angKi, angKd, P_ON_M, REVERSE);
 
 double posInput, posOutput, posSetpoint;
-PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, AUTOMATIC);
+PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, P_ON_E, DIRECT);
 
 double turnInput, turnOutput, turnSetpoint;
-PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, AUTOMATIC);
+PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, REVERSE);
 
 PID* pids[] = {&anglePID, &posPID, &turnPID};
 
diff --git a/robot/webserver.h b/robot/webserver.h
index c08a4c6..edc0fe7 100644
--- a/robot/webserver.h
+++ b/robot/webserver.h
@@ -78,7 +78,8 @@ void initServer(){
     char buff[1024];
     int ret = snprintf(buff, sizeof(buff), 
       R"({
-        "motorTargetAngle": %d,
+        "motorTargetAngle": %f,
+        "distanceReading": %f,
         "position": %f,
         "anglePID": {"setpoint": %lf, "input": %lf, "output": %lf},
         "posPID": {"setpoint": %lf, "input": %lf, "output": %lf},
@@ -92,7 +93,8 @@ void initServer(){
         "aaReal": {"x": %hd, "y": %hd, "z": %hd},
         "aaWorld": {"x": %hd, "y": %hd, "z": %hd}
       })",
-      dbgState.motorTargetAngle, 
+      (float)dbgState.motorTargetAngle, 
+      (float)distanceReading,
       encoder.position(),
       angleSetpoint, angleInput, angleOutput,
       posSetpoint, posInput, posOutput,