From 47883710bac8675ac9368d194222cc935ee1f96b Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Sat, 9 Dec 2023 13:00:34 -0600
Subject: [PATCH] Started to format drive state similar to turn state, tests
 seem to work well for now

---
 .../SodiPizzaAutoFirstDriveState.java          | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
index ad9d6bb..f0adaa3 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
@@ -14,7 +14,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
     final private String groupName, actionName;
     private long lastMoveTime;
     private double drivePower, drivePowerRaw;
-    public int readyToTurn, neededTicks, currentTicks, targetTicks = 1000;
+    public int readyToTurn, neededTicks, currentTicks, targetTicks;
 
     public SodiPizzaAutoFirstDriveState() {
         groupName = " ";
@@ -52,6 +52,16 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
 
         robot.imu.resetYaw();
 
+        robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
     }
 
     @Override
@@ -111,8 +121,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
 
         }
 
-        //Stop and finish set after reached targetTicks within tolerance of 2
-        else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
+        //Stop and finish set after reached targetTicks within tolerance of 10
+        else if (Math.abs(neededTicks) < 10) {
 
             drivePowerRaw = 0;
             getDrivePower();
@@ -122,6 +132,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
             robot.rightFront.setPower(drivePower);
             robot.rightBack.setPower(drivePower);
 
+
+
             engine.blackboardSet("readyToTurn", readyToTurn + 1);
 
             setHasFinished(true);