remove bad state vars :(

MOERobotics · Apr 28, 2023 · f539ecb · f539ecb
1 parent 57c4458
commit f539ecb
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/teleop/DriveCode.java b/src/main/java/frc/robot/teleop/DriveCode.java
@@ -115,7 +115,7 @@ public void teleopInit(GenericRobot robot) {
 
         robot.resetStartPivots();
         robot.setPose();
-        desiredYaw = 0;
+        desiredYaw = robot.getYaw();
         firstTrip = false;
         secondTrip = false;
         pressed = false;
@@ -143,7 +143,10 @@ public void teleopPeriodic(GenericRobot robot) {
         SmartDashboard.putNumber("desiredArmPos", desiredArmPos);
 
         Pose2d currPose = robot.getPose();
-        if (armTimer.get() < .1) desiredArmPos = robot.getPotDegrees();
+        if (armTimer.get() < .1){
+            desiredYaw = robot.getYaw();
+            desiredArmPos = robot.getPotDegrees();
+        }
 
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////Send Pose to Dash