From ce87b41c1bbd663a856b946ab070494dd08a9298 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 17:08:14 -0700 Subject: [PATCH 1/6] =?UTF-8?q?=F0=9F=8E=A8=20S?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 16 +++++++--------- .../swerve/CommandSwerveDrivetrain.java | 4 ---- .../subsystems/swerve/kit/KitSwerveRequest.java | 3 ++- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc36f92e..12ccc33e 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,8 +75,6 @@ public class RobotContainer { /* Drive Controls */ private final int translationAxis = XboxController.Axis.kLeftY.value; - private final int strafeAxis = XboxController.Axis.kLeftX.value; - private final int rotationAxis = XboxController.Axis.kRightX.value; private final int secondaryAxis = XboxController.Axis.kRightY.value; /* Subsystems */ @@ -90,18 +88,18 @@ public class RobotContainer { private final KitSwerveRequest.FieldCentric drive = new KitSwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * Constants.stickDeadband) + .withDeadband(Constants.stickDeadband) .withRotationalDeadband( - MaxAngularRate * Constants.rotationalDeadband) // Add a 10% deadband + Constants.rotationalDeadband) // Add a 10% deadband .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric + SwerveModule.DriveRequestType.Velocity); // I want field-centric private SwerveFieldCentricFacingAngle azi = new SwerveFieldCentricFacingAngle() .withDeadband(MaxSpeed * .1) // TODO: update deadband .withRotationalDeadband(MaxAngularRate * .1) // TODO: update deadband .withHeadingController(SwerveConstants.azimuthController) - .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity); // driving in open loop private final SwerveTelemetry swerveTelemetry = new SwerveTelemetry(MaxSpeed); @@ -420,11 +418,11 @@ public void configureSwerve() { drivetrain.applyRequest( () -> drive - .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with + .withVelocityX(1 * MaxSpeed) // Drive forward with // negative Y (forward) .withVelocityY( - -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-driver.getRightX() * MaxAngularRate))); + 1 * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(1 * MaxAngularRate)).withName("monkey").andThen(new PrintCommand("command ended"))); /* Right stick absolute angle mode on trigger hold, robot adjusts heading to the angle right joystick creates */ diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 5caddbeb..3a6a5da7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -214,10 +214,6 @@ private void configurePathPlanner() { this); // Subsystem for requirements } - public void driveRobotRelative(ChassisSpeeds speeds) { - System.out.println(speeds); - this.setControl(AutoRequest.withSpeeds(speeds)); - } public Command pidToNote(Vision vision) { PIDController xController = new PIDController(0.1, 0, 0); diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java b/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java index e9683437..09ae671f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java +++ b/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java @@ -7,6 +7,8 @@ package frc.robot.subsystems.swerve.kit; +import org.littletonrobotics.junction.Logger; + // // Source code recreated from a .class file by IntelliJ IDEA // (powered by FernFlower decompiler) @@ -422,7 +424,6 @@ public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... m parameters.updatePeriod); SwerveModuleState[] states = parameters.kinematics.toSwerveModuleStates(speeds, this.CenterOfRotation); - for (int i = 0; i < modulesToApply.length; ++i) { modulesToApply[i].apply(states[i], this.DriveRequestType, this.SteerRequestType); } From 6cfbe508f05f337862e98017d86edfb61351737d Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 20:26:31 -0700 Subject: [PATCH 2/6] =?UTF-8?q?=F0=9F=90=9B=20Add=20back=20stuff=20from=20?= =?UTF-8?q?old=20swerve?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 52 +- .../swerve/CommandSwerveDrivetrain.java | 431 ++++----------- .../subsystems/swerve/SwerveTelemetry.java | 7 +- .../subsystems/swerve/TunerConstants.java | 9 +- .../robot/subsystems/swerve/kit/GyroIO.java | 30 - .../subsystems/swerve/kit/GyroIOPigeon2.java | 55 -- .../swerve/kit/KitSimSwerveDrivetrain.java | 152 ----- .../swerve/kit/KitSwerveDrivetrain.java | 480 ---------------- .../swerve/kit/KitSwerveRequest.java | 523 ------------------ .../robot/subsystems/swerve/kit/ModuleIO.java | 95 ---- .../swerve/kit/ModuleIOTalonFX.java | 393 ------------- .../SwerveFieldCentricFacingAngle.java | 6 +- 13 files changed, 130 insertions(+), 2105 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/GyroIO.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/GyroIOPigeon2.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/KitSimSwerveDrivetrain.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveDrivetrain.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/ModuleIO.java delete mode 100644 src/main/java/frc/robot/subsystems/swerve/kit/ModuleIOTalonFX.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ffa756ab..071b1003 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -40,7 +40,7 @@ public static final class FeatureFlags { public static final boolean kShooterEnabled = true; public static final boolean kShooterRegenerativeBrakingEnabled = true; - public static final boolean kSwerveEnabled = true; + public static final boolean kSwerveEnabled = false; public static final boolean kPivotIntakeEnabled = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12ccc33e..50f7d520 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -26,10 +27,8 @@ import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.FeatureFlags; import frc.robot.autos.commands.IntakeSequence; -import frc.robot.autos.routines.AutoRoutines; import frc.robot.helpers.XboxStalker; import frc.robot.subsystems.ampbar.AmpBar; import frc.robot.subsystems.ampbar.AmpBarIOTalonFX; @@ -53,7 +52,6 @@ import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveTelemetry; import frc.robot.subsystems.swerve.TunerConstants; -import frc.robot.subsystems.swerve.kit.KitSwerveRequest; import frc.robot.subsystems.swerve.requests.SwerveFieldCentricFacingAngle; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIOLimelight; @@ -86,13 +84,13 @@ public class RobotContainer { TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed private double MaxAngularRate = 1.5 * Math.PI; // My drivetrain - private final KitSwerveRequest.FieldCentric drive = - new KitSwerveRequest.FieldCentric() - .withDeadband(Constants.stickDeadband) + private final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(Constants.stickDeadband * MaxSpeed) .withRotationalDeadband( - Constants.rotationalDeadband) // Add a 10% deadband + Constants.rotationalDeadband * MaxAngularRate) // Add a 10% deadband .withDriveRequestType( - SwerveModule.DriveRequestType.Velocity); // I want field-centric + SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric private SwerveFieldCentricFacingAngle azi = new SwerveFieldCentricFacingAngle() @@ -141,10 +139,10 @@ public RobotContainer() { .onTrue( drivetrain.applyRequest(() -> azi.withTargetDirection(Rotation2d.fromDegrees(180)))); - test.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - test.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - test.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); - test.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + // test.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // test.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // test.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // test.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureClimb(); // If all the subsystems are enabled, configure "operator" autos @@ -313,9 +311,9 @@ public RobotContainer() { // } autoChooser = new SendableChooser<>(); autoChooser.setDefaultOption("Do Nothing", new InstantCommand()); - autoChooser.addOption( - "5 Note test", - AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); + // autoChooser.addOption( + // "5 Note test", + // AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); // Autos SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -415,14 +413,17 @@ public void configureSwerve() { // default command drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest( - () -> - drive - .withVelocityX(1 * MaxSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY( - 1 * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(1 * MaxAngularRate)).withName("monkey").andThen(new PrintCommand("command ended"))); + drivetrain + .applyRequest( + () -> + drive + .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-driver.getRightX() * MaxAngularRate)) + .withName("monkey") + .andThen(new PrintCommand("command ended"))); /* Right stick absolute angle mode on trigger hold, robot adjusts heading to the angle right joystick creates */ @@ -453,7 +454,7 @@ public void configureSwerve() { -driver.getLeftTriggerAxis() * (MaxAngularRate * 0.3)))); // Reset robot heading on button press - driver.y().onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative)); + driver.y().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative())); // Azimuth angle bindings. isRed == true for red alliance presets. isRed != true for blue. if (isRed == true) { @@ -489,7 +490,8 @@ public void configureSwerve() { if (Utils.isSimulation()) { drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } - drivetrain.registerTelemetry(swerveTelemetry::telemeterize); + // drivetrain.registerTelemetry(logger::telemeterize); + // drivetrain.registerTelemetry(swerveTelemetry::telemeterize); } private void configureShooter() { diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 3a6a5da7..6b559a87 100644 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -7,35 +7,24 @@ package frc.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Volts; - -import com.choreo.lib.*; -import com.ctre.phoenix6.SignalLogger; +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.*; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.limelight.LimelightHelpers; -import frc.robot.subsystems.swerve.kit.KitSwerveDrivetrain; -import frc.robot.subsystems.swerve.kit.KitSwerveRequest; import frc.robot.subsystems.vision.Vision; import java.util.Optional; import java.util.function.Supplier; @@ -45,12 +34,20 @@ * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used * in command-based projects easily. */ -public class CommandSwerveDrivetrain extends KitSwerveDrivetrain implements Subsystem { - private final boolean enabled; +public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; + public final SwerveRequest.ApplyChassisSpeeds AutoRequest = + new SwerveRequest.ApplyChassisSpeeds() + .withDriveRequestType( + SwerveModule.DriveRequestType.Velocity); // TODO: VERY IMPORTANT TUNE THIS + + public final SwerveRequest.RobotCentric NoteRequest = + new SwerveRequest.RobotCentric() + .withDriveRequestType( + SwerveModule.DriveRequestType.Velocity); // TODO: VERY IMPORTANT TUNE THIS /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ @@ -58,163 +55,26 @@ public class CommandSwerveDrivetrain extends KitSwerveDrivetrain implements Subs /* Keep track if we've ever applied the operator perspective before or not */ private boolean hasAppliedOperatorPerspective = false; - public final KitSwerveRequest.ApplyChassisSpeeds AutoRequest = - new KitSwerveRequest.ApplyChassisSpeeds() - .withDriveRequestType(SwerveModule.DriveRequestType.Velocity); - - private final KitSwerveRequest.FieldCentric AutoRequest2 = - new KitSwerveRequest.FieldCentric() - .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); - - private final KitSwerveRequest.SysIdSwerveTranslation TranslationCharacterization = - new KitSwerveRequest.SysIdSwerveTranslation(); - private final KitSwerveRequest.SysIdSwerveRotation RotationCharacterization = - new KitSwerveRequest.SysIdSwerveRotation(); - private final KitSwerveRequest.SysIdSwerveSteerGains SteerCharacterization = - new KitSwerveRequest.SysIdSwerveSteerGains(); - - /* Use one of these sysidroutines for your particular test */ - private SysIdRoutine SysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(TranslationCharacterization.withVolts(volts)), null, this)); - - private final SysIdRoutine SysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(4), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(RotationCharacterization.withVolts(volts)), null, this)); - private final SysIdRoutine SysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - Volts.of(7), - null, - (state) -> SignalLogger.writeString("state", state.toString())), - new SysIdRoutine.Mechanism( - (volts) -> setControl(SteerCharacterization.withVolts(volts)), null, this)); - - /* Change this to the sysid routine you want to test */ - private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation; - public CommandSwerveDrivetrain( - boolean enabled, SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { super(driveTrainConstants, OdometryUpdateFrequency, modules); - this.enabled = enabled; - configurePathPlanner(); if (Utils.isSimulation()) { startSimThread(); } + configurePathPlanner(); } public CommandSwerveDrivetrain( - boolean enabled, - SwerveDrivetrainConstants driveTrainConstants, - SwerveModuleConstants... modules) { + SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { super(driveTrainConstants, modules); - this.enabled = enabled; - configurePathPlanner(); if (Utils.isSimulation()) { startSimThread(); } + configurePathPlanner(); } - @Override - public void setControl(KitSwerveRequest request) { - Logger.recordOutput("/SwerveRequest", request.getClass().getSimpleName()); - if (enabled) { - super.setControl(request); - } - } - - public void updateVision() { - boolean useMegaTag2 = true; // set to false to use MegaTag1 - boolean doRejectUpdate = false; - if (useMegaTag2 == false) { - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); - - if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { - if (mt1.rawFiducials[0].ambiguity > .7) { - doRejectUpdate = true; - } - if (mt1.rawFiducials[0].distToCamera > 3) { - doRejectUpdate = true; - } - } - if (mt1.tagCount == 0) { - doRejectUpdate = true; - } - - if (!doRejectUpdate) { - this.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); - this.addVisionMeasurement(mt1.pose, mt1.timestampSeconds); - } - } else if (useMegaTag2 == true) { - LimelightHelpers.SetRobotOrientation( - "limelight", - this.m_odometry.getEstimatedPosition().getRotation().getDegrees(), - 0, - 0, - 0, - 0, - 0); - LimelightHelpers.PoseEstimate mt2 = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); - if (Math.abs(this.getPigeon2().getRate()) - > 720) // if our angular velocity is greater than 720 degrees per - // second, ignore vision - // updates - { - doRejectUpdate = true; - } - if (mt2.tagCount == 0) { - doRejectUpdate = true; - } - if (!doRejectUpdate) { - this.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); - this.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); - } - } - } - - private void configurePathPlanner() { - double driveBaseRadius = 0; - for (var moduleLocation : m_moduleLocations) { - driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); - } - - AutoBuilder.configureHolonomic( - () -> this.getState().Pose, // Supplier of current robot pose - this::seedFieldRelative, // Consumer for seeding pose against auto - this::getCurrentRobotChassisSpeeds, - (speeds) -> - this.setControl( - AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot - new HolonomicPathFollowerConfig( - SwerveConstants.autoTranslationalController, - SwerveConstants.autoRotationalController, - TunerConstants.kSpeedAt12VoltsMps, - driveBaseRadius, - new ReplanningConfig()), - () -> - DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) - == DriverStation.Alliance.Red, // Assume - this); // Subsystem for requirements - } - - public Command pidToNote(Vision vision) { PIDController xController = new PIDController(0.1, 0, 0); PIDController yController = new PIDController(0.1, 0, 0); @@ -226,101 +86,14 @@ public Command pidToNote(Vision vision) { var ySpeed = yController.calculate(0, vision.getNoteLimelightX()); Logger.recordOutput(this.getClass().getSimpleName() + "ySpeed", ySpeed); this.setControl( - AutoRequest2.withVelocityX(-xSpeed).withVelocityY(ySpeed).withRotationalRate(0)); + NoteRequest.withVelocityX(-xSpeed).withVelocityY(ySpeed).withRotationalRate(0)); }); } - public Command pathfindToNote(Vision vision) { - - double driveBaseRadius = 0; - for (var moduleLocation : m_moduleLocations) { - driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); - } - - PathConstraints constraints = - new PathConstraints( - TunerConstants.kSpeedAt12VoltsMps, // max speed (4.96) - 99, // max acceleration (4 m/s^2) - edu.wpi.first.math.util.Units.degreesToRadians(450), // max angular velocity (450 deg/s) - edu.wpi.first.math.util.Units.degreesToRadians( - 540)); // max angular acceleration (540 deg/s^2) - - // return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile("preload only"), - // constraints, 3.0); - // return new PathfindHolonomic( - // new Pose2d(2,2, - // Rotation2d.fromRadians(1)), - // constraints, - // ()->this.getState().Pose, - // this::getCurrentRobotChassisSpeeds, - // (speeds)->this.setControl(AutoRequest.withSpeeds(speeds)), - // new HolonomicPathFollowerConfig( - // TunerConstants.kSpeedAt12VoltsMps, - // driveBaseRadius, - // new ReplanningConfig())); - // 0.0, - // this); - - return this.defer( - () -> - AutoBuilder.pathfindToPose( - new Pose2d(new Translation2d(2, 2), Rotation2d.fromDegrees(0)), - constraints, - 0.0, - 0.0)); - // return this.defer(()->{System.out.println("hello");return AutoBuilder.pathfindToPose(new - // Pose2d(new Translation2d(2 - // .28,2.10),Rotation2d.fromDegrees(20)),constraints,1,0.0);}); - // return this.defer(()->{return AutoBuilder.pathfindToPose( - // // current pose, path constraints (see above), "goal end velocity", rotation - // // delay distance (how long to travel before rotating) - // vision.getNotePose(this.getState().Pose), constraints, 1, 0.0);}); - } - - public Command rotationTest() { - PathConstraints constraints = - new PathConstraints( - TunerConstants.kSpeedAt12VoltsMps - 1, // max speed (4.96) - 4, // max acceleration (4 m/s^2) - edu.wpi.first.math.util.Units.degreesToRadians(450), // max angular velocity (450 deg/s) - edu.wpi.first.math.util.Units.degreesToRadians( - 540)); // max angular acceleration (540 deg/s^2) - return AutoBuilder.pathfindToPose( - // current pose, path constraints (see above), "goal end velocity", rotation - // delay distance (how long to travel before rotating) - this.getState() - .Pose - .rotateBy(Rotation2d.fromDegrees(90)) - .plus(new Transform2d(new Translation2d(2, 1), Rotation2d.fromDegrees(0))), - constraints, - 0, - 0.0); - } - - public Command applyRequest(Supplier requestSupplier) { + public Command applyRequest(Supplier requestSupplier) { return run(() -> this.setControl(requestSupplier.get())); } - public Command getAutoPath(String pathName) { - return new PathPlannerAuto(pathName); - } - - /* - * Both the sysid commands are specific to one particular sysid routine, change - * which one you're trying to characterize - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return RoutineToApply.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return RoutineToApply.dynamic(direction); - } - - public ChassisSpeeds getCurrentRobotChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } - private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); @@ -338,6 +111,31 @@ private void startSimThread() { m_simNotifier.startPeriodic(kSimLoopPeriod); } + private void configurePathPlanner() { + double driveBaseRadius = 0; + for (var moduleLocation : m_moduleLocations) { + driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); + } + + AutoBuilder.configureHolonomic( + () -> this.getState().Pose, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + () -> this.getState().speeds, + (speeds) -> + this.setControl( + AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot + new HolonomicPathFollowerConfig( + SwerveConstants.autoTranslationalController, + SwerveConstants.autoRotationalController, + TunerConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + new ReplanningConfig()), + () -> + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + == DriverStation.Alliance.Red, // Assume + this); // Subsystem for requirements + } + public Command runChoreoTraj(ChoreoTrajectory trajectory) { return Choreo.choreoSwerveCommand( trajectory, @@ -346,119 +144,82 @@ public Command runChoreoTraj(ChoreoTrajectory trajectory) { SwerveConstants.choreoTranslationController, SwerveConstants.choreoRotationController, ((ChassisSpeeds speeds) -> - this.setControl(new KitSwerveRequest.ApplyChassisSpeeds().withSpeeds(speeds))), + this.setControl(new SwerveRequest.ApplyChassisSpeeds().withSpeeds(speeds))), () -> { - Optional alliance = DriverStation.getAlliance(); + Optional alliance = DriverStation.getAlliance(); return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; }, this); } - /* - * This method comes from 1690's Second Software - * Presentation - * we get the skidding ratio from the current SwerveModuleStates - * The skidding ratio is defined as the ratio between the maximum and the - * minimum magnitudes of the "translational" part of the velocity vector of the - * robot. - * - * @return the skidding ratio of the robot, maximum/minimum, ranges from - * [1,INFINITY) - */ - public double getSkiddingRatio() { - // josh: accessing swerveStates / kinematics like this is INTENTIONAL. - - // grab the current SwerveModuleStates & SwerveDriveKinematics. - final SwerveModuleState[] swerveStates = super.getState().ModuleStates; - final SwerveDriveKinematics kinematics = super.m_kinematics; - - // get the angular velocity of the robot - final double angularVelocityOmegaMeasured = - kinematics.toChassisSpeeds(swerveStates).omegaRadiansPerSecond; // use - // IK to - // get a - // chassis - // speed, - // then - // pull - // out - // the - // angular velocity - - // get the rotational SwerveModuleStates (i.e SwerveModuleState with only angle) - final SwerveModuleState[] swerveStatesRotational = - kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, angularVelocityOmegaMeasured)); + public void updateVision() { + boolean useMegaTag2 = true; // set to false to use MegaTag1 + boolean doRejectUpdate = false; + if (useMegaTag2 == false) { + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); - final double[] swerveStatesTranslationalMagnitudes = - new double[super.getState().ModuleStates.length]; - // josh: intentionally doing this instead of directly pulling - // speedMetersPerSecond from the SwerveModuleState - for (int i = 0; i < swerveStates.length; i++) { - final Translation2d - swerveStateTranslation2d = convertSwerveModuleStateToTranslation2d(swerveStates[i]), - swerveStateRotational = - convertSwerveModuleStateToTranslation2d(swerveStatesRotational[i]), - swerveStateTranslational = swerveStateTranslation2d.minus(swerveStateRotational); - swerveStatesTranslationalMagnitudes[i] = swerveStateTranslational.getNorm(); - } + if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { + if (mt1.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + if (mt1.rawFiducials[0].distToCamera > 3) { + doRejectUpdate = true; + } + } + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } - // find the maximum and minimum magnitudes of the translational parts of the - // SwerveModuleStates - double max = Double.MIN_VALUE, min = Double.MAX_VALUE; - for (double translationalSpeed : swerveStatesTranslationalMagnitudes) { - max = Math.max(max, translationalSpeed); - min = Math.min(min, translationalSpeed); + if (!doRejectUpdate) { + this.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); + this.addVisionMeasurement(mt1.pose, mt1.timestampSeconds); + } + } else if (useMegaTag2 == true) { + LimelightHelpers.SetRobotOrientation( + "limelight", + this.m_odometry.getEstimatedPosition().getRotation().getDegrees(), + 0, + 0, + 0, + 0, + 0); + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); + if (Math.abs(this.getPigeon2().getRate()) + > 720) // if our angular velocity is greater than 720 degrees per + // second, ignore vision + // updates + { + doRejectUpdate = true; + } + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + if (!doRejectUpdate) { + this.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); + this.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); + } } - - // return the skidding ratio - return max / min; - } - - /* - * Translation2d is the wpilib class that represents a 2d vector. this method - * could really be called - * "convertSwerveModuleStateToVector" - */ - private Translation2d convertSwerveModuleStateToTranslation2d(SwerveModuleState state) { - return new Translation2d(state.speedMetersPerSecond, state.angle); } @Override public void periodic() { - super.periodic(); - Logger.recordOutput("Pose", this.getState().Pose); - Logger.recordOutput( - this.getClass().getSimpleName() + "/CurrentCommand", - this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "none"); /* Periodically try to apply the operator perspective */ - /* - * If we haven't applied the operator perspective before, then we should apply - * it regardless of DS state - */ - /* - * This allows us to correct the perspective in case the robot code restarts - * mid-match - */ - /* - * Otherwise, only check and apply the operator perspective if the DS is - * disabled - */ - /* - * This ensures driving behavior doesn't change until an explicit disable event - * occurs during testing - */ + /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ + /* This allows us to correct the perspective in case the robot code restarts mid-match */ + /* Otherwise, only check and apply the operator perspective if the DS is disabled */ + /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { DriverStation.getAlliance() .ifPresent( (allianceColor) -> { this.setOperatorPerspectiveForward( - allianceColor == DriverStation.Alliance.Red + allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation : BlueAlliancePerspectiveRotation); hasAppliedOperatorPerspective = true; }); - updateVision(); } + updateVision(); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java index 2912f0a3..105468cf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java @@ -8,6 +8,7 @@ package frc.robot.subsystems.swerve; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.DoubleArrayPublisher; @@ -20,8 +21,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.subsystems.swerve.kit.KitSwerveDrivetrain.SwerveDriveState; -import org.littletonrobotics.junction.Logger; public class SwerveTelemetry { private final double MaxSpeed; @@ -95,12 +94,10 @@ public SwerveTelemetry(double maxSpeed) { /* Accept the swerve drive state and telemeterize it to smartdashboard */ public void telemeterize(SwerveDriveState state) { /* Telemeterize the pose */ - - // Logger.recordOutput("realBotPose", state.Pose); Pose2d pose = state.Pose; fieldTypePub.set("Field2d"); fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - Logger.recordOutput("SwervePoseCalculated", pose); + /* Telemeterize the robot's general speeds */ double currentTime = Utils.getCurrentTimeSeconds(); double diffTime = currentTime - lastTime; diff --git a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java index c86d52a6..eb03ab21 100644 --- a/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/TunerConstants.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -162,11 +161,5 @@ public class TunerConstants { kInvertRightSide); public static final CommandSwerveDrivetrain DriveTrain = - new CommandSwerveDrivetrain( - Constants.FeatureFlags.kSwerveEnabled, - DrivetrainConstants, - FrontLeft, - FrontRight, - BackLeft, - BackRight); + new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/kit/GyroIO.java deleted file mode 100644 index 9c21fd5b..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/GyroIO.java +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation3d; -import org.littletonrobotics.junction.AutoLog; - -public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public boolean connected = false; - public double rate = 0.0; - public Rotation3d position = new Rotation3d(); - } - - public default void updateInputs(GyroIOInputs inputs) {} - - public StatusSignal getYaw(); - - public StatusSignal getAngularVelocity(); - - public Pigeon2 getPigeon2(); -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/kit/GyroIOPigeon2.java deleted file mode 100644 index 2ce4fb90..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/GyroIOPigeon2.java +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.util.Units; - -/** IO implementation for Pigeon2 */ -public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon; - private final StatusSignal yaw; - private final StatusSignal yawVelocity; - - public GyroIOPigeon2(int id, String canbusName) { - this.pigeon = new Pigeon2(id, canbusName); - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw = pigeon.getYaw().clone(); - yawVelocity = pigeon.getAngularVelocityZWorld().clone(); - yaw.setUpdateFrequency(20); // TODO: check if this is the right frequency - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.position = pigeon.getRotation3d(); - inputs.rate = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - } - - @Override - public StatusSignal getYaw() { - return yaw; - } - - @Override - public StatusSignal getAngularVelocity() { - return yawVelocity; - } - - @Override - public Pigeon2 getPigeon2() { - return pigeon; - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/KitSimSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/kit/KitSimSwerveDrivetrain.java deleted file mode 100644 index 226bee86..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/KitSimSwerveDrivetrain.java +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -// -// Source code recreated from a .class file by IntelliJ IDEA -// (powered by FernFlower decompiler) -// - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.Pigeon2SimState; -import com.ctre.phoenix6.sim.TalonFXSimState; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; - -public class KitSimSwerveDrivetrain { - public final Pigeon2SimState PigeonSim; - protected final SimSwerveModule[] m_modules; - protected final int ModuleCount; - public final SwerveDriveKinematics Kinem; - public Rotation2d LastAngle = new Rotation2d(); - - public KitSimSwerveDrivetrain( - Translation2d[] wheelLocations, - Pigeon2 pigeon, - SwerveDrivetrainConstants driveConstants, - SwerveModuleConstants... moduleConstants) { - this.PigeonSim = pigeon.getSimState(); - this.ModuleCount = moduleConstants.length; - this.m_modules = new SimSwerveModule[this.ModuleCount]; - - for (int i = 0; i < this.ModuleCount; ++i) { - this.m_modules[i] = - new SimSwerveModule( - moduleConstants[i].SteerMotorGearRatio, - moduleConstants[i].SteerInertia, - moduleConstants[i].SteerFrictionVoltage, - moduleConstants[i].SteerMotorInverted, - moduleConstants[i].DriveMotorGearRatio, - moduleConstants[i].DriveInertia, - moduleConstants[i].DriveFrictionVoltage, - moduleConstants[i].DriveMotorInverted); - } - - this.Kinem = new SwerveDriveKinematics(wheelLocations); - } - - public void update(double dtSeconds, double supplyVoltage, ModuleIO... modulesToApply) { - if (this.m_modules.length == this.ModuleCount) { - SwerveModuleState[] states = new SwerveModuleState[this.ModuleCount]; - - for (int i = 0; i < this.ModuleCount; ++i) { - TalonFXSimState steerMotor = modulesToApply[i].getSteerMotor().getSimState(); - TalonFXSimState driveMotor = modulesToApply[i].getDriveMotor().getSimState(); - CANcoderSimState cancoder = modulesToApply[i].getCANcoder().getSimState(); - steerMotor.Orientation = - this.m_modules[i].SteerMotorInverted - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; - driveMotor.Orientation = - this.m_modules[i].DriveMotorInverted - ? ChassisReference.Clockwise_Positive - : ChassisReference.CounterClockwise_Positive; - steerMotor.setSupplyVoltage(supplyVoltage); - driveMotor.setSupplyVoltage(supplyVoltage); - cancoder.setSupplyVoltage(supplyVoltage); - this.m_modules[i].SteerMotor.setInputVoltage( - this.addFriction(steerMotor.getMotorVoltage(), this.m_modules[i].SteerFrictionVoltage)); - this.m_modules[i].DriveMotor.setInputVoltage( - this.addFriction(driveMotor.getMotorVoltage(), this.m_modules[i].DriveFrictionVoltage)); - this.m_modules[i].SteerMotor.update(dtSeconds); - this.m_modules[i].DriveMotor.update(dtSeconds); - steerMotor.setRawRotorPosition( - this.m_modules[i].SteerMotor.getAngularPositionRotations() - * this.m_modules[i].SteerGearing); - steerMotor.setRotorVelocity( - this.m_modules[i].SteerMotor.getAngularVelocityRPM() - / 60.0 - * this.m_modules[i].SteerGearing); - cancoder.setRawPosition(this.m_modules[i].SteerMotor.getAngularPositionRotations()); - cancoder.setVelocity(this.m_modules[i].SteerMotor.getAngularVelocityRPM() / 60.0); - driveMotor.setRawRotorPosition( - this.m_modules[i].DriveMotor.getAngularPositionRotations() - * this.m_modules[i].DriveGearing); - driveMotor.setRotorVelocity( - this.m_modules[i].DriveMotor.getAngularVelocityRPM() - / 60.0 - * this.m_modules[i].DriveGearing); - states[i] = modulesToApply[i].getCurrentState(); - } - - double angleChange = this.Kinem.toChassisSpeeds(states).omegaRadiansPerSecond * dtSeconds; - this.LastAngle = this.LastAngle.plus(Rotation2d.fromRadians(angleChange)); - this.PigeonSim.setRawYaw(this.LastAngle.getDegrees()); - } - } - - protected double addFriction(double motorVoltage, double frictionVoltage) { - if (Math.abs(motorVoltage) < frictionVoltage) { - motorVoltage = 0.0; - } else if (motorVoltage > 0.0) { - motorVoltage -= frictionVoltage; - } else { - motorVoltage += frictionVoltage; - } - - return motorVoltage; - } - - public class SimSwerveModule { - public final DCMotorSim SteerMotor; - public final DCMotorSim DriveMotor; - public final double SteerGearing; - public final double DriveGearing; - public final double SteerFrictionVoltage; - public final double DriveFrictionVoltage; - public final boolean SteerMotorInverted; - public final boolean DriveMotorInverted; - - public SimSwerveModule( - double steerGearing, - double steerInertia, - double steerFrictionVoltage, - boolean steerMotorInverted, - double driveGearing, - double driveInertia, - double driveFrictionVoltage, - boolean driveMotorInverted) { - this.SteerMotor = new DCMotorSim(DCMotor.getFalcon500(1), steerGearing, steerInertia); - this.DriveMotor = new DCMotorSim(DCMotor.getFalcon500(1), driveGearing, driveInertia); - this.SteerGearing = steerGearing; - this.DriveGearing = driveGearing; - this.SteerFrictionVoltage = steerFrictionVoltage; - this.DriveFrictionVoltage = driveFrictionVoltage; - this.SteerMotorInverted = steerMotorInverted; - this.DriveMotorInverted = driveMotorInverted; - } - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveDrivetrain.java deleted file mode 100644 index c8142949..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveDrivetrain.java +++ /dev/null @@ -1,480 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.mechanisms.swerve.*; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.filter.MedianFilter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj.Timer; -import java.util.concurrent.locks.ReadWriteLock; -import java.util.concurrent.locks.ReentrantReadWriteLock; -import java.util.function.Consumer; -import org.littletonrobotics.junction.Logger; - -public class KitSwerveDrivetrain { - protected final boolean IsOnCANFD; - protected final double UpdateFrequency; - protected final int ModuleCount; - protected final ModuleIO[] Modules; - - private final ModuleIOInputsAutoLogged[] moduleInputs = { - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged(), - new ModuleIOInputsAutoLogged() - }; - protected final GyroIO m_gyroIO; - private final GyroIOInputsAutoLogged m_gyroIOAutoLogged = new GyroIOInputsAutoLogged(); - protected final StatusSignal m_yawGetter; - protected final StatusSignal m_angularVelocity; - protected SwerveDriveKinematics m_kinematics; - protected SwerveDrivePoseEstimator m_odometry; - protected SwerveModulePosition[] m_modulePositions; - protected SwerveModuleState[] m_moduleStates; - protected Translation2d[] m_moduleLocations; - protected OdometryThread m_odometryThread; - protected Rotation2d m_fieldRelativeOffset; - protected Rotation2d m_operatorForwardDirection; - protected KitSwerveRequest m_requestToApply; - protected KitSwerveRequest.SwerveControlRequestParameters m_requestParameters; - protected final ReadWriteLock m_stateLock; - protected final KitSimSwerveDrivetrain m_simDrive; - protected Consumer m_telemetryFunction; - protected final SwerveDriveState m_cachedState; - - protected boolean checkIsOnCanFD(String canbusName) { - return CANBus.isNetworkFD(canbusName); - } - - public KitSwerveDrivetrain( - SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - this(driveTrainConstants, 0.0, modules); - } - - public KitSwerveDrivetrain( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - this( - driveTrainConstants, - OdometryUpdateFrequency, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9), - modules); - } - - public KitSwerveDrivetrain( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - this.m_requestToApply = new KitSwerveRequest.Idle(); - this.m_requestParameters = new KitSwerveRequest.SwerveControlRequestParameters(); - this.m_stateLock = new ReentrantReadWriteLock(); - this.m_telemetryFunction = null; - this.m_cachedState = new SwerveDriveState(); - this.IsOnCANFD = this.checkIsOnCanFD(driveTrainConstants.CANbusName); - if (OdometryUpdateFrequency == 0.0) { - this.UpdateFrequency = this.IsOnCANFD ? 250.0 : 100.0; - } else { - this.UpdateFrequency = OdometryUpdateFrequency; - } - - this.ModuleCount = modules.length; - this.m_gyroIO = - new GyroIOPigeon2(driveTrainConstants.Pigeon2Id, driveTrainConstants.CANbusName); - this.m_yawGetter = this.m_gyroIO.getYaw(); - this.m_angularVelocity = this.m_gyroIO.getAngularVelocity(); - this.Modules = new ModuleIO[this.ModuleCount]; - this.m_modulePositions = new SwerveModulePosition[this.ModuleCount]; - this.m_moduleStates = new SwerveModuleState[this.ModuleCount]; - this.m_moduleLocations = new Translation2d[this.ModuleCount]; - int iteration = 0; - SwerveModuleConstants[] var8 = modules; - int var9 = modules.length; - - for (int var10 = 0; var10 < var9; ++var10) { - SwerveModuleConstants module = var8[var10]; - this.Modules[iteration] = new ModuleIOTalonFX(module, driveTrainConstants.CANbusName); - this.m_moduleLocations[iteration] = new Translation2d(module.LocationX, module.LocationY); - this.m_modulePositions[iteration] = this.Modules[iteration].getPosition(true); - this.m_moduleStates[iteration] = this.Modules[iteration].getCurrentState(); - ++iteration; - } - - this.m_kinematics = new SwerveDriveKinematics(this.m_moduleLocations); - this.m_odometry = - new SwerveDrivePoseEstimator( - this.m_kinematics, - new Rotation2d(), - this.m_modulePositions, - new Pose2d(), - odometryStandardDeviation, - visionStandardDeviation); - this.m_fieldRelativeOffset = new Rotation2d(); - this.m_operatorForwardDirection = new Rotation2d(); - this.m_simDrive = - new KitSimSwerveDrivetrain( - this.m_moduleLocations, this.m_gyroIO.getPigeon2(), driveTrainConstants, modules); - this.m_odometryThread = new OdometryThread(); - this.m_odometryThread.start(); - } - - public void periodic() { - for (int i = 0; i < this.Modules.length; i++) { - this.Modules[i].updateInputs(moduleInputs[i]); - Logger.processInputs(this.getClass().getSimpleName() + "/Module" + i, this.moduleInputs[i]); - } - m_gyroIO.updateInputs(m_gyroIOAutoLogged); - Logger.processInputs(this.getClass().getSimpleName() + "/Gyro", m_gyroIOAutoLogged); - } - - public OdometryThread getDaqThread() { - return this.m_odometryThread; - } - - public void setControl(KitSwerveRequest request) { - try { - this.m_stateLock.writeLock().lock(); - this.m_requestToApply = request; - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public StatusCode configNeutralMode(NeutralModeValue neutralMode) { - StatusCode status = StatusCode.OK; - ModuleIO[] var3 = this.Modules; - int var4 = var3.length; - - for (int var5 = 0; var5 < var4; ++var5) { - ModuleIO module = var3[var5]; - StatusCode moduleStatus = module.configNeutralMode(neutralMode); - if (status.isOK()) { - status = moduleStatus; - } - } - - return status; - } - - public void tareEverything() { - try { - this.m_stateLock.writeLock().lock(); - - for (int i = 0; i < this.ModuleCount; ++i) { - this.Modules[i].resetPosition(); - this.m_modulePositions[i] = this.Modules[i].getPosition(true); - } - - this.m_odometry.resetPosition( - Rotation2d.fromDegrees((Double) this.m_yawGetter.getValue()), - this.m_modulePositions, - new Pose2d()); - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public void seedFieldRelative() { - try { - this.m_stateLock.writeLock().lock(); - this.m_fieldRelativeOffset = this.getState().Pose.getRotation(); - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public void setOperatorPerspectiveForward(Rotation2d fieldDirection) { - this.m_operatorForwardDirection = fieldDirection; - } - - public void seedFieldRelative(Pose2d location) { - try { - this.m_stateLock.writeLock().lock(); - this.m_odometry.resetPosition( - Rotation2d.fromDegrees((Double) this.m_yawGetter.getValue()), - this.m_modulePositions, - location); - this.m_cachedState.Pose = location; - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public boolean odometryIsValid() { - boolean var1; - try { - this.m_stateLock.readLock().lock(); - var1 = this.m_odometryThread.odometryIsValid(); - } finally { - this.m_stateLock.readLock().unlock(); - } - - return var1; - } - - public ModuleIO getModule(int index) { - return index >= this.Modules.length ? null : this.Modules[index]; - } - - public SwerveDriveState getState() { - SwerveDriveState var1; - try { - this.m_stateLock.readLock().lock(); - var1 = this.m_cachedState; - } finally { - this.m_stateLock.readLock().unlock(); - } - - return var1; - } - - public Rotation3d getRotation3d() { - return this.m_gyroIOAutoLogged.position; - } - - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - try { - this.m_stateLock.writeLock().lock(); - this.m_odometry.addVisionMeasurement( - visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - try { - this.m_stateLock.writeLock().lock(); - this.m_odometry.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { - try { - this.m_stateLock.writeLock().lock(); - this.m_odometry.setVisionMeasurementStdDevs(visionMeasurementStdDevs); - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public void updateSimState(double dtSeconds, double supplyVoltage) { - this.m_simDrive.update(dtSeconds, supplyVoltage, this.Modules); - } - - public void registerTelemetry(Consumer telemetryFunction) { - try { - this.m_stateLock.writeLock().lock(); - this.m_telemetryFunction = telemetryFunction; - } finally { - this.m_stateLock.writeLock().unlock(); - } - } - - public Pigeon2 getPigeon2() { - return this.m_gyroIO.getPigeon2(); - } - - public class OdometryThread { - protected static final int START_THREAD_PRIORITY = 1; - protected final Thread m_thread = new Thread(this::run); - protected volatile boolean m_running = false; - protected final BaseStatusSignal[] m_allSignals; - protected final MedianFilter peakRemover = new MedianFilter(3); - protected final LinearFilter lowPass = LinearFilter.movingAverage(50); - protected double lastTime = 0.0; - protected double currentTime = 0.0; - protected double averageLoopTime = 0.0; - protected int SuccessfulDaqs = 0; - protected int FailedDaqs = 0; - protected int lastThreadPriority = 1; - protected volatile int threadPriorityToSet = 1; - - public OdometryThread() { - this.m_thread.setDaemon(true); - this.m_allSignals = new BaseStatusSignal[KitSwerveDrivetrain.this.ModuleCount * 4 + 2]; - - for (int i = 0; i < KitSwerveDrivetrain.this.ModuleCount; ++i) { - BaseStatusSignal[] signals = KitSwerveDrivetrain.this.Modules[i].getSignals(); - this.m_allSignals[i * 4 + 0] = signals[0]; - this.m_allSignals[i * 4 + 1] = signals[1]; - this.m_allSignals[i * 4 + 2] = signals[2]; - this.m_allSignals[i * 4 + 3] = signals[3]; - } - - this.m_allSignals[this.m_allSignals.length - 2] = KitSwerveDrivetrain.this.m_yawGetter; - this.m_allSignals[this.m_allSignals.length - 1] = KitSwerveDrivetrain.this.m_angularVelocity; - } - - public void start() { - this.m_running = true; - this.m_thread.start(); - } - - public void stop() { - this.stop(0L); - } - - public void stop(long millis) { - this.m_running = false; - - try { - this.m_thread.join(millis); - } catch (InterruptedException var4) { - Thread.currentThread().interrupt(); - } - } - - public void run() { - BaseStatusSignal.setUpdateFrequencyForAll( - KitSwerveDrivetrain.this.UpdateFrequency, this.m_allSignals); - Threads.setCurrentThreadPriority(true, 1); - - while (this.m_running) { - StatusCode status; - if (KitSwerveDrivetrain.this.IsOnCANFD) { - status = - BaseStatusSignal.waitForAll( - 2.0 / KitSwerveDrivetrain.this.UpdateFrequency, this.m_allSignals); - } else { - Timer.delay(1.0 / KitSwerveDrivetrain.this.UpdateFrequency); - status = BaseStatusSignal.refreshAll(this.m_allSignals); - } - - try { - KitSwerveDrivetrain.this.m_stateLock.writeLock().lock(); - this.lastTime = this.currentTime; - this.currentTime = Utils.getCurrentTimeSeconds(); - this.averageLoopTime = - this.lowPass.calculate(this.peakRemover.calculate(this.currentTime - this.lastTime)); - if (status.isOK()) { - ++this.SuccessfulDaqs; - } else { - ++this.FailedDaqs; - } - - for (int i = 0; i < KitSwerveDrivetrain.this.ModuleCount; ++i) { - KitSwerveDrivetrain.this.m_modulePositions[i] = - KitSwerveDrivetrain.this.Modules[i].getPosition(false); - KitSwerveDrivetrain.this.m_moduleStates[i] = - KitSwerveDrivetrain.this.Modules[i].getCurrentState(); - } - - double yawDegrees = - BaseStatusSignal.getLatencyCompensatedValue( - KitSwerveDrivetrain.this.m_yawGetter, KitSwerveDrivetrain.this.m_angularVelocity); - KitSwerveDrivetrain.this.m_odometry.update( - Rotation2d.fromDegrees(yawDegrees), KitSwerveDrivetrain.this.m_modulePositions); - ChassisSpeeds speeds = - KitSwerveDrivetrain.this.m_kinematics.toChassisSpeeds( - KitSwerveDrivetrain.this.m_moduleStates); - KitSwerveDrivetrain.this.m_requestParameters.currentPose = - KitSwerveDrivetrain.this - .m_odometry - .getEstimatedPosition() - .relativeTo(new Pose2d(0.0, 0.0, KitSwerveDrivetrain.this.m_fieldRelativeOffset)); - KitSwerveDrivetrain.this.m_requestParameters.kinematics = - KitSwerveDrivetrain.this.m_kinematics; - KitSwerveDrivetrain.this.m_requestParameters.swervePositions = - KitSwerveDrivetrain.this.m_moduleLocations; - KitSwerveDrivetrain.this.m_requestParameters.currentChassisSpeed = speeds; - KitSwerveDrivetrain.this.m_requestParameters.timestamp = this.currentTime; - KitSwerveDrivetrain.this.m_requestParameters.updatePeriod = - 1.0 / KitSwerveDrivetrain.this.UpdateFrequency; - KitSwerveDrivetrain.this.m_requestParameters.operatorForwardDirection = - KitSwerveDrivetrain.this.m_operatorForwardDirection; - KitSwerveDrivetrain.this.m_requestToApply.apply( - KitSwerveDrivetrain.this.m_requestParameters, KitSwerveDrivetrain.this.Modules); - KitSwerveDrivetrain.this.m_cachedState.FailedDaqs = this.FailedDaqs; - KitSwerveDrivetrain.this.m_cachedState.SuccessfulDaqs = this.SuccessfulDaqs; - KitSwerveDrivetrain.this.m_cachedState.Pose = - KitSwerveDrivetrain.this.m_odometry.getEstimatedPosition(); - KitSwerveDrivetrain.this.m_cachedState.speeds = speeds; - KitSwerveDrivetrain.this.m_cachedState.OdometryPeriod = this.averageLoopTime; - if (KitSwerveDrivetrain.this.m_cachedState.ModuleStates == null) { - KitSwerveDrivetrain.this.m_cachedState.ModuleStates = - new SwerveModuleState[KitSwerveDrivetrain.this.Modules.length]; - } - - if (KitSwerveDrivetrain.this.m_cachedState.ModuleTargets == null) { - KitSwerveDrivetrain.this.m_cachedState.ModuleTargets = - new SwerveModuleState[KitSwerveDrivetrain.this.Modules.length]; - } - - for (int ix = 0; ix < KitSwerveDrivetrain.this.Modules.length; ++ix) { - KitSwerveDrivetrain.this.m_cachedState.ModuleStates[ix] = - KitSwerveDrivetrain.this.Modules[ix].getCurrentState(); - KitSwerveDrivetrain.this.m_cachedState.ModuleTargets[ix] = - KitSwerveDrivetrain.this.Modules[ix].getTargetState(); - } - - if (KitSwerveDrivetrain.this.m_telemetryFunction != null) { - KitSwerveDrivetrain.this.m_telemetryFunction.accept( - KitSwerveDrivetrain.this.m_cachedState); - } - } finally { - KitSwerveDrivetrain.this.m_stateLock.writeLock().unlock(); - } - - if (this.threadPriorityToSet != this.lastThreadPriority) { - Threads.setCurrentThreadPriority(true, this.threadPriorityToSet); - this.lastThreadPriority = this.threadPriorityToSet; - } - } - } - - public boolean odometryIsValid() { - return this.SuccessfulDaqs > 2; - } - - public void setThreadPriority(int priority) { - this.threadPriorityToSet = priority; - } - } - - public static class SwerveDriveState { - public int SuccessfulDaqs; - public int FailedDaqs; - public Pose2d Pose; - public ChassisSpeeds speeds; - public SwerveModuleState[] ModuleStates; - public SwerveModuleState[] ModuleTargets; - public double OdometryPeriod; - - public SwerveDriveState() {} - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java b/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java deleted file mode 100644 index 09ae671f..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/KitSwerveRequest.java +++ /dev/null @@ -1,523 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import org.littletonrobotics.junction.Logger; - -// -// Source code recreated from a .class file by IntelliJ IDEA -// (powered by FernFlower decompiler) -// - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.Voltage; - -public interface KitSwerveRequest { - StatusCode apply(SwerveControlRequestParameters var1, ModuleIO... var2); - - public static class SysIdSwerveSteerGains implements KitSwerveRequest { - public final MutableMeasure VoltsToApply; - private VoltageOut m_voltRequest; - - public SysIdSwerveSteerGains() { - this.VoltsToApply = MutableMeasure.mutable(Units.Volts.of(0.0)); - this.m_voltRequest = new VoltageOut(0.0); - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i] - .getSteerMotor() - .setControl(this.m_voltRequest.withOutput(this.VoltsToApply.in(Units.Volts))); - modulesToApply[i].getDriveMotor().setControl(this.m_voltRequest.withOutput(0.0)); - } - - return StatusCode.OK; - } - - public SysIdSwerveSteerGains withVolts(Measure Volts) { - this.VoltsToApply.mut_replace(Volts); - return this; - } - } - - public static class SysIdSwerveRotation implements KitSwerveRequest { - public final MutableMeasure VoltsToApply; - private VoltageOut m_voltRequest; - - public SysIdSwerveRotation() { - this.VoltsToApply = MutableMeasure.mutable(Units.Volts.of(0.0)); - this.m_voltRequest = new VoltageOut(0.0); - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].applyCharacterization( - parameters.swervePositions[i].getAngle().plus(Rotation2d.fromDegrees(90.0)), - this.m_voltRequest.withOutput(this.VoltsToApply.in(Units.Volts))); - } - - return StatusCode.OK; - } - - public SysIdSwerveRotation withVolts(Measure Volts) { - this.VoltsToApply.mut_replace(Volts); - return this; - } - } - - public static class SysIdSwerveTranslation implements KitSwerveRequest { - public final MutableMeasure VoltsToApply; - private VoltageOut m_voltRequest; - - public SysIdSwerveTranslation() { - this.VoltsToApply = MutableMeasure.mutable(Units.Volts.of(0.0)); - this.m_voltRequest = new VoltageOut(0.0); - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].applyCharacterization( - Rotation2d.fromDegrees(0.0), - this.m_voltRequest.withOutput(this.VoltsToApply.in(Units.Volts))); - } - - return StatusCode.OK; - } - - public SysIdSwerveTranslation withVolts(Measure Volts) { - this.VoltsToApply.mut_replace(Volts); - return this; - } - } - - public static class ApplyChassisSpeeds implements KitSwerveRequest { - public ChassisSpeeds Speeds = new ChassisSpeeds(); - public Translation2d CenterOfRotation = new Translation2d(0.0, 0.0); - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - - public ApplyChassisSpeeds() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - SwerveModuleState[] states = - parameters.kinematics.toSwerveModuleStates(this.Speeds, this.CenterOfRotation); - - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].apply(states[i], this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public ApplyChassisSpeeds withSpeeds(ChassisSpeeds speeds) { - this.Speeds = speeds; - return this; - } - - public ApplyChassisSpeeds withCenterOfRotation(Translation2d centerOfRotation) { - this.CenterOfRotation = centerOfRotation; - return this; - } - - public ApplyChassisSpeeds withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public ApplyChassisSpeeds withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class RobotCentric implements KitSwerveRequest { - public double VelocityX = 0.0; - public double VelocityY = 0.0; - public double RotationalRate = 0.0; - public double Deadband = 0.0; - public double RotationalDeadband = 0.0; - public Translation2d CenterOfRotation = new Translation2d(); - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - - public RobotCentric() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - double toApplyX = this.VelocityX; - double toApplyY = this.VelocityY; - double toApplyOmega = this.RotationalRate; - if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < this.Deadband) { - toApplyX = 0.0; - toApplyY = 0.0; - } - - if (Math.abs(toApplyOmega) < this.RotationalDeadband) { - toApplyOmega = 0.0; - } - - ChassisSpeeds speeds = new ChassisSpeeds(toApplyX, toApplyY, toApplyOmega); - SwerveModuleState[] states = - parameters.kinematics.toSwerveModuleStates(speeds, this.CenterOfRotation); - - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].apply(states[i], this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public RobotCentric withVelocityX(double velocityX) { - this.VelocityX = velocityX; - return this; - } - - public RobotCentric withVelocityY(double velocityY) { - this.VelocityY = velocityY; - return this; - } - - public RobotCentric withRotationalRate(double rotationalRate) { - this.RotationalRate = rotationalRate; - return this; - } - - public RobotCentric withDeadband(double deadband) { - this.Deadband = deadband; - return this; - } - - public RobotCentric withRotationalDeadband(double rotationalDeadband) { - this.RotationalDeadband = rotationalDeadband; - return this; - } - - public RobotCentric withCenterOfRotation(Translation2d centerOfRotation) { - this.CenterOfRotation = centerOfRotation; - return this; - } - - public RobotCentric withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public RobotCentric withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class PointWheelsAt implements KitSwerveRequest { - public Rotation2d ModuleDirection = new Rotation2d(); - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - - public PointWheelsAt() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - SwerveModuleState state = new SwerveModuleState(0.0, this.ModuleDirection); - modulesToApply[i].apply(state, this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public PointWheelsAt withModuleDirection(Rotation2d moduleDirection) { - this.ModuleDirection = moduleDirection; - return this; - } - - public PointWheelsAt withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public PointWheelsAt withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class Idle implements KitSwerveRequest { - public Idle() {} - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - return StatusCode.OK; - } - } - - public static class FieldCentricFacingAngle implements KitSwerveRequest { - public double VelocityX = 0.0; - public double VelocityY = 0.0; - public Rotation2d TargetDirection = new Rotation2d(); - public double Deadband = 0.0; - public double RotationalDeadband = 0.0; - public Translation2d CenterOfRotation = new Translation2d(); - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - public PhoenixPIDController HeadingController; - public ForwardReference ForwardReference; - - public FieldCentricFacingAngle() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - this.HeadingController = new PhoenixPIDController(0.0, 0.0, 0.0); - this.ForwardReference = KitSwerveRequest.ForwardReference.OperatorPerspective; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - double toApplyX = this.VelocityX; - double toApplyY = this.VelocityY; - Rotation2d angleToFace = this.TargetDirection; - if (this.ForwardReference == KitSwerveRequest.ForwardReference.OperatorPerspective) { - Translation2d tmp = new Translation2d(toApplyX, toApplyY); - tmp = tmp.rotateBy(parameters.operatorForwardDirection); - toApplyX = tmp.getX(); - toApplyY = tmp.getY(); - angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection); - } - - double rotationRate = - this.HeadingController.calculate( - parameters.currentPose.getRotation().getRadians(), - angleToFace.getRadians(), - parameters.timestamp); - double toApplyOmega = rotationRate; - if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < this.Deadband) { - toApplyX = 0.0; - toApplyY = 0.0; - } - - if (Math.abs(toApplyOmega) < this.RotationalDeadband) { - toApplyOmega = 0.0; - } - - ChassisSpeeds speeds = - ChassisSpeeds.discretize( - ChassisSpeeds.fromFieldRelativeSpeeds( - toApplyX, toApplyY, toApplyOmega, parameters.currentPose.getRotation()), - parameters.updatePeriod); - SwerveModuleState[] states = - parameters.kinematics.toSwerveModuleStates(speeds, this.CenterOfRotation); - - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].apply(states[i], this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public FieldCentricFacingAngle withVelocityX(double velocityX) { - this.VelocityX = velocityX; - return this; - } - - public FieldCentricFacingAngle withVelocityY(double velocityY) { - this.VelocityY = velocityY; - return this; - } - - public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) { - this.TargetDirection = targetDirection; - return this; - } - - public FieldCentricFacingAngle withDeadband(double deadband) { - this.Deadband = deadband; - return this; - } - - public FieldCentricFacingAngle withRotationalDeadband(double rotationalDeadband) { - this.RotationalDeadband = rotationalDeadband; - return this; - } - - public FieldCentricFacingAngle withCenterOfRotation(Translation2d centerOfRotation) { - this.CenterOfRotation = centerOfRotation; - return this; - } - - public FieldCentricFacingAngle withDriveRequestType( - SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public FieldCentricFacingAngle withSteerRequestType( - SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class FieldCentric implements KitSwerveRequest { - public double VelocityX = 0.0; - public double VelocityY = 0.0; - public double RotationalRate = 0.0; - public double Deadband = 0.0; - public double RotationalDeadband = 0.0; - public Translation2d CenterOfRotation = new Translation2d(); - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - public ForwardReference ForwardReference; - protected SwerveModuleState[] m_lastAppliedState; - - public FieldCentric() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - this.ForwardReference = KitSwerveRequest.ForwardReference.OperatorPerspective; - this.m_lastAppliedState = null; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - double toApplyX = this.VelocityX; - double toApplyY = this.VelocityY; - if (this.ForwardReference == KitSwerveRequest.ForwardReference.OperatorPerspective) { - Translation2d tmp = new Translation2d(toApplyX, toApplyY); - tmp = tmp.rotateBy(parameters.operatorForwardDirection); - toApplyX = tmp.getX(); - toApplyY = tmp.getY(); - } - - double toApplyOmega = this.RotationalRate; - if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < this.Deadband) { - toApplyX = 0.0; - toApplyY = 0.0; - } - - if (Math.abs(toApplyOmega) < this.RotationalDeadband) { - toApplyOmega = 0.0; - } - - ChassisSpeeds speeds = - ChassisSpeeds.discretize( - ChassisSpeeds.fromFieldRelativeSpeeds( - toApplyX, toApplyY, toApplyOmega, parameters.currentPose.getRotation()), - parameters.updatePeriod); - SwerveModuleState[] states = - parameters.kinematics.toSwerveModuleStates(speeds, this.CenterOfRotation); - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].apply(states[i], this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public FieldCentric withVelocityX(double velocityX) { - this.VelocityX = velocityX; - return this; - } - - public FieldCentric withVelocityY(double velocityY) { - this.VelocityY = velocityY; - return this; - } - - public FieldCentric withRotationalRate(double rotationalRate) { - this.RotationalRate = rotationalRate; - return this; - } - - public FieldCentric withDeadband(double deadband) { - this.Deadband = deadband; - return this; - } - - public FieldCentric withRotationalDeadband(double rotationalDeadband) { - this.RotationalDeadband = rotationalDeadband; - return this; - } - - public FieldCentric withCenterOfRotation(Translation2d centerOfRotation) { - this.CenterOfRotation = centerOfRotation; - return this; - } - - public FieldCentric withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public FieldCentric withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class SwerveDriveBrake implements KitSwerveRequest { - public SwerveModule.DriveRequestType DriveRequestType; - public SwerveModule.SteerRequestType SteerRequestType; - - public SwerveDriveBrake() { - this.DriveRequestType = DriveRequestType.OpenLoopVoltage; - this.SteerRequestType = SteerRequestType.MotionMagic; - } - - public StatusCode apply(SwerveControlRequestParameters parameters, ModuleIO... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - SwerveModuleState state = - new SwerveModuleState(0.0, parameters.swervePositions[i].getAngle()); - modulesToApply[i].apply(state, this.DriveRequestType, this.SteerRequestType); - } - - return StatusCode.OK; - } - - public SwerveDriveBrake withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) { - this.DriveRequestType = driveRequestType; - return this; - } - - public SwerveDriveBrake withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) { - this.SteerRequestType = steerRequestType; - return this; - } - } - - public static class SwerveControlRequestParameters { - public SwerveDriveKinematics kinematics; - public ChassisSpeeds currentChassisSpeed; - public Pose2d currentPose; - public double timestamp; - public Translation2d[] swervePositions; - public Rotation2d operatorForwardDirection; - public double updatePeriod; - - public SwerveControlRequestParameters() {} - } - - public static enum ForwardReference { - RedAlliance, - OperatorPerspective; - - private ForwardReference() {} - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIO.java deleted file mode 100644 index 14c57843..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIO.java +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import org.littletonrobotics.junction.AutoLog; - -public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { - public double driveMotorVoltage = 0.0; - public double driveMotorPosition = 0.0; - public double driveMotorVelocity = 0.0; - public double driveMotorStatorCurrent = 0.0; - public double driveMotorSupplyCurrent = 0.0; - public double driveMotorTemperature = 0.0; - - public double angleMotorVoltage = 0.0; - public double angleMotorPosition = 0.0; - public double angleMotorVelocity = 0.0; - public double angleMotorStatorCurrent = 0.0; - public double angleMotorSupplyCurrent = 0.0; - public double angleMotorTemperature = 0.0; - - public SwerveModuleState currentState = new SwerveModuleState(); - public SwerveModulePosition currentPosition = new SwerveModulePosition(); - } - - public default void updateInputs(ModuleIOInputs inputs) {} - - public SwerveModulePosition getPosition(boolean refresh); - - public void apply( - SwerveModuleState state, - SwerveModule.DriveRequestType driveRequestType, - SwerveModule.SteerRequestType steerRequestType); - - public void applyCharacterization(Rotation2d steerTarget, VoltageOut driveRequest); - - public void applyCharacterization(Rotation2d steerTarget, TorqueCurrentFOC driveRequest); - - public StatusCode configNeutralMode(NeutralModeValue neutralMode); - - public SwerveModulePosition getCachedPosition(); - - public SwerveModuleState getCurrentState(); - - public SwerveModuleState getTargetState(); - - public BaseStatusSignal[] getSignals(); - - public void resetPosition(); - - public TalonFX getDriveMotor(); - - public TalonFX getSteerMotor(); - - public CANcoder getCANcoder(); - - public static enum DriveRequestType { - OpenLoopVoltage, - Velocity; - - private DriveRequestType() {} - } - - public static enum SteerRequestType { - MotionMagic, - MotionMagicExpo; - - private SteerRequestType() {} - } - - public static enum ClosedLoopOutputType { - Voltage, - TorqueCurrentFOC; - - private ClosedLoopOutputType() {} - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIOTalonFX.java deleted file mode 100644 index e0437532..00000000 --- a/src/main/java/frc/robot/subsystems/swerve/kit/ModuleIOTalonFX.java +++ /dev/null @@ -1,393 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.subsystems.swerve.kit; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.controls.*; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import java.io.PrintStream; - -public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX m_driveMotor; - private final TalonFX m_steerMotor; - private final CANcoder m_cancoder; - private final StatusSignal m_driveVoltage; - private final StatusSignal m_drivePosition; - private final StatusSignal m_driveVelocity; - private final StatusSignal m_driveStatorCurrent; - private final StatusSignal m_driveSupplyCurrent; - private final StatusSignal m_driveTemperature; - - private final StatusSignal m_steerVoltage; - private final StatusSignal m_steerPosition; - private final StatusSignal m_steerVelocity; - private final StatusSignal m_steerStatorCurrent; - private final StatusSignal m_steerSupplyCurrent; - private final StatusSignal m_steerTemperature; - private final BaseStatusSignal[] m_signals; - private final double m_driveRotationsPerMeter; - private final double m_couplingRatioDriveRotorToCANcoder; - private final double m_speedAt12VoltsMps; - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0.0); - private final MotionMagicTorqueCurrentFOC m_angleTorqueSetter = - new MotionMagicTorqueCurrentFOC(0.0); - private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0.0); - private final MotionMagicExpoTorqueCurrentFOC m_angleTorqueExpoSetter = - new MotionMagicExpoTorqueCurrentFOC(0.0); - private final VoltageOut m_voltageOpenLoopSetter = new VoltageOut(0.0); - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0.0); - private final VelocityTorqueCurrentFOC m_velocityTorqueSetter = - (new VelocityTorqueCurrentFOC(0.0)).withOverrideCoastDurNeutral(true); - private final SwerveModule.ClosedLoopOutputType m_steerClosedLoopOutput; - private final SwerveModule.ClosedLoopOutputType m_driveClosedLoopOutput; - private final SwerveModulePosition m_internalState = new SwerveModulePosition(); - private SwerveModuleState m_targetState = new SwerveModuleState(); - - public ModuleIOTalonFX(SwerveModuleConstants constants, String canbusName) { - this.m_driveMotor = new TalonFX(constants.DriveMotorId, canbusName); - this.m_steerMotor = new TalonFX(constants.SteerMotorId, canbusName); - this.m_cancoder = new CANcoder(constants.CANcoderId, canbusName); - TalonFXConfiguration talonConfigs = new TalonFXConfiguration(); - talonConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; - talonConfigs.Slot0 = constants.DriveMotorGains; - talonConfigs.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; - talonConfigs.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; - talonConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; - talonConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - talonConfigs.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - StatusCode response = this.m_driveMotor.getConfigurator().apply(talonConfigs); - PrintStream var10000; - int var10001; - if (!response.isOK()) { - var10000 = System.out; - var10001 = this.m_driveMotor.getDeviceID(); - var10000.println( - "TalonFX ID " + var10001 + " failed config with error " + response.toString()); - } - - talonConfigs.TorqueCurrent = new TorqueCurrentConfigs(); - talonConfigs.CurrentLimits = new CurrentLimitsConfigs(); - talonConfigs.Slot0 = constants.SteerMotorGains; - talonConfigs.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; - switch (constants.FeedbackSource) { - case RemoteCANcoder: - talonConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - break; - case FusedCANcoder: - talonConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - break; - case SyncCANcoder: - talonConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder; - } - - talonConfigs.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - talonConfigs.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - talonConfigs.MotionMagic.MotionMagicAcceleration = - talonConfigs.MotionMagic.MotionMagicCruiseVelocity / 0.1; - talonConfigs.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - talonConfigs.MotionMagic.MotionMagicExpo_kA = 0.1; - talonConfigs.ClosedLoopGeneral.ContinuousWrap = true; - talonConfigs.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - response = this.m_steerMotor.getConfigurator().apply(talonConfigs); - if (!response.isOK()) { - var10000 = System.out; - var10001 = this.m_steerMotor.getDeviceID(); - var10000.println( - "TalonFX ID " + var10001 + " failed config with error " + response.toString()); - } - - CANcoderConfiguration cancoderConfigs = new CANcoderConfiguration(); - cancoderConfigs.MagnetSensor.MagnetOffset = constants.CANcoderOffset; - response = this.m_cancoder.getConfigurator().apply(cancoderConfigs); - if (!response.isOK()) { - var10000 = System.out; - var10001 = this.m_cancoder.getDeviceID(); - var10000.println( - "CANcoder ID " + var10001 + " failed config with error " + response.toString()); - } - - this.m_driveVoltage = this.m_driveMotor.getMotorVoltage().clone(); - this.m_drivePosition = this.m_driveMotor.getPosition().clone(); - this.m_driveVelocity = this.m_driveMotor.getVelocity().clone(); - this.m_driveStatorCurrent = this.m_driveMotor.getStatorCurrent().clone(); - this.m_driveSupplyCurrent = this.m_driveMotor.getSupplyCurrent().clone(); - this.m_driveTemperature = this.m_driveMotor.getDeviceTemp().clone(); - - this.m_steerVoltage = this.m_steerMotor.getMotorVoltage().clone(); - this.m_steerPosition = this.m_steerMotor.getPosition().clone(); - this.m_steerVelocity = this.m_steerMotor.getVelocity().clone(); - this.m_steerStatorCurrent = this.m_steerMotor.getStatorCurrent().clone(); - this.m_steerSupplyCurrent = this.m_steerMotor.getSupplyCurrent().clone(); - this.m_steerTemperature = this.m_steerMotor.getDeviceTemp().clone(); - this.m_signals = new BaseStatusSignal[4]; - this.m_signals[0] = this.m_drivePosition; - this.m_signals[1] = this.m_driveVelocity; - this.m_signals[2] = this.m_steerPosition; - this.m_signals[3] = this.m_steerVelocity; - double rotationsPerWheelRotation = constants.DriveMotorGearRatio; - double metersPerWheelRotation = 6.283185307179586 * Units.inchesToMeters(constants.WheelRadius); - this.m_driveRotationsPerMeter = rotationsPerWheelRotation / metersPerWheelRotation; - this.m_couplingRatioDriveRotorToCANcoder = constants.CouplingGearRatio; - this.m_angleVoltageSetter.UpdateFreqHz = 0.0; - this.m_angleTorqueSetter.UpdateFreqHz = 0.0; - this.m_angleVoltageExpoSetter.UpdateFreqHz = 0.0; - this.m_angleTorqueExpoSetter.UpdateFreqHz = 0.0; - this.m_velocityTorqueSetter.UpdateFreqHz = 0.0; - this.m_velocityVoltageSetter.UpdateFreqHz = 0.0; - this.m_voltageOpenLoopSetter.UpdateFreqHz = 0.0; - this.m_steerClosedLoopOutput = constants.SteerMotorClosedLoopOutput; - this.m_driveClosedLoopOutput = constants.DriveMotorClosedLoopOutput; - this.m_speedAt12VoltsMps = constants.SpeedAt12VoltsMps; - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - m_driveVoltage, - m_drivePosition, - m_driveVelocity, - m_driveStatorCurrent, - m_driveSupplyCurrent, - m_driveTemperature, - m_steerVoltage, - m_steerPosition, - m_steerVelocity, - m_steerStatorCurrent, - m_steerSupplyCurrent, - m_steerTemperature); - inputs.driveMotorVoltage = m_driveVoltage.getValueAsDouble(); - inputs.driveMotorPosition = m_drivePosition.getValueAsDouble(); - inputs.driveMotorVelocity = m_driveVelocity.getValueAsDouble(); - inputs.driveMotorStatorCurrent = m_driveStatorCurrent.getValueAsDouble(); - inputs.driveMotorSupplyCurrent = m_driveSupplyCurrent.getValueAsDouble(); - inputs.driveMotorTemperature = m_driveTemperature.getValueAsDouble(); - - inputs.angleMotorVoltage = m_steerVoltage.getValueAsDouble(); - inputs.angleMotorPosition = m_steerPosition.getValueAsDouble(); - inputs.angleMotorVelocity = m_steerVelocity.getValueAsDouble(); - inputs.angleMotorStatorCurrent = m_steerStatorCurrent.getValueAsDouble(); - inputs.angleMotorSupplyCurrent = m_steerSupplyCurrent.getValueAsDouble(); - inputs.angleMotorTemperature = m_steerTemperature.getValueAsDouble(); - - inputs.currentState = getCurrentState(); - inputs.currentPosition = getPosition(false); - } - - @Override - public SwerveModulePosition getPosition(boolean refresh) { - if (refresh) { - this.m_drivePosition.refresh(); - this.m_driveVelocity.refresh(); - this.m_steerPosition.refresh(); - this.m_steerVelocity.refresh(); - } - - double drive_rot = - BaseStatusSignal.getLatencyCompensatedValue(this.m_drivePosition, this.m_driveVelocity); - double angle_rot = - BaseStatusSignal.getLatencyCompensatedValue(this.m_steerPosition, this.m_steerVelocity); - drive_rot -= angle_rot * this.m_couplingRatioDriveRotorToCANcoder; - this.m_internalState.distanceMeters = drive_rot / this.m_driveRotationsPerMeter; - this.m_internalState.angle = Rotation2d.fromRotations(angle_rot); - return this.m_internalState; - } - - public void apply(SwerveModuleState state, SwerveModule.DriveRequestType driveRequestType) { - this.apply(state, driveRequestType, SwerveModule.SteerRequestType.MotionMagic); - } - - @Override - public void apply( - SwerveModuleState state, - SwerveModule.DriveRequestType driveRequestType, - SwerveModule.SteerRequestType steerRequestType) { - SwerveModuleState optimized; - double angleToSetDeg; - optimized = SwerveModuleState.optimize(state, this.m_internalState.angle); - this.m_targetState = optimized; - angleToSetDeg = optimized.angle.getRotations(); - label35: - switch (steerRequestType) { - case MotionMagic: - switch (this.m_steerClosedLoopOutput) { - case Voltage: - this.m_steerMotor.setControl(this.m_angleVoltageSetter.withPosition(angleToSetDeg)); - break label35; - case TorqueCurrentFOC: - this.m_steerMotor.setControl(this.m_angleTorqueSetter.withPosition(angleToSetDeg)); - default: - break label35; - } - case MotionMagicExpo: - switch (this.m_steerClosedLoopOutput) { - case Voltage: - this.m_steerMotor.setControl(this.m_angleVoltageExpoSetter.withPosition(angleToSetDeg)); - break; - case TorqueCurrentFOC: - this.m_steerMotor.setControl(this.m_angleTorqueExpoSetter.withPosition(angleToSetDeg)); - } - } - - double velocityToSet = optimized.speedMetersPerSecond * this.m_driveRotationsPerMeter; - double steerMotorError = angleToSetDeg - (Double) this.m_steerPosition.getValue(); - double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError)); - if (cosineScalar < 0.0) { - cosineScalar = 0.0; - } - - velocityToSet *= cosineScalar; - double azimuthTurnRps = (Double) this.m_steerVelocity.getValue(); - double driveRateBackOut = azimuthTurnRps * this.m_couplingRatioDriveRotorToCANcoder; - velocityToSet += driveRateBackOut; - switch (driveRequestType) { - case OpenLoopVoltage: - velocityToSet /= this.m_driveRotationsPerMeter; - this.m_driveMotor.setControl( - this.m_voltageOpenLoopSetter.withOutput( - velocityToSet / this.m_speedAt12VoltsMps * 12.0)); - break; - case Velocity: - switch (this.m_driveClosedLoopOutput) { - case Voltage: - this.m_driveMotor.setControl(this.m_velocityVoltageSetter.withVelocity(velocityToSet)); - break; - case TorqueCurrentFOC: - this.m_driveMotor.setControl(this.m_velocityTorqueSetter.withVelocity(velocityToSet)); - } - } - } - - @Override - public void applyCharacterization(Rotation2d steerTarget, VoltageOut driveRequest) { - double angleToSetDeg = steerTarget.getRotations(); - switch (this.m_steerClosedLoopOutput) { - case Voltage: - this.m_steerMotor.setControl(this.m_angleVoltageSetter.withPosition(angleToSetDeg)); - break; - case TorqueCurrentFOC: - this.m_steerMotor.setControl(this.m_angleTorqueSetter.withPosition(angleToSetDeg)); - } - - this.m_driveMotor.setControl(driveRequest); - } - - @Override - public void applyCharacterization(Rotation2d steerTarget, TorqueCurrentFOC driveRequest) { - double angleToSetDeg = steerTarget.getRotations(); - switch (this.m_steerClosedLoopOutput) { - case Voltage: - this.m_steerMotor.setControl(this.m_angleVoltageSetter.withPosition(angleToSetDeg)); - break; - case TorqueCurrentFOC: - this.m_steerMotor.setControl(this.m_angleTorqueSetter.withPosition(angleToSetDeg)); - } - - this.m_driveMotor.setControl(driveRequest); - } - - @Override - public StatusCode configNeutralMode(NeutralModeValue neutralMode) { - MotorOutputConfigs configs = new MotorOutputConfigs(); - StatusCode status = this.m_driveMotor.getConfigurator().refresh(configs); - if (status.isOK()) { - configs.NeutralMode = neutralMode; - status = this.m_driveMotor.getConfigurator().apply(configs); - } - - if (!status.isOK()) { - PrintStream var10000 = System.out; - int var10001 = this.m_driveMotor.getDeviceID(); - var10000.println( - "TalonFX ID " + var10001 + " failed config neutral mode with error " + status.toString()); - } - - return status; - } - - @Override - public SwerveModulePosition getCachedPosition() { - return this.m_internalState; - } - - @Override - public SwerveModuleState getCurrentState() { - return new SwerveModuleState( - (Double) this.m_driveVelocity.getValue() / this.m_driveRotationsPerMeter, - Rotation2d.fromRotations((Double) this.m_steerPosition.getValue())); - } - - @Override - public SwerveModuleState getTargetState() { - return this.m_targetState; - } - - @Override - public BaseStatusSignal[] getSignals() { - return this.m_signals; - } - - @Override - public void resetPosition() { - this.m_driveMotor.setPosition(0.0); - } - - @Override - public TalonFX getDriveMotor() { - return this.m_driveMotor; - } - - @Override - public TalonFX getSteerMotor() { - return this.m_steerMotor; - } - - @Override - public CANcoder getCANcoder() { - return this.m_cancoder; - } - - public static enum DriveRequestType { - OpenLoopVoltage, - Velocity; - - private DriveRequestType() {} - } - - public static enum SteerRequestType { - MotionMagic, - MotionMagicExpo; - - private SteerRequestType() {} - } - - public static enum ClosedLoopOutputType { - Voltage, - TorqueCurrentFOC; - - private ClosedLoopOutputType() {} - } -} diff --git a/src/main/java/frc/robot/subsystems/swerve/requests/SwerveFieldCentricFacingAngle.java b/src/main/java/frc/robot/subsystems/swerve/requests/SwerveFieldCentricFacingAngle.java index ad16a06c..9cb163d4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/requests/SwerveFieldCentricFacingAngle.java +++ b/src/main/java/frc/robot/subsystems/swerve/requests/SwerveFieldCentricFacingAngle.java @@ -8,13 +8,13 @@ package frc.robot.subsystems.swerve.requests; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.subsystems.swerve.kit.KitSwerveRequest; -public class SwerveFieldCentricFacingAngle extends KitSwerveRequest.FieldCentricFacingAngle - implements KitSwerveRequest { +public class SwerveFieldCentricFacingAngle extends SwerveRequest.FieldCentricFacingAngle + implements SwerveRequest { /** * Sets the velocity in the X direction, in m/s. X is defined as forward according to WPILib From 557697670f0419151bde011ed4842e0a98391db6 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 20:29:11 -0700 Subject: [PATCH 3/6] =?UTF-8?q?=F0=9F=8E=A8=20Make=20autos=20cleaner?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robot/autos/routines/AutoRoutines.java | 262 +++++++----------- 1 file changed, 95 insertions(+), 167 deletions(-) diff --git a/src/main/java/frc/robot/autos/routines/AutoRoutines.java b/src/main/java/frc/robot/autos/routines/AutoRoutines.java index df83c386..4ff26d63 100644 --- a/src/main/java/frc/robot/autos/routines/AutoRoutines.java +++ b/src/main/java/frc/robot/autos/routines/AutoRoutines.java @@ -72,178 +72,106 @@ public static Command center5Note( new Trigger(() -> !intake.isBeamBroken()).debounce(RoutineConstants.beamBreakDelay); return Commands.sequence( - Commands.runOnce( - () -> - swerve.seedFieldRelative( - DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) - == DriverStation.Alliance.Blue - ? center_w1.getInitialPose() - : center_w1.getFlippedInitialPose())), - intake - .intakeIn() - .deadlineWith( - swerve.runChoreoTraj(center_w1), - pivotIntake.setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing)), - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(w1_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken), - intake - .intakeIn() - .deadlineWith( - pivotShooter.setPosition(0), - swerve.runChoreoTraj(center_w2), - pivotIntake.setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing)), - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(w2_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken), - intake - .intakeIn() - .deadlineWith( - pivotShooter.setPosition(0), - swerve.runChoreoTraj(center_w3), - pivotIntake.setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing)), - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(w3_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken), - swerve - .runChoreoTraj(center_c1) - .andThen(Commands.waitSeconds(1)) - .deadlineWith( - pivotShooter.setPosition(0), - intake.intakeIn(), - pivotIntake.setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing)), + AutoHelperCommands.resetPose(center_w1, swerve), + AutoHelperCommands.preLoad(pivotShooter, intake, shooter, noteOuttaken), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, center_w1), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, w1_center, noteOuttaken), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, center_w2) + .deadlineWith(pivotShooter.setPosition(0)), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, w2_center, noteOuttaken), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, center_w3) + .deadlineWith(pivotShooter.setPosition(0)), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, w3_center, noteOuttaken), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, center_c1) + .deadlineWith(pivotShooter.setPosition(0)), Commands.either( - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(c1_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, c1_center, noteOuttaken), Commands.sequence( - swerve - .runChoreoTraj(c1_c2) - .deadlineWith( - pivotShooter.setPosition(0), - pivotIntake - .setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing) - .alongWith(intake.intakeIn())), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, c1_c2), Commands.either( - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(c2_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage( - IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, c2_center, noteOuttaken), Commands.sequence( - swerve - .runChoreoTraj(c2_c3) - .deadlineWith( - pivotShooter.setPosition(0), - pivotIntake - .setPosition( - PivotIntakeConstants.kPivotGroundPos - * PivotIntakeConstants.kPivotMotorGearing) - .alongWith(intake.intakeIn())), - Commands.parallel( - Commands.sequence( - swerve.runChoreoTraj(c3_center), - Commands.waitUntil( - () -> - Util.epsilonEquals( - pivotShooter.getPosition(), - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing, - 5)), - intake.setPassthroughVoltage( - IntakeConstants.kPassthroughIntakeVoltage)), - shooter.setVelocity( - ShooterConstants.kShooterSubwooferRPS, - ShooterConstants.kShooterFollowerSubwooferRPS), - pivotShooter.setPosition( - PivotShooterConstants.kSubWooferPreset - * PivotShooterConstants.kPivotMotorGearing)) - .until(noteOuttaken)), + AutoHelperCommands.intakeIn(intake, swerve, pivotIntake, c2_c3), + AutoHelperCommands.shootSubwoofer( + intake, shooter, swerve, pivotShooter, c3_center, noteOuttaken)), intake::isBeamBroken)), intake::isBeamBroken)); } + + private static class AutoHelperCommands { + public static Command preLoad( + PivotShooter pivotShooter, Intake intake, Shooter shooter, Trigger noteOuttaken) { + return Commands.parallel( + Commands.waitUntil( + () -> + Util.epsilonEquals( + pivotShooter.getPosition(), + PivotShooterConstants.kSubWooferPreset + * PivotShooterConstants.kPivotMotorGearing, + 5)) + .andThen(intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), + shooter.setVelocity( + ShooterConstants.kShooterSubwooferRPS, + ShooterConstants.kShooterFollowerSubwooferRPS), + pivotShooter.setPosition( + PivotShooterConstants.kSubWooferPreset + * PivotShooterConstants.kPivotMotorGearing)) + .until(noteOuttaken); + } + + public static Command resetPose(ChoreoTrajectory trajectory, CommandSwerveDrivetrain swerve) { + return Commands.runOnce( + () -> + swerve.seedFieldRelative( + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + == DriverStation.Alliance.Blue + ? trajectory.getInitialPose() + : trajectory.getFlippedInitialPose())); + } + + public static Command intakeIn( + Intake intake, + CommandSwerveDrivetrain swerve, + PivotIntake pivotIntake, + ChoreoTrajectory traj) { + return intake + .intakeIn() + .deadlineWith( + swerve.runChoreoTraj(traj), + pivotIntake.setPosition( + PivotIntakeConstants.kPivotGroundPos * PivotIntakeConstants.kPivotMotorGearing)); + } + + public static Command shootSubwoofer( + Intake intake, + Shooter shooter, + CommandSwerveDrivetrain swerve, + PivotShooter pivotShooter, + ChoreoTrajectory traj, + Trigger noteOuttaken) { + return Commands.parallel( + Commands.sequence( + swerve.runChoreoTraj(traj), + Commands.waitUntil( + () -> + Util.epsilonEquals( + pivotShooter.getPosition(), + PivotShooterConstants.kSubWooferPreset + * PivotShooterConstants.kPivotMotorGearing, + 5)), + intake.setPassthroughVoltage(IntakeConstants.kPassthroughIntakeVoltage)), + shooter.setVelocity( + ShooterConstants.kShooterSubwooferRPS, + ShooterConstants.kShooterFollowerSubwooferRPS), + pivotShooter.setPosition( + PivotShooterConstants.kSubWooferPreset + * PivotShooterConstants.kPivotMotorGearing)) + .until(noteOuttaken); + } + } } From 7585f51c0c1ebc5ec833e30687455432b7e12f32 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 20:34:06 -0700 Subject: [PATCH 4/6] =?UTF-8?q?=F0=9F=94=A5=20Add=20back=20telemetry=20and?= =?UTF-8?q?=20remove=20debug=20logs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50f7d520..a58007b6 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -413,17 +413,14 @@ public void configureSwerve() { // default command drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain - .applyRequest( - () -> - drive - .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY( - -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-driver.getRightX() * MaxAngularRate)) - .withName("monkey") - .andThen(new PrintCommand("command ended"))); + drivetrain.applyRequest( + () -> + drive + .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-driver.getRightX() * MaxAngularRate))); /* Right stick absolute angle mode on trigger hold, robot adjusts heading to the angle right joystick creates */ @@ -490,6 +487,7 @@ public void configureSwerve() { if (Utils.isSimulation()) { drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } + drivetrain.registerTelemetry(swerveTelemetry::telemeterize); // drivetrain.registerTelemetry(logger::telemeterize); // drivetrain.registerTelemetry(swerveTelemetry::telemeterize); } From d4f8e8a3602ca9223ff84c369f41554bb022da63 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 20:50:36 -0700 Subject: [PATCH 5/6] =?UTF-8?q?=F0=9F=94=A5=20Cleanup?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 20 --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 144 +----------------- .../java/frc/robot/TrainingDataPoint.java | 27 ---- .../java/frc/robot/autos/AutoConstants.java | 15 -- .../frc/robot/commands/EndCurrentCommand.java | 20 --- .../robot/helpers/SubsystemCurrentLimits.java | 19 --- .../java/frc/robot/helpers/TimedBoolean.java | 40 ----- .../java/frc/robot/utils/CommandQueue.java | 43 ------ .../frc/robot/utils/StaticThreadChecker.java | 28 ---- 10 files changed, 6 insertions(+), 351 deletions(-) delete mode 100644 src/main/java/frc/robot/TrainingDataPoint.java delete mode 100644 src/main/java/frc/robot/commands/EndCurrentCommand.java delete mode 100644 src/main/java/frc/robot/helpers/SubsystemCurrentLimits.java delete mode 100644 src/main/java/frc/robot/helpers/TimedBoolean.java delete mode 100644 src/main/java/frc/robot/utils/CommandQueue.java delete mode 100644 src/main/java/frc/robot/utils/StaticThreadChecker.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 071b1003..c8242caa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,8 +33,6 @@ public final class Constants { public static final class FeatureFlags { // subsystems - public static final boolean kEasterEggEnabled = false; - public static final boolean kIntakeEnabled = true; public static final boolean kShooterEnabled = true; @@ -45,7 +43,6 @@ public static final class FeatureFlags { public static final boolean kPivotIntakeEnabled = true; public static final boolean kClimbEnabled = true; - public static final boolean kLEDEnabled = true; public static final boolean kAmpBarEnabled = true; @@ -55,24 +52,7 @@ public static final class FeatureFlags { public static final boolean DebugCommandEnabled = false; public static final boolean kRobotVizEnabled = true && !Robot.isReal(); - // features - public static final boolean kAutoAlignEnabled = false; - public static final boolean kIntakeAutoScoreDistanceSensorOffset = false; - public static final boolean kShuffleboardLayoutEnabled = true; - public static final boolean kUsePrefs = true; - - public static final boolean kPitRoutineEnabled = false; - - public static final boolean kCanTestEnabled = false; public static boolean kPivotShooterEnabled = true; } - public static final class ShuffleboardConstants { - public static final String kDriverTabName = "Driver"; - public static final String kOperatorTabName = "Operator"; - public static final String kIntakeLayoutName = "Intake"; - public static final String kSwerveLayoutName = "Swerve"; - public static final String kArmLayoutName = "Arm"; - public static final String kShooterLayoutName = "Shooter"; - } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c0402101..4c84295a 100755 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -252,7 +252,6 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); // Run CAN Test // m_robotContainer.CANTest(); - m_robotContainer.runPitTestRoutine(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a58007b6..a44730a3 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import static frc.robot.subsystems.pivotshooter.PivotShooterConstants.*; import static frc.robot.subsystems.swerve.AzimuthConstants.*; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; @@ -29,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.FeatureFlags; import frc.robot.autos.commands.IntakeSequence; +import frc.robot.autos.routines.AutoRoutines; import frc.robot.helpers.XboxStalker; import frc.robot.subsystems.ampbar.AmpBar; import frc.robot.subsystems.ampbar.AmpBarIOTalonFX; @@ -38,7 +38,6 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeIOTalonFX; import frc.robot.subsystems.led.LED; -import frc.robot.subsystems.led.commands.*; import frc.robot.subsystems.pivotintake.PivotIntake; import frc.robot.subsystems.pivotintake.PivotIntakeConstants; import frc.robot.subsystems.pivotintake.PivotIntakeIOTalonFX; @@ -55,7 +54,6 @@ import frc.robot.subsystems.swerve.requests.SwerveFieldCentricFacingAngle; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIOLimelight; -import frc.robot.utils.CommandQueue; import org.littletonrobotics.junction.Logger; /** @@ -69,7 +67,6 @@ public class RobotContainer { /* Controllers */ private final CommandXboxController driver = new CommandXboxController(0); private final CommandXboxController operator = new CommandXboxController(1); - private final CommandXboxController test = new CommandXboxController(2); /* Drive Controls */ private final int translationAxis = XboxController.Axis.kLeftY.value; @@ -107,7 +104,6 @@ public class RobotContainer { public AmpBar ampbar; public PivotIntake pivotIntake; public Climb climb; - public CommandQueue commandQueue; public Vision vision; @@ -122,7 +118,6 @@ public RobotContainer() { // Cancel any previous commands running CommandScheduler.getInstance().cancelAll(); - commandQueue = new CommandQueue(); vision = new Vision(new VisionIOLimelight()); // Setup subsystems & button-bindings @@ -132,17 +127,8 @@ public RobotContainer() { configurePivotIntake(); configureIntake(); configureSwerve(); - test.leftBumper().onTrue(Commands.runOnce(SignalLogger::start)); - test.rightBumper().onTrue(Commands.runOnce(SignalLogger::stop)); - test.leftTrigger() - .onTrue( - drivetrain.applyRequest(() -> azi.withTargetDirection(Rotation2d.fromDegrees(180)))); - // test.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - // test.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - // test.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); - // test.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureClimb(); // If all the subsystems are enabled, configure "operator" autos @@ -152,9 +138,6 @@ public RobotContainer() { configureOperatorAutos(); } - if (FeatureFlags.kLEDEnabled) { - configureLED(); - } // Named commands { @@ -298,31 +281,20 @@ public RobotContainer() { pivotIntake.setPosition(kPivotGroundPos).withTimeout(0.75), pivotIntake.slamAndPID()))); } - /* Run checks */ configureCheeks(); - // operator.povLeft().onTrue(cancelCommand); - // Configure the auto - // if (FeatureFlags.kSwerveEnabled) { - // autoChooser = AutoBuilder.buildAutoChooser(); - // } else { - // autoChooser = new SendableChooser<>(); - // } autoChooser = new SendableChooser<>(); autoChooser.setDefaultOption("Do Nothing", new InstantCommand()); - // autoChooser.addOption( - // "5 Note test", - // AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); - // Autos + autoChooser.addOption( + "5 Note test", + AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); +// Autos SmartDashboard.putData("Auto Chooser", autoChooser); } private void configurePivotShooter() { pivotShooter = new PivotShooter(FeatureFlags.kPivotShooterEnabled, new PivotShooterIOTalonFX()); - // operator.b().onTrue(new bruh(pivotShooter)); - // operator.x().onTrue(new SequentialCommandGroup(new - // PivotShootSubwoofer(pivotShooter))); operator .x() .onTrue( @@ -380,11 +352,8 @@ private void configurePivotIntake() { private void configureClimb() { climb = new Climb(FeatureFlags.kClimbEnabled, new ClimbIOTalonFX()); - // zeroClimb = new ZeroClimb(climb); // NEED FOR SHUFFLEBOARD - operator.povDown().onTrue(climb.zero()); - // new Trigger(() -> operator.getRawAxis(translationAxis) < -0.5).onTrue(new - // UpClimb(climb)); + new Trigger(() -> operator.getRawAxis(translationAxis) > 0.5).onTrue(climb.retractClimber()); if (this.ampbar != null && this.pivotShooter != null) { new Trigger(() -> operator.getRawAxis(translationAxis) < -0.5) @@ -398,11 +367,6 @@ private void configureClimb() { new Trigger(() -> Math.abs(operator.getRawAxis(secondaryAxis)) > 0.5) .onTrue(new PrintCommand("u suck")); // old command waws dehook climb } - // Josh: HangSequence is broken and Rhea does not want to use it; we should - // rmove this - // later. - // new ScheduleCommand(new HangSequence(climb, operator)).schedule(); - // operator.povDownLeft().onTrue(new TestClimbFlip(climb)); } public void setAllianceCol(boolean col) { @@ -488,23 +452,10 @@ public void configureSwerve() { drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } drivetrain.registerTelemetry(swerveTelemetry::telemeterize); - // drivetrain.registerTelemetry(logger::telemeterize); - // drivetrain.registerTelemetry(swerveTelemetry::telemeterize); } private void configureShooter() { shooter = new Shooter(FeatureFlags.kShooterEnabled, new ShooterIOTalonFX()); - // new Trigger(() -> Math.abs(shooter.getShooterRps() - 100) <= 5) - // .onTrue( - // new InstantCommand( - // () -> { - // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 100); - // })) - // .onFalse( - // new InstantCommand( - // () -> { - // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); - // })); if (FeatureFlags.kAmpBarEnabled) { ampbar = new AmpBar(FeatureFlags.kAmpBarEnabled, new AmpBarIOTalonFX()); operator @@ -565,82 +516,6 @@ public void disableRumble() { operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); } - public void configureLED() { - int[][] ledList = new int[][] {new int[] {2, 3}, new int[] {1, 1}}; - - led = new LED(); - led.setDefaultCommand(new CoordinatesButItsMultiple(led, ledList, 100, 0, 0, 10)); - // led.setDefaultCommand(new SetLEDsFromBinaryString(led, LEDConstants.based, - // 100, 0, 0, 5)); - - /* - * Intake LED, flashes RED while intake is down and running, - * flashes GREEN on successful intake - */ - if (FeatureFlags.kIntakeEnabled) { - // Trigger intakeDetectedNote = new Trigger(intake::isBeamBroken); - // // intakeDetectedNote.whileTrue(new SetSuccessfulIntake(led)); - - // intakeDetectedNote - // .onTrue( - // new InstantCommand( - // () -> { - // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 50); - // driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 50); - // })) - // .onFalse( - // new InstantCommand( - // () -> { - // operator.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); - // driver.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0); - // })); - - // This boolean is true when velocity is LESS than 0. - // Trigger intakeRunning = new Trigger(intake::isMotorSpinning); - // intakeRunning.whileTrue(new SetGroundIntakeRunning(led)); - } - - /* - * Shooter LED, solid ORANGE while shooter is running, flashes ORANGE if note - * fed - */ - // if (FeatureFlags.kShooterEnabled) { - - // Trigger shooterRunning = new Trigger( - // () -> (shooter.getShooterFollowerRps() > 75 || shooter.getShooterRps() > - // 75)); - // shooterRunning.whileTrue(new SetSpeakerScore(led)); - // } - // if (FeatureFlags.kSwerveEnabled) { - // if (DriverStation.isAutonomousEnabled() || - - // FeatureFlags.kSwerveUseVisionForPoseEst) { - // Trigger swerveSpeakerAligned = new Trigger(swerveDrive::isAlignedToSpeaker); - // swerveSpeakerAligned.whileTrue(new SetRobotAligned(led)); - // } - // if (DriverStation.isTeleopEnabled()) { - // // if (DriverStation.isTeleopEnabled()) { - // // Trigger azimuthRan = - // // new Trigger( - // // () -> - // // (driver.leftBumper().getAsBoolean() || - // driver.rightBumper().getAsBoolean()) - // // || driver.x().getAsBoolean() - // // || driver.b().getAsBoolean() - // // || driver.a().getAsBoolean() - // // || driver.povUp().getAsBoolean()); - // // azimuthRan.whileTrue(new SetAzimuthRan(led)); - // // } - // } - - // if (FeatureFlags.kClimbEnabled) { - // Trigger climbRunning = - // new Trigger( - // () -> ((climb.getLeftCurrent() > 0) || (operator.povDown().getAsBoolean()))); - // } - // } - } - private void configureCheeks() { // Here, put checks for the configuration of the robot if (DriverStation.isFMSAttached()) { @@ -673,13 +548,6 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - /* Test Routines */ - - public void runPitTestRoutine() { - // Command pitRoutine = new PitRoutine(swerveDrive, climb, intake, pivotIntake, - // shooter); - // pitRoutine.schedule(); - } public void periodic(double dt) { XboxStalker.stalk(driver, operator); diff --git a/src/main/java/frc/robot/TrainingDataPoint.java b/src/main/java/frc/robot/TrainingDataPoint.java deleted file mode 100644 index e2723d92..00000000 --- a/src/main/java/frc/robot/TrainingDataPoint.java +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot; - -// A helper class to store training data points for the robot's localization algorithm. -// Each data point contains the distance traveled by the robot, and the standard -// deviation of the robot's x and y translation and angle. -public class TrainingDataPoint { - - public double distance; - public double stdDevXTranslation; - public double stdDevYTranslation; - public double stdDevAngle; - - public TrainingDataPoint( - double distance, double stdDevXTranslation, double stdDevYTranslation, double stdDevAngle) { - this.distance = distance; - this.stdDevXTranslation = stdDevXTranslation; - this.stdDevYTranslation = stdDevYTranslation; - this.stdDevAngle = stdDevAngle; - } -} diff --git a/src/main/java/frc/robot/autos/AutoConstants.java b/src/main/java/frc/robot/autos/AutoConstants.java index dbcb0a6b..555d9c7e 100644 --- a/src/main/java/frc/robot/autos/AutoConstants.java +++ b/src/main/java/frc/robot/autos/AutoConstants.java @@ -17,21 +17,6 @@ public final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final Pose2d kRedSpeakerLocation = - new Pose2d(15.26, 5.56, Rotation2d.fromDegrees(-180)); - - public static final Pose2d kBlueSpeakerLocation = - new Pose2d(1.27, 5.56, Rotation2d.fromDegrees(180)); - - public static final Pose2d kRedAmpLocation = new Pose2d(14.74, 7.48, Rotation2d.fromDegrees(90)); - - public static final Pose2d kBlueAmpLocation = new Pose2d(1.81, 7.57, Rotation2d.fromDegrees(90)); - - public static final double kSpeakerAlignmentThreshold = 0.2; // meters - - public static final double kPXController = 1; - public static final double kPYController = 1; - public static final double kPThetaController = 1; /* Constraint for the motion profilied robot angle controller */ public static final TrapezoidProfile.Constraints kThetaControllerConstraints = diff --git a/src/main/java/frc/robot/commands/EndCurrentCommand.java b/src/main/java/frc/robot/commands/EndCurrentCommand.java deleted file mode 100644 index bc5e0932..00000000 --- a/src/main/java/frc/robot/commands/EndCurrentCommand.java +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.helpers.DebugCommandBase; - -public class EndCurrentCommand extends DebugCommandBase { - Command currentCommand; - - public EndCurrentCommand(Command command) { - currentCommand = command; - currentCommand.end(true); - } -} diff --git a/src/main/java/frc/robot/helpers/SubsystemCurrentLimits.java b/src/main/java/frc/robot/helpers/SubsystemCurrentLimits.java deleted file mode 100644 index dde7f1ac..00000000 --- a/src/main/java/frc/robot/helpers/SubsystemCurrentLimits.java +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.helpers; - -public abstract interface SubsystemCurrentLimits { - public static final boolean kSetCurrentLimits = false; - public static final double kCurrentLimit = 0; - public double kCurrentThreshold = 40; - public double kCurrentThresholdTime = 0.1; - public boolean kEnableCurrentLimit = true; - - public boolean kEnableStatorLimits = false; - public double kStatorLimit = 10; -} diff --git a/src/main/java/frc/robot/helpers/TimedBoolean.java b/src/main/java/frc/robot/helpers/TimedBoolean.java deleted file mode 100644 index 3dbb65b3..00000000 --- a/src/main/java/frc/robot/helpers/TimedBoolean.java +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.helpers; - -import edu.wpi.first.wpilibj.Timer; -import java.util.function.BooleanSupplier; - -public class TimedBoolean { - private BooleanSupplier condition; - private Timer timer = new Timer(); - private double triggerThreshold; - - public TimedBoolean(BooleanSupplier condition, double triggerThreshold) { - this.condition = condition; - this.triggerThreshold = triggerThreshold; - } - - public void initialize() { - timer.stop(); - timer.reset(); - } - - public void update() { - if (condition.getAsBoolean()) { - timer.start(); - } else { - timer.stop(); - timer.reset(); - } - } - - public boolean hasBeenTrueForThreshold() { - return timer.hasElapsed(triggerThreshold) && condition.getAsBoolean(); - } -} diff --git a/src/main/java/frc/robot/utils/CommandQueue.java b/src/main/java/frc/robot/utils/CommandQueue.java deleted file mode 100644 index ca9eba6f..00000000 --- a/src/main/java/frc/robot/utils/CommandQueue.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.utils; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.HashMap; -import java.util.function.BooleanSupplier; - -public class CommandQueue extends SubsystemBase { - - private HashMap commandMap = new HashMap<>(); - - public CommandQueue() {} - - public void periodic() { - for (BooleanSupplier trigger : commandMap.keySet()) { - if (trigger.getAsBoolean()) { - commandMap.get(trigger).schedule(); - commandMap.remove(trigger); - } - } - } - - public Command addCommand(BooleanSupplier trigger, Command command) { - return new InstantCommand(() -> commandMap.put(trigger, command)); - } - - public Command addCommand(Trigger trigger, Command command) { - return addCommand(trigger::getAsBoolean, command); - } - - public Command removeCommand(BooleanSupplier trigger) { - return new InstantCommand(() -> commandMap.remove(trigger)); - } -} diff --git a/src/main/java/frc/robot/utils/StaticThreadChecker.java b/src/main/java/frc/robot/utils/StaticThreadChecker.java deleted file mode 100644 index cd04e8b5..00000000 --- a/src/main/java/frc/robot/utils/StaticThreadChecker.java +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) 2024 FRC 3256 -// https://github.com/Team3256 -// -// Use of this source code is governed by a -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.utils; - -import frc.robot.Constants.FeatureFlags; - -public class StaticThreadChecker { - // A piece of code that checks if the current thread is the main thread. - // If it is, it will print a message and stop the thread to prevent drive lag. - // If kDebugEnabled is true, it will throw a RuntimeException. - - public static void checkCurrentThread() { - if (Thread.currentThread().getId() == 1) { - String functionName = Thread.currentThread().getStackTrace()[2].getMethodName(); - System.out.println(functionName + " called from main thread; stopping to prevent drive lag!"); - if (FeatureFlags.kDebugEnabled) { - throw new RuntimeException( - functionName + " called from main thread; crashing since kDebugEnabled is true!"); - } - return; - } - } -} From 0a89c47e2c6b7d24587d45d566c27b42db1a04c4 Mon Sep 17 00:00:00 2001 From: Ryan Abraham Date: Fri, 20 Sep 2024 20:52:09 -0700 Subject: [PATCH 6/6] =?UTF-8?q?=F0=9F=8E=A8=20Spotless?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 11 ++++------- src/main/java/frc/robot/autos/AutoConstants.java | 3 --- 3 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c8242caa..a7dc0c91 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -54,5 +54,4 @@ public static final class FeatureFlags { public static boolean kPivotShooterEnabled = true; } - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a6bdeb85..5c8441a7 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.FeatureFlags; import frc.robot.autos.commands.IntakeSequence; +import frc.robot.autos.routines.AutoRoutines; import frc.robot.helpers.XboxStalker; import frc.robot.subsystems.ampbar.AmpBar; import frc.robot.subsystems.ampbar.AmpBarIOTalonFX; @@ -127,8 +128,6 @@ public RobotContainer() { configureIntake(); configureSwerve(); - - configureClimb(); // If all the subsystems are enabled, configure "operator" autos if (FeatureFlags.kIntakeEnabled @@ -137,7 +136,6 @@ public RobotContainer() { configureOperatorAutos(); } - // Named commands { // Auto named commands @@ -285,9 +283,9 @@ public RobotContainer() { autoChooser = new SendableChooser<>(); autoChooser.setDefaultOption("Do Nothing", new InstantCommand()); - autoChooser.addOption( - "5 Note test", - AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); + autoChooser.addOption( + "5 Note test", + AutoRoutines.center5Note(drivetrain, intake, shooter, pivotShooter, pivotIntake)); SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -548,7 +546,6 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - public void periodic(double dt) { XboxStalker.stalk(driver, operator); Logger.recordOutput("Note pose", vision.getNotePose(drivetrain.getState().Pose)); diff --git a/src/main/java/frc/robot/autos/AutoConstants.java b/src/main/java/frc/robot/autos/AutoConstants.java index 555d9c7e..3b9a010f 100644 --- a/src/main/java/frc/robot/autos/AutoConstants.java +++ b/src/main/java/frc/robot/autos/AutoConstants.java @@ -7,8 +7,6 @@ package frc.robot.autos; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutoConstants { @@ -17,7 +15,6 @@ public final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - /* Constraint for the motion profilied robot angle controller */ public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(