diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..28cf0c3 --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +*.glb filter=lfs diff=lfs merge=lfs -text diff --git a/.github/workflows/fix-format.yml b/.github/workflows/fix-format.yml new file mode 100644 index 0000000..769727a --- /dev/null +++ b/.github/workflows/fix-format.yml @@ -0,0 +1,48 @@ +# stolen from 8033 + +name: Format + +# Controls when the action will run. Triggers the workflow on push or pull request +# events but only for the main branch. +on: + # push: + # branches: [ main ] + # pull_request: + # branches: [ main ] + workflow_dispatch: + +jobs: + # This workflow contains a single job called "build" + fix-format: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + permissions: + # Give the default GITHUB_TOKEN write permission to commit and push the + # added or changed files to the repository. + contents: write + + # This grabs the WPILib docker container + container: wpilib/roborio-cross-ubuntu:2023-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew spotlessApply + + # Commit changes + - uses: stefanzweifel/git-auto-commit-action@v5 + with: + commit_message: run spotless diff --git a/README.md b/README.md index 316c3d7..ca28c07 100644 --- a/README.md +++ b/README.md @@ -67,9 +67,8 @@ Gradle build will automatically run checkers to make sure there are no null poin ## ...generate controller maps? ```shell -./gradlew spotlessApply -# Go to NetworkTables > NetworkTables View > Transitory Values > -# /AdvantageKit/RealOutputs/controllerMap +./gradlew simulateJava +# Running simulateJava will dump the controller map onto a file. Quit it right after the program starts python scripts/gen_controller_map.py # Now, open the index.html # (`open` is a macOS-only command) diff --git a/advscope_assets/Robot_Kirby/config.json b/advscope_assets/Robot_Kirby/config.json new file mode 100644 index 0000000..a9cbeb8 --- /dev/null +++ b/advscope_assets/Robot_Kirby/config.json @@ -0,0 +1,15 @@ +{ + "name": "Kirby", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [0, 0, 0], + "cameras": [] +} diff --git a/advscope_assets/Robot_Kirby/documented_config.json5 b/advscope_assets/Robot_Kirby/documented_config.json5 new file mode 100644 index 0000000..a9b54ce --- /dev/null +++ b/advscope_assets/Robot_Kirby/documented_config.json5 @@ -0,0 +1,30 @@ +{ + "name": "Kirby", + // "sourceUrl": string // Link to the original file, optional + // Sequence of rotations along the x, y, and z axes + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + // Position offset in meters, applied after rotation + "position": [0, 0, 0], + + "cameras": [ + // Fixed camera positions, can be empty + // { + // "name": string // Camera name + // "rotations": { "axis": "x" | "y" | "z", "degrees": number }[] // Sequence of rotations along the x, y, and z axes + // "position": [number, number, number] // Position offset in meters relative to the robot, applied after rotation + // "resolution": [number, number] // Resolution in pixels, used to set the fixed aspect ratio + // "fov": number // Horizontal field of view in degrees + // } + ] + // Not going to implement articulated components for now + // "components": [...] // See "Articulated Components" +} diff --git a/advscope_assets/Robot_Kirby/model.glb b/advscope_assets/Robot_Kirby/model.glb new file mode 100644 index 0000000..2aa1598 --- /dev/null +++ b/advscope_assets/Robot_Kirby/model.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c3e57fefe22408bed0d5efefc0d4dc351c29684b3632bc7b13b793482111bae8 +size 63275852 diff --git a/scripts/map.json b/scripts/map.json index 6dd1e91..a26ea33 100644 --- a/scripts/map.json +++ b/scripts/map.json @@ -1 +1 @@ -{"driver":{"leftBumper":"smth","a":"smth","b":"shoot","povRight":"smth","povDownLeft":"azi","x":"pivot shooter wow","povDownRight":"azi","y":"seed relative","povDown":"azi","leftTrigger":"something","rightBumper":"smth"},"operator":{}} \ No newline at end of file +{"driver":{"b":"shoot","x":"pivot shooter no","y":"reset heading","leftTrigger":"Slow Drive"},"operator":{"rightStick":"Turret"}} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 970ffd2..235b88c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,11 +10,13 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.utils.NT4PublisherNoFMS; import java.util.HashMap; import java.util.Map; +import java.util.Optional; import java.util.function.BiConsumer; import monologue.Logged; import monologue.Monologue; @@ -36,12 +38,18 @@ public class Robot extends LoggedRobot implements Logged { private RobotContainer m_robotContainer; + private double correctionAngle; + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ @Override public void robotInit() { + + RobotController.setBrownoutVoltage( + 4.75); // the "blackout" voltage is 4.5 // lowk if this bot dont work im setting this to + // like 0 configureAdvantageKit(); if (Constants.FeatureFlags.kMonologueEnabled) { configureMonologue(); @@ -235,6 +243,20 @@ public void testInit() { @Override public void testPeriodic() {} + @Override + public void driverStationConnected() { + boolean isRedAlliance = true; + Optional ally = Optional.of(DriverStation.getAlliance().get()); + if (ally.get() == DriverStation.Alliance.Red) { + System.out.println("Set Azimuth Red Alliance"); + } + if (ally.get() == DriverStation.Alliance.Blue) { + isRedAlliance = false; + System.out.println("Set Azimuth Blue Alliance"); + } + m_robotContainer.setAllianceCol(isRedAlliance); + } + /** This function is called once when the robot is first started up. */ @Override public void simulationInit() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 109737f..1c15052 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,8 @@ package frc.robot; +import static frc.robot.subsystems.swerve.AzimuthConstants.*; + import choreo.auto.AutoChooser; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; @@ -14,15 +16,14 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; import frc.robot.commands.AutoRoutines; import frc.robot.sim.SimMechs; -import frc.robot.subsystems.BeamBreakIOBanner; +import frc.robot.subsystems.BeamBreakIOAdafruit; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.ampevator.Ampevator; import frc.robot.subsystems.ampevator.AmpevatorIOSim; @@ -41,11 +42,11 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; +import frc.robot.subsystems.spindex.ShooterFeederIOTalonFX; import frc.robot.subsystems.spindex.Spindex; import frc.robot.subsystems.spindex.SpindexConstants; import frc.robot.subsystems.spindex.SpindexIOTalonFX; import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.swerve.requests.SwerveFieldCentricFacingAngle; import frc.robot.subsystems.turret.*; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIOLimelight; @@ -62,9 +63,12 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final CommandSwerveDrivetrain swerve = TunerConstants.DriveTrain; + private boolean isRed; private final Ampevator ampevator = - new Ampevator(true, (Utils.isSimulation()) ? new AmpevatorIOSim() : new AmpevatorIOTalonFX()); + new Ampevator( + Constants.FeatureFlags.kAmpevatorEnabled, + (Utils.isSimulation()) ? new AmpevatorIOSim() : new AmpevatorIOTalonFX()); private final Turret turret = new Turret( @@ -75,7 +79,7 @@ public class RobotContainer { private final Shooter shooter = new Shooter( Constants.FeatureFlags.kShooterEnabled, - RobotBase.isReal() ? new ShooterIOTalonFX() : new ShooterIOSim()); + Utils.isSimulation() ? new ShooterIOSim() : new ShooterIOTalonFX()); private final PivotShooter pivotShooter = new PivotShooter( @@ -86,19 +90,20 @@ public class RobotContainer { new Roller( Constants.FeatureFlags.kAmpevatorRollersEnabled, new RollerIOTalonFX(), - new BeamBreakIOBanner(RollerConstants.kRollerBeamBreakDIO)); + new BeamBreakIOAdafruit(RollerConstants.kRollerBeamBreakDIO)); private final Climb climb = new Climb(Constants.FeatureFlags.kClimbEnabled, new ClimbIOTalonFX()); private final Intake intake = new Intake( Constants.FeatureFlags.kIntakeEnabled, new IntakeIOTalonFX(), - new BeamBreakIOBanner(IntakeConstants.kIntakeBeamBreakDIO)); + new BeamBreakIOAdafruit(IntakeConstants.kIntakeBeamBreakDIO)); private final Spindex spindex = new Spindex( Constants.FeatureFlags.kSpindexEnabled, new SpindexIOTalonFX(), - new BeamBreakIOBanner(SpindexConstants.kSpindexBeamBreakDIO)); + new ShooterFeederIOTalonFX(), + new BeamBreakIOAdafruit(SpindexConstants.kSpindexBeamBreakDIO)); private final Vision vision = new Vision(new VisionIOLimelight()); private final Superstructure superstructure = @@ -114,9 +119,9 @@ public class RobotContainer { vision); // Replace with CommandPS4Controller or CommandJoystick if needed - private final MappedXboxController m_driverController = + public final MappedXboxController m_driverController = new MappedXboxController(ControllerConstants.kDriverControllerPort, "driver"); - private final MappedXboxController m_operatorController = + public final MappedXboxController m_operatorController = new MappedXboxController(ControllerConstants.kOperatorControllerPort, "operator"); private final AutoRoutines autoRoutines = new AutoRoutines(swerve); @@ -135,9 +140,6 @@ public RobotContainer() { SimMechs.init(); // No other bindings should be added after this line. - if (Constants.FeatureFlags.kControllerMapEnabled) { - MappedXboxController.dumpControllerMap(m_driverController, m_operatorController); - } } /** @@ -151,24 +153,36 @@ public RobotContainer() { */ private void configureBindings() { m_driverController.b("shoot").whileTrue(shooter.setVelocity(100, 100)); - m_driverController.x("pivot shooter wow").onTrue(pivotShooter.setPosition(100)); + m_driverController.x("pivot shooter no").onTrue(pivotShooter.setPosition(100)); m_driverController - .y("pivot shooter wow 2") + .y("pivot shooter wow heheheh") .onTrue( NamedCommands.parallel( pivotShooter.setPosition(-100).withName("hi test"), shooter.setVelocity(100, 100).withName("test 2"))); + + m_operatorController + .rightStick("Turret") + .whileTrue( + turret.setPositionFieldRelative( + new Rotation2d(m_operatorController.getLeftX(), m_operatorController.getLeftY()), + swerve)); } private void configureAutoChooser() { autoChooser.addAutoRoutine("Box", autoRoutines::boxAuto); } - private void configureSwerve() { + public void setAllianceCol(boolean red) { + isRed = red; + } + + public void configureSwerve() { double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; double MaxAngularRate = 1.5 * Math.PI; // My drivetrain double SlowMaxSpeed = MaxSpeed * 0.3; double SlowMaxAngular = MaxAngularRate * 0.3; + SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(Constants.ControllerConstants.DriverConstants.kStickDeadband * MaxSpeed) @@ -177,95 +191,199 @@ private void configureSwerve() { * MaxAngularRate) // Add a 10% deadband .withDriveRequestType( SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric - SwerveFieldCentricFacingAngle azi = - new SwerveFieldCentricFacingAngle() - .withDeadband(MaxSpeed * .15) // TODO: update deadband - .withRotationalDeadband(MaxAngularRate * .15) // TODO: update deadband - .withHeadingController(SwerveConstants.azimuthController) - .withDriveRequestType(SwerveModule.DriveRequestType.Velocity); + /* Default swerve command */ swerve.setDefaultCommand( - // Drivetrain will execute this command periodically + /* Drivetrain will execute this command periodically */ swerve.applyRequest( () -> drive .withVelocityX(m_driverController.getLeftY() * MaxSpeed) // Drive -y is forward .withVelocityY(m_driverController.getLeftX() * MaxSpeed) // Drive -x is left .withRotationalRate(-m_driverController.getRightX() * MaxAngularRate))); - /* - * Right stick absolute angle mode on trigger hold, - * robot adjusts heading to the angle right joystick creates - */ + + /* invert axes */ m_driverController - .rightTrigger("something") + .rightTrigger("Drive with Inverted Axes") .whileTrue( swerve.applyRequest( () -> drive - .withVelocityX( - -m_driverController.getLeftY() * MaxSpeed) // Drive -y is forward - .withVelocityY( - -m_driverController.getLeftX() * MaxSpeed) // Drive -x is left - .withRotationalRate(-m_driverController.getRightX() * MaxAngularRate))); - // Slows translational and rotational speed to 30% + .withVelocityX(-m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(-m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate(-m_driverController.getRightX() * SlowMaxAngular))); + + /* translational and rotational speed are slowed */ m_driverController - .leftTrigger("something") + .leftTrigger("Slow Drive") .whileTrue( swerve.applyRequest( () -> drive - .withVelocityX(m_driverController.getLeftY() * (MaxSpeed * 0.17)) - .withVelocityY(m_driverController.getLeftX() * (MaxSpeed * 0.17)) - .withRotationalRate( - -m_driverController.getRightX() * (1.3 * 0.2 * Math.PI)))); - // Reset robot heading on button press - m_driverController.y("seed relative").onTrue(swerve.runOnce(() -> swerve.seedFieldRelative())); - // Azimuth angle bindings. isRed == true for red alliance presets. isRed != true - // for blue. - if (DriverStation.getAlliance() - .orElse(DriverStation.Alliance.Blue) - .equals(DriverStation.Alliance.Red)) { + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate(-m_driverController.getRightX() * SlowMaxAngular))); + + /* Reset robot heading to current orientation on button press */ + m_driverController.y("reset heading").onTrue(swerve.runOnce(swerve::seedFieldRelative)); + + /* + * Azimuth angle bindings. isRed == true for red alliance presets. + * isRed != true for blue. + */ + if (isRed) { + // azi source red m_driverController - .rightBumper("smth") + .rightBumper() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSourceRed))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziSourceRed.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi amp red m_driverController - .a("smth") + .a() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziAmpRed))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziAmpRed.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi feeder red m_driverController - .povRight("smth") + .povRight() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziFeederRed))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziFeederRed.getDegrees(), + Timer.getFPGATimestamp())))); } else { + // azi source blue m_driverController - .rightBumper("smth") + .rightBumper() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSourceBlue))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziSourceBlue.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi amp blue m_driverController - .a("smth") + .a() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziAmpBlue))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziAmpBlue.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi feeder blue m_driverController - .povRight("smth") + .povRight() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziFeederBlue))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziFeederBlue.getDegrees(), + Timer.getFPGATimestamp())))); } - // Universal azimuth bindings + + /* Universal Azimuth Bindings */ + + // azi subwoofer front m_driverController - .leftBumper("smth") + .leftBumper() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferFront))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziSubwooferFront.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi subwoofer left m_driverController - .povDownLeft("azi") + .x() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferLeft))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziSubwooferLeft.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi subwoofer right m_driverController - .povDownRight("azi") + .b() .whileTrue( - swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziSubwooferRight))); + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziSubwooferRight.getDegrees(), + Timer.getFPGATimestamp())))); + + // azi cleanup m_driverController - .povDown("azi") - .whileTrue(swerve.applyRequest(() -> azi.withTargetDirection(AzimuthConstants.aziCleanUp))); + .povDown() + .whileTrue( + swerve.applyRequest( + () -> + drive + .withVelocityX(m_driverController.getLeftY() * SlowMaxSpeed) + .withVelocityY(m_driverController.getLeftX() * SlowMaxSpeed) + .withRotationalRate( + SwerveConstants.azimuthController.calculate( + swerve.getPigeon2().getAngle(), + aziCleanUp.getDegrees(), + Timer.getFPGATimestamp())))); + if (Utils.isSimulation()) { swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } diff --git a/src/main/java/frc/robot/subsystems/BeamBreakIOBanner.java b/src/main/java/frc/robot/subsystems/BeamBreakIOAdafruit.java similarity index 83% rename from src/main/java/frc/robot/subsystems/BeamBreakIOBanner.java rename to src/main/java/frc/robot/subsystems/BeamBreakIOAdafruit.java index a8573f1..8021667 100644 --- a/src/main/java/frc/robot/subsystems/BeamBreakIOBanner.java +++ b/src/main/java/frc/robot/subsystems/BeamBreakIOAdafruit.java @@ -9,10 +9,10 @@ import edu.wpi.first.wpilibj.DigitalInput; -public class BeamBreakIOBanner implements BeamBreakIO { +public class BeamBreakIOAdafruit implements BeamBreakIO { private final DigitalInput beamBreak; - public BeamBreakIOBanner(int beambreakID) { + public BeamBreakIOAdafruit(int beambreakID) { beamBreak = new DigitalInput(beambreakID); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4576e54..a582282 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -16,9 +16,12 @@ import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.pivotshooter.PivotShooter; +import frc.robot.subsystems.pivotshooter.PivotShooterConstants; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.spindex.Spindex; import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.turret.TurretConstants; import frc.robot.subsystems.vision.Vision; import java.util.HashMap; import java.util.Map; @@ -26,18 +29,18 @@ public class Superstructure { public static enum StructureState { + AUTO, IDLE, - HOMED, - PREINTAKE, - INTAKE, - PRECLIMB, - CLIMB, - PRESUB, - PREPODIUM, - PREFEED, - SHOOT, - PREAMP, - OUTTAKE, + HOME, + AMP_PREP, + AMPING, + SUB_PREP, + SUBING, + FEED_PREP, + FEEDING, + TRAP_PREP, + TRAPPING, + POST_TRAP } private final Ampevator ampevator; @@ -50,8 +53,8 @@ public static enum StructureState { private final Turret turret; private final Vision vision; - private StructureState state = StructureState.IDLE; - private StructureState prevState = StructureState.IDLE; + private StructureState state = StructureState.AUTO; + private StructureState prevState = StructureState.AUTO; private Map stateTriggers = new HashMap(); @@ -96,6 +99,48 @@ public void configStateTransitions() { .onTrue(spindex.off()) .onTrue(pivotShooter.off()) .onTrue(shooter.off()); + + stateTriggers + .get(StructureState.HOME) + .onTrue(roller.off()) + .onTrue(ampevator.setStowPosition()) + .onTrue(climb.retractClimber()) + .onTrue(intake.off()) + .onTrue(spindex.off()) + .onTrue(pivotShooter.setPosition(0)) + .onTrue(shooter.off()) + .whileTrue(turret.getDefaultCommand()); + + stateTriggers + .get(StructureState.AMP_PREP) + .onTrue(spindex.goToAmpevator().until(roller.debouncedBeamBreak)) + .onTrue(intake.redirectToAmp().until(roller.debouncedBeamBreak)) + .onTrue(roller.intakeNote()) + .and(roller.debouncedBeamBreak) + .onTrue(ampevator.setAmpPosition()); + stateTriggers + .get(StructureState.AMPING) + .whileTrue(roller.outtake()) + .and(roller.debouncedBeamBreak) + .onTrue(setState(StructureState.HOME)); + stateTriggers + .get(StructureState.SUB_PREP) + .onTrue(spindex.goToShooter()) + .onTrue( + pivotShooter.setPosition( + PivotShooterConstants.kSubWooferPreset * PivotShooterConstants.kPivotMotorGearing)) + .onTrue( + shooter.setVelocity( + ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS)) + .onTrue(turret.setPosition(TurretConstants.kSubPreset)); + stateTriggers + .get(StructureState.SUBING) + .onTrue( + shooter.setVelocity( + ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS)) + .onTrue(spindex.feedNoteToShooter()) + .and(spindex.debouncedBeamBreak.debounce(1)) + .onTrue(setState(StructureState.HOME)); } // call manually diff --git a/src/main/java/frc/robot/subsystems/ampevatorrollers/Roller.java b/src/main/java/frc/robot/subsystems/ampevatorrollers/Roller.java index 5963660..368b21b 100644 --- a/src/main/java/frc/robot/subsystems/ampevatorrollers/Roller.java +++ b/src/main/java/frc/robot/subsystems/ampevatorrollers/Roller.java @@ -29,7 +29,7 @@ public class Roller extends DisableSubsystem { private final SysIdRoutine roller_sysIdRoutine; - private final Trigger debouncedBeamBreak = new Trigger(this::isBeamBroken).debounce(0.1); + public final Trigger debouncedBeamBreak = new Trigger(this::isBeamBroken).debounce(0.1); ; public Roller(boolean disabled, RollerIO rollerIO, BeamBreakIO beamBreakIO) { @@ -75,10 +75,16 @@ public Command off() { return this.runOnce(rollerIO::off); } + public Command intakeNote() { + return this.run(() -> rollerIO.setRollerVoltage(RollerConstants.kRollerIntakeVoltage)) + .until(debouncedBeamBreak) + .finallyDo(rollerIO::off); + } + public Command outtake() { - return this.run(() -> rollerIO.setRollerVoltage(RollerConstants.kRollerRollerVoltage)) + return this.run(() -> rollerIO.setRollerVoltage(RollerConstants.kRollerOuttakeVoltage)) .until(debouncedBeamBreak) - .andThen(this.off()); + .finallyDo(rollerIO::off); } public Command rollerSysIdQuasistatic(SysIdRoutine.Direction direction) { diff --git a/src/main/java/frc/robot/subsystems/ampevatorrollers/RollerConstants.java b/src/main/java/frc/robot/subsystems/ampevatorrollers/RollerConstants.java index 467d31a..3b916f7 100644 --- a/src/main/java/frc/robot/subsystems/ampevatorrollers/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/ampevatorrollers/RollerConstants.java @@ -15,7 +15,7 @@ public final class RollerConstants { /* CAN */ public static final int kRollerMotorID = 33; - public static final double kRollerRollerVoltage = 12; + public static final double kRollerOuttakeVoltage = 12; // Time of Flight constants public static final double kBeamBreakDelayTime = 0; @@ -42,4 +42,5 @@ public final class RollerConstants { .withStatorCurrentLimitEnable(true) .withStatorCurrentLimit(80)); public static int flashConfigRetries = 5; + public static double kRollerIntakeVoltage = 4; } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index b5036b5..a482073 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -77,7 +77,12 @@ public Command off() { public Command intakeIn() { return this.run(() -> intakeIO.setIntakeVoltage(IntakeConstants.kIntakeIntakeVoltage)) .until(debouncedBeamBreak) - .andThen(this.off()); + .finallyDo(intakeIO::off); + } + + public Command redirectToAmp() { + return this.run(() -> intakeIO.setIntakeVoltage(IntakeConstants.kIntakeRedirectVoltage)) + .finallyDo(intakeIO::off); } public Command intakeSysIdQuasistatic(SysIdRoutine.Direction direction) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 9974ac8..a8343c0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -42,4 +42,5 @@ public final class IntakeConstants { .withStatorCurrentLimitEnable(true) .withStatorCurrentLimit(80)); public static int flashConfigRetries = 5; + public static double kIntakeRedirectVoltage = -2; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 1ca47fc..59126d7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -58,21 +58,7 @@ public void updateInputs(ShooterIOInputs inputs) { RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage( leftFlywheelSimModel.getCurrentDrawAmps(), rightFlywheelSimModel.getCurrentDrawAmps())); - // For Advantage Kit >>> - inputs.shooterMotorVoltage = shooterMotorSim.getMotorVoltage(); - inputs.shooterMotorVelocity = leftFlywheelSimModel.getAngularVelocityRPM() / 60; - inputs.shooterMotorStatorCurrent = leftFlywheelSimModel.getCurrentDrawAmps(); - inputs.shooterMotorSupplyCurrent = shooterMotorSim.getSupplyCurrent(); - inputs.shooterMotorTemperature = 0.0; // In a perfect motor, no heat is generated - inputs.shooterMotorReferenceSlope = 69420; // No idea how to simulate this - - inputs.shooterMotorFollowerVoltage = shooterFollowerMotorSim.getMotorVoltage(); - inputs.shooterMotorFollowerVelocity = rightFlywheelSimModel.getAngularVelocityRPM() / 60; - inputs.shooterMotorFollowerStatorCurrent = rightFlywheelSimModel.getCurrentDrawAmps(); - inputs.shooterMotorFollowerSupplyCurrent = shooterFollowerMotorSim.getSupplyCurrent(); - inputs.shooterMotorFollowerTemperature = 0.0; - inputs.shooterMotorFollowerReferenceSlope = 69420; - // <<< For Advantage Kit + super.updateInputs(inputs); SimMechs.addToShooterFlywheelAngle( Math.toDegrees(leftRps) diff --git a/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIO.java b/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIO.java new file mode 100644 index 0000000..d40a6e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIO.java @@ -0,0 +1,29 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.spindex; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterFeederIO { + @AutoLog + public static class ShooterFeederIOInputs { + public double shooterFeederMotorVoltage = 0.0; + public double shooterFeederMotorVelocity = 0.0; + public double shooterFeederMotorStatorCurrent = 0.0; + public double shooterFeederMotorSupplyCurrent = 0.0; + public double shooterFeederMotorTemperature = 0.0; + } + + public default void updateInputs(ShooterFeederIOInputs inputs) {} + + public default void setFeederVoltage(double voltage) {} + + public default void setFeederVelocity(double velocity) {} + + public default void off() {} +} diff --git a/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIOTalonFX.java new file mode 100644 index 0000000..7b7dee0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindex/ShooterFeederIOTalonFX.java @@ -0,0 +1,84 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.spindex; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.utils.PhoenixUtil; + +public class ShooterFeederIOTalonFX implements ShooterFeederIO { + private final TalonFX shooterFeederMotor = new TalonFX(SpindexConstants.shooterFeederMotorID); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + private final MotionMagicVelocityVoltage motionMagicRequest = + new MotionMagicVelocityVoltage(0).withSlot(0); + + private final StatusSignal shooterFeederMotorVoltage = + shooterFeederMotor.getMotorVoltage(); + private final StatusSignal shooterFeederMotorVelocity = shooterFeederMotor.getVelocity(); + private final StatusSignal shooterFeederMotorStatorCurrent = + shooterFeederMotor.getStatorCurrent(); + private final StatusSignal shooterFeederMotorSupplyCurrent = + shooterFeederMotor.getSupplyCurrent(); + private final StatusSignal shooterFeederMotorTemperature = + shooterFeederMotor.getDeviceTemp(); + + public ShooterFeederIOTalonFX() { + PhoenixUtil.applyMotorConfigs( + shooterFeederMotor, + SpindexConstants.feederMotorConfigs, + SpindexConstants.flashConfigRetries); + + BaseStatusSignal.setUpdateFrequencyForAll( + SpindexConstants.updateFrequency, + shooterFeederMotorVoltage, + shooterFeederMotorVelocity, + shooterFeederMotorStatorCurrent, + shooterFeederMotorSupplyCurrent, + shooterFeederMotorTemperature); + shooterFeederMotor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ShooterFeederIOInputs inputs) { + BaseStatusSignal.refreshAll( + shooterFeederMotorVoltage, + shooterFeederMotorVelocity, + shooterFeederMotorStatorCurrent, + shooterFeederMotorSupplyCurrent, + shooterFeederMotorTemperature); + inputs.shooterFeederMotorVoltage = shooterFeederMotorVoltage.getValueAsDouble(); + inputs.shooterFeederMotorVelocity = shooterFeederMotorVelocity.getValueAsDouble(); + inputs.shooterFeederMotorStatorCurrent = shooterFeederMotorStatorCurrent.getValueAsDouble(); + inputs.shooterFeederMotorSupplyCurrent = shooterFeederMotorSupplyCurrent.getValueAsDouble(); + inputs.shooterFeederMotorTemperature = shooterFeederMotorTemperature.getValueAsDouble(); + } + + @Override + public void setFeederVoltage(double voltage) { + + shooterFeederMotor.setVoltage(voltage); + } + + @Override + public void setFeederVelocity(double velocity) { + if (SpindexConstants.useMotionMagic) { + shooterFeederMotor.setControl(motionMagicRequest.withVelocity(velocity)); + } else { + shooterFeederMotor.setControl(velocityRequest.withVelocity(velocity)); + } + } + + @Override + public void off() { + shooterFeederMotor.setControl(new NeutralOut()); + } +} diff --git a/src/main/java/frc/robot/subsystems/spindex/Spindex.java b/src/main/java/frc/robot/subsystems/spindex/Spindex.java index da46a48..52b7c92 100644 --- a/src/main/java/frc/robot/subsystems/spindex/Spindex.java +++ b/src/main/java/frc/robot/subsystems/spindex/Spindex.java @@ -8,6 +8,7 @@ package frc.robot.subsystems.spindex; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.BeamBreakIO; import frc.robot.subsystems.BeamBreakIOInputsAutoLogged; import frc.robot.utils.DisableSubsystem; @@ -29,11 +30,22 @@ public class Spindex extends DisableSubsystem { private final BeamBreakIOInputsAutoLogged beamBreakIOAutoLogged = new BeamBreakIOInputsAutoLogged(); + private final ShooterFeederIO shooterFeederIO; + private final ShooterFeederIOInputsAutoLogged shooterFeederIOAutoLogged = + new ShooterFeederIOInputsAutoLogged(); + + public final Trigger debouncedBeamBreak = new Trigger(() -> beamBreakIOAutoLogged.beamBroken); + // private beambreak Beambreak = new beamreak(1) - public Spindex(boolean disabled, SpindexIO spindexIO, BeamBreakIO beamBreakIO) { + public Spindex( + boolean disabled, + SpindexIO spindexIO, + ShooterFeederIO shooterFeeder, + BeamBreakIO beamBreakIO) { super(disabled); this.spindexIO = spindexIO; this.beamBreakIO = beamBreakIO; + this.shooterFeederIO = shooterFeeder; } @Override @@ -47,20 +59,42 @@ public void periodic() { } public Command setSpindexVoltage(double voltage) { - return this.run(() -> spindexIO.setSpindexVoltage(voltage)).finallyDo(() -> spindexIO.off()); + return this.run(() -> spindexIO.setSpindexVoltage(voltage)).finallyDo(spindexIO::off); } public Command setSpindexVelocity(double velocity) { - return this.run(() -> spindexIO.setSpindexVelocity(velocity)).finallyDo(() -> spindexIO.off()); + return this.run(() -> spindexIO.setSpindexVelocity(velocity)).finallyDo(spindexIO::off); + } + + public Command setShooterFeederVoltage(double voltage) { + return this.run(() -> shooterFeederIO.setFeederVoltage(voltage)) + .finallyDo(shooterFeederIO::off); + } + + public Command setShooterFeederVelocity(double velocity) { + return this.run(() -> shooterFeederIO.setFeederVelocity(velocity)) + .finallyDo(shooterFeederIO::off); } public Command goToShooter() { return setSpindexVelocity(SpindexConstants.spindexMotorSpeedRPS) .until(() -> beamBreakIOAutoLogged.beamBroken) - .andThen(this.off()); + .finallyDo(spindexIO::off); + } + + public Command feedNoteToShooter() { + return setShooterFeederVoltage(SpindexConstants.shooterFeederVoltage) + .until(() -> beamBreakIOAutoLogged.beamBroken) + .finallyDo(shooterFeederIO::off); + } + + public Command goToAmpevator() { + return setSpindexVelocity(-SpindexConstants.spindexMotorSpeedRPS) + .until(() -> beamBreakIOAutoLogged.beamBroken) + .finallyDo(spindexIO::off); } public Command off() { - return this.run(() -> spindexIO.off()); + return this.run(spindexIO::off); } } diff --git a/src/main/java/frc/robot/subsystems/spindex/SpindexConstants.java b/src/main/java/frc/robot/subsystems/spindex/SpindexConstants.java index 8eb18d0..26933e6 100644 --- a/src/main/java/frc/robot/subsystems/spindex/SpindexConstants.java +++ b/src/main/java/frc/robot/subsystems/spindex/SpindexConstants.java @@ -14,7 +14,7 @@ public class SpindexConstants { public static final int spindexMotorID = 0; public static final double spindexMotorSpeedRPS = 0.8; - public static TalonFXConfiguration motorConfigs = + public static TalonFXConfiguration spindexMotorConfigs = new TalonFXConfiguration() // TODO: tune .withSlot0(new Slot0Configs().withKS(0).withKV(0.1).withKP(1).withKI(0).withKD(0)) .withMotorOutput( @@ -35,4 +35,23 @@ public class SpindexConstants { public static double updateFrequency = 50; // idk if this is right public static boolean useMotionMagic = false; public static int kSpindexBeamBreakDIO = 1; + public static TalonFXConfiguration feederMotorConfigs = + new TalonFXConfiguration() // TODO: tune + .withSlot0(new Slot0Configs().withKS(0).withKV(0.1).withKP(1).withKI(0).withKD(0)) + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.Clockwise_Positive)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicAcceleration(120) + .withMotionMagicCruiseVelocity(60) + .withMotionMagicJerk(1200)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(80)); + ; + public static int shooterFeederMotorID = 43; + public static double shooterFeederVoltage = 8; } diff --git a/src/main/java/frc/robot/subsystems/spindex/SpindexIOTalonFX.java b/src/main/java/frc/robot/subsystems/spindex/SpindexIOTalonFX.java index 443be56..b9e164f 100644 --- a/src/main/java/frc/robot/subsystems/spindex/SpindexIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/spindex/SpindexIOTalonFX.java @@ -29,7 +29,7 @@ public class SpindexIOTalonFX implements SpindexIO { public SpindexIOTalonFX() { PhoenixUtil.applyMotorConfigs( - spindexMotor, SpindexConstants.motorConfigs, SpindexConstants.flashConfigRetries); + spindexMotor, SpindexConstants.spindexMotorConfigs, SpindexConstants.flashConfigRetries); BaseStatusSignal.setUpdateFrequencyForAll( SpindexConstants.updateFrequency, diff --git a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java index 078596f..cac4d86 100644 --- a/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java +++ b/src/main/java/frc/robot/subsystems/turret/EncoderIOCancoder.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.util.Units; +import frc.robot.utils.PhoenixUtil; public class EncoderIOCancoder implements EncoderIO { @@ -21,12 +22,8 @@ public class EncoderIOCancoder implements EncoderIO { public EncoderIOCancoder(int canCoderID) { encoder = new CANcoder(canCoderID, "mani"); - var response = encoder.getConfigurator().apply(TurretConstants.canCoderConfig); - - if (!response.isOK()) { - System.out.println( - "CANcoder ID " + canCoderID + " failed config with error " + response.getDescription()); - } + PhoenixUtil.applyCancoderConfig( + encoder, TurretConstants.canCoderConfig, TurretConstants.flashConfigRetries); encoderPosition = encoder.getAbsolutePosition(); encoderVelocity = encoder.getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 593f149..188b010 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -79,7 +79,7 @@ public static Rotation2d getTurretPosition(Rotation2d cancoder1, Rotation2d canc * @param swerveAngle Current angle of the swerve drivetrain * @return command to set the position of the turret absolute to the swerve angle */ - public Command setPositionRelativeToSwerve(Rotation2d position, CommandSwerveDrivetrain swerve) { + public Command setPositionFieldRelative(Rotation2d position, CommandSwerveDrivetrain swerve) { return this.run( () -> turretIO.setPosition( diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 2f8bcd4..fbe374b 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; public final class TurretConstants { @@ -31,6 +32,8 @@ public final class TurretConstants { public static final double kForwardLimit = 69; // TODO: Set limit public static final double kReverseLimit = -69; // TODO: Set limit + public static final Rotation2d kSubPreset = Rotation2d.fromRotations(0); + public static final TalonFXConfiguration motorConfigs = // TODO: Set configs new TalonFXConfiguration() .withSlot0( diff --git a/src/main/java/frc/robot/utils/TalonConfigEquality.java b/src/main/java/frc/robot/utils/PhoenixConfigEquality.java similarity index 93% rename from src/main/java/frc/robot/utils/TalonConfigEquality.java rename to src/main/java/frc/robot/utils/PhoenixConfigEquality.java index 5f9729e..3a16718 100644 --- a/src/main/java/frc/robot/utils/TalonConfigEquality.java +++ b/src/main/java/frc/robot/utils/PhoenixConfigEquality.java @@ -9,12 +9,13 @@ import com.ctre.phoenix6.configs.*; -public class TalonConfigEquality { +public class PhoenixConfigEquality { // Stolen from 254, as it's a dependency of PhoenixUtil. // Should probably make this configured like how we normally configure code public static final boolean ENABLE_LOGGING_INEQ = true; public static final double TALON_CONFIG_EPSILON = 0.05; + public static final double CANCODER_CONFIG_EPSILON = 0.05; private static final double kEpsilon = 1e-12; private static boolean epsilonEquals(double a, double b, double epsilon) { @@ -25,9 +26,9 @@ private static boolean epsilonEquals(double a, double b) { return epsilonEquals(a, b, kEpsilon); } - // private static boolean epsilonEquals(int a, int b, int epsilon) { - // return (a - epsilon <= b) && (a + epsilon >= b); - // } + // private static boolean epsilonEquals(int a, int b, int epsilon) { + // return (a - epsilon <= b) && (a + epsilon >= b); + // } public static boolean isEqual(TalonFXConfiguration a, TalonFXConfiguration b) { return isEqual(a.Slot0, b.Slot0) @@ -46,6 +47,16 @@ && isEqual(a.SoftwareLimitSwitch, b.SoftwareLimitSwitch) && isEqual(a.MotionMagic, b.MotionMagic); } + public static boolean isEqual(CANcoderConfiguration a, CANcoderConfiguration b) { + return a.FutureProofConfigs == b.FutureProofConfigs && isEqual(a.MagnetSensor, b.MagnetSensor); + } + + public static boolean isEqual(MagnetSensorConfigs a, MagnetSensorConfigs b) { + return a.SensorDirection == b.SensorDirection + && epsilonEquals(a.MagnetOffset, b.MagnetOffset, CANCODER_CONFIG_EPSILON) + && a.AbsoluteSensorRange == b.AbsoluteSensorRange; + } + public static boolean isEqual(Slot0Configs a, Slot0Configs b) { boolean val = epsilonEquals(a.kP, b.kP, TALON_CONFIG_EPSILON) diff --git a/src/main/java/frc/robot/utils/PhoenixUtil.java b/src/main/java/frc/robot/utils/PhoenixUtil.java index 8a7dec8..ea9a663 100644 --- a/src/main/java/frc/robot/utils/PhoenixUtil.java +++ b/src/main/java/frc/robot/utils/PhoenixUtil.java @@ -8,7 +8,9 @@ package frc.robot.utils; import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; @@ -45,7 +47,7 @@ public static boolean readAndVerifyConfiguration(TalonFX talon, TalonFXConfigura DriverStation.reportWarning( "Failed to read config for talon [" + talon.getDescription() + "]", false); return false; - } else if (!TalonConfigEquality.isEqual(config, readConfig)) { + } else if (!PhoenixConfigEquality.isEqual(config, readConfig)) { // configs did not match DriverStation.reportWarning( "Configuration verification failed for talon [" + talon.getDescription() + "]", false); @@ -56,6 +58,25 @@ public static boolean readAndVerifyConfiguration(TalonFX talon, TalonFXConfigura } } + public static boolean readAndVerifyConfiguration( + CANcoder cancoder, CANcoderConfiguration config) { + CANcoderConfiguration readConfig = new CANcoderConfiguration(); + if (!spamGetStatusCode(() -> cancoder.getConfigurator().refresh(readConfig))) { + // could not get config! + DriverStation.reportWarning( + "Failed to read config for CANCoder [" + cancoder.getDeviceID() + "]", false); + return false; + } else if (!PhoenixConfigEquality.isEqual(config, readConfig)) { + // configs did not match + DriverStation.reportWarning( + "Configuration verification failed for cancoder [" + cancoder.getDeviceID() + "]", false); + return false; + } else { + // configs read and match, Talon OK + return true; + } + } + // The main function you should use for most purposes public static boolean applyMotorConfigs( TalonFX motor, TalonFXConfiguration motorConfig, int numTries) { @@ -94,4 +115,44 @@ public static boolean applyMotorConfigs( "Failed to apply config for talon after " + numTries + " attempts", false); return false; } + + // TODO(Bryan): in the future I want to make this generic LOL + // but right now it's not worth the effort + public static boolean applyCancoderConfig( + CANcoder cancoder, CANcoderConfiguration cancoderConfig, int numTries) { + for (int i = 0; i < numTries; i++) { + if (RobotBase.isSimulation()) { + return cancoder.getConfigurator().apply(cancoderConfig).isOK(); + } + if (spamGetStatusCode(() -> cancoder.getConfigurator().apply(cancoderConfig))) { + // API says we applied config, lets make sure it's right + if (readAndVerifyConfiguration(cancoder, cancoderConfig)) { + return true; + } else { + DriverStation.reportWarning( + "Failed to verify config for cancoder [" + + cancoder.getDeviceID() + + "] (attempt " + + (i + 1) + + " of " + + numTries + + ")", + false); + } + } else { + DriverStation.reportWarning( + "Failed to apply config for cancoder [" + + cancoder.getDeviceID() + + "] (attempt " + + (i + 1) + + " of " + + numTries + + ")", + false); + } + } + DriverStation.reportError( + "Failed to apply config for talon after " + numTries + " attempts", false); + return false; + } } diff --git a/src/test/java/RobotContainerTest.java b/src/test/java/RobotContainerTest.java index a603761..bc05aba 100644 --- a/src/test/java/RobotContainerTest.java +++ b/src/test/java/RobotContainerTest.java @@ -1,5 +1,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import frc.robot.utils.MappedXboxController; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; @@ -18,6 +19,7 @@ public void setup() { @Test public void test() { frcRobot = new RobotContainer(); + MappedXboxController.dumpControllerMap(frcRobot.m_driverController, frcRobot.m_operatorController); assertEquals(1, 1); } } \ No newline at end of file