diff --git a/build.gradle b/build.gradle index 87b72cd3..d52c20ee 100644 --- a/build.gradle +++ b/build.gradle @@ -161,9 +161,6 @@ tasks.register('eventDeploy') { commandLine 'git', 'commit', '-m', commitMessage } - //'git add -A'.execute() - // commandLine 'git', 'add', '-A' - println "Deployed to branch: '$branch'" println "Commit message: '$commitMessage'" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9224b514..a057877c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.RAINBOWT; public static final boolean tuningMode = true; public static final boolean characterizationMode = false; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5da33283..a6c9adc1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,11 +20,10 @@ import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.VirtualSubsystem; import java.util.HashMap; import java.util.Map; import java.util.function.BiConsumer; - -import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -147,10 +146,10 @@ public void robotPeriodic() { if (!autoCommand.isScheduled() && !autoMessagePrinted) { if (DriverStation.isAutonomousEnabled()) { System.out.printf( - "*** Auto finished in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); + "*** Auto finished in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); } else { System.out.printf( - "*** Auto cancelled in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); + "*** Auto cancelled in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart); } autoMessagePrinted = true; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c638ff5..64ee3b49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,9 +24,6 @@ import frc.robot.commands.FeedForwardCharacterization; import frc.robot.commands.auto.TestAutos; import frc.robot.subsystems.apriltagvision.AprilTagVision; -import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; -import frc.robot.subsystems.apriltagvision.AprilTagVisionIO; -import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.subsystems.superstructure.intake.IntakeIO; @@ -45,207 +42,184 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state as field - private final RobotState robotState = RobotState.getInstance(); - - // Subsystems - private Drive drive; - private AprilTagVision aprilTagVision; - private Shooter shooter; - private Intake intake; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - switch (Constants.getMode()) { - case REAL -> { - // Real robot, instantiate hardware IO implementations\ - switch (Constants.getRobot()) { - default -> { - drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0]), - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[1])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); - } - } - } - case SIM -> { - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() { - }, - new ModuleIOSim(DriveConstants.moduleConfigs[0]), - new ModuleIOSim(DriveConstants.moduleConfigs[1]), - new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3])); - shooter = new Shooter(new ShooterIOSim()); - intake = new Intake(new IntakeIOSim()); - } - default -> { - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); - shooter = new Shooter(new ShooterIO() { - }); - intake = new Intake(new IntakeIO() { - }); - } - } - - if (drive == null) { - drive = new Drive( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }); - } - - if (aprilTagVision == null) { - aprilTagVision = - switch (Constants.getRobot()) { - case RAINBOWT -> new AprilTagVision( - new AprilTagVisionIO() { - }, - new AprilTagVisionIO() { - }); - default -> new AprilTagVision(); - }; - } - - if (shooter == null) { - shooter = new Shooter(new ShooterIO() {}); - } - - if (intake == null) { - intake = new Intake(new IntakeIO() {}); + // Load robot state as field + private final RobotState robotState = RobotState.getInstance(); + + // Subsystems + private Drive drive; + private AprilTagVision aprilTagVision; + private Shooter shooter; + private Intake intake; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + switch (Constants.getMode()) { + case REAL -> { + // Real robot, instantiate hardware IO implementations\ + switch (Constants.getRobot()) { + default -> { + drive = + new Drive( + new GyroIOPigeon2(false), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); + aprilTagVision = new AprilTagVision(); + shooter = new Shooter(new ShooterIOSparkMax()); + intake = new Intake(new IntakeIOSparkMax()); + } } + } + case SIM -> { + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3])); + shooter = new Shooter(new ShooterIOSim()); + intake = new Intake(new IntakeIOSim()); + } + default -> { + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + shooter = new Shooter(new ShooterIO() {}); + intake = new Intake(new IntakeIO() {}); + } + } + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + } - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); - // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) - .finallyDo(drive::endCharacterization)); - autoChooser.addOption( - "Left Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runLeftCharacterizationVolts, - shooter::getLeftCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - autoChooser.addOption( - "Right Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - - autoChooser.addOption( - "Davis Auto Centerline first", TestAutos.davisAutoDefensive(drive, intake)); - autoChooser.addOption("Davis Auto (4 note)", TestAutos.davisAuto(drive, intake)); - - // Testing autos paths - // Function> trajectoryCommandFactory = - // trajectoryFile -> { - // Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); - // return trajectory.map( - // traj -> - // Commands.runOnce( - // () -> { - // robotState.resetPose( - // AllianceFlipUtil.apply(traj.getStartPose()), - // drive.getWheelPositions(), - // drive.getGyroYaw()); - // drive.getMotionPlanner().setTrajectory(traj); - // }, - // drive)); - // }; - // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - // trajectoryCommandFactory - // .apply(trajectoryFile) - // .ifPresent( - // command -> { - // autoChooser.addOption(trajectoryFile.getName(), command); - // }); - // } - - // Configure the button bindings - configureButtonBindings(); + if (aprilTagVision == null) { + aprilTagVision = new AprilTagVision(); } - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - drive - .setDriveInput( - () -> - DriveCommands.getDriveInputSpeeds( - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())) - .schedule(); - controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - new Pose2d( - robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) - .ignoringDisable(true)); - controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + if (shooter == null) { + shooter = new Shooter(new ShooterIO() {}); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); + if (intake == null) { + intake = new Intake(new IntakeIO() {}); } + + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + // Set up feedforward characterization + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); + autoChooser.addOption( + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runLeftCharacterizationVolts, + shooter::getLeftCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runRightCharacterizationVolts, + shooter::getRightCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + + autoChooser.addOption( + "Davis Auto Centerline first", TestAutos.davisAutoDefensive(drive, intake)); + autoChooser.addOption("Davis Auto (4 note)", TestAutos.davisAuto(drive, intake)); + + // Testing autos paths + // Function> trajectoryCommandFactory = + // trajectoryFile -> { + // Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + // return trajectory.map( + // traj -> + // Commands.runOnce( + // () -> { + // robotState.resetPose( + // AllianceFlipUtil.apply(traj.getStartPose()), + // drive.getWheelPositions(), + // drive.getGyroYaw()); + // drive.getMotionPlanner().setTrajectory(traj); + // }, + // drive)); + // }; + // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + // trajectoryCommandFactory + // .apply(trajectoryFile) + // .ifPresent( + // command -> { + // autoChooser.addOption(trajectoryFile.getName(), command); + // }); + // } + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller + .a() + .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), intake::running)); + controller + .x() + .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), intake::running)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + new Pose2d( + robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) + .ignoringDisable(true)); + controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index c62da8be..9deb60d9 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,30 +30,38 @@ public class DriveCommands { private static final double DEADBAND = 0.1; - public static ChassisSpeeds getDriveInputSpeeds( - DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return drive.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); - return ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(), - linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(), - omega * DriveConstants.drivetrainConfig.maxAngularVelocity(), - AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose().getRotation())); + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(), + linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(), + omega * DriveConstants.drivetrainConfig.maxAngularVelocity()); + drive.setDriveInputSpeeds(speeds); + }); } private static final Supplier shotRotation = diff --git a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVision.java index 222fdea7..853e531e 100644 --- a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVision.java @@ -1,5 +1,9 @@ package frc.robot.subsystems.apriltagvision; +import static frc.robot.RobotState.VisionObservation; +import static frc.robot.subsystems.apriltagvision.AprilTagVisionConstants.*; +import static frc.robot.subsystems.apriltagvision.AprilTagVisionIO.AprilTagVisionIOInputs; + import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -7,235 +11,217 @@ import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Optional; - import frc.robot.FieldConstants; import frc.robot.RobotState; import frc.robot.util.Alert; import frc.robot.util.GeomUtil; import frc.robot.util.VirtualSubsystem; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Optional; import org.littletonrobotics.junction.Logger; -import static frc.robot.subsystems.apriltagvision.AprilTagVisionConstants.*; -import static frc.robot.subsystems.apriltagvision.AprilTagVisionIO.AprilTagVisionIOInputs; -import static frc.robot.RobotState.VisionObservation; - public class AprilTagVision extends VirtualSubsystem { - private final AprilTagVisionIO[] io; - private final AprilTagVisionIOInputs[] inputs; - - private final RobotState robotState = RobotState.getInstance(); - private boolean enableVisionUpdates = true; - private final Alert enableVisionUpdatesAlert = - new Alert("Vision updates are temporarily disabled.", Alert.AlertType.WARNING); - private final Map lastFrameTimes = new HashMap<>(); - private final Map lastTagDetectionTimes = new HashMap<>(); - - public AprilTagVision(AprilTagVisionIO... io) { - System.out.println("[Init] Creating AprilTagVision"); - if (io.length != cameraPoses.length) { - throw new IllegalArgumentException( - "Number of camera positions must match number of AprilTagVisionIO instances."); - } - this.io = io; - inputs = new AprilTagVisionIOInputs[io.length]; - for (int i = 0; i < io.length; i++) { - inputs[i] = new AprilTagVisionIOInputs(); - } - - // Create map of last frame times for instances - for (int i = 0; i < io.length; i++) { - lastFrameTimes.put(i, 0.0); - } + private final AprilTagVisionIO[] io; + private final AprilTagVisionIOInputs[] inputs; + + private final RobotState robotState = RobotState.getInstance(); + private boolean enableVisionUpdates = true; + private final Alert enableVisionUpdatesAlert = + new Alert("Vision updates are temporarily disabled.", Alert.AlertType.WARNING); + private final Map lastFrameTimes = new HashMap<>(); + private final Map lastTagDetectionTimes = new HashMap<>(); + + public AprilTagVision(AprilTagVisionIO... io) { + System.out.println("[Init] Creating AprilTagVision"); + if (io.length != cameraPoses.length) { + throw new IllegalArgumentException( + "Number of camera positions must match number of AprilTagVisionIO instances."); + } + this.io = io; + inputs = new AprilTagVisionIOInputs[io.length]; + for (int i = 0; i < io.length; i++) { + inputs[i] = new AprilTagVisionIOInputs(); + } - // Create map of last detection times for tags - FieldConstants.aprilTags - .getTags() - .forEach( - (AprilTag tag) -> { - lastTagDetectionTimes.put(tag.ID, 0.0); - }); + // Create map of last frame times for instances + for (int i = 0; i < io.length; i++) { + lastFrameTimes.put(i, 0.0); } - /** Sets whether vision updates for odometry are enabled. */ - public void setVisionUpdatesEnabled(boolean enabled) { - enableVisionUpdates = enabled; - enableVisionUpdatesAlert.set(!enabled); + // Create map of last detection times for tags + FieldConstants.aprilTags + .getTags() + .forEach( + (AprilTag tag) -> { + lastTagDetectionTimes.put(tag.ID, 0.0); + }); + } + + /** Sets whether vision updates for odometry are enabled. */ + public void setVisionUpdatesEnabled(boolean enabled) { + enableVisionUpdates = enabled; + enableVisionUpdatesAlert.set(!enabled); + } + + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]); } - public void periodic() { - for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[i]); - Logger.processInputs("AprilTagVision/Inst" + i, inputs[i]); + // Loop over instances + List allRobotPoses = new ArrayList<>(); + List allRobotPoses3d = new ArrayList<>(); + List allVisionObservations = new ArrayList<>(); + for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { + // Loop over frames + for (int frameIndex = 0; frameIndex < inputs[instanceIndex].timestamps.length; frameIndex++) { + lastFrameTimes.put(instanceIndex, Timer.getFPGATimestamp()); + var timestamp = inputs[instanceIndex].timestamps[frameIndex]; + var values = inputs[instanceIndex].frames[frameIndex]; + + // Exit if blank frame + if (values.length == 0 || values[0] == 0) { + continue; } - // Loop over instances - List allRobotPoses = new ArrayList<>(); - List allRobotPoses3d = new ArrayList<>(); - List allVisionObservations = new ArrayList<>(); - for (int instanceIndex = 0; instanceIndex < io.length; instanceIndex++) { - // Loop over frames - for (int frameIndex = 0; frameIndex < inputs[instanceIndex].timestamps.length; frameIndex++) { - lastFrameTimes.put(instanceIndex, Timer.getFPGATimestamp()); - var timestamp = inputs[instanceIndex].timestamps[frameIndex]; - var values = inputs[instanceIndex].frames[frameIndex]; - - // Exit if blank frame - if (values.length == 0 || values[0] == 0) { - continue; - } - - // Switch based on number of poses - Pose3d cameraPose = null; - Pose3d robotPose3d = null; - switch ((int) values[0]) { - case 1: - // One pose (multi-tag), use directly - cameraPose = - new Pose3d( - values[2], - values[3], - values[4], - new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); - robotPose3d = - cameraPose.transformBy( - GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); - break; - - case 2: - // Two poses (one tag), disambiguate - double error0 = values[1]; - double error1 = values[9]; - Pose3d cameraPose0 = - new Pose3d( - values[2], - values[3], - values[4], - new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); - Pose3d cameraPose1 = - new Pose3d( - values[10], - values[11], - values[12], - new Rotation3d(new Quaternion(values[13], values[14], values[15], values[16]))); - Pose3d robotPose3d0 = - cameraPose0.transformBy( - GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); - Pose3d robotPose3d1 = - cameraPose1.transformBy( - GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); - - // Select pose using projection errors - if (error0 < error1 * ambiguityThreshold) { - cameraPose = cameraPose0; - robotPose3d = robotPose3d0; - } else if (error1 < error0 * ambiguityThreshold) { - cameraPose = cameraPose1; - robotPose3d = robotPose3d1; - } - break; - } - - // Exit if no data - if (cameraPose == null || robotPose3d == null) { - continue; - } - - // Exit if robot pose is off the field - if (robotPose3d.getX() < -fieldBorderMargin - || robotPose3d.getX() > FieldConstants.fieldLength + fieldBorderMargin - || robotPose3d.getY() < -fieldBorderMargin - || robotPose3d.getY() > FieldConstants.fieldWidth + fieldBorderMargin - || robotPose3d.getZ() < -zMargin - || robotPose3d.getZ() > zMargin) { - continue; - } - - // Get 2D robot pose - Pose2d robotPose = robotPose3d.toPose2d(); - - // Get tag poses and update last detection times - List tagPoses = new ArrayList<>(); - for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i++) { - int tagId = (int) values[i]; - lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); - Optional tagPose = FieldConstants.aprilTags.getTagPose((int) values[i]); - tagPose.ifPresent(tagPoses::add); - } - - // Calculate average distance to tag - double totalDistance = 0.0; - for (Pose3d tagPose : tagPoses) { - totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); - } - double avgDistance = totalDistance / tagPoses.size(); - - // Add observation to list - double xyStdDev = xyStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); - double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); - allVisionObservations.add( - new VisionObservation( - robotPose, timestamp, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); - allRobotPoses.add(robotPose); - allRobotPoses3d.add(robotPose3d); - - // Log data from instance - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/LatencySecs", - Timer.getFPGATimestamp() - timestamp); - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/RobotPose", robotPose); - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", - robotPose3d); - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/TagPoses", - tagPoses.toArray(Pose3d[]::new)); + // Switch based on number of poses + Pose3d cameraPose = null; + Pose3d robotPose3d = null; + switch ((int) values[0]) { + case 1: + // One pose (multi-tag), use directly + cameraPose = + new Pose3d( + values[2], + values[3], + values[4], + new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); + robotPose3d = + cameraPose.transformBy( + GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); + break; + + case 2: + // Two poses (one tag), disambiguate + double error0 = values[1]; + double error1 = values[9]; + Pose3d cameraPose0 = + new Pose3d( + values[2], + values[3], + values[4], + new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); + Pose3d cameraPose1 = + new Pose3d( + values[10], + values[11], + values[12], + new Rotation3d(new Quaternion(values[13], values[14], values[15], values[16]))); + Pose3d robotPose3d0 = + cameraPose0.transformBy( + GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); + Pose3d robotPose3d1 = + cameraPose1.transformBy( + GeomUtil.pose3dToTransform3d(cameraPoses[instanceIndex]).inverse()); + + // Select pose using projection errors + if (error0 < error1 * ambiguityThreshold) { + cameraPose = cameraPose0; + robotPose3d = robotPose3d0; + } else if (error1 < error0 * ambiguityThreshold) { + cameraPose = cameraPose1; + robotPose3d = robotPose3d1; } + break; + } - // If no frames from instances, clear robot pose - if (inputs[instanceIndex].timestamps.length == 0) { - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/RobotPose", - new double[] {}); - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", - new double[] {}); - } + // Exit if no data + if (cameraPose == null || robotPose3d == null) { + continue; + } - // If no recent frames from instance, clear tag poses - if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { - Logger.recordOutput( - "AprilTagVision/Inst" + instanceIndex + "/TagPoses", - new double[] {}); - } + // Exit if robot pose is off the field + if (robotPose3d.getX() < -fieldBorderMargin + || robotPose3d.getX() > FieldConstants.fieldLength + fieldBorderMargin + || robotPose3d.getY() < -fieldBorderMargin + || robotPose3d.getY() > FieldConstants.fieldWidth + fieldBorderMargin + || robotPose3d.getZ() < -zMargin + || robotPose3d.getZ() > zMargin) { + continue; + } + + // Get 2D robot pose + Pose2d robotPose = robotPose3d.toPose2d(); + + // Get tag poses and update last detection times + List tagPoses = new ArrayList<>(); + for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i++) { + int tagId = (int) values[i]; + lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); + Optional tagPose = FieldConstants.aprilTags.getTagPose((int) values[i]); + tagPose.ifPresent(tagPoses::add); } - // Log robot poses + // Calculate average distance to tag + double totalDistance = 0.0; + for (Pose3d tagPose : tagPoses) { + totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); + } + double avgDistance = totalDistance / tagPoses.size(); + + // Add observation to list + double xyStdDev = xyStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); + double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDistance, 2.0) / tagPoses.size(); + allVisionObservations.add( + new VisionObservation( + robotPose, timestamp, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); + allRobotPoses.add(robotPose); + allRobotPoses3d.add(robotPose3d); + + // Log data from instance Logger.recordOutput( - "AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); + "AprilTagVision/Inst" + instanceIndex + "/LatencySecs", + Timer.getFPGATimestamp() - timestamp); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", robotPose); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", robotPose3d); Logger.recordOutput( - "AprilTagVision/RobotPoses3d", - allRobotPoses3d.toArray(Pose3d[]::new)); - - // Log tag poses - List allTagPoses = new ArrayList<>(); - for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { - if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { - allTagPoses.add(FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()).get()); - } - } + "AprilTagVision/Inst" + instanceIndex + "/TagPoses", tagPoses.toArray(Pose3d[]::new)); + } + + // If no frames from instances, clear robot pose + if (inputs[instanceIndex].timestamps.length == 0) { + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new double[] {}); Logger.recordOutput( - "AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); + "AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new double[] {}); + } - // Send results to robot state - if (enableVisionUpdates) { - allVisionObservations.forEach(robotState::addVisionObservation); - } + // If no recent frames from instance, clear tag poses + if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new double[] {}); + } + } + + // Log robot poses + Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); + Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new)); + + // Log tag poses + List allTagPoses = new ArrayList<>(); + for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { + if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { + allTagPoses.add(FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()).get()); + } + } + Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); + + // Send results to robot state + if (enableVisionUpdates) { + allVisionObservations.forEach(robotState::addVisionObservation); } -} \ No newline at end of file + } +} diff --git a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java index cb567c15..c6125198 100644 --- a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java +++ b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java @@ -4,28 +4,22 @@ import frc.robot.Constants; public class AprilTagVisionConstants { - public static final double ambiguityThreshold = 0.15; - public static final double targetLogTimeSecs = 0.1; - public static final double fieldBorderMargin = 0.5; - public static final double zMargin = 0.75; - public static final double xyStdDevCoefficient = 0.01; - public static final double thetaStdDevCoefficient = 0.01; + public static final double ambiguityThreshold = 0.15; + public static final double targetLogTimeSecs = 0.1; + public static final double fieldBorderMargin = 0.5; + public static final double zMargin = 0.75; + public static final double xyStdDevCoefficient = 0.01; + public static final double thetaStdDevCoefficient = 0.01; - public static final Pose3d[] cameraPoses = - switch (Constants.getRobot()) { - case RAINBOWT -> new Pose3d[] { - new Pose3d(), // TODO: what are these again? - new Pose3d() - }; - default -> new Pose3d[] {}; - }; + public static final Pose3d[] cameraPoses = + switch (Constants.getRobot()) { + case RAINBOWT -> new Pose3d[] {}; + default -> new Pose3d[] {}; + }; - public static final String[] cameraNames = - switch (Constants.getRobot()) { - case RAINBOWT -> new String[] { - "northstar_0", - "northstar_1" - }; - default -> new String[] {}; - }; + public static final String[] cameraNames = + switch (Constants.getRobot()) { + case RAINBOWT -> new String[] {"northstar_0", "northstar_1"}; + default -> new String[] {}; + }; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bfc46228..2ea094a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -74,9 +74,6 @@ public void periodic() { Arrays.stream(modules).forEach(Module::updateInputs); odometryLock.unlock(); - // Call periodic on module - Arrays.stream(modules).forEach(Module::periodic); - // Calculate the min odometry position updates across all modules int minOdometryUpdates = IntStream.of( @@ -131,8 +128,8 @@ public void periodic() { SwerveModuleState[] setpointStates = motionPlanner.update( Timer.getFPGATimestamp(), RobotState.getInstance().getEstimatedPose(), fieldVelocity); - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; // Run robot if (!characterizing) { SwerveDriveKinematics.desaturateWheelSpeeds( @@ -178,9 +175,8 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } - // Command factories for motion planner commands - public Command setDriveInput(Supplier driveInput) { - return Commands.runOnce(() -> motionPlanner.setDriveInputSpeeds(driveInput)); + public void setDriveInputSpeeds(ChassisSpeeds speeds) { + motionPlanner.setDriveInputSpeeds(speeds); } public Command followTrajectoryCommand(Trajectory trajectory) { @@ -201,9 +197,7 @@ public Command orientModules(Rotation2d[] orientations) { public Command orientModules(Rotation2d orientation) { Rotation2d[] orientations = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - orientations[i] = orientation; - } + Arrays.fill(orientations, orientation); return orientModules(orientations); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index e4a5463a..8350847f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -148,9 +148,9 @@ public record ModuleConstants( double ffKs, double ffKv, double driveKp, - double drivekD, + double driveKd, double turnKp, - double turnkD, + double turnKd, double driveReduction, double turnReduction) {} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java index 831c488e..8528fc8b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java @@ -59,7 +59,7 @@ public class DriveMotionPlanner { private final SwerveDriveKinematics swerveDriveKinematics; - private Supplier driveInputSpeeds = ChassisSpeeds::new; + private ChassisSpeeds driveInputSpeeds = new ChassisSpeeds(); private Optional currentTrajectory = Optional.empty(); private Optional trajectoryStartTime = Optional.empty(); private Optional> headingSupplier = Optional.empty(); @@ -107,10 +107,7 @@ protected void disableHeadingSupplier() { if (currentTrajectory.isPresent()) trajectoryController.resetThetaController(); } - protected void setDriveInputSpeeds(Supplier driveInputSpeeds) { - if (driveInputSpeeds == null || driveInputSpeeds.get() == null) { - return; - } + protected void setDriveInputSpeeds(ChassisSpeeds driveInputSpeeds) { this.driveInputSpeeds = driveInputSpeeds; } @@ -179,7 +176,7 @@ protected SwerveModuleState[] update(double timestamp, Pose2d robot, Twist2d fie currentTrajectory = Optional.empty(); trajectoryStartTime = Optional.empty(); // Use drive input speeds (none if in auto) - return driveInputSpeeds.get(); + return new ChassisSpeeds(); } HolonomicDriveState currentState = @@ -202,15 +199,17 @@ protected SwerveModuleState[] update(double timestamp, Pose2d robot, Twist2d fie trajectoryController.getPoseError().getRotation().getRadians()); return trajectorySpeeds; }) - .orElseGet(driveInputSpeeds); + .orElse(driveInputSpeeds); // Otherwise use inputted speeds // Use heading controller - ChassisSpeeds finalSpeeds = speeds; + // Change to robot relative speeds + final ChassisSpeeds robotRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robot.getRotation()); headingSupplier.ifPresent( headingSupplier -> { Rotation2d setpointHeading = headingSupplier.get(); - finalSpeeds.omegaRadiansPerSecond = + robotRelativeSpeeds.omegaRadiansPerSecond = headingController.calculate( robot.getRotation().getRadians(), setpointHeading.getRadians()); Logger.recordOutput("Drive/headingControl", setpointHeading); @@ -222,12 +221,14 @@ protected SwerveModuleState[] update(double timestamp, Pose2d robot, Twist2d fie // Calculate output setpoint states SwerveModuleState[] outputStates = new SwerveModuleState[4]; if (moduleOrientations.isEmpty()) { - speeds = ChassisSpeeds.discretize(speeds, 0.02); - outputStates = swerveDriveKinematics.toSwerveModuleStates(speeds); + ChassisSpeeds discretizedSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); + outputStates = swerveDriveKinematics.toSwerveModuleStates(discretizedSpeeds); Logger.recordOutput( "Drive/speeds", new double[] { - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond + discretizedSpeeds.vxMetersPerSecond, + discretizedSpeeds.vyMetersPerSecond, + discretizedSpeeds.omegaRadiansPerSecond }); } else { // If module orientations are present diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 35afda8a..e37282a4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -1,37 +1,78 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import java.util.stream.IntStream; +import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class Module { + private static final LoggedTunableNumber wheelRadius = + new LoggedTunableNumber("Drive/Module/WheelRadius", DriveConstants.wheelRadius); + private static final LoggedTunableNumber driveKp = + new LoggedTunableNumber("Drive/Module/DriveKp", DriveConstants.moduleConstants.driveKp()); + private static final LoggedTunableNumber driveKd = + new LoggedTunableNumber("Drive/Module/DriveKd", DriveConstants.moduleConstants.driveKd()); + private static final LoggedTunableNumber driveKs = + new LoggedTunableNumber("Drive/Module/DriveKs", DriveConstants.moduleConstants.ffKs()); + private static final LoggedTunableNumber driveKv = + new LoggedTunableNumber("Drive/Module/DriveKv", DriveConstants.moduleConstants.ffKv()); + private static final LoggedTunableNumber turnKp = + new LoggedTunableNumber("Drive/Module/TurnKp", DriveConstants.moduleConstants.turnKp()); + private static final LoggedTunableNumber turnKd = + new LoggedTunableNumber("Drive/Module/TurnKd", DriveConstants.moduleConstants.turnKd()); + private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; + private final PIDController driveController = + new PIDController( + DriveConstants.moduleConstants.driveKp(), 0.0, DriveConstants.moduleConstants.driveKd()); + private final PIDController turnController = + new PIDController( + DriveConstants.moduleConstants.turnKp(), 0.0, DriveConstants.moduleConstants.turnKd()); + private SimpleMotorFeedforward driveFf = + new SimpleMotorFeedforward( + DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); + public Module(ModuleIO io, int index) { this.io = io; this.index = index; + + turnController.enableContinuousInput(-Math.PI, Math.PI); } /** Called while blocking odometry thread */ public void updateInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module" + index, inputs); - } - public void periodic() { - io.periodic(); + driveController.setPID(driveKp.get(), 0, driveKd.get()); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> driveFf = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0), + driveKs, + driveKv); + turnController.setPID(turnKp.get(), 0, driveKd.get()); } public void runSetpoint(SwerveModuleState setpoint) { - io.runSetpoint(setpoint); + io.setDriveVoltage( + driveController.calculate( + inputs.driveVelocityRadPerSec, + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) + + driveFf.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setTurnVoltage( + turnController.calculate( + inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); } public void runCharacterization(double volts) { - io.runCharacterization(volts); + io.setTurnVoltage(turnController.calculate(inputs.turnAbsolutePosition.getRadians(), 0)); + io.setDriveVoltage(volts); } public void setBrakeMode(boolean enabled) { @@ -46,13 +87,13 @@ public void stop() { public SwerveModulePosition[] getModulePositions() { int minOdometryPositions = Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length); - return IntStream.range(0, minOdometryPositions) - .mapToObj( - odometryIndex -> - new SwerveModulePosition( - inputs.odometryDrivePositionsMeters[odometryIndex], - inputs.odometryTurnPositions[odometryIndex])) - .toArray(SwerveModulePosition[]::new); + SwerveModulePosition[] positions = new SwerveModulePosition[minOdometryPositions]; + for (int i = 0; i < minOdometryPositions; i++) { + positions[i] = + new SwerveModulePosition( + inputs.odometryDrivePositionsMeters[i], inputs.odometryTurnPositions[i]); + } + return positions; } public Rotation2d getAngle() { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 2855e58a..407cd2c9 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -14,7 +14,6 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -23,13 +22,13 @@ class ModuleIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; + public double driveCurrentAmps = 0.0; public Rotation2d turnAbsolutePosition = new Rotation2d(); public Rotation2d turnPosition = new Rotation2d(); public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; + public double turnCurrentAmps = 0.0; public double[] odometryDrivePositionsMeters = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; @@ -38,13 +37,13 @@ class ModuleIOInputs { /** Updates the set of loggable inputs. */ default void updateInputs(ModuleIOInputs inputs) {} - default void periodic() {} + default void setDriveVoltage(double volts) {} - /** Set setpoint for module */ - default void runSetpoint(SwerveModuleState state) {} + default void setTurnVoltage(double volts) {} - /** Runs the module with the specified voltage while controlling to zero degrees. */ - default void runCharacterization(double volts) {} + default void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) {} + + default void setTurnPositionSetpoint(double angleRadians, double ffVolts) {} /** Enable or disable brake mode on the drive motor. */ default void setDriveBrakeMode(boolean enable) {} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index de602cce..0368f494 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -1,277 +1,279 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage +//// Copyright 2021-2024 FRC 6328 +//// http://github.com/Mechanical-Advantage +//// +//// This program is free software; you can redistribute it and/or +//// modify it under the terms of the GNU General Public License +//// version 3 as published by the Free Software Foundation or +//// available in the root directory of this project. +//// +//// This program is distributed in the hope that it will be useful, +//// but WITHOUT ANY WARRANTY; without even the implied warranty of +//// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +//// GNU General Public License for more details. // -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. +// package frc.robot.subsystems.drive; // -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.drive; - -import static frc.robot.subsystems.drive.DriveConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -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.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; -import java.util.Queue; -import java.util.function.Supplier; - -public class ModuleIOKrakenFOC implements ModuleIO { - // Reseed relative encoder to absolute encoder value - private static final int reseedRelativeEncoderCounts = 100; - private int currentReseedCount = reseedRelativeEncoderCounts; - - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnPosition; - private final Supplier turnAbsolutePosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - private final boolean isTurnMotorInverted = true; - - // queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final AnalogInput turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; - - // Setpoint control - private final VelocityTorqueCurrentFOC driveVelocityControl = - new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false, false); - private final PositionTorqueCurrentFOC turnPositionControl = - new PositionTorqueCurrentFOC(0.0, 0.0, 0.0, 1, false, false, false); - private final VoltageOut driveVoltageControl = new VoltageOut(0.0).withEnableFOC(true); - private final NeutralOut driveBrake = new NeutralOut(); - private final NeutralOut turnBrake = new NeutralOut(); - private Mode currentMode = Mode.SETPOINT; - private double driveSpeedSetpoint = 0.0; - private Rotation2d turnAngleSetpoint = new Rotation2d(); - private double driveVoltage = 0.0; - - public ModuleIOKrakenFOC(ModuleConfig config) { - // init controllers and encoders from config constants - driveTalon = new TalonFX(config.driveID()); - turnTalon = new TalonFX(config.turnID()); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.Voltage.PeakForwardVoltage = 12.0; - driveConfig.Voltage.PeakReverseVoltage = -12.0; - // TUNE PID CONSTANTS - driveConfig.Slot0.kP = moduleConstants.driveKp(); - driveConfig.Slot0.kI = 0.0; - driveConfig.Slot0.kD = moduleConstants.drivekD(); - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; - turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; - turnConfig.Voltage.PeakForwardVoltage = 12.0; - turnConfig.Voltage.PeakReverseVoltage = -12.0; - // TUNE PID CONSTANTS - turnConfig.Slot0.kP = moduleConstants.turnKp(); - turnConfig.Slot0.kI = 0.0; - turnConfig.Slot0.kD = moduleConstants.turnkD(); - turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; - turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - // Map rotation of motor shaft to 1 rotation of turn shaft - turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); - - // Apply configs - for (int i = 0; i < 4; i++) { - boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; - setDriveBrakeMode(true); - error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); - setTurnBrakeMode(true); - if (!error) break; - } - - // Get signals and set update rate - // 50hz signals - drivePosition = driveTalon.getPosition(); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - - turnPosition = turnTalon.getPosition(); - turnAbsolutePosition = - () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) - .minus(absoluteEncoderOffset); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - - // 250hz signals - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnVelocity, - turnAppliedVolts, - turnCurrent); - BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) - / moduleConstants.driveReduction(); - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) - / moduleConstants.driveReduction(); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = turnAbsolutePosition.get(); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / moduleConstants.turnReduction()); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / moduleConstants.turnReduction(); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - signal -> - Units.rotationsToRadians(signal) - * wheelRadius - / moduleConstants.driveReduction()) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) - .toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void periodic() { - // Only reseed if turn abs encoder is online - if (++currentReseedCount >= reseedRelativeEncoderCounts - && turnAbsoluteEncoder.getVoltage() != 0.0) { - // heading of wheel --> turn motor shaft rotations - // timeout of 20 ms - boolean passed = - turnTalon.setPosition( - Units.radiansToRotations(turnAbsolutePosition.get().getRadians()) - * moduleConstants.turnReduction(), - 0.02) - == StatusCode.OK; - // redo next cycle if did not pass - currentReseedCount = passed ? 0 : currentReseedCount; - } - - // Control motors - if (currentMode == Mode.NEUTRAL) { - driveTalon.setControl(driveBrake); - turnTalon.setControl(turnBrake); - } else { - if (currentMode == Mode.SETPOINT) { - driveTalon.setControl(driveVelocityControl.withVelocity(driveSpeedSetpoint)); - } else if (currentMode == Mode.CHARACTERIZATION) { - driveTalon.setControl(driveVoltageControl.withOutput(driveVoltage)); - } - turnTalon.setControl(turnPositionControl.withPosition(turnAngleSetpoint.getRotations())); - } - } - - @Override - public void runSetpoint(SwerveModuleState state) { - currentMode = Mode.SETPOINT; - driveSpeedSetpoint = state.speedMetersPerSecond / wheelRadius; - turnAngleSetpoint = state.angle; - } - - @Override - public void runCharacterization(double volts) { - currentMode = Mode.CHARACTERIZATION; - driveVoltage = volts; - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } - - @Override - public void stop() { - currentMode = Mode.NEUTRAL; - } - - private enum Mode { - SETPOINT, - CHARACTERIZATION, - NEUTRAL; - } -} +// import static frc.robot.subsystems.drive.DriveConstants.*; +// +// import com.ctre.phoenix6.BaseStatusSignal; +// import com.ctre.phoenix6.StatusCode; +// import com.ctre.phoenix6.StatusSignal; +// import com.ctre.phoenix6.configs.MotorOutputConfigs; +// import com.ctre.phoenix6.configs.TalonFXConfiguration; +// import com.ctre.phoenix6.controls.NeutralOut; +// import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +// import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +// import com.ctre.phoenix6.controls.VoltageOut; +// import com.ctre.phoenix6.hardware.TalonFX; +// 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.SwerveModuleState; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.AnalogInput; +// import edu.wpi.first.wpilibj.RobotController; +// import java.util.Queue; +// import java.util.function.Supplier; +// +// public class ModuleIOKrakenFOC implements ModuleIO { +// // Reseed relative encoder to absolute encoder value +// private static final int reseedRelativeEncoderCounts = 100; +// private int currentReseedCount = reseedRelativeEncoderCounts; +// +// private final StatusSignal drivePosition; +// private final StatusSignal driveVelocity; +// private final StatusSignal driveAppliedVolts; +// private final StatusSignal driveCurrent; +// +// private final StatusSignal turnPosition; +// private final Supplier turnAbsolutePosition; +// private final StatusSignal turnVelocity; +// private final StatusSignal turnAppliedVolts; +// private final StatusSignal turnCurrent; +// private final boolean isTurnMotorInverted = true; +// +// // queues +// private final Queue drivePositionQueue; +// private final Queue turnPositionQueue; +// +// private final TalonFX driveTalon; +// private final TalonFX turnTalon; +// private final AnalogInput turnAbsoluteEncoder; +// private final Rotation2d absoluteEncoderOffset; +// +// // Setpoint control +// private final VelocityTorqueCurrentFOC driveVelocityControl = +// new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false, false); +// private final PositionTorqueCurrentFOC turnPositionControl = +// new PositionTorqueCurrentFOC(0.0, 0.0, 0.0, 1, false, false, false); +// private final VoltageOut driveVoltageControl = new VoltageOut(0.0).withEnableFOC(true); +// private final NeutralOut driveBrake = new NeutralOut(); +// private final NeutralOut turnBrake = new NeutralOut(); +// private Mode currentMode = Mode.SETPOINT; +// private double driveSpeedSetpoint = 0.0; +// private Rotation2d turnAngleSetpoint = new Rotation2d(); +// private double driveVoltage = 0.0; +// +// public ModuleIOKrakenFOC(ModuleConfig config) { +// // init controllers and encoders from config constants +// driveTalon = new TalonFX(config.driveID()); +// turnTalon = new TalonFX(config.turnID()); +// turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); +// absoluteEncoderOffset = config.absoluteEncoderOffset(); +// +// var driveConfig = new TalonFXConfiguration(); +// driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; +// driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; +// driveConfig.Voltage.PeakForwardVoltage = 12.0; +// driveConfig.Voltage.PeakReverseVoltage = -12.0; +// // TUNE PID CONSTANTS +// driveConfig.Slot0.kP = moduleConstants.driveKp(); +// driveConfig.Slot0.kI = 0.0; +// driveConfig.Slot0.kD = moduleConstants.driveKd(); +// driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; +// driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; +// +// var turnConfig = new TalonFXConfiguration(); +// turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; +// turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; +// turnConfig.Voltage.PeakForwardVoltage = 12.0; +// turnConfig.Voltage.PeakReverseVoltage = -12.0; +// // TUNE PID CONSTANTS +// turnConfig.Slot0.kP = moduleConstants.turnKp(); +// turnConfig.Slot0.kI = 0.0; +// turnConfig.Slot0.kD = moduleConstants.turnKd(); +// turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; +// turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; +// turnConfig.ClosedLoopGeneral.ContinuousWrap = true; +// // Map rotation of motor shaft to 1 rotation of turn shaft +// turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); +// +// // Apply configs +// for (int i = 0; i < 4; i++) { +// boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; +// setDriveBrakeMode(true); +// error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); +// setTurnBrakeMode(true); +// if (!error) break; +// } +// +// // Get signals and set update rate +// // 50hz signals +// drivePosition = driveTalon.getPosition(); +// driveVelocity = driveTalon.getVelocity(); +// driveAppliedVolts = driveTalon.getMotorVoltage(); +// driveCurrent = driveTalon.getStatorCurrent(); +// +// turnPosition = turnTalon.getPosition(); +// turnAbsolutePosition = +// () -> +// Rotation2d.fromRadians( +// turnAbsoluteEncoder.getVoltage() +// / RobotController.getVoltage5V() +// * 2.0 +// * Math.PI) +// .minus(absoluteEncoderOffset); +// turnVelocity = turnTalon.getVelocity(); +// turnAppliedVolts = turnTalon.getMotorVoltage(); +// turnCurrent = turnTalon.getStatorCurrent(); +// +// // 250hz signals +// drivePositionQueue = +// PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); +// turnPositionQueue = +// PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); +// +// BaseStatusSignal.setUpdateFrequencyForAll( +// 50.0, +// driveVelocity, +// driveAppliedVolts, +// driveCurrent, +// turnVelocity, +// turnAppliedVolts, +// turnCurrent); +// BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); +// driveTalon.optimizeBusUtilization(); +// turnTalon.optimizeBusUtilization(); +// } +// +// @Override +// public void updateInputs(ModuleIOInputs inputs) { +// BaseStatusSignal.refreshAll( +// drivePosition, +// driveVelocity, +// driveAppliedVolts, +// driveCurrent, +// turnPosition, +// turnVelocity, +// turnAppliedVolts, +// turnCurrent); +// +// inputs.drivePositionRad = +// Units.rotationsToRadians(drivePosition.getValueAsDouble()) +// / moduleConstants.driveReduction(); +// inputs.driveVelocityRadPerSec = +// Units.rotationsToRadians(driveVelocity.getValueAsDouble()) +// / moduleConstants.driveReduction(); +// inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); +// inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; +// +// inputs.turnAbsolutePosition = turnAbsolutePosition.get(); +// inputs.turnPosition = +// Rotation2d.fromRotations(turnPosition.getValueAsDouble() / +// moduleConstants.turnReduction()); +// inputs.turnVelocityRadPerSec = +// Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / +// moduleConstants.turnReduction(); +// inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); +// inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; +// +// inputs.odometryDrivePositionsMeters = +// drivePositionQueue.stream() +// .mapToDouble( +// signal -> +// Units.rotationsToRadians(signal) +// * wheelRadius +// / moduleConstants.driveReduction()) +// .toArray(); +// inputs.odometryTurnPositions = +// turnPositionQueue.stream() +// .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) +// .toArray(Rotation2d[]::new); +// drivePositionQueue.clear(); +// turnPositionQueue.clear(); +// } +// +// @Override +// public void periodic() { +// // Only reseed if turn abs encoder is online +// if (++currentReseedCount >= reseedRelativeEncoderCounts +// && turnAbsoluteEncoder.getVoltage() != 0.0) { +// // heading of wheel --> turn motor shaft rotations +// // timeout of 20 ms +// boolean passed = +// turnTalon.setPosition( +// Units.radiansToRotations(turnAbsolutePosition.get().getRadians()) +// * moduleConstants.turnReduction(), +// 0.02) +// == StatusCode.OK; +// // redo next cycle if did not pass +// currentReseedCount = passed ? 0 : currentReseedCount; +// } +// +// // Control motors +// if (currentMode == Mode.NEUTRAL) { +// driveTalon.setControl(driveBrake); +// turnTalon.setControl(turnBrake); +// } else { +// if (currentMode == Mode.SETPOINT) { +// driveTalon.setControl(driveVelocityControl.withVelocity(driveSpeedSetpoint)); +// } else if (currentMode == Mode.CHARACTERIZATION) { +// driveTalon.setControl(driveVoltageControl.withOutput(driveVoltage)); +// } +// turnTalon.setControl(turnPositionControl.withPosition(turnAngleSetpoint.getRotations())); +// } +// } +// +// @Override +// public void runSetpoint(SwerveModuleState state) { +// currentMode = Mode.SETPOINT; +// driveSpeedSetpoint = state.speedMetersPerSecond / wheelRadius; +// turnAngleSetpoint = state.angle; +// } +// +// @Override +// public void runCharacterization(double volts) { +// currentMode = Mode.CHARACTERIZATION; +// driveVoltage = volts; +// } +// +// @Override +// public void setDriveBrakeMode(boolean enable) { +// var config = new MotorOutputConfigs(); +// config.Inverted = InvertedValue.CounterClockwise_Positive; +// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; +// driveTalon.getConfigurator().apply(config); +// } +// +// @Override +// public void setTurnBrakeMode(boolean enable) { +// var config = new MotorOutputConfigs(); +// config.Inverted = +// isTurnMotorInverted +// ? InvertedValue.Clockwise_Positive +// : InvertedValue.CounterClockwise_Positive; +// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; +// turnTalon.getConfigurator().apply(config); +// } +// +// @Override +// public void stop() { +// currentMode = Mode.NEUTRAL; +// } +// +// private enum Mode { +// SETPOINT, +// CHARACTERIZATION, +// NEUTRAL; +// } +// } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 1a5b54f8..60b96123 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -15,10 +15,8 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -40,22 +38,11 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; private final Rotation2d turnAbsoluteInitPosition; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; private Rotation2d turnAbsolutePosition; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop public ModuleIOSim(ModuleConfig config) { turnAbsoluteInitPosition = config.absoluteEncoderOffset(); turnAbsolutePosition = turnAbsoluteInitPosition; - - driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv()); - driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD()); - turnFeedback = new PIDController(moduleConstants.turnKp(), 0.0, moduleConstants.turnkD()); - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override @@ -66,7 +53,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); @@ -74,7 +61,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); inputs.odometryDrivePositionsMeters = new double[] {driveSim.getAngularPositionRad() * wheelRadius}; @@ -82,49 +69,23 @@ public void updateInputs(ModuleIOInputs inputs) { new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())}; } - @Override - public void periodic() { - // Run closed loop turn control - if (angleSetpoint != null) { - turnAppliedVolts = - turnFeedback.calculate(turnAbsolutePosition.getRadians(), angleSetpoint.getRadians()); - turnSim.setInputVoltage(turnAppliedVolts); - // Run closed loop drive control - if (speedSetpoint != null) { - // Scale velocity based on turn error - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; - driveAppliedVolts = - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate( - driveSim.getAngularVelocityRadPerSec(), velocityRadPerSec); - driveSim.setInputVoltage(driveAppliedVolts); - } - } + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); } - @Override - public void runSetpoint(SwerveModuleState state) { - angleSetpoint = state.angle; - speedSetpoint = state.speedMetersPerSecond; + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(turnAppliedVolts); } - @Override - public void runCharacterization(double volts) { - // Lock wheels to forward and no speed setpoint - angleSetpoint = new Rotation2d(); - speedSetpoint = null; + public void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) {} - driveAppliedVolts = volts; - driveSim.setInputVoltage(volts); - } + public void setTurnPositionSetpoint(double angleRadians, double ffVolts) {} @Override public void stop() { driveSim.setInputVoltage(0.0); turnSim.setInputVoltage(0.0); - - speedSetpoint = null; - angleSetpoint = null; } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index f873f631..38112b06 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -20,19 +20,14 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; import java.util.Queue; import java.util.function.Supplier; public class ModuleIOSparkMax implements ModuleIO { - // Every 100 times through periodic (2 secs) we reset relative encoder to absolute encoder - private static final int reSeedIterations = 100; - private final CANSparkMax driveMotor; private final CANSparkMax turnMotor; private final RelativeEncoder driveEncoder; @@ -44,14 +39,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final Queue turnPositionQueue; private final Rotation2d absoluteEncoderOffset; - private int currentIteration = reSeedIterations; - private Supplier absoluteEncoderValue; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private final Supplier absoluteEncoderValue; public ModuleIOSparkMax(ModuleConfig config) { // Init motor & encoder objects @@ -77,20 +65,10 @@ public ModuleIOSparkMax(ModuleConfig config) { driveEncoder.setPosition(0.0); driveEncoder.setMeasurementPeriod(10); driveEncoder.setAverageDepth(2); - driveEncoder.setPositionConversionFactor(2 * Math.PI / moduleConstants.driveReduction()); - driveEncoder.setVelocityConversionFactor( - 2 * Math.PI / (60.0 * moduleConstants.driveReduction())); turnRelativeEncoder.setPosition(0.0); turnRelativeEncoder.setMeasurementPeriod(10); turnRelativeEncoder.setAverageDepth(2); - turnRelativeEncoder.setPositionConversionFactor( - 2 * Math.PI / moduleConstants.turnReduction()); - turnRelativeEncoder.setVelocityConversionFactor( - 2 * Math.PI / (60.0 * moduleConstants.turnReduction())); - - driveMotor.setCANTimeout(0); - turnMotor.setCANTimeout(0); driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); @@ -98,6 +76,8 @@ public ModuleIOSparkMax(ModuleConfig config) { if (driveMotor.burnFlash().equals(REVLibError.kOk) && turnMotor.burnFlash().equals(REVLibError.kOk)) break; } + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); absoluteEncoderValue = () -> @@ -113,28 +93,36 @@ public ModuleIOSparkMax(ModuleConfig config) { turnPositionQueue = SparkMaxOdometryThread.getInstance() .registerSignal(() -> absoluteEncoderValue.get().getRadians()); - - driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv()); - driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD()); - turnFeedback = new PIDController(moduleConstants.turnKp(), 0.0, moduleConstants.turnkD()); - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = driveEncoder.getPosition(); - inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() / moduleConstants.driveReduction()); inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveMotor.getOutputCurrent()}; + inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); inputs.turnAbsolutePosition = absoluteEncoderValue.get(); - inputs.turnPosition = Rotation2d.fromRadians(turnRelativeEncoder.getPosition()); - inputs.turnVelocityRadPerSec = turnRelativeEncoder.getVelocity(); + inputs.turnPosition = + Rotation2d.fromRadians( + Units.rotationsToRadians( + turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnMotor.getOutputCurrent()}; + inputs.turnCurrentAmps = turnMotor.getOutputCurrent(); inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream().mapToDouble(rads -> rads * wheelRadius).toArray(); + drivePositionQueue.stream() + .mapToDouble( + motorPositionRevs -> + Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) + * wheelRadius) + .toArray(); inputs.odometryTurnPositions = turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); drivePositionQueue.clear(); @@ -142,46 +130,23 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void periodic() { - // Reset to absolute encoder if abs encoder is non zero - if (++currentIteration >= reSeedIterations && absoluteEncoderValue.get().getRadians() != 0.0) { - REVLibError error = - turnRelativeEncoder.setPosition( - (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) - * moduleConstants.turnReduction()); - // Reset current iteration to 0 if reset went through - if (error == REVLibError.kOk) currentIteration = 0; - } - - // Run closed loop turn control - if (angleSetpoint != null) { - turnMotor.setVoltage( - turnFeedback.calculate( - absoluteEncoderValue.get().getRadians(), angleSetpoint.getRadians())); - // Run closed loop drive control - if (speedSetpoint != null) { - // Scale velocity based on turn error - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; - driveMotor.setVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(driveEncoder.getVelocity(), velocityRadPerSec)); - } - } + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); } @Override - public void runSetpoint(SwerveModuleState state) { - angleSetpoint = state.angle; - speedSetpoint = state.speedMetersPerSecond; + public void setTurnVoltage(double volts) { + turnMotor.setVoltage(volts); } @Override - public void runCharacterization(double volts) { - angleSetpoint = new Rotation2d(); - speedSetpoint = null; + public void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) { + // TODO + } - driveMotor.setVoltage(volts); + @Override + public void setTurnPositionSetpoint(double angleRadians, double ffVolts) { + // TODO } @Override @@ -196,10 +161,7 @@ public void setTurnBrakeMode(boolean enable) { @Override public void stop() { - driveMotor.setVoltage(0.0); - turnMotor.setVoltage(0.0); - - speedSetpoint = null; - angleSetpoint = null; + driveMotor.stopMotor(); + turnMotor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java index fb3256aa..5589a0f9 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java @@ -41,11 +41,11 @@ public static class IntakeConstants { public static double reduction = (1.0 / 1.0); public static int id = switch (Constants.getRobot()) { - default -> 4; + default -> 45; }; public static boolean inverted = switch (Constants.getRobot()) { - default -> false; + default -> true; }; } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/Intake.java b/src/main/java/frc/robot/subsystems/superstructure/intake/Intake.java index 43faa0c9..72b26f16 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/Intake.java @@ -13,7 +13,8 @@ public class Intake extends SubsystemBase { private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - private boolean run = false; + private boolean intake = false; + private boolean eject = false; public Intake(IntakeIO io) { System.out.println("[Init] Creating Intake"); @@ -29,25 +30,49 @@ public void periodic() { if (DriverStation.isDisabled()) { stop(); - run = false; } else { - if (run) { + if (intake) { io.setVoltage(intakeVoltage.get()); + } else if (eject) { + io.setVoltage(-intakeVoltage.get()); } } } - private void run() { - run = true; + public boolean intaking() { + return intake; + } + + public boolean ejecting() { + return eject; + } + + public boolean running() { + return eject || intake; + } + + private void intake() { + intake = true; + eject = false; + } + + private void eject() { + eject = true; + intake = false; } private void stop() { - run = false; + intake = false; + eject = false; io.stop(); } - public Command runCommand() { - return Commands.runOnce(this::run); + public Command intakeCommand() { + return Commands.runOnce(this::intake); + } + + public Command ejectCommand() { + return Commands.runOnce(this::eject); } public Command stopCommand() { diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIOSparkMax.java index b7d2c775..fb3c9a66 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIOSparkMax.java @@ -13,7 +13,7 @@ public IntakeIOSparkMax() { motor = new CANSparkMax(id, CANSparkMax.MotorType.kBrushless); motor.restoreFactoryDefaults(); motor.setInverted(inverted); - motor.setSmartCurrentLimit(20); + motor.setSmartCurrentLimit(80); motor.enableVoltageCompensation(12.0); encoder = motor.getEncoder(); diff --git a/src/main/java/frc/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/superstructure/shooter/Shooter.java index 56781b62..d571c0e3 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/superstructure/shooter/Shooter.java @@ -10,129 +10,129 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class Shooter extends SubsystemBase { - private static final LoggedTunableNumber feedVolts = - new LoggedTunableNumber("Shooter/FeedVolts", 6.0); - private static final LoggedTunableNumber leftkP = - new LoggedTunableNumber("Shooter/leftkP", leftFlywheelConstants.kP()); - private static final LoggedTunableNumber leftkI = - new LoggedTunableNumber("Shooter/leftkI", leftFlywheelConstants.kI()); - private static final LoggedTunableNumber leftkD = - new LoggedTunableNumber("Shooter/leftkD", leftFlywheelConstants.kD()); - private static final LoggedTunableNumber leftkS = - new LoggedTunableNumber("Shooter/leftkS", leftFlywheelConstants.kS()); - private static final LoggedTunableNumber leftkV = - new LoggedTunableNumber("Shooter/leftkV", leftFlywheelConstants.kV()); - private static final LoggedTunableNumber leftkA = - new LoggedTunableNumber("Shooter/leftkA", leftFlywheelConstants.kA()); - private static final LoggedTunableNumber rightkP = - new LoggedTunableNumber("Shooter/rightkP", rightFlywheelConstants.kP()); - private static final LoggedTunableNumber rightkI = - new LoggedTunableNumber("Shooter/rightkI", rightFlywheelConstants.kI()); - private static final LoggedTunableNumber rightkD = - new LoggedTunableNumber("Shooter/rightkD", rightFlywheelConstants.kD()); - private static final LoggedTunableNumber rightkS = - new LoggedTunableNumber("Shooter/rightkS", rightFlywheelConstants.kS()); - private static final LoggedTunableNumber rightkV = - new LoggedTunableNumber("Shooter/rightkV", rightFlywheelConstants.kV()); - private static final LoggedTunableNumber rightkA = - new LoggedTunableNumber("Shooter/rightkA", rightFlywheelConstants.kA()); - private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Shooter/ToleranceRPM", shooterToleranceRPM); - private final LoggedDashboardNumber leftSpeedRpm = - new LoggedDashboardNumber("Left Speed RPM", 6000); - private final LoggedDashboardNumber rightSpeedRpm = - new LoggedDashboardNumber("Right Speed RPM", 4000); - - private final ShooterIO shooterIO; - private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); - - private boolean characterizing = false; - - public Shooter(ShooterIO io) { - System.out.println("[Init] Creating Shooter"); - shooterIO = io; - shooterIO.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()); - shooterIO.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()); - shooterIO.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()); - shooterIO.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()); - } - - @Override - public void periodic() { - // check controllers - LoggedTunableNumber.ifChanged( - hashCode(), - () -> shooterIO.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()), - leftkP, - leftkI, - leftkD); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> shooterIO.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()), - leftkS, - leftkV, - leftkA); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> shooterIO.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()), - rightkP, - rightkI, - rightkD); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> shooterIO.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()), - rightkS, - rightkV, - rightkA); - - shooterIO.updateInputs(shooterInputs); - Logger.processInputs("Shooter", shooterInputs); - - if (DriverStation.isDisabled()) { - shooterIO.stop(); + private static final LoggedTunableNumber feedVolts = + new LoggedTunableNumber("Shooter/FeedVolts", 6.0); + private static final LoggedTunableNumber leftkP = + new LoggedTunableNumber("Shooter/leftkP", leftFlywheelConstants.kP()); + private static final LoggedTunableNumber leftkI = + new LoggedTunableNumber("Shooter/leftkI", leftFlywheelConstants.kI()); + private static final LoggedTunableNumber leftkD = + new LoggedTunableNumber("Shooter/leftkD", leftFlywheelConstants.kD()); + private static final LoggedTunableNumber leftkS = + new LoggedTunableNumber("Shooter/leftkS", leftFlywheelConstants.kS()); + private static final LoggedTunableNumber leftkV = + new LoggedTunableNumber("Shooter/leftkV", leftFlywheelConstants.kV()); + private static final LoggedTunableNumber leftkA = + new LoggedTunableNumber("Shooter/leftkA", leftFlywheelConstants.kA()); + private static final LoggedTunableNumber rightkP = + new LoggedTunableNumber("Shooter/rightkP", rightFlywheelConstants.kP()); + private static final LoggedTunableNumber rightkI = + new LoggedTunableNumber("Shooter/rightkI", rightFlywheelConstants.kI()); + private static final LoggedTunableNumber rightkD = + new LoggedTunableNumber("Shooter/rightkD", rightFlywheelConstants.kD()); + private static final LoggedTunableNumber rightkS = + new LoggedTunableNumber("Shooter/rightkS", rightFlywheelConstants.kS()); + private static final LoggedTunableNumber rightkV = + new LoggedTunableNumber("Shooter/rightkV", rightFlywheelConstants.kV()); + private static final LoggedTunableNumber rightkA = + new LoggedTunableNumber("Shooter/rightkA", rightFlywheelConstants.kA()); + private static final LoggedTunableNumber shooterTolerance = + new LoggedTunableNumber("Shooter/ToleranceRPM", shooterToleranceRPM); + private final LoggedDashboardNumber leftSpeedRpm = + new LoggedDashboardNumber("Left Speed RPM", 6000); + private final LoggedDashboardNumber rightSpeedRpm = + new LoggedDashboardNumber("Right Speed RPM", 4000); + + private final ShooterIO shooterIO; + private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + + private boolean characterizing = false; + + public Shooter(ShooterIO io) { + System.out.println("[Init] Creating Shooter"); + shooterIO = io; + shooterIO.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()); + shooterIO.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()); + shooterIO.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()); + shooterIO.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()); + } + + @Override + public void periodic() { + // check controllers + LoggedTunableNumber.ifChanged( + hashCode(), + () -> shooterIO.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()), + leftkP, + leftkI, + leftkD); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> shooterIO.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()), + leftkS, + leftkV, + leftkA); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> shooterIO.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()), + rightkP, + rightkI, + rightkD); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> shooterIO.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()), + rightkS, + rightkV, + rightkA); + + shooterIO.updateInputs(shooterInputs); + Logger.processInputs("Shooter", shooterInputs); + + if (DriverStation.isDisabled()) { + shooterIO.stop(); + } else { + if (!characterizing) { + shooterIO.setRPM(leftSpeedRpm.get(), rightSpeedRpm.get()); + double feederSetpointVolts = 0.0; + if (leftSpeedRpm.get() > 0 && rightSpeedRpm.get() > 0 && atSetpoint()) { + feederSetpointVolts = feedVolts.get(); } else { - if (!characterizing) { - shooterIO.setRPM(leftSpeedRpm.get(), rightSpeedRpm.get()); - double feederSetpointVolts = 0.0; - if (leftSpeedRpm.get() > 0 && rightSpeedRpm.get() > 0 && atSetpoint()) { - feederSetpointVolts = feedVolts.get(); - } else { - feederSetpointVolts = 0; - } - shooterIO.setFeederVoltage(feederSetpointVolts); - } + feederSetpointVolts = 0; } - - Logger.recordOutput("Shooter/LeftRPM", shooterInputs.leftFlywheelVelocityRPM); - Logger.recordOutput("Shooter/RightRPM", shooterInputs.rightFlywheelVelocityRPM); - Logger.recordOutput("Shooter/FeederRPM", shooterInputs.feederVelocityRPM); - } - - public void runLeftCharacterizationVolts(double volts) { - shooterIO.setLeftCharacterizationVoltage(volts); - } - - public void runRightCharacterizationVolts(double volts) { - shooterIO.setRightCharacterizationVoltage(volts); + shooterIO.setFeederVoltage(feederSetpointVolts); + } } - public double getLeftCharacterizationVelocity() { - return shooterInputs.leftFlywheelVelocityRPM; - } - - public void setCharacterizing(boolean characterizing) { - this.characterizing = characterizing; - } - - public double getRightCharacterizationVelocity() { - return shooterInputs.rightFlywheelVelocityRPM; - } - - @AutoLogOutput(key = "Shooter/AtSetpoint") - public boolean atSetpoint() { - return Math.abs(shooterInputs.leftFlywheelVelocityRPM - leftSpeedRpm.get()) - <= shooterTolerance.get() - && Math.abs(shooterInputs.rightFlywheelVelocityRPM - rightSpeedRpm.get()) - <= shooterTolerance.get(); - } + Logger.recordOutput("Shooter/LeftRPM", shooterInputs.leftFlywheelVelocityRPM); + Logger.recordOutput("Shooter/RightRPM", shooterInputs.rightFlywheelVelocityRPM); + Logger.recordOutput("Shooter/FeederRPM", shooterInputs.feederVelocityRPM); + } + + public void runLeftCharacterizationVolts(double volts) { + shooterIO.setLeftCharacterizationVoltage(volts); + } + + public void runRightCharacterizationVolts(double volts) { + shooterIO.setRightCharacterizationVoltage(volts); + } + + public double getLeftCharacterizationVelocity() { + return shooterInputs.leftFlywheelVelocityRPM; + } + + public void setCharacterizing(boolean characterizing) { + this.characterizing = characterizing; + } + + public double getRightCharacterizationVelocity() { + return shooterInputs.rightFlywheelVelocityRPM; + } + + @AutoLogOutput(key = "Shooter/AtSetpoint") + public boolean atSetpoint() { + return Math.abs(shooterInputs.leftFlywheelVelocityRPM - leftSpeedRpm.get()) + <= shooterTolerance.get() + && Math.abs(shooterInputs.rightFlywheelVelocityRPM - rightSpeedRpm.get()) + <= shooterTolerance.get(); + } } diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 80b6e20c..22378fd3 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -4,17 +4,17 @@ import java.util.List; public abstract class VirtualSubsystem { - private static List subsystems = new ArrayList<>(); + private static List subsystems = new ArrayList<>(); - public VirtualSubsystem() { - subsystems.add(this); - } + public VirtualSubsystem() { + subsystems.add(this); + } - public static void periodicAll() { - for (VirtualSubsystem subsystem : subsystems) { - subsystem.periodic(); - } + public static void periodicAll() { + for (VirtualSubsystem subsystem : subsystems) { + subsystem.periodic(); } + } - public abstract void periodic(); + public abstract void periodic(); }