Integration Between Subsystems, Simulation, and Bugfixes #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft

blaze-developer wants to merge 25 commits into main from integration

src/main/java/frc/robot/Constants.java

-Original file line number
+Diff line change
@@ Expand Up / @@ -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;
         }
@@ Expand Down Expand Up / @@ -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.
@@ Expand Down @@

src/main/java/frc/robot/RobotContainer.java

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -6,56 +6,73 @@
  
    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;

    import frc.robot.subsystems.vision.AprilTagVision;

    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 {

        // 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<Command> 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()))

            );

        }

src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java

-Original file line number
+Diff line change
@@ -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();
+        }
+    }

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Integration Between Subsystems, Simulation, and Bugfixes #27

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!

Integration Between Subsystems, Simulation, and Bugfixes #27

Are you sure you want to change the base?

Uh oh!

Integration Between Subsystems, Simulation, and Bugfixes #27

Uh oh!

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!