From 749b0d34df330280da1f0a4a164fb49f18a6b541 Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Thu, 30 Nov 2023 20:37:54 -0600
Subject: [PATCH] Why won't TurnState turn?

---
 .../States/SodiPizzaAutoFirstDriveState.java       |  2 ++
 .../Autonomous/States/SodiPizzaAutoTurnState.java  | 14 +++++++++++---
 .../CenterStage/Common/SodiPizzaMinibotObject.java |  7 ++++++-
 3 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java
index 4c2905f..87556d7 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java
@@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.Autonomous.States;
 
 import android.annotation.SuppressLint;
 
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
 
 import dev.cyberarm.engine.V2.CyberarmState;
@@ -39,6 +40,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
         robot.leftFront.getCurrentPosition());
         engine.telemetry.addLine();
         engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
+        engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
 
     }
 
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
index b8b206e..8c364c5 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
@@ -1,6 +1,7 @@
 package org.timecrafters.CenterStage.Autonomous.States;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
 import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
@@ -40,8 +41,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Target Pos", robot.leftFront.getTargetPosition());
+        engine.telemetry.addData("Target Rotation", targetRot);
         engine.telemetry.addData("Power", robot.leftFront.getPower());
+        engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
+        engine.telemetry.addData("Engine Ready To Turn Value", engine.blackboardGetInt("readyToTurn"));
+
+
     }
 
     @Override
@@ -56,7 +63,8 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
         robot.rightBack.setPower(turnSpeed);
 
         neededRot = (targetRot - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
-//        robot.imu.resetYaw();
+        engine.blackboardSet("readyToTurn", 0);
+
     }
 
 
@@ -67,7 +75,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
 
         currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 
-        if (readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) {
+        if (readyToTurn == 1 && Math.abs(neededRot) > 10) {
 
             targetRot = -90;
 
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
index 1102635..8e4ca89 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
@@ -1,9 +1,12 @@
 package org.timecrafters.CenterStage.Common;
 
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
@@ -18,6 +21,7 @@ public class SodiPizzaMinibotObject extends Robot {
     public DcMotor leftFront, rightFront, leftBack, rightBack;
     public Servo shoulder, gripper;
     public IMU imu;
+    public Rev2mDistanceSensor distSensor;
     private String string;
 
     public static double GRIPPER_CLOSED = 0.333; // ~90 degrees
@@ -69,8 +73,9 @@ public class SodiPizzaMinibotObject extends Robot {
         shoulder = engine.hardwareMap.servo.get("arm");
         gripper = engine.hardwareMap.servo.get("gripper");
 
-        //readyToTurn
+        //Distance Sensor
 
+        distSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distSensor");
 
     }
 }