Log Goal

Mechanical-Advantage · Feb 12, 2024 · 4f8e355 · 4f8e355
1 parent 94655eb
commit 4f8e355
Showing 1 changed file with 8 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/commands/Turn.java b/src/main/java/frc/robot/commands/Turn.java
@@ -4,20 +4,17 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.subsystems.drive.Drive;
 import org.littletonrobotics.junction.Logger;
 
 public class Turn extends Command {
   private final Drive drive;
-  private final Rotation2d setpoint;
+  private Rotation2d setpoint;
   private final boolean relative;
 
   private final PIDController controller;
-  private final Timer toleranceTimer = new Timer();
-
   private static double kP = 0;
   private static double kI = 0;
   private static double kD = 0;
@@ -28,7 +25,7 @@ public class Turn extends Command {
   public Turn(Drive drive, Rotation2d setpoint, boolean relative) {
     addRequirements(drive);
     this.drive = drive;
-    this.setpoint = setpoint;
+    this.setpoint = Rotation2d.fromDegrees(setpoint.getDegrees());
     this.relative = relative;
 
     switch (Constants.currentMode) {
@@ -40,7 +37,7 @@ public Turn(Drive drive, Rotation2d setpoint, boolean relative) {
         toleranceDegrees = 1.0;
         break;
       default: // for SIM
-        kP = 0.5;
+        kP = 0.1;
         kI = 0.0;
         kD = 0.001;
         minVelocity = 0.0;
@@ -57,10 +54,11 @@ public Turn(Drive drive, Rotation2d setpoint, boolean relative) {
   @Override
   public void initialize() {
     controller.reset();
-
+    Logger.recordOutput("Turn Input", setpoint.getDegrees());
     if (relative) {
       controller.setSetpoint(
           Rotation2d.fromDegrees(drive.getGyroYawDegrees()).rotateBy(setpoint).getDegrees());
+      setpoint = Rotation2d.fromDegrees((drive.getGyroYawDegrees()) + setpoint.getDegrees());
     } else {
       controller.setSetpoint(setpoint.getDegrees());
     }
@@ -70,6 +68,9 @@ public void initialize() {
   @Override
   public void execute() {
     Logger.recordOutput("TurnActive", true);
+    Logger.recordOutput("Turn X Goal", setpoint.getDegrees());
+    /*Units.radiansToDegrees(
+    (Math.atan2(Constants.SPEAKER_Y - drive.getPose().getY(), drive.getPose().getX()))));*/
     // Update output speeds
 
     double angularSpeed = controller.calculate(drive.getGyroYawDegrees());