From f771b936f51960c61a8a9d2e9f54a88072d3317b Mon Sep 17 00:00:00 2001
From: Iooob <94081903+Iooob@users.noreply.github.com>
Date: Mon, 6 Nov 2023 15:31:42 -0500
Subject: [PATCH] commit

n00bs
---
 .../java/frc/robot/commands/AimCommand.java   |  4 ++-
 .../frc/robot/subsystems/DriveSubsystem.java  | 28 ++++++++++---------
 2 files changed, 18 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java
index 1a01d94..5f9bdd3 100644
--- a/src/main/java/frc/robot/commands/AimCommand.java
+++ b/src/main/java/frc/robot/commands/AimCommand.java
@@ -62,7 +62,9 @@ public void execute() {
                   Units.degreesToRadians(CamResult.getBestTarget().getPitch()))
               - GOAL_RANGE_METERS;
       // turn and move towards target.
-      m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      //m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      m_driveSubsystem.turnToAngle(m_driveSubsystem.getYaw(), angleGoal);
+      m_driveSubsystem.driveToDistance(distanceFromTarget);
       // we reset the angle everytime as the target could change / move.
       m_driveSubsystem.turnSetGoal(angleGoal);
       m_driveSubsystem.distanceSetGoal(distanceFromTarget);
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 5d9d4f2..822196a 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -218,6 +218,21 @@ public void driveToDistance(double targetDistance) {
     }
   }
 
+  public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) {
+    /*
+     * When this function is activated, execute another command to rotate to target angle. Since a Tank drive
+     * system cannot move forward simultaneously while rotating, all joystick input
+     * is ignored until this button is released.
+     */
+    this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees);
+    double leftStickValue = turnRotateToAngleRate;
+    double rightStickValue = -turnRotateToAngleRate;
+    if (!m_turnController.atGoal()) {
+      this.tankDrive(leftStickValue, rightStickValue);
+    }
+  }
+
+
   public void driveAndTurn(double gyroYawAngle, double TargetAngleDegrees, double targetDistance) {
     /*
     This lets you set a gyro angle and a distance you need to travel.
@@ -251,19 +266,6 @@ private void calcuateAngleRate(double gyroYawAngle, double targetAngleDegrees) {
     turnRotateToAngleRate = MathUtil.clamp(m_turnController.calculate(gyroYawAngle), -1.0, 1.0);
   }
 
-  public void turnToAngle(double gyroYawAngle, double TargetAngleDegrees) {
-    /*
-     * When this function is activated, execute another command to rotate to target angle. Since a Tank drive
-     * system cannot move forward simultaneously while rotating, all joystick input
-     * is ignored until this button is released.
-     */
-    this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees);
-    double leftStickValue = turnRotateToAngleRate;
-    double rightStickValue = -turnRotateToAngleRate;
-    if (!m_turnController.atGoal()) {
-      this.tankDrive(leftStickValue, rightStickValue);
-    }
-  }
 
   // magnitude = (joystickL + joystickR) / 2;
   public void driveStraight(