diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fecb0825..cf5239cc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,12 +39,10 @@ import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; - import java.io.File; import java.util.Objects; import java.util.Optional; import java.util.function.Function; - import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -54,209 +52,192 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state - 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]), - false); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - 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]), - false); - 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() { - }, - false); - shooter = new Shooter(new ShooterIO() { - }); - intake = new Intake(new IntakeIO() { - }); - } - } - - if (drive == null) { + // Load robot state + 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 GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - false); - } - - if (aprilTagVision == null) { - aprilTagVision = new AprilTagVision(); - } - - if (shooter == null) { - shooter = new Shooter(new ShooterIO() { - }); - } - - if (intake == null) { - intake = new Intake(new IntakeIO() { - }); + 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]), + false); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + 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]), + false); + 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() {}, + false); + shooter = new Shooter(new ShooterIO() {}); + 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))); + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + false); + } - // Testing autos paths - Function> trajectoryCommandFactory = - trajectoryFile -> { - Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); - return trajectory.map( - traj -> - Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.followTrajectory(traj))); - }; - 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)); - } + if (aprilTagVision == null) { + aprilTagVision = new AprilTagVision(); + } - // Configure the button bindings - configureButtonBindings(); + if (shooter == null) { + shooter = new Shooter(new ShooterIO() {}); } - /** - * 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() - .whileTrue( - drive.autoAlignToPose( - () -> - AllianceFlipUtil.apply( - new Pose2d( - new Translation2d( - FieldConstants.ampCenter.getX(), - FieldConstants.ampCenter.getY() - - DriveConstants.driveConfig.trackwidthY() / 2.0), - new Rotation2d(-Math.PI / 2.0))))); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - // 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()))); + if (intake == null) { + intake = new Intake(new IntakeIO() {}); } - /** - * 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(); + 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))); + + // Testing autos paths + Function> trajectoryCommandFactory = + trajectoryFile -> { + Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + return trajectory.map( + traj -> + Commands.runOnce( + () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + .andThen(drive.followTrajectory(traj))); + }; + 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() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() + - DriveConstants.driveConfig.trackwidthY() / 2.0), + new Rotation2d(-Math.PI / 2.0))))); + // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); + // 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/RobotState.java b/src/main/java/frc/robot/RobotState.java index 5dfd195c..1aa3a850 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -56,7 +56,6 @@ private RobotState() { /** Add odometry observation */ public void addOdometryObservation(OdometryObservation observation) { - Pose2d lastOdometryPose = odometryPose; Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); lastWheelPositions = observation.wheelPositions(); // Check gyro connected @@ -72,7 +71,7 @@ public void addOdometryObservation(OdometryObservation observation) { // Add pose to buffer at timestamp poseBuffer.addSample(observation.timestamp(), odometryPose); // Calculate diff from last odometry pose and add onto pose estimate - estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose)); + estimatedPose = estimatedPose.exp(twist); } public void addVisionObservation(VisionObservation observation) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 83b3c7da..6d1e3f3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -43,13 +43,14 @@ @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - private static final LoggedTunableNumber coastSpeedLimit = new LoggedTunableNumber( + private static final LoggedTunableNumber coastSpeedLimit = + new LoggedTunableNumber( "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); - private static final LoggedTunableNumber coastDisableTime = new LoggedTunableNumber( - "Drive/CoastDisableTimeSeconds", 0.5); + private static final LoggedTunableNumber coastDisableTime = + new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); @AutoLog - public static class OdometryTimeestampInputs { + public static class OdometryTimestampInputs { public double[] timestamps = new double[] {}; } @@ -57,14 +58,16 @@ public static class OdometryTimeestampInputs { // TODO: DO THIS BETTER! public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); - private final OdometryTimeestampInputsAutoLogged odometryTimestampInputs = - new OdometryTimeestampInputsAutoLogged(); + private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimestampInputsAutoLogged(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; private boolean modulesOrienting = false; private boolean characterizing = false; + private final Timer coastTimer = new Timer(); + private boolean brakeModeEnabled = true; private double characterizationVolts = 0.0; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; @@ -82,11 +85,13 @@ public static class OdometryTimeestampInputs { private final TrajectoryMotionPlanner trajectoryMotionPlanner; private final AutoAlignMotionPlanner autoAlignMotionPlanner; - private final Timer coastTimer = new Timer(); - private boolean shouldCoast = false; - - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br, - boolean useMotorConroller) { + public Drive( + GyroIO gyroIO, + ModuleIO fl, + ModuleIO fr, + ModuleIO bl, + ModuleIO br, + boolean useMotorConroller) { System.out.println("[Init] Creating Drive"); this.gyroIO = gyroIO; modules[0] = new Module(fl, 0, useMotorConroller); @@ -153,8 +158,7 @@ public void periodic() { } // update current velocities use gyro when possible - ChassisSpeeds robotRelativeVelocity = - DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + ChassisSpeeds robotRelativeVelocity = getSpeeds(); robotRelativeVelocity.omegaRadiansPerSecond = gyroInputs.connected ? gyroInputs.yawVelocityRadPerSec @@ -164,22 +168,20 @@ public void periodic() { // Disabled, stop modules and coast if (DriverStation.isDisabled()) { Arrays.stream(modules).forEach(Module::stop); - if (Math.hypot(robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) <= coastSpeedLimit.get()) { + if (Math.hypot( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) + <= coastSpeedLimit.get() + && brakeModeEnabled) { setBrakeMode(false); coastTimer.stop(); coastTimer.reset(); - } else if (coastTimer.hasElapsed(coastDisableTime.get())) { + } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { setBrakeMode(false); coastTimer.stop(); coastTimer.reset(); } else { coastTimer.start(); } - // Clear logs - Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); - Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); - Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); - Logger.recordOutput("Drive/SetpointSpeeds", new double[] {}); return; } @@ -188,21 +190,11 @@ public void periodic() { for (Module module : modules) { module.runCharacterization(characterizationVolts); } - // Clear logs - Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); - Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); - Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); - Logger.recordOutput("Drive/SetpointSpeeds", new double[] {}); return; } // Skip if orienting modules if (modulesOrienting) { - // Clear logs - Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); - Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); - Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); - Logger.recordOutput("Drive/SetpointSpeeds", new double[] {}); return; } @@ -258,6 +250,7 @@ public double getCharacterizationVelocity() { /** Set brake mode enabled */ public void setBrakeMode(boolean enabled) { + brakeModeEnabled = enabled; Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } @@ -267,14 +260,21 @@ public Command orientModules(Rotation2d orientation) { public Command orientModules(Rotation2d[] orientations) { return run(() -> { - for (int i = 0; i < orientations.length; i++) { - modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); - }}) - .beforeStarting(() -> modulesOrienting = true) - .until(() -> Arrays.stream(modules).allMatch(module -> - Math.abs( - module.getAngle().getDegrees() - module.getSetpointState().angle.getDegrees()) <= 2.0)) - .andThen(() -> modulesOrienting = false); + for (int i = 0; i < orientations.length; i++) { + modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); + } + }) + .until( + () -> + Arrays.stream(modules) + .allMatch( + module -> + Math.abs( + module.getAngle().getDegrees() + - module.getSetpointState().angle.getDegrees()) + <= 2.0)) + .beforeStarting(() -> modulesOrienting = true) + .finallyDo(() -> modulesOrienting = false); } /** Follows a trajectory using the trajectory motion planner. */ @@ -304,6 +304,12 @@ private SwerveModuleState[] getModuleStates() { return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } + /** Returns the measured speeds of the robot in the robot's frame of reference. */ + @AutoLogOutput(key = "Drive/MeasuredSpeeds") + private ChassisSpeeds getSpeeds() { + return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + } + public static Rotation2d[] getStraightOrientations() { return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index aa9480be..f0d858ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -11,232 +11,233 @@ import frc.robot.Constants; import frc.robot.util.swerve.ModuleLimits; -/** - * All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) - */ +/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - // TODO: get effective wheel radius - public static final double wheelRadius = Units.inchesToMeters(2.0); - - // For Kraken - public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = true; - public static final boolean useMotionMagic = false; - public static final double motionMagicCruiseVelocity = 0.0; - public static final double motionMagicAcceleration = 0.0; - } - - // Drive Constants - public static DriveConfig driveConfig = - switch (Constants.getRobot()) { - case SIMBOT, COMPBOT -> new DriveConfig( - Units.inchesToMeters(25.0), - Units.inchesToMeters(25.0), - Units.feetToMeters(13.05), - Units.feetToMeters(30.02), - 8.86, - 43.97); - default -> new DriveConfig( - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(12.16), - Units.feetToMeters(21.32), - 7.93, - 29.89); - }; - public static final Translation2d[] moduleTranslations = - new Translation2d[]{ - new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), - new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0), - new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), - new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) - }; - public static final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics(moduleTranslations); - - // Odometry Constants - public static final double odometryFrequency = - switch (Constants.getRobot()) { - case SIMBOT -> 50.0; - case RAINBOWT -> 100.0; - case COMPBOT -> 250.0; - }; - - public static final Matrix odometryStateStdDevs = - switch (Constants.getRobot()) { - default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); - }; - - // Module Constants - public static ModuleConfig[] moduleConfigs = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> new ModuleConfig[]{ - new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), - new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), - new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), - new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) - }; - case SIMBOT -> { - ModuleConfig[] configs = new ModuleConfig[4]; - for (int i = 0; i < configs.length; i++) - configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); - yield configs; - } + // TODO: get effective wheel radius + public static final double wheelRadius = Units.inchesToMeters(2.0); + + // For Kraken + public static class KrakenDriveConstants { + public static final boolean useTorqueCurrentFOC = true; + public static final boolean useMotionMagic = false; + public static final double motionMagicCruiseVelocity = 0.0; + public static final double motionMagicAcceleration = 0.0; + } + + // Drive Constants + public static DriveConfig driveConfig = + switch (Constants.getRobot()) { + case SIMBOT, COMPBOT -> + new DriveConfig( + Units.inchesToMeters(25.0), + Units.inchesToMeters(25.0), + Units.feetToMeters(13.05), + Units.feetToMeters(30.02), + 8.86, + 43.97); + default -> + new DriveConfig( + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) + }; + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(moduleTranslations); + + // Odometry Constants + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case RAINBOWT -> 100.0; + case COMPBOT -> 250.0; + }; + + public static final Matrix odometryStateStdDevs = + switch (Constants.getRobot()) { + default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); + }; + + // Module Constants + public static ModuleConfig[] moduleConfigs = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new ModuleConfig[] { + new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), + new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), + new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), + new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) }; - - public static ModuleConstants moduleConstants = - switch (Constants.getRobot()) { - case COMPBOT -> new ModuleConstants( - 1.0, - 0.0, - 0.0, - 0.0, - 200.0, - 20.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - case RAINBOWT -> new ModuleConstants( - 0.1, - 0.13, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L2.reduction, - Mk4iReductions.TURN.reduction); - case SIMBOT -> new ModuleConstants( - 0.014, - 0.134, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - }; - - public static ModuleLimits moduleLimits = - new ModuleLimits( - driveConfig.maxLinearVelocity(), - driveConfig.maxLinearVelocity() * 5, - Units.degreesToRadians(1080.0)); - - // Trajectory Following - public static TrajectoryConstants trajectoryConstants = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> new TrajectoryConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - case SIMBOT -> new TrajectoryConstants( - 4.0, - 0.0, - 2.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - }; - - // Auto Align - public static AutoAlignConstants autoAlignConstants = - new AutoAlignConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(2.0), - Units.degreesToRadians(2.0), - driveConfig.maxLinearVelocity() * 0.6, - driveConfig.maxLinearAcceleration() * 0.5, - driveConfig.maxAngularVelocity() * 0.3, - driveConfig.maxAngularAcceleration() * 0.5); - - // Swerve Heading Control - public static HeadingControllerConstants headingControllerConstants = - switch (Constants.getRobot()) { - default -> new HeadingControllerConstants(3.0, 0.0); - }; - - public record DriveConfig( - double trackwidthX, - double trackwidthY, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - public double driveBaseRadius() { - return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); + yield configs; } + }; + + public static ModuleConstants moduleConstants = + switch (Constants.getRobot()) { + case COMPBOT -> + new ModuleConstants( + 2.0, + 0.0, + 200.0, + 0.0, + 200.0, + 20.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + case RAINBOWT -> + new ModuleConstants( + 0.1, + 0.13, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + case SIMBOT -> + new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + }; + + public static ModuleLimits moduleLimits = + new ModuleLimits( + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearVelocity() * 5, + Units.degreesToRadians(1080.0)); + + // Trajectory Following + public static TrajectoryConstants trajectoryConstants = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new TrajectoryConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + case SIMBOT -> + new TrajectoryConstants( + 4.0, + 0.0, + 2.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + }; + + // Auto Align + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + + // Swerve Heading Control + public static HeadingControllerConstants headingControllerConstants = + switch (Constants.getRobot()) { + default -> new HeadingControllerConstants(3.0, 0.0); + }; + + public record DriveConfig( + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); } - - public record ModuleConfig( - int driveID, - int turnID, - int absoluteEncoderChannel, - Rotation2d absoluteEncoderOffset, - boolean turnMotorInverted) { - } - - public record ModuleConstants( - double ffKs, - double ffKv, - double driveKp, - double driveKd, - double turnKp, - double turnKd, - double driveReduction, - double turnReduction) { - } - - public record TrajectoryConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double goalLinearTolerance, - double goalThetaTolerance, - double linearVelocityTolerance, - double angularVelocityTolerance) { - } - - public record AutoAlignConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - } - - public record HeadingControllerConstants(double Kp, double Kd) { - } - - private enum Mk4iReductions { - L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), - L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), - TURN((150.0 / 7.0)); - - final double reduction; - - Mk4iReductions(double reduction) { - this.reduction = reduction; - } + } + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) {} + + public record ModuleConstants( + double ffKs, + double ffKv, + double driveKp, + double driveKd, + double turnKp, + double turnKd, + double driveReduction, + double turnReduction) {} + + public record TrajectoryConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double goalLinearTolerance, + double goalThetaTolerance, + double linearVelocityTolerance, + double angularVelocityTolerance) {} + + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + + public record HeadingControllerConstants(double Kp, double Kd) {} + + private enum Mk4iReductions { + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + TURN((150.0 / 7.0)); + + final double reduction; + + Mk4iReductions(double reduction) { + this.reduction = reduction; } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 87ddb0b8..fbfde029 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -38,8 +38,7 @@ public class Module { new SimpleMotorFeedforward( DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); - @Getter - private SwerveModuleState setpointState = new SwerveModuleState(); + @Getter private SwerveModuleState setpointState = new SwerveModuleState(); public Module(ModuleIO io, int index, boolean useMotorController) { this.io = io; @@ -56,22 +55,18 @@ public void updateInputs() { // Update FF and controllers LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); - io.setDriveFF(driveKs.get(), driveKv.get(), 0); - }, - driveKs, - driveKv); + hashCode(), + () -> { + driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); + io.setDriveFF(driveKs.get(), driveKv.get(), 0); + }, + driveKs, + driveKv); if (useMotorController) { LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), - driveKp, driveKd); + hashCode(), () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), driveKp, driveKd); LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), - turnKp, turnKd); + hashCode(), () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), turnKp, turnKd); } else { driveController.setPID(driveKp.get(), 0, driveKd.get()); turnController.setPID(turnKp.get(), 0, turnKd.get()); @@ -81,18 +76,19 @@ public void updateInputs() { public void runSetpoint(SwerveModuleState setpoint) { setpointState = setpoint; if (useMotorController) { - io.setDriveVelocitySetpoint(setpoint.speedMetersPerSecond, - driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setDriveVelocitySetpoint( + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius, + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); io.setTurnPositionSetpoint(setpoint.angle.getRadians()); } else { io.setDriveVoltage( - driveController.calculate( - inputs.driveVelocityRadPerSec, - setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) - + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + driveController.calculate( + inputs.driveVelocityRadPerSec, + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) + + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); io.setTurnVoltage( - turnController.calculate( - inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + turnController.calculate( + inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 40158763..d60f0e8c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,86 +17,55 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - 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 = 0.0; - - public double[] odometryDrivePositionsMeters = new double[]{}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; - } - - /** - * Updates the set of loggable inputs. - */ - default void updateInputs(ModuleIOInputs inputs) { - } - - /** - * Run drive motor at volts - */ - default void setDriveVoltage(double volts) { - } - - /** - * Run turn motor at volts - */ - default void setTurnVoltage(double volts) { - } - - /** - * Set drive velocity setpoint - */ - default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - } - - /** - * Set turn position setpoint - */ - default void setTurnPositionSetpoint(double angleRads) { - } - - /** - * Configure drive PID - */ - default void setDrivePID(double kP, double kI, double kD) { - } - - /** - * Configure turn PID - */ - default void setTurnPID(double kP, double kI, double kD) { - } - - /** - * Configure drive feedforward for TorqueCurrentFOC - */ - default void setDriveFF(double kS, double kV, double kA) { - } - - /** - * Enable or disable brake mode on the drive motor. - */ - default void setDriveBrakeMode(boolean enable) { - } - - /** - * Enable or disable brake mode on the turn motor. - */ - default void setTurnBrakeMode(boolean enable) { - } - - /** - * Disable output to all motors - */ - default void stop() { - } + @AutoLog + class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTorqueCurrentAmps = 0.0; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTorqueCurrentAmps = 0.0; + + public double[] odometryDrivePositionsMeters = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + default void updateInputs(ModuleIOInputs inputs) {} + + /** Run drive motor at volts */ + default void setDriveVoltage(double volts) {} + + /** Run turn motor at volts */ + default void setTurnVoltage(double volts) {} + + /** Set drive velocity setpoint */ + default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} + + /** Set turn position setpoint */ + default void setTurnPositionSetpoint(double angleRads) {} + + /** Configure drive PID */ + default void setDrivePID(double kP, double kI, double kD) {} + + /** Configure turn PID */ + default void setTurnPID(double kP, double kI, double kD) {} + + /** Configure drive feedforward for TorqueCurrentFOC */ + default void setDriveFF(double kS, double kV, double kA) {} + + /** Enable or disable brake mode on the drive motor. */ + default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + default void setTurnBrakeMode(boolean enable) {} + + /** Disable output to all motors */ + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index 15fbd768..ee06881d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -15,250 +15,252 @@ 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 { - // Hardware - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final AnalogInput turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; - - // Status Signals - 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; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private static final int shouldResetCounts = 100; - private int resetCounter = shouldResetCounts; - - private Slot0Configs driveFeedbackConfig = new Slot0Configs(); - private Slot0Configs turnFeedbackConfig = new Slot0Configs(); - - public ModuleIOKrakenFOC(ModuleConfig config) { - // Init controllers and encoders from config constants - driveTalon = new TalonFX(config.driveID(), "canivore"); - turnTalon = new TalonFX(config.turnID(), "canivore"); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - - // Config Motors - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.Voltage.PeakForwardVoltage = 12.0; - driveConfig.Voltage.PeakReverseVoltage = -12.0; - 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; - turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; - turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; - turnConfig.MotorOutput.Inverted = config.turnMotorInverted() ? - InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - - // If in motoControl mode, set reference points in rotations convert from radians - // Affects getPosition() and getVelocity() - driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); - turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - - // Config Motion Magic - if (KrakenDriveConstants.useMotionMagic) { - turnConfig.MotionMagic.MotionMagicCruiseVelocity = KrakenDriveConstants.motionMagicCruiseVelocity; - turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; - } - - // 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 - // 100hz signals - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - turnAbsolutePosition = - () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) - .minus(absoluteEncoderOffset); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - // 250hz signals - drivePosition = driveTalon.getPosition(); - turnPosition = turnTalon.getPosition(); - BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - - // TODO: what does this do? -// driveTalon.optimizeBusUtilization(); -// turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCounts - && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { - turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); - resetCounter = 0; - } - - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - - inputs.turnAbsolutePosition = turnAbsolutePosition.get(); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map(Rotation2d::fromRotations) - .toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts) - .withEnableFOC(true)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts) - .withEnableFOC(true)); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); - } else { - driveTalon.setControl(new VelocityVoltage(velocityRotationsPerSec) - .withFeedForward(ffVolts) - .withEnableFOC(true)); - } - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - double angleRotations = Units.radiansToRotations(angleRads); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); - } else { - turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); - } - } else { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicVoltage(angleRotations) - .withEnableFOC(true)); - } else { - turnTalon.setControl(new PositionVoltage(angleRotations) - .withEnableFOC(true)); - } - } - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveFeedbackConfig.kP = kP; - driveFeedbackConfig.kI = kI; - driveFeedbackConfig.kD = kD; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnFeedbackConfig.kP = kP; - turnFeedbackConfig.kI = kI; - turnFeedbackConfig.kD = kD; - turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + // Hardware + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private static final int shouldResetCounts = 100; + private int resetCounter = shouldResetCounts; + + private Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + + public ModuleIOKrakenFOC(ModuleConfig config) { + // Init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID(), "canivore"); + turnTalon = new TalonFX(config.turnID(), "canivore"); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + // Config Motors + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + turnConfig.MotorOutput.Inverted = + config.turnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + + // If in motoControl mode, set reference points in rotations convert from radians + // Affects getPosition() and getVelocity() + driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + + // Config Motion Magic + if (KrakenDriveConstants.useMotionMagic) { + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + KrakenDriveConstants.motionMagicCruiseVelocity; + turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; } - @Override - public void setDriveFF(double kS, double kV, double kA) { - driveFeedbackConfig.kS = kS; - driveFeedbackConfig.kV = kV; - driveFeedbackConfig.kA = kA; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + // 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; } - @Override - public void setDriveBrakeMode(boolean enable) { - driveTalon.setNeutralMode(enable ? - NeutralModeValue.Brake : NeutralModeValue.Coast); + // Get signals and set update rate + // 100hz signals + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTorqueCurrent = driveTalon.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTorqueCurrent = turnTalon.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + // 250hz signals + drivePosition = driveTalon.getPosition(); + turnPosition = turnTalon.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + // TODO: what does this do? + // driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCounts + && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); + resetCounter = 0; } - @Override - public void setTurnBrakeMode(boolean enable) { - turnTalon.setNeutralMode(enable ? - NeutralModeValue.Brake : NeutralModeValue.Coast); + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTorqueCurrentAmps = driveTorqueCurrent.getValueAsDouble(); + + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTorqueCurrentAmps = turnTorqueCurrent.getValueAsDouble(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble(signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); + } else { + driveTalon.setControl( + new VelocityVoltage(velocityRotationsPerSec) + .withFeedForward(ffVolts) + .withEnableFOC(true)); } - - @Override - public void stop() { - driveTalon.setControl(new NeutralOut()); - turnTalon.setControl(new NeutralOut()); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + double angleRotations = Units.radiansToRotations(angleRads); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); + } else { + turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); + } + } else { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); + } else { + turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); + } } + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedbackConfig.kP = kP; + driveFeedbackConfig.kI = kI; + driveFeedbackConfig.kD = kD; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedbackConfig.kP = kP; + turnFeedbackConfig.kI = kI; + turnFeedbackConfig.kD = kD; + turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + } + + @Override + public void setDriveFF(double kS, double kV, double kA) { + driveFeedbackConfig.kS = kS; + driveFeedbackConfig.kV = kV; + driveFeedbackConfig.kA = kA; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void stop() { + driveTalon.setControl(new NeutralOut()); + turnTalon.setControl(new NeutralOut()); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 6f72e9bc..e0ca78cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -24,9 +24,9 @@ public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.driveReduction(), 0.025); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); private final DCMotorSim turnSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.turnReduction(), 0.004); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.turnReduction(), 0.004); private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); @@ -48,14 +48,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); inputs.odometryDrivePositionsMeters = new double[] {driveSim.getAngularPositionRad() * wheelRadius}; @@ -76,14 +76,13 @@ public void setTurnVoltage(double volts) { @Override public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { setDriveVoltage( - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) - + ffVolts); + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) + + ffVolts); } @Override public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage( - turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index d4a29f16..4f688368 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -22,193 +22,191 @@ 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 { - // Hardware - private final CANSparkMax driveMotor; - private final CANSparkMax turnMotor; - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - - // Controllers - private final SparkPIDController driveController; - private final SparkPIDController turnController; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final Rotation2d absoluteEncoderOffset; - private final Supplier absoluteEncoderValue; - private static final int shouldResetCount = 100; - private int resetCounter = shouldResetCount; - - public ModuleIOSparkMax(ModuleConfig config) { - // Init motor & encoder objects - driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); - turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - driveEncoder = driveMotor.getEncoder(); - turnRelativeEncoder = turnMotor.getEncoder(); - - driveController = driveMotor.getPIDController(); - turnController = turnMotor.getPIDController(); - turnController.setPositionPIDWrappingEnabled(true); - final double maxInput = Math.PI * moduleConstants.driveReduction(); - turnController.setPositionPIDWrappingMinInput(-maxInput); - turnController.setPositionPIDWrappingMaxInput(maxInput); - - driveMotor.restoreFactoryDefaults(); - turnMotor.restoreFactoryDefaults(); - driveMotor.setCANTimeout(250); - turnMotor.setCANTimeout(250); - - for (int i = 0; i < 4; i++) { - turnMotor.setInverted(config.turnMotorInverted()); - driveMotor.setSmartCurrentLimit(40); - turnMotor.setSmartCurrentLimit(30); - driveMotor.enableVoltageCompensation(12.0); - turnMotor.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - - if (driveMotor.burnFlash().equals(REVLibError.kOk) - && turnMotor.burnFlash().equals(REVLibError.kOk)) break; - } - driveMotor.setCANTimeout(0); - turnMotor.setCANTimeout(0); - - absoluteEncoderValue = - () -> Rotation2d.fromRotations( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V()) - .minus(absoluteEncoderOffset); - - drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + // Hardware + private final CANSparkMax driveMotor; + private final CANSparkMax turnMotor; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + + // Controllers + private final SparkPIDController driveController; + private final SparkPIDController turnController; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final Rotation2d absoluteEncoderOffset; + private final Supplier absoluteEncoderValue; + private static final int shouldResetCount = 100; + private int resetCounter = shouldResetCount; + + public ModuleIOSparkMax(ModuleConfig config) { + // Init motor & encoder objects + driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); + turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + driveEncoder = driveMotor.getEncoder(); + turnRelativeEncoder = turnMotor.getEncoder(); + + driveController = driveMotor.getPIDController(); + turnController = turnMotor.getPIDController(); + turnController.setPositionPIDWrappingEnabled(true); + final double maxInput = Math.PI * moduleConstants.driveReduction(); + turnController.setPositionPIDWrappingMinInput(-maxInput); + turnController.setPositionPIDWrappingMaxInput(maxInput); + + driveMotor.restoreFactoryDefaults(); + turnMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(250); + turnMotor.setCANTimeout(250); + + for (int i = 0; i < 4; i++) { + turnMotor.setInverted(config.turnMotorInverted()); + driveMotor.setSmartCurrentLimit(40); + turnMotor.setSmartCurrentLimit(30); + driveMotor.enableVoltageCompensation(12.0); + turnMotor.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + + if (driveMotor.burnFlash().equals(REVLibError.kOk) + && turnMotor.burnFlash().equals(REVLibError.kOk)) break; } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCount - && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { - for (int i = 0; i < 4; i++) { - turnRelativeEncoder.setPosition( - absoluteEncoderValue.get().getRotations() - * moduleConstants.turnReduction()); - } - resetCounter = 0; - } - - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - driveEncoder.getVelocity() / moduleConstants.driveReduction()); - inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); - - inputs.turnAbsolutePosition = absoluteEncoderValue.get(); - 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 = turnMotor.getOutputCurrent(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - motorPositionRevs -> - Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) - * wheelRadius) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); + + absoluteEncoderValue = + () -> + Rotation2d.fromRotations( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) + .minus(absoluteEncoderOffset); + + drivePositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCount + && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { + for (int i = 0; i < 4; i++) { + turnRelativeEncoder.setPosition( + absoluteEncoderValue.get().getRotations() * moduleConstants.turnReduction()); + } + resetCounter = 0; } - @Override - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() / moduleConstants.driveReduction()); + inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); + inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); + + inputs.turnAbsolutePosition = absoluteEncoderValue.get(); + 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.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + motorPositionRevs -> + Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) + * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnMotor.setVoltage(volts); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + driveController.setReference( + Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), + CANSparkBase.ControlType.kVelocity, + 0, + ffVolts, + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + turnController.setReference( + Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), + CANSparkBase.ControlType.kPosition); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + driveController.setP(kP); + driveController.setI(kI); + driveController.setD(kD); } - - @Override - public void setTurnVoltage(double volts) { - turnMotor.setVoltage(volts); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - driveController.setReference( - Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), - CANSparkBase.ControlType.kVelocity, - 0, - ffVolts, - SparkPIDController.ArbFFUnits.kVoltage); - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - turnController.setReference( - Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), - CANSparkBase.ControlType.kPosition); - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - for (int i = 0; i < 4; i++) { - driveController.setP(kP); - driveController.setI(kI); - driveController.setD(kD); - } - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - for (int i = 0; i < 4; i++) { - turnController.setP(kP); - turnController.setI(kI); - turnController.setD(kD); - } - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void stop() { - driveMotor.stopMotor(); - turnMotor.stopMotor(); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + turnController.setP(kP); + turnController.setI(kI); + turnController.setD(kD); } + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void stop() { + driveMotor.stopMotor(); + turnMotor.stopMotor(); + } }