commit

n00bs
MiamiBeachBots · Nov 6, 2023 · f771b93 · f771b93
1 parent 1130ee0
commit f771b93
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 14 deletions.
diff --git 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
@@ -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(