diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bde88b0..8445897 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -2,16 +2,15 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { - public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Rebuilt"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 3; - public static final String GIT_SHA = "c2ce782ddc4a2e3387dbdfd9c896ce84e806aabf"; - public static final String GIT_DATE = "2026-01-20 14:12:18 EST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2026-01-20 14:26:55 EST"; - public static final long BUILD_UNIX_TIME = 1768937215602L; + public static final int GIT_REVISION = 18; + public static final String GIT_SHA = "a08c18aa3742a8182231092667a5c3ca832602aa"; + public static final String GIT_DATE = "2026-01-22 13:45:30 EST"; + public static final String GIT_BRANCH = "spindexer"; + public static final String BUILD_DATE = "2026-01-30 12:59:27 EST"; + public static final long BUILD_UNIX_TIME = 1769795967688L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccc1150..9e5526a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,9 @@ import frc.robot.constants.Constants; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.spindexer.SpindexerIOSim; +import frc.robot.subsystems.spindexer.SpindexerIOTalonFX; import frc.robot.util.Telemetry; import frc.robot.util.TunerConstants; import org.ironmaple.simulation.SimulatedArena; @@ -32,16 +35,19 @@ public class RobotContainer { // Subsystems public static CommandSwerveDrivetrain drivetrain; + public static Spindexer spindexer; public RobotContainer() { switch (Constants.currentMode) { case REAL: + spindexer = new Spindexer(new SpindexerIOTalonFX()); drivetrain = TunerConstants.createDrivetrain(); break; case SIM: + spindexer = new Spindexer(new SpindexerIOSim()); drivetrain = TunerConstants.createDrivetrain(); driveSimulation = drivetrain.mapleSimSwerveDrivetrain.mapleSimDrive; diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index fccc6ad..2ee0d95 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -15,22 +15,16 @@ public class DriveConstants { - public static final Distance bumperWidth = Inches.of(0); // TODO: set - - public static final LinearVelocity MAX_SPEED = TunerConstants.kSpeedAt12Volts; // TODO: set - public static final LinearAcceleration MAX_ACCELERATION = - MetersPerSecondPerSecond.of(12); // TODO: set + public static final Distance bumperWidth = Inches.of(0); // TODO: set - public static final AngularVelocity MAX_ANGULAR_SPEED = - RotationsPerSecond.of(4); // TODO: set - public static final AngularAcceleration MAX_ANGULAR_ACCELERATION = - RotationsPerSecondPerSecond.of(9); // TODO: set + public static final LinearVelocity MAX_SPEED = TunerConstants.kSpeedAt12Volts; // TODO: set + public static final LinearAcceleration MAX_ACCELERATION = + MetersPerSecondPerSecond.of(12); // TODO: set - public static final PathConstraints DEFAULT_CONSTRAINTS = - new PathConstraints( - MAX_SPEED, - MAX_ACCELERATION, - MAX_ANGULAR_SPEED, - MAX_ANGULAR_ACCELERATION - ); + public static final AngularVelocity MAX_ANGULAR_SPEED = RotationsPerSecond.of(4); // TODO: set + public static final AngularAcceleration MAX_ANGULAR_ACCELERATION = + RotationsPerSecondPerSecond.of(9); // TODO: set + + public static final PathConstraints DEFAULT_CONSTRAINTS = + new PathConstraints(MAX_SPEED, MAX_ACCELERATION, MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCELERATION); } diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java new file mode 100644 index 0000000..65a7d1b --- /dev/null +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -0,0 +1,13 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.Slot0Configs; + +public class SpindexerConstants { + public static final int CAN_ID = 0; + public static final Slot0Configs SLOT0_CONFIGS = + new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0).withKG(0); + + public static final double MOMENT_OF_INERTIA = 0; + public static final double GEARING = 1; + public static final double SPIN_VOLTAGE = 0.0; +} diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index 501757d..5bd493b 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -7,27 +7,29 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.ImmutableAngularAcceleration; -import edu.wpi.first.units.measure.ImmutableAngularVelocity; public class TurretConstants { - // TODO: IMPLEMENT - public static final int CAN_ID = -1; - public static final Angle MAX_ANGLE = Rotations.of(1); - public static final double GEAR_RATIO = 0; // how many rotations of the output shaft per rotation of the turret - public static final double MOMENT_OF_INERTIA = 0; - public static final double LENGTH = 0; - public static final double SIM_KP = 0; - public static final double SIM_KI = 0; - public static final double SIM_KD = 0; - public static final double TALON_KS = 0; - public static final double TALON_KV = 0; - public static final double TALON_KA = 0; - public static final double TALON_KP = 0; - public static final double TALON_KI = 0; - public static final double TALON_KD = 0; - public static final AngularVelocity TURRET_CRUISE_VELOCITY = RotationsPerSecond.of(4); // target cruise velocity of the turret, 4 rps - public static final AngularAcceleration TURRET_ACCELERATION = RotationsPerSecondPerSecond.of(16);// target acceleration of the turret, 16 rps^2 - public static final double TURRET_JERK = 160; // target jerk of the turret, _160 rps^3 -> there's no AngularJerk sadly - public static final Angle ANGLE_TOLERANCE = Rotations.of(0.001); + // TODO: IMPLEMENT + public static final int CAN_ID = -1; + public static final Angle MAX_ANGLE = Rotations.of(1); + public static final double GEAR_RATIO = + 0; // how many rotations of the output shaft per rotation of the turret + public static final double MOMENT_OF_INERTIA = 0; + public static final double LENGTH = 0; + public static final double SIM_KP = 0; + public static final double SIM_KI = 0; + public static final double SIM_KD = 0; + public static final double TALON_KS = 0; + public static final double TALON_KV = 0; + public static final double TALON_KA = 0; + public static final double TALON_KP = 0; + public static final double TALON_KI = 0; + public static final double TALON_KD = 0; + public static final AngularVelocity TURRET_CRUISE_VELOCITY = + RotationsPerSecond.of(4); // target cruise velocity of the turret, 4 rps + public static final AngularAcceleration TURRET_ACCELERATION = + RotationsPerSecondPerSecond.of(16); // target acceleration of the turret, 16 rps^2 + public static final double TURRET_JERK = + 160; // target jerk of the turret, _160 rps^3 -> there's no AngularJerk sadly + public static final Angle ANGLE_TOLERANCE = Rotations.of(0.001); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 66d4804..bc5e3e4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -49,425 +49,422 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.002; // 2 ms - private Notifier m_simNotifier = null; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /** Swerve request to apply during robot-centric path following */ - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = - new SwerveRequest.ApplyRobotSpeeds(); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; - - public AprilTagFieldLayout tagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); // TODO: SET - - /* Heading PID Controller for things like automatic alignment buttons */ - public PIDController thetaController = new PIDController(5, 0, 0.1); - - /* variable to store our heading */ - public Rotation2d odometryHeading = new Rotation2d(); - - private boolean isRobotAtAngleSetPoint; // for angle turning - private boolean fieldRelative; - - public RobotConfig config; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to - * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]ᵀ, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, - * theta]ᵀ, with units in meters and radians - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - - super( - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - Constants.currentMode == Constants.Mode.SIM ? MapleSimSwerveDrivetrain.regulateModuleConstantsForSimulation(modules) : modules - ); - - odometryHeading = Rotation2d.fromRotations(0); - - if (Utils.isSimulation()) { - startSimThread(); - } - - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) {} - - tagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - fieldRelative = true; - configureAutoBuilder(); - - thetaController.setTolerance(Degrees.of(1).in(Radians)); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - } - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> - setControl( - m_pathApplyRobotSpeeds - .withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(10, 0, 0), - // PID constants for rotation - new PIDConstants(7, 0, 0)), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError( - "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - public static Rotation2d wrapAngle(Rotation2d ang) { - double angle = ang.getDegrees(); - angle = (angle + 180) % 360; // Step 1 and 2 - if (angle < 0) { - angle += 360; // Make sure it's positive - } - return Rotation2d.fromDegrees(angle - 180); // Step 3 - } - - public Rotation2d getWrappedHeading() { - return wrapAngle(odometryHeading); - } - - public double getVelocityXFromController() { - double xInput = -MathUtil.applyDeadband(RobotContainer.driverController.getLeftX(), 0.06); - return Math.pow(xInput, 3) * MAX_SPEED.in(MetersPerSecond); - } - - public double getVelocityYFromController() { - double yInput = MathUtil.applyDeadband(RobotContainer.driverController.getLeftY(), 0.06); - return Math.pow(yInput, 3) * MAX_SPEED.in(MetersPerSecond); - } - - public void driveJoystick() { - double rotInput = -MathUtil.applyDeadband(RobotContainer.driverController.getRightX(), 0.06); - double rotVelocity = Math.pow(rotInput, 3) * MAX_ANGULAR_SPEED.in(RadiansPerSecond); - - drive( - getVelocityYFromController(), - getVelocityXFromController(), - rotVelocity, - fieldRelative, - true - ); // drives - } - - public void drive(ChassisSpeeds speeds, boolean fieldRelative, boolean respectOperatorPerspective) { - drive( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - fieldRelative, - respectOperatorPerspective - ); - } - - public void drive( - double xSpeed, - double ySpeed, - double thetaSpeed, - boolean fieldRelative, - boolean respectOperatorPerspective) { - - if (respectOperatorPerspective) { - if (DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red - && fieldRelative) { - xSpeed *= -1; - ySpeed *= -1; - } - } - - ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, thetaSpeed); - - if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getWrappedHeading()); - } - - SwerveRequest req = - new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(speeds) - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.Position); - - setControl(req); //actually drives - } - - public void setRobotRelativeAngle(Rotation2d angDeg) { - double wrappedSetPoint = wrapAngle(odometryHeading.plus(angDeg)).getRadians(); - thetaController.setSetpoint(wrappedSetPoint); - } - - public void alignToAngleRobotRelative(boolean lockDrive) { - double response = thetaController.calculate(odometryHeading.getRadians()); - if (lockDrive) { - drive(0, 0, response, false, true); // perspective doesn't matter in robot relative - } else { - drive(getVelocityYFromController(), getVelocityXFromController(), response, false, true); - } - } - - public void setFieldRelativeAngle(Rotation2d angle) { - double wrappedAngle = wrapAngle(angle).getRadians(); - thetaController.setSetpoint(wrappedAngle); - } - - public void alignToAngleFieldRelative(boolean lockDrive) { - double response = thetaController.calculate(odometryHeading.getRadians()); - if (lockDrive) { - drive(0, 0, response, true, true); - } else { - drive(getVelocityYFromController(), getVelocityXFromController(), response, true, true); - } - } - - public void alignToAngleRobotRelativeContinuous( - Supplier angleSup, boolean lockDrive) { - setRobotRelativeAngle(angleSup.get()); - alignToAngleRobotRelative(lockDrive); - } - - public void toRobotRelative() { - fieldRelative = false; - } - - public void toFieldRelative() { - fieldRelative = true; - } - - @AutoLogOutput - public Pose2d getRobotPose() { - return this.getState().Pose; - } - - public void zeroGyro() { - resetRotation(PoseUtils.flipRotAlliance(Rotation2d.fromDegrees(0))); - } - - public Command driveJoystickInputCommand() { - return Commands.run(() -> driveJoystick(), this); - } - - public Command zeroGyroCommand() { - return Commands.runOnce(() -> zeroGyro(), RobotContainer.drivetrain); - } - - public Command toRobotRelativeCommand() { - return Commands.runOnce(() -> toRobotRelative(), RobotContainer.drivetrain); - } - - public Command toFieldRelativeCommand() { - return Commands.runOnce(() -> toFieldRelative(), RobotContainer.drivetrain); - } - - public Command alignToAngleRobotRelativeCommand(Rotation2d angle, boolean lockDrive) { - return Commands.sequence( - Commands.runOnce(() -> setRobotRelativeAngle(angle), RobotContainer.drivetrain), - Commands.run(() -> alignToAngleRobotRelative(lockDrive), RobotContainer.drivetrain) - .until(() -> isRobotAtAngleSetPoint)); - } - - public Command alignToAngleRobotRelativeContinuousCommand( - Supplier angle, boolean lockDrive) { - return Commands.run(() -> alignToAngleRobotRelativeContinuous(angle, lockDrive), this) - .until(() -> isRobotAtAngleSetPoint); - } - - public Command alignToAngleFieldRelativeCommand(Rotation2d angle, boolean lockDrive) { - return Commands.sequence( - Commands.runOnce(() -> setFieldRelativeAngle(angle), RobotContainer.drivetrain), - Commands.run(() -> alignToAngleFieldRelative(lockDrive), this) - .until(() -> isRobotAtAngleSetPoint)); - } - - @Override - public void periodic() { - - odometryHeading = getRobotPose().getRotation(); - fieldRelative = !RobotContainer.driverController.L2().getAsBoolean(); - Logger.recordOutput("DriveState/FieldRelative", fieldRelative); - isRobotAtAngleSetPoint = thetaController.atSetpoint(); - - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); - m_hasAppliedOperatorPerspective = true; - }); - } - - DogLog.log("BatteryVoltage", RobotController.getBatteryVoltage()); - DogLog.log("Drive/OdometryPose", getState().Pose); - DogLog.log("Drive/TargetStates", getState().ModuleTargets); - DogLog.log("Drive/MeasuredStates", getState().ModuleStates); - DogLog.log("Drive/MeasuredSpeeds", getState().Speeds); - - if (mapleSimSwerveDrivetrain != null) { - DogLog.log( - "Drive/SimulationPose", - mapleSimSwerveDrivetrain.mapleSimDrive.getSimulatedDriveTrainPose() - ); - } - } - - public MapleSimSwerveDrivetrain mapleSimSwerveDrivetrain = null; - - @SuppressWarnings("unchecked") - private void startSimThread() { - mapleSimSwerveDrivetrain = - new MapleSimSwerveDrivetrain( - Seconds.of(kSimLoopPeriod), - // TODO: modify the following constants according to our robot - ROBOT_MASS, // robot weight - BUMPER_WIDTH, // bumper length - BUMPER_WIDTH, // bumper width - DCMotor.getKrakenX60Foc(1), // drive motor type - DCMotor.getKrakenX60Foc(1), // steer motor type - 1.2, // wheel COF - getModuleLocations(), - getPigeon2(), - getModules(), - TunerConstants.FrontLeft, - TunerConstants.FrontRight, - TunerConstants.BackLeft, - TunerConstants.BackRight); - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - @Override - public void resetPose(Pose2d pose) { - if (this.mapleSimSwerveDrivetrain != null) { - mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(pose); - Timer.delay(0.1); // wait for simulation to update - } - super.resetPose(pose); - } + private static final double kSimLoopPeriod = 0.002; // 2 ms + private Notifier m_simNotifier = null; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /** Swerve request to apply during robot-centric path following */ + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds(); + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; + + public AprilTagFieldLayout tagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); // TODO: SET + + /* Heading PID Controller for things like automatic alignment buttons */ + public PIDController thetaController = new PIDController(5, 0, 0.1); + + /* variable to store our heading */ + public Rotation2d odometryHeading = new Rotation2d(); + + private boolean isRobotAtAngleSetPoint; // for angle turning + private boolean fieldRelative; + + public RobotConfig config; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, + * theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + Constants.currentMode == Constants.Mode.SIM + ? MapleSimSwerveDrivetrain.regulateModuleConstantsForSimulation(modules) + : modules); + + odometryHeading = Rotation2d.fromRotations(0); + + if (Utils.isSimulation()) { + startSimThread(); + } + + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + } + + tagLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + fieldRelative = true; + configureAutoBuilder(); + + thetaController.setTolerance(Degrees.of(1).in(Radians)); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> + setControl( + m_pathApplyRobotSpeeds + .withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(10, 0, 0), + // PID constants for rotation + new PIDConstants(7, 0, 0)), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError( + "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + } + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + public static Rotation2d wrapAngle(Rotation2d ang) { + double angle = ang.getDegrees(); + angle = (angle + 180) % 360; // Step 1 and 2 + if (angle < 0) { + angle += 360; // Make sure it's positive + } + return Rotation2d.fromDegrees(angle - 180); // Step 3 + } + + public Rotation2d getWrappedHeading() { + return wrapAngle(odometryHeading); + } + + public double getVelocityXFromController() { + double xInput = -MathUtil.applyDeadband(RobotContainer.driverController.getLeftX(), 0.06); + return Math.pow(xInput, 3) * MAX_SPEED.in(MetersPerSecond); + } + + public double getVelocityYFromController() { + double yInput = MathUtil.applyDeadband(RobotContainer.driverController.getLeftY(), 0.06); + return Math.pow(yInput, 3) * MAX_SPEED.in(MetersPerSecond); + } + + public void driveJoystick() { + double rotInput = -MathUtil.applyDeadband(RobotContainer.driverController.getRightX(), 0.06); + double rotVelocity = Math.pow(rotInput, 3) * MAX_ANGULAR_SPEED.in(RadiansPerSecond); + + drive( + getVelocityYFromController(), + getVelocityXFromController(), + rotVelocity, + fieldRelative, + true); // drives + } + + public void drive( + ChassisSpeeds speeds, boolean fieldRelative, boolean respectOperatorPerspective) { + drive( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + fieldRelative, + respectOperatorPerspective); + } + + public void drive( + double xSpeed, + double ySpeed, + double thetaSpeed, + boolean fieldRelative, + boolean respectOperatorPerspective) { + + if (respectOperatorPerspective) { + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red + && fieldRelative) { + xSpeed *= -1; + ySpeed *= -1; + } + } + + ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, thetaSpeed); + + if (fieldRelative) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getWrappedHeading()); + } + + SwerveRequest req = + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(speeds) + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.Position); + + setControl(req); // actually drives + } + + public void setRobotRelativeAngle(Rotation2d angDeg) { + double wrappedSetPoint = wrapAngle(odometryHeading.plus(angDeg)).getRadians(); + thetaController.setSetpoint(wrappedSetPoint); + } + + public void alignToAngleRobotRelative(boolean lockDrive) { + double response = thetaController.calculate(odometryHeading.getRadians()); + if (lockDrive) { + drive(0, 0, response, false, true); // perspective doesn't matter in robot relative + } else { + drive(getVelocityYFromController(), getVelocityXFromController(), response, false, true); + } + } + + public void setFieldRelativeAngle(Rotation2d angle) { + double wrappedAngle = wrapAngle(angle).getRadians(); + thetaController.setSetpoint(wrappedAngle); + } + + public void alignToAngleFieldRelative(boolean lockDrive) { + double response = thetaController.calculate(odometryHeading.getRadians()); + if (lockDrive) { + drive(0, 0, response, true, true); + } else { + drive(getVelocityYFromController(), getVelocityXFromController(), response, true, true); + } + } + + public void alignToAngleRobotRelativeContinuous( + Supplier angleSup, boolean lockDrive) { + setRobotRelativeAngle(angleSup.get()); + alignToAngleRobotRelative(lockDrive); + } + + public void toRobotRelative() { + fieldRelative = false; + } + + public void toFieldRelative() { + fieldRelative = true; + } + + @AutoLogOutput + public Pose2d getRobotPose() { + return this.getState().Pose; + } + + public void zeroGyro() { + resetRotation(PoseUtils.flipRotAlliance(Rotation2d.fromDegrees(0))); + } + + public Command driveJoystickInputCommand() { + return Commands.run(() -> driveJoystick(), this); + } + + public Command zeroGyroCommand() { + return Commands.runOnce(() -> zeroGyro(), RobotContainer.drivetrain); + } + + public Command toRobotRelativeCommand() { + return Commands.runOnce(() -> toRobotRelative(), RobotContainer.drivetrain); + } + + public Command toFieldRelativeCommand() { + return Commands.runOnce(() -> toFieldRelative(), RobotContainer.drivetrain); + } + + public Command alignToAngleRobotRelativeCommand(Rotation2d angle, boolean lockDrive) { + return Commands.sequence( + Commands.runOnce(() -> setRobotRelativeAngle(angle), RobotContainer.drivetrain), + Commands.run(() -> alignToAngleRobotRelative(lockDrive), RobotContainer.drivetrain) + .until(() -> isRobotAtAngleSetPoint)); + } + + public Command alignToAngleRobotRelativeContinuousCommand( + Supplier angle, boolean lockDrive) { + return Commands.run(() -> alignToAngleRobotRelativeContinuous(angle, lockDrive), this) + .until(() -> isRobotAtAngleSetPoint); + } + + public Command alignToAngleFieldRelativeCommand(Rotation2d angle, boolean lockDrive) { + return Commands.sequence( + Commands.runOnce(() -> setFieldRelativeAngle(angle), RobotContainer.drivetrain), + Commands.run(() -> alignToAngleFieldRelative(lockDrive), this) + .until(() -> isRobotAtAngleSetPoint)); + } + + @Override + public void periodic() { + + odometryHeading = getRobotPose().getRotation(); + fieldRelative = !RobotContainer.driverController.L2().getAsBoolean(); + Logger.recordOutput("DriveState/FieldRelative", fieldRelative); + isRobotAtAngleSetPoint = thetaController.atSetpoint(); + + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); + } + + DogLog.log("BatteryVoltage", RobotController.getBatteryVoltage()); + DogLog.log("Drive/OdometryPose", getState().Pose); + DogLog.log("Drive/TargetStates", getState().ModuleTargets); + DogLog.log("Drive/MeasuredStates", getState().ModuleStates); + DogLog.log("Drive/MeasuredSpeeds", getState().Speeds); + + if (mapleSimSwerveDrivetrain != null) { + DogLog.log( + "Drive/SimulationPose", + mapleSimSwerveDrivetrain.mapleSimDrive.getSimulatedDriveTrainPose()); + } + } + + public MapleSimSwerveDrivetrain mapleSimSwerveDrivetrain = null; + + @SuppressWarnings("unchecked") + private void startSimThread() { + mapleSimSwerveDrivetrain = + new MapleSimSwerveDrivetrain( + Seconds.of(kSimLoopPeriod), + // TODO: modify the following constants according to our robot + ROBOT_MASS, // robot weight + BUMPER_WIDTH, // bumper length + BUMPER_WIDTH, // bumper width + DCMotor.getKrakenX60Foc(1), // drive motor type + DCMotor.getKrakenX60Foc(1), // steer motor type + 1.2, // wheel COF + getModuleLocations(), + getPigeon2(), + getModules(), + TunerConstants.FrontLeft, + TunerConstants.FrontRight, + TunerConstants.BackLeft, + TunerConstants.BackRight); + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + @Override + public void resetPose(Pose2d pose) { + if (this.mapleSimSwerveDrivetrain != null) { + mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(pose); + Timer.delay(0.1); // wait for simulation to update + } + super.resetPose(pose); + } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java new file mode 100644 index 0000000..e282e16 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.spindexer; + +import static frc.robot.constants.SpindexerConstants.*; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; +import org.littletonrobotics.junction.Logger; + +public class Spindexer extends SubsystemBase { + + private SpindexerIO io; + private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); + + public Spindexer(SpindexerIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Spindexer", inputs); + } + + public Command spinCommand() { + return Commands.run(() -> { + io.setVoltage(SPIN_VOLTAGE); + }); + } + + public Command reverseSpinCommand() { + return Commands.run(() -> { + io.setVoltage(-SPIN_VOLTAGE); + }); + } + + public Command manualControlCommand() { + return Commands.run(() -> { + io.setVoltage(12 * (Math.pow(RobotContainer.operatorController.getLeftX(), 3))); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java new file mode 100644 index 0000000..a2c6df0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.spindexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface SpindexerIO { + + @AutoLog + public class SpindexerIOInputs { + public double appliedVoltage = 0.0; + public double velocity = 0.0; // rps + public double tempCelcius = 0.0; + } + + public void updateInputs(SpindexerIOInputs inputs); + + public void setVoltage(double currentVoltage); + + public void stop(); + +} diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java new file mode 100644 index 0000000..ec43400 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOSim.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.spindexer; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.constants.SpindexerConstants; + +public class SpindexerIOSim implements SpindexerIO { + + private FlywheelSim sim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + SpindexerConstants.MOMENT_OF_INERTIA, + SpindexerConstants.GEARING + ), + DCMotor.getKrakenX60(1), + 0.0 + ); + + public void updateInputs(SpindexerIOInputs inputs) { + inputs.appliedVoltage = sim.getInputVoltage(); + inputs.velocity = (sim.getAngularVelocityRadPerSec() / 2 * Math.PI); + inputs.tempCelcius = 0; + } + + @Override + public void setVoltage(double currentVoltage) { + sim.setInputVoltage(currentVoltage); + } + + @Override + public void stop() { + setVoltage(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java new file mode 100644 index 0000000..cfe409f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIOTalonFX.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.spindexer; + +import static frc.robot.constants.SpindexerConstants.*; +import com.ctre.phoenix6.hardware.TalonFX; + +public class SpindexerIOTalonFX implements SpindexerIO { + + private TalonFX motor; + + public SpindexerIOTalonFX() { + motor = new TalonFX(CAN_ID); + } + + public void updateInputs(SpindexerIOInputs inputs) { + inputs.appliedVoltage = motor.getMotorVoltage().getValueAsDouble(); + inputs.tempCelcius = motor.getDeviceTemp().getValueAsDouble(); // celsius + inputs.velocity = motor.getVelocity().getValueAsDouble(); // RPS + } + + public void stop() { + motor.stopMotor(); + } + + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + +} diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8c07dfe..38739e0 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,12 +1,10 @@ package frc.robot.subsystems.turret; -import java.util.function.DoubleSupplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** * The TurretIOHardware class interfaces with the TalonFX motor controller and CANCoders to manage @@ -20,19 +18,24 @@ public class Turret extends SubsystemBase { public Turret(TurretIO io) { this.io = io; } - public Command stopCommand(){ + + public Command stopCommand() { return Commands.runOnce(() -> io.stop()); } - public Command setRobotRelativeAngleCommand(double target){ + + public Command setRobotRelativeAngleCommand(double target) { return Commands.run(() -> io.setRobotRelativeAngle(target), this).until(() -> io.atSetpoint()); } - public Command setRobotRelativeAngleContinuousCommand(DoubleSupplier target){ + + public Command setRobotRelativeAngleContinuousCommand(DoubleSupplier target) { return Commands.run(() -> io.setRobotRelativeAngle(target), this); } - public Command setFieldRelativeAngleCommand(double target){ + + public Command setFieldRelativeAngleCommand(double target) { return Commands.run(() -> io.setFieldRelativeAngle(target), this).until(() -> io.atSetpoint()); } - public Command setFieldRelativeAngleContinuousCommand(DoubleSupplier target){ + + public Command setFieldRelativeAngleContinuousCommand(DoubleSupplier target) { return Commands.run(() -> io.setFieldRelativeAngle(target), this); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index e3d5984..9ca190d 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -8,7 +8,7 @@ public interface TurretIO { public static class TurretIOInputs { public double velocity; public double appliedVoltage; - public double robotRelativeAngle; //angles are in rotations + public double robotRelativeAngle; // angles are in rotations public double robotRelativeAngleSetpoint; public double fieldRelativeAngle; public double fieldRelativeAngleSetpoint; @@ -21,7 +21,7 @@ public static class TurretIOInputs { public double getFieldRelativeAngle(); public boolean atSetpoint(); - + public double wrapAngleSetpoint(double angle); public void zeroRelativeEncoder(); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java index 7704b9c..a523930 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOSim.java @@ -1,21 +1,24 @@ package frc.robot.subsystems.turret; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; +import static frc.robot.constants.TurretConstants.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.RobotContainer; import java.util.function.DoubleSupplier; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; -import static frc.robot.constants.TurretConstants.*; + public class TurretIOSim implements TurretIO { - private SingleJointedArmSim sim; - PIDController pidController; - double appliedVoltage = 0; - double fieldRelativeSetpoint =0; + private SingleJointedArmSim sim; + PIDController pidController; + double appliedVoltage = 0; + double fieldRelativeSetpoint = 0; - public TurretIOSim() { - sim = new SingleJointedArmSim( + public TurretIOSim() { + sim = + new SingleJointedArmSim( DCMotor.getKrakenX60(0), GEAR_RATIO, MOMENT_OF_INERTIA, @@ -25,82 +28,82 @@ public TurretIOSim() { false, 0, 0); - pidController = new PIDController(SIM_KP, SIM_KI, SIM_KD); - - } - - @Override - public void updateInputs(TurretIOInputs input) { - input.velocity = sim.getVelocityRadPerSec() / (2 * Math.PI); - input.appliedVoltage = appliedVoltage; - input.robotRelativeAngle = getRobotRelativeAngle(); - input.robotRelativeAngleSetpoint = pidController.getSetpoint(); - input.fieldRelativeAngle = getFieldRelativeAngle(); - input.fieldRelativeAngleSetpoint = fieldRelativeSetpoint; - } + pidController = new PIDController(SIM_KP, SIM_KI, SIM_KD); + } - @Override - public void setRobotRelativeAngle(double angle) { - pidController.setSetpoint(wrapAngleSetpoint(angle)); - setVoltage(pidController.calculate(getRobotRelativeAngle())); - } + @Override + public void updateInputs(TurretIOInputs input) { + input.velocity = sim.getVelocityRadPerSec() / (2 * Math.PI); + input.appliedVoltage = appliedVoltage; + input.robotRelativeAngle = getRobotRelativeAngle(); + input.robotRelativeAngleSetpoint = pidController.getSetpoint(); + input.fieldRelativeAngle = getFieldRelativeAngle(); + input.fieldRelativeAngleSetpoint = fieldRelativeSetpoint; + } - @Override - public double getRobotRelativeAngle() { - return Radians.of(sim.getAngleRads()).in(Rotations); - } + @Override + public void setRobotRelativeAngle(double angle) { + pidController.setSetpoint(wrapAngleSetpoint(angle)); + setVoltage(pidController.calculate(getRobotRelativeAngle())); + } - @Override - public double getFieldRelativeAngle(){ - return getRobotRelativeAngle()+RobotContainer.drivetrain.odometryHeading.getRotations(); - } + @Override + public double getRobotRelativeAngle() { + return Radians.of(sim.getAngleRads()).in(Rotations); + } - @Override - public void zeroRelativeEncoder() { - sim.setState(0,0); - } + @Override + public double getFieldRelativeAngle() { + return getRobotRelativeAngle() + RobotContainer.drivetrain.odometryHeading.getRotations(); + } - @Override - public void setRobotRelativeAngle(DoubleSupplier supplier) { - setRobotRelativeAngle(supplier.getAsDouble()); - } + @Override + public void zeroRelativeEncoder() { + sim.setState(0, 0); + } - @Override - public void setVoltage(double voltage) { - appliedVoltage = voltage; - sim.setInputVoltage(voltage); - } + @Override + public void setRobotRelativeAngle(DoubleSupplier supplier) { + setRobotRelativeAngle(supplier.getAsDouble()); + } - @Override - public void stop() { - setVoltage(0); - } + @Override + public void setVoltage(double voltage) { + appliedVoltage = voltage; + sim.setInputVoltage(voltage); + } - @Override - public void setFieldRelativeAngle(double angle) { - double robotRelativeAngleSetpoint = angle - RobotContainer.drivetrain.odometryHeading.getRotations(); - fieldRelativeSetpoint = angle; - setRobotRelativeAngle(robotRelativeAngleSetpoint); - } + @Override + public void stop() { + setVoltage(0); + } - @Override - public void setFieldRelativeAngle(DoubleSupplier supplier) { - setFieldRelativeAngle(supplier.getAsDouble()); - } + @Override + public void setFieldRelativeAngle(double angle) { + double robotRelativeAngleSetpoint = + angle - RobotContainer.drivetrain.odometryHeading.getRotations(); + fieldRelativeSetpoint = angle; + setRobotRelativeAngle(robotRelativeAngleSetpoint); + } + + @Override + public void setFieldRelativeAngle(DoubleSupplier supplier) { + setFieldRelativeAngle(supplier.getAsDouble()); + } - @Override - public boolean atSetpoint() { - return pidController.atSetpoint(); - } - - @Override - public double wrapAngleSetpoint(double angle) { - if(angle>MAX_ANGLE.in(Rotations)) { - return angle-1; - }else if(angle<0){ - return angle+1; - }else { - return angle; - } - } + @Override + public boolean atSetpoint() { + return pidController.atSetpoint(); + } + + @Override + public double wrapAngleSetpoint(double angle) { + if (angle > MAX_ANGLE.in(Rotations)) { + return angle - 1; + } else if (angle < 0) { + return angle + 1; + } else { + return angle; + } + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java index 2dc55e6..d2ca1c9 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java @@ -1,49 +1,50 @@ package frc.robot.subsystems.turret; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static frc.robot.constants.TurretConstants.*; + import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.RobotContainer; -import frc.robot.constants.TurretConstants; - -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static frc.robot.constants.TurretConstants.*; import java.util.function.DoubleSupplier; public class TurretIOTalonFX implements TurretIO { - private TalonFX motor; - private MotionMagicVoltage mm_req; - private double robotRelativeSetpoint; - private double fieldRelativeSetpoint; - public TurretIOTalonFX() { - - robotRelativeSetpoint = 0; - fieldRelativeSetpoint = 0; - - motor = new TalonFX(CAN_ID); - TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); - Slot0Configs slot0Configs = talonFXConfigs.Slot0; - - slot0Configs.kS = TALON_KS; - slot0Configs.kV = TALON_KV; - slot0Configs.kA = TALON_KA; - slot0Configs.kP = TALON_KP; - slot0Configs.kI = TALON_KI; - slot0Configs.kD = TALON_KD; - - mm_req = new MotionMagicVoltage(0); - - MotionMagicConfigs motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = TURRET_CRUISE_VELOCITY.in(RotationsPerSecond)*GEAR_RATIO; - motionMagicConfigs.MotionMagicAcceleration = TURRET_ACCELERATION.in(RotationsPerSecondPerSecond)*GEAR_RATIO; - motionMagicConfigs.MotionMagicJerk = TURRET_JERK*GEAR_RATIO; - motor.getConfigurator().apply(talonFXConfigs); + private TalonFX motor; + private MotionMagicVoltage mm_req; + private double robotRelativeSetpoint; + private double fieldRelativeSetpoint; + + public TurretIOTalonFX() { + + robotRelativeSetpoint = 0; + fieldRelativeSetpoint = 0; + + motor = new TalonFX(CAN_ID); + TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); + Slot0Configs slot0Configs = talonFXConfigs.Slot0; + + slot0Configs.kS = TALON_KS; + slot0Configs.kV = TALON_KV; + slot0Configs.kA = TALON_KA; + slot0Configs.kP = TALON_KP; + slot0Configs.kI = TALON_KI; + slot0Configs.kD = TALON_KD; + + mm_req = new MotionMagicVoltage(0); + + MotionMagicConfigs motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = + TURRET_CRUISE_VELOCITY.in(RotationsPerSecond) * GEAR_RATIO; + motionMagicConfigs.MotionMagicAcceleration = + TURRET_ACCELERATION.in(RotationsPerSecondPerSecond) * GEAR_RATIO; + motionMagicConfigs.MotionMagicJerk = TURRET_JERK * GEAR_RATIO; + motor.getConfigurator().apply(talonFXConfigs); } @Override @@ -58,9 +59,14 @@ public void updateInputs(TurretIOInputs inputs) { @Override public void setRobotRelativeAngle(double angle) { - robotRelativeSetpoint = angle; - // converts to motor output shaft rotations and makes request to motion magic for a new setpoint. - motor.setControl(mm_req.withPosition(wrapAngleSetpoint(angle)*GEAR_RATIO).withSlot(0).withEnableFOC(false)); + robotRelativeSetpoint = angle; + // converts to motor output shaft rotations and makes request to motion magic for a new + // setpoint. + motor.setControl( + mm_req + .withPosition(wrapAngleSetpoint(angle) * GEAR_RATIO) + .withSlot(0) + .withEnableFOC(false)); } @Override @@ -90,8 +96,9 @@ public void stop() { @Override public void setFieldRelativeAngle(double angle) { - fieldRelativeSetpoint = angle; - double robotRelativeAngleSetpoint = angle - RobotContainer.drivetrain.odometryHeading.getRotations(); + fieldRelativeSetpoint = angle; + double robotRelativeAngleSetpoint = + angle - RobotContainer.drivetrain.odometryHeading.getRotations(); setRobotRelativeAngle(robotRelativeAngleSetpoint); } @@ -102,22 +109,24 @@ public void setFieldRelativeAngle(DoubleSupplier supplier) { @Override public double getFieldRelativeAngle() { - return getRobotRelativeAngle()+RobotContainer.drivetrain.odometryHeading.getRotations(); + return getRobotRelativeAngle() + RobotContainer.drivetrain.odometryHeading.getRotations(); } @Override public boolean atSetpoint() { - return Math.abs(getRobotRelativeAngle() - mm_req.getPositionMeasure().in(Rotations)/GEAR_RATIO) < ANGLE_TOLERANCE.in(Rotations); + return Math.abs( + getRobotRelativeAngle() - mm_req.getPositionMeasure().in(Rotations) / GEAR_RATIO) + < ANGLE_TOLERANCE.in(Rotations); } @Override public double wrapAngleSetpoint(double angle) { - if(angle>MAX_ANGLE.in(Rotations)) { - return angle-1; - }else if(angle<0){ - return angle+1; - }else { - return angle; - } - } + if (angle > MAX_ANGLE.in(Rotations)) { + return angle - 1; + } else if (angle < 0) { + return angle + 1; + } else { + return angle; + } + } }