Se/swerve commands

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,6 +7,8 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
+import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
+import com.stuypulse.robot.constants.Field;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.amper.Amper;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
@@ -62,7 +64,9 @@ private void configureDefaultCommands() {
     /*** BUTTONS ***/
     /***************/
 
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+        driver.getBottomButton().whileTrue(new SwerveDriveToPose(Field.getFiducial(2).getLocation().toPose2d()));
+    }
 
     /**************/
     /*** AUTONS ***/

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -1,10 +1,15 @@
 package com.stuypulse.robot.commands.swerve;
 
+import java.util.Optional;
+
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.Driver.Drive;
 import com.stuypulse.robot.constants.Settings.Driver.Turn;
+import com.stuypulse.robot.constants.Settings.Driver.Turn.GyroFeedback;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.control.angle.AngleController;
 import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.Angle;
 import com.stuypulse.stuylib.math.SLMath;
 import com.stuypulse.stuylib.streams.numbers.IStream;
 import com.stuypulse.stuylib.streams.numbers.filters.RateLimit;
@@ -13,6 +18,7 @@
 import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
 import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class SwerveDriveDrive extends Command {
@@ -24,6 +30,8 @@ public class SwerveDriveDrive extends Command {
 
     private final Gamepad driver;
 
+    private Optional<Rotation2d> holdAngle;
+
     public SwerveDriveDrive(Gamepad driver) {
         this.driver = driver;
 
@@ -41,17 +49,37 @@ public SwerveDriveDrive(Gamepad driver) {
 
         turn = IStream.create(driver::getRightX)
             .filtered(
-                x -> SLMath.deadband(x, Turn.DEADBAND.get()),
-                x -> SLMath.spow(x, Turn.POWER.get()),
-                x -> x * Turn.MAX_TELEOP_TURNING.get(),
-                new RateLimit(Turn.RC.get())
+          //      x -> SLMath.deadband(x, Turn.DEADBAND.get()),
+          //      x -> SLMath.spow(x, Turn.POWER.get()),
+                x -> x * Turn.MAX_TELEOP_TURNING.get()
+          //      new RateLimit(Turn.RC.get())
         ); 
 
+        holdAngle = Optional.empty();
+
         addRequirements(swerve);
     }
 
+    @Override
+    public void initialize() {
+        holdAngle = Optional.empty();
+    }
+
+    private boolean isWithinTurnDeadband() {
+        return Math.abs(turn.get()) < Turn.DEADBAND.get();
+    }
+
     @Override
     public void execute() {
-        swerve.drive(speed.get(), turn.get());
+        double angularVel = turn.get();
+
+        if(isWithinTurnDeadband() && holdAngle.isEmpty()){
+            holdAngle = Optional.of(swerve.getGyroAngle());
+        }
+        else {
+            holdAngle = Optional.empty(); 
+        }
+
+        swerve.drive(speed.get(), angularVel);
     }
 }

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java

@@ -0,0 +1,74 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
+import com.stuypulse.robot.constants.Settings.Alignment.Translation;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveToPose extends Command {
+    private final SwerveDrive swerve;
+    private Pose2d targetPose;
+
+    private final HolonomicController controller;
+
+    private final BStream isAligned;
+
+    private final FieldObject2d targetPose2d;
+
+    public SwerveDriveToPose(Pose2d targetPose) {
+        swerve = SwerveDrive.getInstance();
+        this.targetPose = targetPose;
+
+        targetPose2d = Odometry.getInstance().getField().getObject("Target Pose");
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)
+        );
+
+        isAligned = BStream.create(this::isAligned)
+            .filtered(new BDebounceRC.Both(Alignment.DEBOUNCE_TIME.get()));
+
+        addRequirements(swerve);
+    }
+
+    private boolean isAligned() {
+        return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get());
+    }
+
+    @Override
+    public void initialize() {}
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = Odometry.getInstance().getPose();
+        targetPose2d.setPose(targetPose);
+
+        controller.update(targetPose, currentPose);
+        swerve.setChassisSpeeds(controller.getOutput());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return isAligned.get();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerve.stop();
+        targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java

@@ -0,0 +1,113 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
+import com.stuypulse.robot.constants.Settings.Alignment.Translation;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.math.Vector2D;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveToShoot extends Command {
+    /*
+     * swerve
+     * holonomi controller
+     * targetpose pose2d
+     * targetPose2d as a field object 2d
+     * 
+     * contrusctor (init them, add requirements)
+     * initialize (do the math to get the target pose)
+     * execute (set the target pose to the controller and then setChassisSpeeds to it)
+     * isFinished (check if the controller is done within the tolerances)
+     * end (stop the swerve)    
+     * 
+     */
+    private final SwerveDrive swerve;
+    private Pose2d targetPose;
+    private final HolonomicController controller;
+    private final FieldObject2d targetPose2d;
+
+    public SwerveDriveToShoot(Pose2d targetPose) {
+        swerve = SwerveDrive.getInstance();
+        this.targetPose = targetPose;
+
+        this.targetPose2d = Odometry.getInstance().getField().getObject("Target Pose");
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)
+        );
+
+        addRequirements(swerve);
+    }
+
+    private double linearInterpolate(double lowerY, double upperY, double lowerBound, double upperBound, double input) {
+        if (input == lowerBound) {
+            return lowerY;
+        } else if (input == upperBound) {
+            return upperY;
+        } else if (input < lowerBound || input > upperBound) {
+            throw new IllegalArgumentException("Input must be between lowerBound and upperBound");
+        }
+        else {
+            return (upperY - lowerY) * (input - lowerBound) / (upperBound - lowerBound) + lowerY;
+        }
+    }
+
+    //TODO: Make this work lmao
+    private Pose2d getSpeakerTargetPose(Rotation2d angleToSpeaker) {
+        //Everything under is from aiming at center of speaker
+        Vector2D robotPose = new Vector2D(Odometry.getInstance().getPose().getTranslation());
+        Vector2D targetPose = new Vector2D(this.targetPose.getTranslation());
+        Vector2D targetVector = robotPose.add(robotPose.sub(targetPose).normalize().mul(Alignment.TARGET_DISTANCE_IN.get()));
+        //Rotation2d angleToSpeaker = new Rotation2d(Math.atan2(targetPose.y - robotPose.y, targetPose.x - robotPose.x));
+
+        double speakerOpeningLength = Units.inchesToMeters(41.625);
+        Rotation2d speakerTargetAngle = Rotation2d.fromDegrees(linearInterpolate(-speakerOpeningLength / 2, speakerOpeningLength / 2, -70, 70, angleToSpeaker.getDegrees()));
+
+        return new Pose2d(targetVector.x, targetVector.y, speakerTargetAngle);
+    }
+
+    @Override
+    public void initialize() {
+        Vector2D robotPose = new Vector2D(Odometry.getInstance().getPose().getTranslation());
+        Vector2D targetPose = new Vector2D(this.targetPose.getTranslation());
+        Rotation2d angleToSpeaker = new Rotation2d(Math.atan2(targetPose.y - robotPose.y, targetPose.x - robotPose.x));
+        Vector2D targetVector = robotPose.add(robotPose.sub(targetPose).normalize().mul(Alignment.TARGET_DISTANCE_IN.get()));
+        //this.targetPose = getSpeakerTargetPose(angleToSpeaker);
+
+        this.targetPose = new Pose2d(targetVector.x, targetVector.y, angleToSpeaker);
+    }
+
+    @Override 
+    public void execute() {
+        Pose2d currentPose = Odometry.getInstance().getPose();
+        targetPose2d.setPose(targetPose);
+
+        controller.update(targetPose, currentPose);
+        swerve.setChassisSpeeds(controller.getOutput());
+    }
+
+
+    @Override
+    public boolean isFinished() {
+        return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerve.stop();
+        targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java

@@ -0,0 +1,32 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class SwerveDriveXMode extends Command {
+    private final SwerveDrive swerve;
+
+    public SwerveDriveXMode() {
+        swerve = SwerveDrive.getInstance();
+        addRequirements(swerve);
+    }
+
+    @Override 
+    public void initialize() {
+    }    
+
+    @Override
+    public void execute() {
+        SwerveModuleState[] states = new SwerveModuleState[] {
+            new SwerveModuleState(0,Rotation2d.fromDegrees(135)),
+            new SwerveModuleState(0,Rotation2d.fromDegrees(45)),
+            new SwerveModuleState(0,Rotation2d.fromDegrees(225)),
+            new SwerveModuleState(0,Rotation2d.fromDegrees(315))
+        };
+        swerve.setModuleStates(states);
+    }
+}

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -4,11 +4,13 @@
 
 import com.stuypulse.robot.util.Fiducial;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * This interface stores information about the field elements.
@@ -84,4 +86,14 @@ public static Fiducial getFiducial(int fid) {
         }
         return null;
     }
+
+    Pose2d SPEAKER_POSES[] = {
+        getFiducial(7).getLocation().toPose2d(), // BLUE CENTER
+        getFiducial(4).getLocation().toPose2d(), // RED CENTER
+    };
+
+    public static Pose2d getAllianceSpeakerPose() {
+        boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue;
+        return SPEAKER_POSES[isBlue ? 0 : 1];
+    }
 }