From f5805567e7aec6f7e08dea4861f84c71d62c48ea Mon Sep 17 00:00:00 2001
From: Cyberarm <matthewlikesrobots@gmail.com>
Date: Sat, 5 Nov 2022 16:58:32 -0500
Subject: [PATCH] Added debugging telemetry to CollectorDistanceState that
 showed that there was most likely not a bug with the distance delta- but that
 the 2M distance sensor is flakey at long ranges, Fixed mecanum minibot unable
 to grab

---
 .../States/CollectorDistanceState.java        | 33 ++++++++++++++-----
 .../states/MecanumMinibotTeleOpState.java     |  4 +--
 2 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
index 9fc3355..fc7e32a 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
@@ -17,6 +17,9 @@ public class CollectorDistanceState extends CyberarmState {
     private double lastMeasuredTime;
     private double currentDistance;
     private double LastDistanceRead;
+    private double distanceDelta;
+    private double debugRunTime;
+    private String debugStatus = "?";
 
 
     public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
@@ -30,20 +33,30 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
         engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
-
-        engine.telemetry.addData("drivePower", drivePower);
-        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+        engine.telemetry.addLine();
 
         engine.telemetry.addData("traveledDistance", traveledDistance);
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
         engine.telemetry.addData("Current Distance", currentDistance);
         engine.telemetry.addData("last Distance", LastDistanceRead);
+        engine.telemetry.addLine();
+
+        engine.telemetry.addData("distanceDelta", distanceDelta);
+        engine.telemetry.addData("DEBUG RunTime", debugRunTime);
+        engine.telemetry.addData("DEBUG Status", debugStatus);
 
     }
 
@@ -82,15 +95,19 @@ public class CollectorDistanceState extends CyberarmState {
 
             currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM);
 
-            if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) {
+            distanceDelta = LastDistanceRead - currentDistance;
+
+            if (distanceDelta >= 0.0 || currentDistance > 500) {
                 // I am moving forward
                 // and im close too my target.
                 LastDistanceRead = currentDistance;
-
+                debugRunTime = runTime();
+                debugStatus = "RUNNING";
             } else {
                 // I have stopped
-                targetDrivePower = 0;
-//                setHasFinished(true);
+                debugStatus = "STOPPED";
+                setHasFinished(true);
+                return;
             }
         }
 
diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
index 99890ae..3d46479 100644
--- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
@@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
         }
 
         /* ............................................................................ grab */
-        if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 ||
+        if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 ||
                 engine.gamepad2.x){ // in
             robot.pServoGrab.setPosition(0.9);
         }
@@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
                 engine.gamepad2.back){ // small out
             robot.pServoGrab.setPosition(0.50);
         }
-        if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 ||
+        if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 ||
                 engine.gamepad2.b){ // big out
             robot.pServoGrab.setPosition(0.0);
         }