diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7e38922..e141e48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,6 +35,8 @@ public final class Constants { // falling back to REPLAY. private static final RobotType robotType = RobotType.SIMBOT; + public static final boolean spawnLessFuelInSim = true; + public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; } @@ -66,7 +68,7 @@ public static enum RobotType { BETA, /** The Sim Bot */ - SIMBOT, + SIMBOT } // This is only a fallback! This will not change the robot type. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c29ff88..5330559 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,39 +6,54 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.FeetPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.Arrays; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; +import frc.robot.Constants.RobotType; import frc.robot.commands.drive.DriveCommands; import frc.robot.generated.AlphaTunerConstants; import frc.robot.generated.BetaTunerConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOMaple; import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIOMaple; import frc.robot.subsystems.drive.ModuleIOReplay; -import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.hopper.HopperIO; +import frc.robot.subsystems.hopper.HopperIOSim; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.HoodIO; +import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.thriftyclimb.ThriftyClimb; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIO; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIOSim; @@ -46,6 +61,7 @@ import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOPhotonSim; import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; public class RobotContainer { @@ -53,9 +69,10 @@ public class RobotContainer { // Subsystems private Drive drivebase_; private AprilTagVision vision_; - private ThriftyClimb thriftyClimb_; private IntakeSubsystem intake_; private Shooter shooter_; + private Hopper hopper_; + private ThriftyClimb climb_; // Choosers private final LoggedDashboardChooser autoChooser_; @@ -101,9 +118,35 @@ public RobotContainer() { case SIMBOT: // Sim robot, instantiate physics sim IO implementations + // Create and configure a drivetrain simulation configuration + DriveTrainSimulationConfig config = DriveTrainSimulationConfig.Default() + .withGyro(COTS.ofPigeon2()) + .withSwerveModule(COTS.ofMark4( + DCMotor.getKrakenX60(1), + DCMotor.getKrakenX60(1), + COTS.WHEELS.COLSONS.cof, + 2 + )) + .withTrackLengthTrackWidth( + Meters.of(Math.abs( + CompTunerConstants.FrontLeft.LocationX - + CompTunerConstants.BackLeft.LocationX + )), + Meters.of(Math.abs( + CompTunerConstants.FrontLeft.LocationY - + CompTunerConstants.FrontRight.LocationY + )) + ) + .withBumperSize(Inches.of(30.75), Inches.of(37.25)); + + // Add sim drivebase to simulation and where modules can get it. + // CALL THIS BEFORE CREATING THE DRIVEBASE! + MapleSimUtil.createSwerve(config, new Pose2d(2.0, 2.0, Rotation2d.kZero)); + MapleSimUtil.createIntake(); + drivebase_ = new Drive( - new GyroIO() {}, - ModuleIOSim::new, + new GyroIOMaple(), + ModuleIOMaple::new, CompTunerConstants.FrontLeft, CompTunerConstants.FrontRight, CompTunerConstants.BackLeft, @@ -114,14 +157,16 @@ public RobotContainer() { vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, - new CameraIOPhotonSim("front", VisionConstants.frontTransform, drivebase_::getPose, true) + new CameraIOPhotonSim("front", VisionConstants.frontTransform, MapleSimUtil::getPosition, true) ); intake_= new IntakeSubsystem(new IntakeIOSim()); + + shooter_ = new Shooter(new ShooterIOSim(), new HoodIOSim()); + + hopper_ = new Hopper(new HopperIOSim()); - thriftyClimb_ = new ThriftyClimb( - new ThriftyClimbIOSim() - ); + climb_ = new ThriftyClimb(new ThriftyClimbIOSim()); break; default: // Comp Bot @@ -202,17 +247,21 @@ public RobotContainer() { cams ); } - - if (thriftyClimb_ == null) { - thriftyClimb_ = new ThriftyClimb(new ThriftyClimbIO() {}); - } if (intake_ == null) { intake_ = new IntakeSubsystem(new IntakeIO() {}); } if (shooter_ == null) { - shooter_ = new Shooter(new ShooterIO() {}); + shooter_ = new Shooter(new ShooterIO() {}, new HoodIO() {}); + } + + if (hopper_ == null) { + hopper_ = new Hopper(new HopperIO() {}); + } + + if (climb_ == null) { + climb_ = new ThriftyClimb(new ThriftyClimbIO() {}); } DriveCommands.configure( @@ -226,6 +275,11 @@ public RobotContainer() { Mechanism3d.measured.zero(); Mechanism3d.setpoints.zero(); + // Maple Sim + if (Constants.getRobot() == RobotType.SIMBOT) { + MapleSimUtil.start(); + } + // Choosers autoChooser_ = new LoggedDashboardChooser<>("Auto Choices"); autoChooser_.onChange(auto -> { @@ -243,44 +297,25 @@ public RobotContainer() { // Bind robot actions to commands here. private void configureBindings() { - gamepad_.a().onTrue(thriftyClimb_.toggle()); - //Testing out each of the commands in the simulator - gamepad_.a().whileTrue( - intake_.setRollerVoltageCommand(IntakeConstants.rollerCollectVoltage) - ); - - gamepad_.b().whileTrue( - intake_.setPivotAngleCommand(IntakeConstants.pivotTargetAngle) - ); - - gamepad_.x().whileTrue( - intake_.deployIntakeCommand() - ); - - gamepad_.y().whileTrue( - intake_.stowIntakeCommand() - ); - - gamepad_.leftBumper().whileTrue( - intake_.stopRollerCommand() - ); - - gamepad_.rightBumper().whileTrue( - intake_.setRollerVelocityCommand(IntakeConstants.rollerMaxVelocity) - ); - - gamepad_.back().whileTrue( - intake_.setPivotVoltageCommand(IntakeConstants.pivotVoltage) - ); - - gamepad_.rightTrigger().whileTrue( - intake_.intakeDeployCommand() - ); - + // Manually deploying and undeploying the intake. + gamepad_.start().onTrue(Commands.either( + intake_.deployCmd(), + intake_.stowCmd(), + intake_::isIntakeStowed + )); + + // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().whileTrue( - intake_.stopStowCommand() + intake_.runIntakeCmd().beforeStarting( + intake_.startDeployCmd().onlyIf(intake_::isIntakeStowed) + ) ); + // While the right trigger is held, we will shoot into the hub. + gamepad_.rightTrigger().whileTrue(shooter_.shootCmd(hopper_)); + + // When the hopper isnt shooting, set it to run its idle velocity. + hopper_.setDefaultCommand(hopper_.idleScrambler()); } private void configureDriveBindings() { @@ -339,7 +374,7 @@ private void configureTestModeBindings() { LoggedNetworkNumber hoodAngle = new LoggedNetworkNumber("Tuning/Shooter/TargetHoodAngle", ShooterConstants.SoftwareLimits.hoodMinAngle); gamepad_.a().and(RobotModeTriggers.test()).toggleOnTrue( - shooter_.runDynamicSetpoint(() -> RotationsPerSecond.of(shooterVelocity.get()), () -> Degrees.of(hoodAngle.get())) + shooter_.runDynamicSetpoints(() -> RotationsPerSecond.of(shooterVelocity.get()), () -> Degrees.of(hoodAngle.get())) ); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java b/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java new file mode 100644 index 0000000..9c0aedb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import org.ironmaple.simulation.drivesims.GyroSimulation; + +import frc.robot.util.MapleSimUtil; +import frc.robot.util.PhoenixUtil; + +public class GyroIOMaple implements GyroIO { + private final GyroSimulation sim; + + public GyroIOMaple() { + sim = MapleSimUtil.getGyroSimulation(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = true; + inputs.yawPosition = sim.getGyroReading(); + inputs.yawVelocityRadPerSec = sim.getMeasuredAngularVelocity().in(RadiansPerSecond); + inputs.odometryYawPositions = sim.getCachedGyroReadings(); + inputs.odometryYawTimestamps = PhoenixUtil.getSimulationOdometryTimestamps(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java new file mode 100644 index 0000000..4b3ceef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java @@ -0,0 +1,153 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import java.util.Arrays; + +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.util.MapleSimUtil; +import frc.robot.util.PhoenixUtil; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module constants from Phoenix. + * Simulation is always based on voltage control. + */ +public class ModuleIOMaple implements ModuleIO { + // TunerConstants doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KS = 0.03; + private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController turnMotor; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private final PIDController driveController; + private final PIDController turnController; + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + private static int moduleIndex = 0; + + public ModuleIOMaple( + SwerveModuleConstants constants, + CANBus bus + ) { + moduleSimulation = MapleSimUtil.getModuleSimulations()[moduleIndex]; + moduleIndex++; + + this.driveMotor = moduleSimulation + .useGenericMotorControllerForDrive() + .withCurrentLimit(Amps.of(constants.SlipCurrent)); + this.turnMotor = moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); + + this.driveController = new PIDController(0.05, 0.0, 0.0); + this.turnController = new PIDController(8.0, 0.0, 0.0); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = driveFFVolts + + driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond)); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians()); + } else { + turnController.reset(); + } + + // Update simulation state + driveMotor.requestVoltage(Volts.of(driveAppliedVolts)); + turnMotor.requestVoltage(Volts.of(turnAppliedVolts)); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + inputs.driveVelocityRadPerSec = + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = + Math.abs(moduleSimulation.getDriveMotorStatorCurrent().in(Amps)); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnPosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnVelocityRadPerSec = + moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = + Math.abs(moduleSimulation.getSteerMotorStatorCurrent().in(Amps)); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = PhoenixUtil.getSimulationOdometryTimestamps(); + inputs.odometryDrivePositionsRad = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositions()) + .mapToDouble(angle -> angle.in(Radians)) + .toArray(); + inputs.odometryTurnPositions = moduleSimulation.getCachedSteerAbsolutePositions(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 625047a..cf408b9 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -1,158 +1,175 @@ package frc.robot.subsystems.hopper; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + 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 edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; - -import static edu.wpi.first.units.Units.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.util.MapleSimUtil; public class Hopper extends SubsystemBase { - - private final HopperIO io; - private final HopperIOInputsAutoLogged inputs = new HopperIOInputsAutoLogged(); - - private AngularVelocity scramblerGoal = RotationsPerSecond.of(0.0); - private AngularVelocity feederGoal = RotationsPerSecond.of(0.0); - - public Hopper(HopperIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Hopper", inputs); - - Logger.recordOutput("Hopper/ScramblerGoalRPS", - scramblerGoal.in(RotationsPerSecond)); - Logger.recordOutput("Hopper/FeederGoalRPS", - feederGoal.in(RotationsPerSecond)); - Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); - Logger.recordOutput("Hopper/FeederAtGoal", isFeederAtGoal()); - } - - // Scrambler control - public void setScramblerVelocity(AngularVelocity velocity) { - scramblerGoal = velocity; - io.setScramblerVelocity(velocity); - } - - public void setScramblerVoltage(Voltage voltage) { - scramblerGoal = RotationsPerSecond.of(0.0); - io.setScramblerVoltage(voltage); - } - - public void idleScrambler() { - setScramblerVelocity( - HopperConstants.scramblerMaxVelocity.times(0.15) - ); -} - - public void activeScrambler() { - setScramblerVelocity(HopperConstants.scramblerMaxVelocity); - } - - public void stopScrambler() { - setScramblerVoltage(Volts.of(0.0)); - } - - // Feeder control - public void setFeederVelocity(AngularVelocity velocity) { - feederGoal = velocity; - io.setFeederVelocity(velocity); - } - - public void setFeederVoltage(Voltage voltage) { - feederGoal = RotationsPerSecond.of(0.0); - io.setFeederVoltage(voltage); - } - - public void stopFeeder() { - setFeederVoltage(Volts.of(0.0)); - } - - public void stopAll() { - stopScrambler(); - stopFeeder(); - } - - public void feed() { - setFeederVelocity(HopperConstants.feederMaxVelocity); - } - - public void feedSlow() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(0.5) - ); -} - - public void reverseFeed() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(-0.5) - ); -} - - public void scrambleAndFeed() { - activeScrambler(); - feed(); - } - - // Readbacks + state checks - public AngularVelocity getScramblerVelocity() { - return inputs.scramblerVelocity; - } - - public AngularVelocity getFeederVelocity() { - return inputs.feederVelocity; - } - - public double getScramblerCurrent() { - return inputs.scramblerCurrent.in(Amps); - } - - public double getFeederCurrent() { - return inputs.feederCurrent.in(Amps); - } - - public boolean isScramblerAtGoal() { - return Math.abs( - inputs.scramblerVelocity.in(RotationsPerSecond) - - scramblerGoal.in(RotationsPerSecond)) < 1.0; - } - - public boolean isFeederAtGoal() { - return Math.abs( - inputs.feederVelocity.in(RotationsPerSecond) - - feederGoal.in(RotationsPerSecond)) < 1.0; - } - - // Command factories - public Command setFeederVoltageCommand(Voltage voltage) { - return Commands.run(() -> setFeederVoltage(voltage), this) - .withName("Hopper.SetFeederVoltage"); - } - - public Command setScramblerVoltageCommand(Voltage voltage) { - return Commands.run(() -> setScramblerVoltage(voltage), this) - .withName("Hopper.SetScramblerVoltage"); - } - - public Command stopFeederCommand() { - return Commands.run(this::stopFeeder, this) - .withName("Hopper.StopFeeder"); - } - - public Command stopScramblerCommand() { - return Commands.run(this::stopScrambler, this) - .withName("Hopper.StopScrambler"); - } - - public Command stopAllCommand() { - return Commands.run(this::stopAll, this) - .withName("Hopper.StopAll"); - } + + private final HopperIO io; + private final HopperIOInputsAutoLogged inputs = new HopperIOInputsAutoLogged(); + + private AngularVelocity scramblerGoal = RotationsPerSecond.of(0.0); + private AngularVelocity feederGoal = RotationsPerSecond.of(0.0); + + public Hopper(HopperIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Hopper", inputs); + + if (Constants.getMode() == Mode.SIM) { + // Set shooter running if feeder and scrambler are both rolling. + MapleSimUtil.setShooterRunning( + inputs.feederVelocity.gt(RadiansPerSecond.zero()) && + inputs.scramblerVelocity.gt(RadiansPerSecond.zero()) + ); + } + + Logger.recordOutput("Hopper/ScramblerGoal", scramblerGoal); + Logger.recordOutput("Hopper/FeederGoal", feederGoal); + Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); + Logger.recordOutput("Hopper/FeederAtGoal", isFeederAtGoal()); + } + + // Scrambler control + private void setScramblerVelocity(AngularVelocity velocity) { + scramblerGoal = velocity; + io.setScramblerVelocity(velocity); + } + + private void setScramblerVoltage(Voltage voltage) { + scramblerGoal = RotationsPerSecond.of(0.0); + io.setScramblerVoltage(voltage); + } + + private void stopScrambler() { + setScramblerVoltage(Volts.of(0.0)); + } + + // Feeder control + private void setFeederVelocity(AngularVelocity velocity) { + feederGoal = velocity; + io.setFeederVelocity(velocity); + } + + private void setFeederVoltage(Voltage voltage) { + feederGoal = RotationsPerSecond.of(0.0); + io.setFeederVoltage(voltage); + } + + private void stopFeeder() { + setFeederVoltage(Volts.of(0.0)); + } + + private void stopAll() { + stopScrambler(); + stopFeeder(); + } + + // Commands + + /** + * Runs the scrambler at its idling speed until the command ends. + * @return + */ + public Command idleScrambler() { + return startEnd( + () -> setFeederVelocity(HopperConstants.feederMaxVelocity.times(0.15)), + this::stopFeeder + ); + } + + /** + * Runs the feeder and scrambler at specified speeds. + * @param feeder + * @param scrambler + * @return + */ + public Command feed(AngularVelocity feeder, AngularVelocity scrambler) { + return startEnd(() -> { + setFeederVelocity(feeder); + setScramblerVelocity(scrambler); + }, this::stopAll); + } + + /** + * Runs the scrambler at its active speed, and the feeder. + * @return + */ + public Command forwardFeed() { + return feed(HopperConstants.scramblerActiveVelocity, HopperConstants.feedingVelocity); + } + + /** + * Runs the scramble and feeder in reverse. + * @return + */ + public Command reverseFeed() { + return feed(HopperConstants.scramblerActiveVelocity.times(-1), HopperConstants.feedingVelocity.times(-1)); + } + + // Readbacks + state checks + public AngularVelocity getScramblerVelocity() { + return inputs.scramblerVelocity; + } + + public AngularVelocity getFeederVelocity() { + return inputs.feederVelocity; + } + + public double getScramblerCurrent() { + return inputs.scramblerCurrent.in(Amps); + } + + public double getFeederCurrent() { + return inputs.feederCurrent.in(Amps); + } + + public boolean isScramblerAtGoal() { + return inputs.scramblerVelocity.isNear(scramblerGoal, RotationsPerSecond.one()); + } + + public boolean isFeederAtGoal() { + return inputs.feederVelocity.isNear(feederGoal, RotationsPerSecond.one()); + } + + public Command setFeederVoltageCommand(Voltage voltage) { + return runOnce(() -> setFeederVoltage(voltage)) + .withName("Hopper.SetFeederVoltage"); + } + + public Command setScramblerVoltageCommand(Voltage voltage) { + return runOnce(() -> setScramblerVoltage(voltage)) + .withName("Hopper.SetScramblerVoltage"); + } + + public Command stopFeederCommand() { + return runOnce(this::stopFeeder) + .withName("Hopper.StopFeeder"); + } + + public Command stopScramblerCommand() { + return runOnce(this::stopScrambler) + .withName("Hopper.StopScrambler"); + } + + public Command stopAllCommand() { + return runOnce(this::stopAll) + .withName("Hopper.StopAll"); + } } diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java b/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java deleted file mode 100644 index fb46ba2..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.subsystems.hopper; - -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -public class HopperCommands { - //Start Motor Commands - public static Command setFeederVoltage(Hopper hopper, Voltage voltage) { - return Commands.run(() -> hopper.setFeederVoltage(voltage), hopper) - .withName("setFeederVoltage"); - } - - public static Command setScramblerVoltage(Hopper hopper, Voltage voltage) { - return Commands.run(() -> hopper.setScramblerVoltage(voltage), hopper) - .withName("setScramblerVoltage"); - } - - //Stop Motor Commands - public static Command stopFeeder(Hopper hopper) { - return Commands.run(hopper::stopFeeder, hopper) - .withName("stopFeeder"); - } - - public static Command stopScrambler(Hopper hopper) { - return Commands.run(hopper::stopScrambler, hopper) - .withName("stopScrambler"); - } - - public static Command stopAll(Hopper hopper) { - return Commands.run(hopper::stopAll, hopper) - .withName("stopAll"); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java index fe9f2fd..1bb0538 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java @@ -5,13 +5,17 @@ import static edu.wpi.first.units.Units.*; public class HopperConstants { + + // Command Constants + public static final AngularVelocity feedingVelocity = RotationsPerSecond.of(20); + public static final AngularVelocity scramblerActiveVelocity = RotationsPerSecond.of(10); //Feeder Constants - public static final int feederMotorCANID = 0; //To be updated - public static final double feederGearRatio = 0.0; //To be updated + public static final int feederMotorCANID = 5; //To be updated + public static final double feederGearRatio = 1.0; //To be updated - public static final double feederKP = 0.0; //To be updated + public static final double feederKP = 1.0; //To be updated public static final double feederKI = 0.0; //To be updated public static final double feederKD = 0.0; //To be updated public static final double feederKS = 0.0; //To be updated @@ -23,8 +27,8 @@ public class HopperConstants { public static final Current feederCurrentLimit = Amps.of(0.0); //To be updated //Scrambler Constants - public static final int scramblerMotorCANID = 0; //To be updated - public static final double scramblerGearRatio = 0.0; //To be updated + public static final int scramblerMotorCANID = 6; //To be updated + public static final double scramblerGearRatio = 1.0; //To be updated public static final double scramblerKP = 0.0; //To be updated public static final double scramblerKI = 0.0; //To be updated diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java b/src/main/java/frc/robot/subsystems/hopper/HopperIO.java index 431d6fe..5bc8820 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperIO.java @@ -1,10 +1,14 @@ package frc.robot.subsystems.hopper; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.units.*; -import edu.wpi.first.units.measure.*; import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + public interface HopperIO { @AutoLog public static class HopperIOInputs { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 2a749ba..3e39276 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -16,15 +16,14 @@ public final class IntakeConstants { - public static final int rollerMotorCANID= 1;//Temporary - public static final int pivotMotorCANID= 2; //Temporary + public static final int rollerMotorCANID= 8;//Temporary + public static final int pivotMotorCANID= 9; //Temporary public static final int currentLimit= 40; public static final Time currentLimitTime= Seconds.of(1); //Temporary - public static final double pivotKP= 0.5; //later for turning - public static final double pivotKD= 0.05; //later for turning + public static final double pivotKD= 0.04; //later for turning public static final double pivotKV= 0; //later for turning public static final double pivotKI= 0; //later for turning public static final double pivotKA= 0; //later for turning @@ -35,15 +34,15 @@ public final class IntakeConstants { public static final double rollerKD= 0; //change later public static final double rollerKV= 0; //change later public static final double rollerKI= 0; //change later - public static final double rollerKA=0; //change later + public static final double rollerKA = 0; //change later public static final double rollerKS= 0; //change later public static final double rollerKG= 0; //change later - public static final Angle stowedAngle= Degrees.of(-20); //Temporary angle - public static final Angle deployedAngle= Degrees.of(-35); //Temporary angle + public static final Angle stowedAngle= Degrees.of(0); //Temporary angle + public static final Angle deployedAngle= Degrees.of(125); //Temporary angle public static final Angle pivotMinAngle= Degrees.of(-15); //Temporary angle - public static final Angle pivotMaxAngle= Degrees.of(-40); //Temporary angle - public static final Angle pivotTargetAngle= Degrees.of(-25); //This variable can be changed if we want to run a command to tell intake to go to a specific position + public static final Angle pivotMaxAngle= Degrees.of(130); //Temporary angle + public static final Angle pivotTargetAngle = Degrees.of(-25); //This variable can be changed if we want to run a command to tell intake to go to a specific position public static final Voltage pivotVoltage= Volts.of(2); //Temporary if we want to set the pivot voltage to a specifc value at all public static final AngularVelocity pivotCruiseVelocity= DegreesPerSecond.of(90); //Temporary speed @@ -53,10 +52,10 @@ public final class IntakeConstants { public static final AngularVelocity rollerMaxVelocity= DegreesPerSecond.of(360); //Temporary speed to be changed as needed public static final Voltage rollerCollectVoltage= Volts.of(6); //Temporary voltage - public static final double pivotDegreeTolerance= 2; //Tolerance to compare current angle to target + public static final Angle pivotTolerance = Degrees.of(5); //Tolerance to compare current angle to target - public static final double pivotGearRatio= 0; //Unsure yet, will need it for the simulator - public static final double rollerGearRatio= 0; //Unsure yet, will need it for the simulator - public static final MomentOfInertia PIVOT_MOMENTOFINERTIA= KilogramSquareMeters.of(0.01); - public static final MomentOfInertia ROLLER_MOMENTOFINERTIA= KilogramSquareMeters.of(0.01); + public static final double motorToPivotGearRatio = 2.0; //Unsure yet, will need it for the simulator + public static final double rollerGearRatio = 1.0; //Unsure yet, will need it for the simulator + public static final MomentOfInertia PIVOT_MOMENTOFINERTIA = KilogramSquareMeters.of(0.01); + public static final MomentOfInertia ROLLER_MOMENTOFINERTIA = KilogramSquareMeters.of(0.001); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index eef5d0f..c2e73cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,32 +1,35 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.generated.CompTunerConstants; +import static edu.wpi.first.units.Units.KilogramSquareMeters; import com.ctre.phoenix6.sim.TalonFXSimState; -import edu.wpi.first.math.system.plant.LinearSystemId; + import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; -import static edu.wpi.first.units.Units.KilogramSquareMeters; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Robot; +import frc.robot.generated.CompTunerConstants; -public class IntakeIOSim extends IntakeIOTalonFX{ +public class IntakeIOSim extends IntakeIOTalonFX { public final DCMotorSim pivotMotorSim; public final DCMotorSim rollerMotorSim; public IntakeIOSim() { - super(CompTunerConstants.kCANBus, CompTunerConstants.kCANBus); + super(CompTunerConstants.kCANBus, CompTunerConstants.kCANBus); // This needs to be changed + pivotMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.pivotGearRatio + DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.motorToPivotGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) ); rollerMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX60Foc(1), IntakeConstants.ROLLER_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.rollerGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) ); } @@ -39,15 +42,15 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ pivotMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); pivotMotorSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); - pivotMotorSim.update(0.020); //20 millisecond robot sim loop + pivotMotorSim.update(Robot.defaultPeriodSecs); - pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.pivotGearRatio)); - pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.pivotGearRatio)); + pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.motorToPivotGearRatio)); + pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.motorToPivotGearRatio)); rollerMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); rollerMotorSim.setInputVoltage(rollerMotorSimState.getMotorVoltage()); - rollerMotorSim.update(0.020); //20 millisecond robot sim loop + rollerMotorSim.update(Robot.defaultPeriodSecs); rollerMotorSimState.setRawRotorPosition(rollerMotorSim.getAngularPosition().times(IntakeConstants.rollerGearRatio)); rollerMotorSimState.setRotorVelocity(rollerMotorSim.getAngularVelocity().times(IntakeConstants.rollerGearRatio)); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 7c31cf4..41991cc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -115,8 +115,8 @@ public IntakeIOTalonFX(CANBus pivotBus, CANBus rollerBus) { pivotAngleSignal = pivotMotor.getPosition(); pivotAngularVelocitySignal = pivotMotor.getVelocity(); rollerAngularVelocitySignal = rollerMotor.getVelocity(); - rollerAppliedVoltsSignal = rollerMotor.getSupplyVoltage(); - pivotAppliedVoltsSignal = pivotMotor.getSupplyVoltage(); + rollerAppliedVoltsSignal = rollerMotor.getMotorVoltage(); + pivotAppliedVoltsSignal = pivotMotor.getMotorVoltage(); rollerCurrentAmpsSignal = rollerMotor.getStatorCurrent(); pivotCurrentAmpsSignal = pivotMotor.getStatorCurrent(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 64aee11..1d8a8c9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -1,17 +1,22 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.units.measure.Voltage; + import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Time; - -import static edu.wpi.first.units.Units.*; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.util.MapleSimUtil; +import frc.robot.util.Mechanism3d; public class IntakeSubsystem extends SubsystemBase { private final IntakeIO io; @@ -19,151 +24,127 @@ public class IntakeSubsystem extends SubsystemBase { private final Angle pivotDeployedAngle = IntakeConstants.deployedAngle; private final Angle pivotStowedAngle = IntakeConstants.stowedAngle; + private Angle setpointAngle = pivotStowedAngle; + public IntakeSubsystem(IntakeIO io) { this.io = io; - Logger.processInputs("Intake", inputs); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); + + Logger.recordOutput("Intake/PivotSetpoint", setpointAngle); + + Mechanism3d.measured.setIntake(inputs.PivotAngle); + Mechanism3d.setpoints.setIntake(setpointAngle); + + if (Constants.getMode() == Mode.SIM) { + MapleSimUtil.setIntakeRunning(isIntakeDeployed() && inputs.RollerAngularVelocity.gt(RadiansPerSecond.zero())); + } } //Intake control methods - public void setRollerVoltage(Voltage volts) { + private void setRollerVoltage(Voltage volts) { io.setRollerVoltage(volts); } - public void setPivotAngle(Angle angle) { + private void setPivotAngle(Angle angle) { + setpointAngle = angle; io.setPivotAngle(angle); } - public void stopRoller(){ - io.setRollerVoltage(Volts.of(0)); + /** + * Runs the roller. + */ + private void startIntaking() { + io.setRollerVoltage(IntakeConstants.rollerCollectVoltage); + } + + /** + * Stops the roller. + */ + private void stopIntaking() { + io.stopRoller(); } - public void stowIntake(){ + /** + * Stows the intake and hopper. + */ + private void stow() { setPivotAngle(IntakeConstants.stowedAngle); } - public void deployIntake(){ + /** + * Deploys the intake and hopper. + */ + private void deploy() { setPivotAngle(IntakeConstants.deployedAngle); } - public void setRollerVelocity(AngularVelocity velocity) { - io.setRollerVelocity(velocity); - } - - public void setPivotVoltage(Voltage voltage) { - io.setPivotVoltage(voltage); - } - public Angle getPivotAngle(){ return inputs.PivotAngle; } - public boolean isIntakeDeployed(){ - // Compare pivot angle to deployed angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = pivotDeployedAngle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + public boolean isIntakeDeployed() { + return isPivotAtAngle(pivotDeployedAngle); } public boolean isIntakeStowed(){ - // Compare pivot angle to stowed angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = pivotStowedAngle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + return isPivotAtAngle(pivotStowedAngle); } public boolean isPivotAtAngle(Angle angle){ - // Compare pivot angle to target angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = angle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + return inputs.PivotAngle.isNear(angle, IntakeConstants.pivotTolerance); } ///////////// ///Commands// ///////////// - ////////////////////////////////////////////////////// - /// First stow the intake, and then wait until it is stowed - ////////////////////////////////////////////////////// - public Command stowIntakeCommand() { - return Commands.runOnce(() -> stowIntake()) - .andThen(Commands.waitUntil(() -> isIntakeStowed()) - .withTimeout(2)).withName("Stow Intake"); - } - - ////////////////////////////////////////////////////// - /// First deploy the intake, and then wait until it is deployed - ////////////////////////////////////////////////////// - public Command deployIntakeCommand() { - return Commands.runOnce(() -> deployIntake()) - .andThen(Commands.waitUntil(() -> isIntakeDeployed()) - .withTimeout(2)).withName("Deploy Intake"); - } - - ///////////////////////// - /// Stop the rollers once - ///////////////////////// - public Command stopRollerCommand() { - return Commands.runOnce(() -> stopRoller()) - .withName("Stop Roller"); - } - - //////////////////////////////////////////////////////// - /// Continously set the roller voltage until interrupted - //////////////////////////////////////////////////////// - public Command setRollerVoltageCommand(Voltage volts) { - return Commands.run(() -> setRollerVoltage(volts)) - .withName("Set Roller Voltage"); - } - - ////////////////////////////////////////////////////////////////// - /// Continously move the rollers at set velocity until interrupted - ////////////////////////////////////////////////////////////////// - public Command setRollerVelocityCommand(AngularVelocity velocity) { - return Commands.run(() -> setRollerVelocity(velocity)) - .withName("Set Roller Velocity"); - } - - //////////////////////////////////////////////////////////////// - /// Continously move the pivot at set velocity until interrupted - //////////////////////////////////////////////////////////////// - public Command setPivotVoltageCommand(Voltage voltage) { - return Commands.run(() -> setPivotVoltage(voltage)) - .withName("Set Pivot Voltage"); - } - - ///////////////////////////////////////////////////////////// - /// First set the angle target, then wait until it is reached - ///////////////////////////////////////////////////////////// - public Command setPivotAngleCommand(Angle angle) { - return Commands.runOnce(() ->setPivotAngle(angle)) - .andThen(Commands.waitUntil(() -> isPivotAtAngle(angle)) - .withTimeout(2)).withName("Set Pivot Angle"); - } - - //////////////////////////////// - ///Deploy and intake in parallel - //////////////////////////////// - public Command intakeDeployCommand(){ - return Commands.runOnce(() -> setRollerVoltage(IntakeConstants.rollerCollectVoltage)) - .andThen(Commands.runOnce(()->deployIntake() )); - } - - //////////////////////////////////////// - ///Stow and stop the rollers in parallel - //////////////////////////////////////// - public Command stopStowCommand(){ - return Commands.runOnce(() -> stopRollerCommand()) - .andThen(Commands.runOnce(()-> stowIntakeCommand())); + /** + * Command that stows the intake and ends when it gets there. + * @return + */ + public Command stowCmd() { + return Commands.runOnce(() -> stow()) + .andThen(Commands.waitUntil(() -> isIntakeStowed()) + .withTimeout(2)).withName("Stow Intake"); + } + + /** + * Command that deploys the intake and ends when it gets there. + * @return + */ + public Command deployCmd() { + return startDeployCmd() + .andThen(Commands.waitUntil(this::isIntakeDeployed) + .withTimeout(2)).withName("Deploy Intake"); + } + + /** + * Command that starts the deployment of the intake, but doesnt wait until its done. + * @return + */ + public Command startDeployCmd() { + return runOnce(this::deploy); + } + + /** + * Command that runs the intake until the command ends. + * @return The command + */ + public Command runIntakeCmd() { + return startEnd(this::startIntaking, this::stopIntaking); + } + + public Command intakeSequence() { + return runIntakeCmd().beforeStarting( + deployCmd().onlyIf(this::isIntakeStowed) + ); } - //////////////////////////// /// Sys ID Routine creation/ /// //////////////////////// @@ -179,7 +160,7 @@ public final SysIdRoutine pivotSysIdRoutine(){ (state) -> Logger.recordOutput("state", state.toString()) //Logging the state of the routine ), new SysIdRoutine.Mechanism( - (Voltage voltage)-> setPivotVoltage(voltage), + (Voltage voltage)-> io.setPivotVoltage(voltage), null, this ) diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java new file mode 100644 index 0000000..48dacf9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.measure.Angle; + +public interface HoodIO { + @AutoLog + public static class HoodInputs { + public Angle position; + } + + public default void updateInputs(HoodInputs inputs) {}; + + public default void runPosition(Angle angle) {}; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java new file mode 100644 index 0000000..2be5f36 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Servo; + +public class HoodIOServo implements HoodIO { + + private Servo hoodLeft; + private Servo hoodRight; + + @Override + public void updateInputs(HoodInputs inputs) { + inputs.position = Degrees.of(hoodLeft.getAngle()); + } + + @Override + public void runPosition(Angle angle) { + double degrees = angle.in(Degrees); + hoodLeft.setAngle(degrees); + hoodRight.setAngle(degrees); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java new file mode 100644 index 0000000..c9cc9a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; + +public class HoodIOSim implements HoodIO { + private Angle setpointAngle = Degrees.zero(); + + @Override + public void updateInputs(HoodInputs inputs) { + inputs.position = setpointAngle; + } + + @Override + public void runPosition(Angle angle) { + setpointAngle = angle; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f7b12c0..8a0007d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,7 +1,9 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RevolutionsPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -19,83 +21,101 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.subsystems.hopper.Hopper; +import frc.robot.util.MapleSimUtil; +import frc.robot.util.Mechanism3d; public class Shooter extends SubsystemBase { - private final ShooterIO io_; - private final ShooterIOInputsAutoLogged inputs_; - private AngularVelocity shooterTarget; - private Angle hoodTarget; - - public Shooter(ShooterIO io) { - io_ = io; - inputs_ = new ShooterIOInputsAutoLogged(); - shooterTarget = RotationsPerSecond.zero(); - hoodTarget = Radians.zero(); + private final ShooterIO shooterIO; + private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + + private final HoodIO hoodIO; + private final HoodInputsAutoLogged hoodInputs = new HoodInputsAutoLogged(); + + private AngularVelocity shooterTarget = RadiansPerSecond.zero(); + private Angle hoodTarget = Radians.zero(); + + public Shooter(ShooterIO ioShooter, HoodIO ioHood) { + this.shooterIO = ioShooter; + this.hoodIO = ioHood; } @Override public void periodic() { - io_.updateInputs(inputs_); - Logger.processInputs("Shooter", inputs_); + shooterIO.updateInputs(shooterInputs); + Logger.processInputs("Shooter", shooterInputs); + hoodIO.updateInputs(hoodInputs); + Logger.processInputs("Shooter/Hood", hoodInputs); + + Mechanism3d.measured.setHood(hoodInputs.position); + Mechanism3d.setpoints.setHood(hoodTarget); + + if (Constants.getMode() == Mode.SIM) { + MapleSimUtil.setShooterVelocity(shooterInputs.wheelVelocity); + MapleSimUtil.setHoodAngle(hoodInputs.position); + } + Logger.recordOutput("Shooter/VelocitySetPoint", shooterTarget); Logger.recordOutput("Shooter/HoodSetPoint", hoodTarget); } // Shooter Methods - public void setShooterVelocity(AngularVelocity vel) { + private void setShooterVelocity(AngularVelocity vel) { shooterTarget = vel; - io_.setShooterVelocity(vel); + shooterIO.setVelocity(vel); } - public void setShooterVoltage(Voltage vol) { - shooterTarget = RotationsPerSecond.of(0.0); - io_.setShooterVoltage(vol); + private void setShooterVoltage(Voltage vol) { + shooterTarget = RotationsPerSecond.zero(); + shooterIO.setVoltage(vol); } - public void stopShooter() { - shooterTarget = RotationsPerSecond.of(0.0); - io_.stopShooter(); + private void stopShooter() { + shooterTarget = RotationsPerSecond.zero(); + shooterIO.stop(); } public AngularVelocity getShooterVelocity() { - return inputs_.shooter1Velocity; - } - - public boolean isShooterReady() { - return Math.abs(inputs_.shooter1Velocity.in(RotationsPerSecond) - shooterTarget.in(RotationsPerSecond)) < ShooterConstants.shooterTolerance.in(RevolutionsPerSecond); + return shooterInputs.wheelVelocity; } public Voltage getShooterVoltage() { - return inputs_.shooter1Voltage; + return shooterInputs.shooter1Voltage; + } + + public boolean isShooterReady() { + return shooterInputs.wheelVelocity.isNear(shooterTarget, ShooterConstants.shooterTolerance); } public Current getShooterCurrent() { - return (inputs_.shooter1Current) - .plus(inputs_.shooter2Current) - .plus(inputs_.shooter3Current) - .plus(inputs_.shooter4Current); + return (shooterInputs.shooter1Current) + .plus(shooterInputs.shooter2Current) + .plus(shooterInputs.shooter3Current) + .plus(shooterInputs.shooter4Current); } - public Command setVelocityCmd(AngularVelocity vel) { + public Command runToVelocityCmd(AngularVelocity vel) { return runOnce(() -> setShooterVelocity(vel)) - .andThen(Commands.waitUntil(() -> isShooterReady())).withName("Set Shooter Velocity"); + .andThen(Commands.waitUntil(this::isShooterReady)).withName("Set Shooter Velocity"); } - public Command stopCommand() { + public Command stopCmd() { return runOnce(() -> stopShooter()) - .andThen(Commands.waitUntil(() -> isShooterReady())).withName("Stop Shooter"); + .andThen(Commands.waitUntil(this::isShooterReady)).withName("Stop Shooter"); } - public Command setVoltageCmd(Voltage vol) { + public Command runVoltageCmd(Voltage vol) { return runOnce(() -> setShooterVoltage(vol)).withName("Set Shooter Voltage"); } // Hood Methods - public void setHoodAngle(Angle pos) { + private void setHoodAngle(Angle pos) { hoodTarget = pos; - io_.setHoodPosition(pos); + hoodIO.runPosition(pos); } public Command hoodToPosCmd(Angle pos) { @@ -103,19 +123,36 @@ public Command hoodToPosCmd(Angle pos) { } // Both - public void goToShooterPosition(AngularVelocity vel, Angle pos) { + private void setSetpoints(AngularVelocity vel, Angle pos) { shooterTarget = vel; - this.setShooterVelocity(shooterTarget); + setShooterVelocity(shooterTarget); hoodTarget = pos; - this.setHoodAngle(hoodTarget); + setHoodAngle(hoodTarget); + } + + /** + * Shoot balls from the shooter until the command ends. + * @return + */ + public Command shootCmd(Hopper hopper) { + return Commands.parallel( + runDynamicSetpoints(() -> RPM.of(1000), () -> Degrees.of(45)), + hopper.forwardFeed() + ); } - public Command runSetpointCmd(AngularVelocity vel, Angle pos) { - return runOnce(() -> goToShooterPosition(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady)); + public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) { + return runOnce(() -> setSetpoints(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady)); } - public Command runDynamicSetpoint(Supplier vel, Supplier pos ) { - return run(() -> goToShooterPosition(vel.get(), pos.get())); + /** + * Runs supplied setpoints until the command ends, then stops. + * @param vel + * @param pos + * @return + */ + public Command runDynamicSetpoints(Supplier vel, Supplier pos ) { + return runEnd(() -> setSetpoints(vel.get(), pos.get()), this::stopShooter); } // Sys ID @@ -130,10 +167,10 @@ public Command shooterSysIdDynamic(SysIdRoutine.Direction dir) { private SysIdRoutine shooterIdRoutine() { Voltage step = Volts.of(7); Time to = Seconds.of(10.0); - SysIdRoutine.Config cfg = new SysIdRoutine.Config(null, step, to, (state) -> Logger.recordOutput("HoodSysIdTestState", state.toString())); + SysIdRoutine.Config cfg = new SysIdRoutine.Config(null, step, to, (state) -> Logger.recordOutput("SysIdTestState", state.toString())); SysIdRoutine.Mechanism mfg = new SysIdRoutine.Mechanism( - (volts) -> io_.setShooterVoltage(volts), + (volts) -> shooterIO.setVoltage(volts), null, this); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f3b8171..100dca1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -15,18 +15,17 @@ public class ShooterConstants { public static final int hoodCANID = 4; - public static final int currentLimit = 40; public static final Time currentLimitTime = Seconds.of(1); - public static final double gearRatio = 75/27; + public static final double gearRatio = 75.0 / 27.0; public static final AngularVelocity shooterTolerance = RotationsPerSecond.of(1.0); public class PID { // shooter - public static final double shooterkP = 0.0; + public static final double shooterkP = 0.5; public static final double shooterkI = 0.0; public static final double shooterkD = 0.0; public static final double shooterkV = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 3073880..2bd953b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,13 +1,11 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -35,20 +33,15 @@ public static class ShooterIOInputs { public Voltage shooter4Voltage = Volts.zero(); public Current shooter4Current = Amps.zero(); public AngularVelocity shooter4Velocity = RadiansPerSecond.zero(); - - public Angle hoodPosition = Radians.zero(); - public Voltage hoodVoltage = Volts.zero(); - public Current hoodCurrent = Amps.zero(); - public AngularVelocity hoodVelocity = RadiansPerSecond.zero(); + + public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } public default void updateInputs(ShooterIOInputs inputs) {} - public default void setShooterVelocity(AngularVelocity vel) {} - - public default void setShooterVoltage(Voltage vol) {} + public default void setVelocity(AngularVelocity vel) {} - public default void stopShooter() {} + public default void setVoltage(Voltage vol) {} - public default void setHoodPosition(Angle pos) {} + public default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java new file mode 100644 index 0000000..c61cb32 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Robot; + +public class ShooterIOSim extends ShooterIOTalonFX { + + private final FlywheelSim flywheelSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60Foc(3), 0.01, ShooterConstants.gearRatio + ), + DCMotor.getKrakenX60Foc(3) + ); + + public ShooterIOSim() { + super(new CANBus()); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + TalonFXSimState[] motorStates = { + shooter1Motor.getSimState(), + shooter2Motor.getSimState(), + shooter3Motor.getSimState() + }; + + flywheelSim.setInputVoltage(motorStates[0].getMotorVoltage()); + flywheelSim.update(Robot.defaultPeriodSecs); + + for (TalonFXSimState simState : motorStates) { + simState.setSupplyVoltage(RobotController.getBatteryVoltage()); + simState.setRotorVelocity(flywheelSim.getAngularVelocity().times(ShooterConstants.gearRatio)); + simState.setRotorAcceleration(flywheelSim.getAngularAcceleration().times(ShooterConstants.gearRatio)); + } + + super.updateInputs(inputs); + } + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 99cd9ed..98439c9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.StatusSignalCollection; @@ -9,25 +11,17 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; -import static edu.wpi.first.units.Units.Degrees; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.Servo; public class ShooterIOTalonFX implements ShooterIO { - private TalonFX shooter1Motor; - private TalonFX shooter2Motor; - private TalonFX shooter3Motor; - - private Servo hood1; - private Servo hood2; - + protected TalonFX shooter1Motor; + protected TalonFX shooter2Motor; + protected TalonFX shooter3Motor; private StatusSignalCollection signals; @@ -49,13 +43,8 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooter2Motor = new TalonFX(ShooterConstants.shooter2CANID, shooterCANBus); shooter3Motor = new TalonFX(ShooterConstants.shooter3CANID, shooterCANBus); //shooter4Motor = new TalonFX(ShooterConstants.shooter4CANID, shooterCANBus); - - //hoodMotor = new TalonFX(ShooterConstants.hoodCANID, shooterCANBus); - hood1 = new Servo(0); - hood2 = new Servo(1); final TalonFXConfiguration shooterConfigs = new TalonFXConfiguration(); - final TalonFXConfiguration hoodConfigs = new TalonFXConfiguration(); // PID shooterConfigs.Slot0.kP = ShooterConstants.PID.shooterkP; @@ -66,22 +55,6 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.Slot0.kG = ShooterConstants.PID.shooterkG; shooterConfigs.Slot0.kS = ShooterConstants.PID.shooterkS; - - hoodConfigs.Slot0.kP = ShooterConstants.PID.hoodkP; - hoodConfigs.Slot0.kD = ShooterConstants.PID.hoodkD; - hoodConfigs.Slot0.kI = ShooterConstants.PID.hoodkI; - hoodConfigs.Slot0.kV = ShooterConstants.PID.hoodkV; - hoodConfigs.Slot0.kA = ShooterConstants.PID.hoodkA; - hoodConfigs.Slot0.kG = ShooterConstants.PID.hoodkG; - hoodConfigs.Slot0.kS = ShooterConstants.PID.hoodkS; - - - // Software Limits - only hood - hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMaxAngle; - hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMinAngle; - // Motion Magic Configurations shooterConfigs.MotionMagic.MotionMagicCruiseVelocity = ShooterConstants.MotionMagic.shooterkMaxVelocity; shooterConfigs.MotionMagic.MotionMagicAcceleration = ShooterConstants.MotionMagic.shooterkMaxAcceleration; @@ -91,18 +64,14 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; shooterConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - hoodConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; - shooterConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - // Similar to our checkError function tryUntilOk(5, () -> shooter1Motor.getConfigurator().apply(shooterConfigs, 0.25)); tryUntilOk(5, () -> shooter2Motor.getConfigurator().apply(shooterConfigs, 0.25)); tryUntilOk(5, () -> shooter3Motor.getConfigurator().apply(shooterConfigs, 0.25)); // Setting other shooter motors as followers - shooter2Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); - shooter3Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); - + shooter2Motor.setControl(new Follower(ShooterConstants.shooter1CANID, MotorAlignmentValue.Aligned)); + shooter3Motor.setControl(new Follower(ShooterConstants.shooter1CANID, MotorAlignmentValue.Aligned)); shooter1AngularVelocity = shooter1Motor.getVelocity(); shooter1AppliedVolts = shooter1Motor.getSupplyVoltage(); @@ -115,7 +84,7 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooter3CurrentAmps = shooter3Motor.getSupplyCurrent(); // Status Signal Collection, less repetitive code - StatusSignalCollection signals = new StatusSignalCollection( + signals = new StatusSignalCollection( shooter1AngularVelocity, shooter1AppliedVolts, shooter1CurrentAmps, @@ -147,24 +116,20 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.shooter1Current = shooter1CurrentAmps.getValue(); inputs.shooter2Current = shooter2CurrentAmps.getValue(); inputs.shooter3Current = shooter3CurrentAmps.getValue(); + + inputs.wheelVelocity = inputs.shooter1Velocity.times(ShooterConstants.gearRatio); } - public void setShooterVelocity(AngularVelocity vel) { - AngularVelocity velocity = vel.times(ShooterConstants.gearRatio); + public void setVelocity(AngularVelocity vel) { + AngularVelocity velocity = vel.div(ShooterConstants.gearRatio); shooter1Motor.setControl(new MotionMagicVelocityVoltage(velocity)); } - public void setShooterVoltage(Voltage vol) { + public void setVoltage(Voltage vol) { shooter1Motor.setControl(new VoltageOut(vol)); } - public void stopShooter() { - shooter1Motor.set(0); - } - - public void setHoodPosition(Angle pos) { - double position = pos.in(Degrees); - hood1.setAngle(position); - hood2.setAngle(position); + public void stop() { + shooter1Motor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java index 42a9b8c..ffcbc8c 100644 --- a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java +++ b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.thriftyclimb; import static edu.wpi.first.units.Units.Centimeter; -import static edu.wpi.first.units.Units.Meters; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java index f7619a9..428ced0 100644 --- a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java @@ -6,7 +6,7 @@ import edu.wpi.first.units.measure.MomentOfInertia; public class ThriftyClimbConstants { - public static final int thriftyClimbId = 1; + public static final int thriftyClimbId = 7; public static final Distance thriftyClimbSpoolDiameter = Inches.of(1.5 * 2); public static final double thriftyGearRatio = 1.0 / 50.0; diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java new file mode 100644 index 0000000..d02054a --- /dev/null +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -0,0 +1,178 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.Centimeters; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import org.ironmaple.simulation.IntakeSimulation; +import org.ironmaple.simulation.IntakeSimulation.IntakeSide; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.gamepieces.GamePieceProjectile; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; + +public class MapleSimUtil { + private static SwerveDriveSimulation drivebaseSimulation; + + private static IntakeSimulation intakeSimulation; + + private static Angle hoodAngle = Degrees.zero(); + private static boolean shooterRunning = false; + private static AngularVelocity shooterVelocity = RadiansPerSecond.zero(); + + private static final Distance shooterRadius = Centimeters.of(5); + + private static final Command run = + Commands.run(MapleSimUtil::periodic) + .ignoringDisable(true) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + + private static final Command runShooter = + Commands.runOnce(() -> { + if (getRemainingGamepieces() == 0) return; + + var fuelLeft = createProjectile(Inches.of(3.7)); + loseGamepiece(); + SimulatedArena.getInstance().addGamePieceProjectile(fuelLeft); + + if (getRemainingGamepieces() == 0) return; + + var fuelRight = createProjectile(Inches.of(-3.7)); + loseGamepiece(); + SimulatedArena.getInstance().addGamePieceProjectile(fuelRight); + }) + .andThen(Commands.waitTime(Milliseconds.of(1000 / 10))) + .repeatedly(); + + /** Starts the MapleSim simulation, kicks off a periodic method. */ + public static void start() { + var rebuilt = (Arena2026Rebuilt) SimulatedArena.getInstance(); + rebuilt.setEfficiencyMode(Constants.spawnLessFuelInSim); + rebuilt.resetFieldForAuto(); + + new Trigger(() -> shooterRunning).whileTrue(runShooter); + + CommandScheduler.getInstance().schedule(run); + } + + private static void periodic() { + var arena = (Arena2026Rebuilt) SimulatedArena.getInstance(); + + arena.simulationPeriodic(); + + var fuel = arena.getGamePiecesArrayByType("Fuel"); + + Logger.recordOutput("MapleSim/Gamepieces", fuel); + Logger.recordOutput("MapleSim/Pose", getPosition()); + Logger.recordOutput("MapleSim/Score/Red", arena.getScore(Alliance.Red)); + Logger.recordOutput("MapleSim/Score/Blue", arena.getScore(Alliance.Blue)); + Logger.recordOutput("MapleSim/FuelRemaining", getRemainingGamepieces()); + Logger.recordOutput("MapleSim/Intaking", intakeSimulation.isRunning()); + Logger.recordOutput("MapleSim/ActiveHub", arena.isActive(true) ? Alliance.Blue : Alliance.Red); + } + + public static SwerveDriveSimulation createSwerve(DriveTrainSimulationConfig config, Pose2d initialPose) { + drivebaseSimulation = new SwerveDriveSimulation(config, initialPose); + SimulatedArena.getInstance().addDriveTrainSimulation(drivebaseSimulation); + return drivebaseSimulation; + } + + public static IntakeSimulation createIntake() { + if (drivebaseSimulation == null) throw new IllegalStateException("Intake cannot be created before swerve is!"); + + intakeSimulation = IntakeSimulation.OverTheBumperIntake( + "Fuel", + drivebaseSimulation, + Inches.of(29.5), + Inches.of(8.125), + IntakeSide.FRONT, + 80 + ); + + return intakeSimulation; + } + + public static GamePieceProjectile createProjectile(Distance yOffset) { + return new RebuiltFuelOnFly( + getPosition().getTranslation(), + new Translation2d(Inches.of(-8), Inches.of(3.7)), // Initial Shooter Position + getFieldChassisSpeeds(), + getPosition().getRotation(), + Inches.of(18.5), // Initial Height + MetersPerSecond.of(shooterVelocity.in(RadiansPerSecond) * shooterRadius.in(Meters)), + hoodAngle + ) + .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) + .withProjectileTrajectoryDisplayCallBack( + poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) + ); + } + + public static Pose2d getPosition() { + return drivebaseSimulation.getSimulatedDriveTrainPose(); + } + + public static ChassisSpeeds getFieldChassisSpeeds() { + return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); + } + + public static int getRemainingGamepieces() { + return intakeSimulation.getGamePiecesAmount(); + } + + public static void setIntakeRunning(boolean running) { + if (running) { + intakeSimulation.startIntake(); + } else { + intakeSimulation.stopIntake(); + } + } + + public static void loseGamepiece() { + intakeSimulation.obtainGamePieceFromIntake(); + } + + public static void setShooterRunning(boolean running) { + shooterRunning = running; + } + + public static void setShooterVelocity(AngularVelocity velocity) { + shooterVelocity = velocity; + } + + public static void setHoodAngle(Angle angle) { + hoodAngle = angle; + } + + public static GyroSimulation getGyroSimulation() { + return drivebaseSimulation.getGyroSimulation(); + } + + public static SwerveModuleSimulation[] getModuleSimulations() { + return drivebaseSimulation.getModules(); + } + } diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index 95c9d02..bc08e5e 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -14,8 +14,15 @@ package frc.robot.util; import com.ctre.phoenix6.StatusCode; + +import edu.wpi.first.wpilibj.Timer; + +import static edu.wpi.first.units.Units.Seconds; + import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; + public class PhoenixUtil { /** Attempts to run the command until no error is produced. */ public static void tryUntilOk(int maxAttempts, Supplier command) { @@ -24,4 +31,16 @@ public static void tryUntilOk(int maxAttempts, Supplier command) { if (error.isOK()) break; } } + + public static double[] getSimulationOdometryTimestamps() { + final double[] odometryTimeStamps = new double[SimulatedArena.getSimulationSubTicksIn1Period()]; + + for (int i = 0; i < odometryTimeStamps.length; i++) { + odometryTimeStamps[i] = Timer.getFPGATimestamp() + - 0.02 + + i * SimulatedArena.getSimulationDt().in(Seconds); + } + + return odometryTimeStamps; + } } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 0000000..c73bea4 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.4.0-beta", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.4.0-beta" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file