Updated Shoot and Rotate for RobotState

Robocubs · Jan 27, 2024 · 3082bac · 3082bac
1 parent e63f250
commit 3082bac
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 10 deletions.
diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java
@@ -22,6 +22,7 @@
 import com.team1701.lib.util.LoggedTunableNumber;
 import com.team1701.robot.Configuration.Mode;
 import com.team1701.robot.commands.AutonomousCommands;
+import com.team1701.robot.commands.DriveCommands;
 import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import com.team1701.robot.subsystems.drive.DriveMotorFactory;
@@ -191,6 +192,10 @@ mDrive, new Rotation2d(2.5), Constants.Drive.kFastKinematicLimits, true),
                                         ? GeometryUtil.kRotationIdentity
                                         : GeometryUtil.kRotationPi))
                         .withName("ZeroGyroscopeToHeading"));
+        mDriverController
+                .rightBumper()
+                .whileTrue(DriveCommands.rotateRelativeToRobot(
+                        mDrive, new Rotation2d(2), Constants.Drive.kFastKinematicLimits, true));
         mDriverController.leftTrigger().whileTrue(swerveLock(mDrive));
         TriggeredAlert.info("Driver right bumper pressed", mDriverController.rightBumper());
 

diff --git a/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java b/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java
@@ -5,7 +5,7 @@
 import com.team1701.lib.util.LoggedTunableNumber;
 import com.team1701.lib.util.Util;
 import com.team1701.robot.Constants;
-import com.team1701.robot.estimation.PoseEstimator;
+import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -18,6 +18,7 @@
 /* Note: positive rotations are counterclockwise (turning left) */
 
 public class RotateRelativeToRobot extends Command {
+    private RobotState mRobotState = new RobotState();
     private static final String kLoggingPrefix = "Command/RotateRelativeToRobot/";
     private static final double kModuleRadius = Constants.Drive.kModuleRadius;
     private static final KinematicLimits kMaxKinematicLimits = Constants.Drive.kFastTrapezoidalKinematicLimits;
@@ -63,14 +64,14 @@ public class RotateRelativeToRobot extends Command {
     @Override
     public void initialize() {
         mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);
-        mRotationalOffset = PoseEstimator.getInstance().getPose2d().getRotation();
+        mRotationalOffset = mRobotState.getPose2d().getRotation();
 
-        Logger.recordOutput(kLoggingPrefix + "/requestedRotation/robotRelative", mTargetRelativeRotation);
-        Logger.recordOutput(kLoggingPrefix + "/requestedRotation/fieldRelative", mTargetFieldRotation);
+        Logger.recordOutput(kLoggingPrefix + "requestedRotation/robotRelative", mTargetRelativeRotation);
+        Logger.recordOutput(kLoggingPrefix + "requestedRotation/fieldRelative", mTargetFieldRotation);
 
         mRotationController.reset();
 
-        var currentActualRotation = PoseEstimator.getInstance().getPose2d().getRotation();
+        var currentActualRotation = mRobotState.getPose2d().getRotation();
         mTargetFieldRotation = currentActualRotation.plus(mTargetRelativeRotation);
         var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity();
         mRotationState = new TrapezoidProfile.State(
@@ -96,7 +97,7 @@ public void execute() {
             mRotationController.setPID(kRotationKp.get(), kRotationKi.get(), kRotationKd.get());
         }
 
-        var currentActualRotation = PoseEstimator.getInstance().getPose2d().getRotation();
+        var currentActualRotation = mRobotState.getPose2d().getRotation();
 
         // Calculate rotational velocity
         var rotationPidOutput =
@@ -112,7 +113,7 @@ public void execute() {
                 Rotation2d.fromRadians(kRotationToleranceRadians.get()));
         if (atTargetRotation) {
             mDrive.stop();
-            currentActualRotation = PoseEstimator.getInstance().getPose2d().getRotation();
+            currentActualRotation = mRobotState.getPose2d().getRotation();
         } else {
             mDrive.setVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, rotationalVelocity, currentActualRotation));
             currentActualRotation = Rotation2d.fromRadians(mRotationState.position);
@@ -135,7 +136,7 @@ public boolean isFinished() {
     }
 
     public boolean atTargetPose() {
-        var currentRotation = PoseEstimator.getInstance().getPose2d().getRotation();
+        var currentRotation = mRobotState.getPose2d().getRotation();
         var rotationError = (mTargetRelativeRotation.plus(mRotationalOffset)).minus(currentRotation);
 
         return Util.inRange(rotationError.getRadians(), kRotationToleranceRadians.get())

diff --git a/src/main/java/com/team1701/robot/commands/Shoot.java b/src/main/java/com/team1701/robot/commands/Shoot.java
@@ -2,7 +2,7 @@
 
 import com.team1701.robot.Constants;
 import com.team1701.robot.FieldConstants;
-import com.team1701.robot.estimation.PoseEstimator;
+import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
 import com.team1701.robot.subsystems.shooter.Shooter;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -14,6 +14,9 @@
 public class Shoot extends Command {
     private final Shooter mShooter;
     private final Drive mDrive;
+
+    private final RobotState mRobotState = new RobotState();
+
     private double distanceToTarget;
     private Rotation2d headingToTarget;
     private Rotation2d shooterAngleFromHorizontal;
@@ -34,7 +37,7 @@ public Shoot(Shooter shooter, Drive drive, boolean allianceIsBlue) {
     @Override
     public void initialize() {
         shotFired = false;
-        currentPose = PoseEstimator.getInstance().getPose2d();
+        currentPose = mRobotState.getPose2d();
         distanceToTarget = currentPose.getTranslation().getDistance(targetSpeaker);
         headingToTarget =
                 new Rotation2d(targetSpeaker.getX() - currentPose.getX(), targetSpeaker.getY() - currentPose.getY());