From d783db146b5e06e38d8cbdeb4108c5c84dace947 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 22 Jan 2026 20:01:27 -0500 Subject: [PATCH 1/3] WIP 1/22/26 Instantiate Hopper Subsystem and Command! --- src/main/java/frc/robot/RobotContainer.java | 262 +++++++++++--------- 1 file changed, 138 insertions(+), 124 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4372b8d..417b95c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,14 +15,20 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.pathplanner.lib.auto.AutoBuilder; +import com.revrobotics.spark.config.SparkBaseConfig; + 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.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; 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.sysid.SysIdRoutine; +import frc.lib.W8.io.motor.*; +import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.util.Device.CAN; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; @@ -31,132 +37,140 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.hopper.Hopper; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and button mappings) should be declared here. - */ +* This class is where the bulk of the robot should be declared. Since Command-based is a +* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} +* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including +* subsystems, commands, and button mappings) should be declared here. +*/ public class RobotContainer { - // Subsystems - private final Drive drive; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - drive = - new Drive( - new GyroIOPigeon2(), - new ModuleIOTalonFX(TunerConstants.FrontLeft), - new ModuleIOTalonFX(TunerConstants.FrontRight), - new ModuleIOTalonFX(TunerConstants.BackLeft), - new ModuleIOTalonFX(TunerConstants.BackRight)); - break; - - case SIM: - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(TunerConstants.FrontLeft), - new ModuleIOSim(TunerConstants.FrontRight), - new ModuleIOSim(TunerConstants.BackLeft), - new ModuleIOSim(TunerConstants.BackRight)); - break; - - default: - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - break; - } - - // Set up auto routines - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - - // Set up SysId routines - autoChooser.addOption( - "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); - autoChooser.addOption( - "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); - autoChooser.addOption( - "Drive SysId (Quasistatic Forward)", - drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive SysId (Quasistatic Reverse)", - drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // Default command, normal field-relative drive - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> controller.getLeftY(), - () -> controller.getLeftX(), - () -> -controller.getRightX())); - - // Lock to 0° when A button is held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> new Rotation2d())); - - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - - // Reset gyro to 0° when B button is pressed - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); - } - - TalonFX billyBob = new TalonFX(30, TunerConstants.DrivetrainConstants.CANBusName); +// Subsystems +private final Drive drive; +//private final Hopper hopper; + +// Controller +private final CommandXboxController controller = new CommandXboxController(0); + +// Dashboard inputs +private final LoggedDashboardChooser autoChooser; + +/** The container for the robot. Contains subsystems, OI devices, and commands. */ +public RobotContainer() { +switch (Constants.currentMode) { +case REAL: +// Real robot, instantiate hardware IO implementations +drive = +new Drive( +new GyroIOPigeon2(), +new ModuleIOTalonFX(TunerConstants.FrontLeft), +new ModuleIOTalonFX(TunerConstants.FrontRight), +new ModuleIOTalonFX(TunerConstants.BackLeft), +new ModuleIOTalonFX(TunerConstants.BackRight)); + +/*hopper = +new Hopper(new FlywheelMechanismReal(new MotorIORev("HopperMotor", new CAN(0, "rio"), false, null, null)));*/ +break; + +case SIM: +// Sim robot, instantiate physics sim IO implementations +drive = +new Drive( +new GyroIO() {}, +new ModuleIOSim(TunerConstants.FrontLeft), +new ModuleIOSim(TunerConstants.FrontRight), +new ModuleIOSim(TunerConstants.BackLeft), +new ModuleIOSim(TunerConstants.BackRight)); + +/*hopper = +new Hopper(new FlywheelMechanismSim(new MotorIOSim("HopperMotor", new CAN(0, "rio"), false, null, null)), new DCMotor(0, 0, 0, 0, 0, 0));*/ +break; + +default: +// Replayed robot, disable IO implementations +drive = +new Drive( +new GyroIO() {}, +new ModuleIO() {}, +new ModuleIO() {}, +new ModuleIO() {}, +new ModuleIO() {}); +break; +} + +// Set up auto routines +autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + +// Set up SysId routines +autoChooser.addOption( +"Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); +autoChooser.addOption( +"Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); +autoChooser.addOption( +"Drive SysId (Quasistatic Forward)", +drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); +autoChooser.addOption( +"Drive SysId (Quasistatic Reverse)", +drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); +autoChooser.addOption( +"Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); +autoChooser.addOption( +"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + +// Configure the button bindings +configureButtonBindings(); +} + +/** +* Use this method to define your button->command mappings. Buttons can be created by +* instantiating a {@link GenericHID} or one of its subclasses ({@link +* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link +* edu.wpi.first.wpilibj2.command.button.JoystickButton}. +*/ +private void configureButtonBindings() { +// Default command, normal field-relative drive +drive.setDefaultCommand( +DriveCommands.joystickDrive( +drive, +() -> controller.getLeftY(), +() -> controller.getLeftX(), +() -> -controller.getRightX())); + +// Lock to 0° when A button is held +controller +.a() +.whileTrue( +DriveCommands.joystickDriveAtAngle( +drive, +() -> -controller.getLeftY(), +() -> -controller.getLeftX(), +() -> new Rotation2d())); + +// Switch to X pattern when X button is pressed +controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + +// Reset gyro to 0° when B button is pressed +controller +.b() +.onTrue( +Commands.runOnce( +() -> +drive.setPose( +new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), +drive) +.ignoringDisable(true)); } + +/** +* Use this to pass the autonomous command to the main {@link Robot} class. +* +* @return the command to run in autonomous +*/ +public Command getAutonomousCommand() { +return autoChooser.get(); +} + +TalonFX billyBob = new TalonFX(30, TunerConstants.DrivetrainConstants.CANBusName); +} \ No newline at end of file From ed7aafc96a5d020be2a3a64764784cd3fd95af9e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Wed, 28 Jan 2026 19:55:40 -0500 Subject: [PATCH 2/3] Mapped 'setGoal' and instantiated the hopper subsystem in RobotContainer.java - 1 / 28 / 26 --- src/main/java/frc/robot/Constants.java | 5 +++++ src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++++++++----- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4133372..f2cb9f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Second; +import com.revrobotics.spark.config.SparkMaxConfig; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Translation2d; @@ -90,6 +92,9 @@ public class HopperConstants { public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity ANGULAR_VELOCITY = RotationsPerSecond.of(1); public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); + public static final int CAN_ID = 0; + public static final SparkMaxConfig MOTOR_CONFIG = new SparkMaxConfig(); + public static final int HOPPER_POSITION = 1; } public class Ports { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 417b95c..a96825d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ 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.units.measure.MomentOfInertia; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -29,6 +30,7 @@ import frc.lib.W8.io.motor.*; import frc.lib.W8.mechanisms.flywheel.*; import frc.lib.W8.util.Device.CAN; +import frc.robot.Constants.HopperConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; @@ -38,6 +40,11 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.hopper.Hopper; + +import static edu.wpi.first.units.Units.Kilogram; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -49,7 +56,7 @@ public class RobotContainer { // Subsystems private final Drive drive; -//private final Hopper hopper; +private final Hopper hopper; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -70,8 +77,8 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackLeft), new ModuleIOTalonFX(TunerConstants.BackRight)); -/*hopper = -new Hopper(new FlywheelMechanismReal(new MotorIORev("HopperMotor", new CAN(0, "rio"), false, null, null)));*/ +hopper = +new Hopper(new FlywheelMechanismReal(new MotorIORev("HopperMotor", new CAN(HopperConstants.CAN_ID, "rio"), false, HopperConstants.MOTOR_CONFIG))); break; case SIM: @@ -84,8 +91,8 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); -/*hopper = -new Hopper(new FlywheelMechanismSim(new MotorIOSim("HopperMotor", new CAN(0, "rio"), false, null, null)), new DCMotor(0, 0, 0, 0, 0, 0));*/ +hopper = +new Hopper(new FlywheelMechanismSim(new MotorIORevSim("Simulated Rev Motor", new CAN(HopperConstants.CAN_ID, "rio"), false, 1, 1, DCMotor.getNEO(1), HopperConstants.MOTOR_CONFIG), DCMotor.getNEO(1), KilogramSquareMeters.of(0.5), RotationsPerSecond.of(0.1))); break; default: @@ -97,6 +104,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); +hopper = +new Hopper(new FlywheelMechanism() {}); break; } @@ -161,6 +170,9 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); + +// Followed example on line 161 +controller.x().onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); } /** From 06fb346412136b82930b875c768368c83509cb77 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Thu, 29 Jan 2026 20:12:20 -0500 Subject: [PATCH 3/3] 1/29/26 Fixed small issues --- src/main/java/frc/robot/Constants.java | 20 +- src/main/java/frc/robot/RobotContainer.java | 300 +++++++++--------- .../frc/robot/subsystems/hopper/Hopper.java | 25 +- 3 files changed, 183 insertions(+), 162 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f2cb9f0..684137d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,7 +21,6 @@ import static edu.wpi.first.units.Units.Second; import com.revrobotics.spark.config.SparkMaxConfig; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Translation2d; @@ -91,7 +90,8 @@ public class HopperConstants { private static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity ANGULAR_VELOCITY = RotationsPerSecond.of(1); - public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); + public static final AngularAcceleration ANGULAR_ACCELERATION = + RotationsPerSecondPerSecond.of(1); public static final int CAN_ID = 0; public static final SparkMaxConfig MOTOR_CONFIG = new SparkMaxConfig(); public static final int HOPPER_POSITION = 1; @@ -106,7 +106,7 @@ public class Ports { } public final class ShooterConstants { - // Constants for the Shooter + // Constants for the Shooter public static final Angle ANGLE_TOLERANCE = Rotations.of(0.01); public static final AngularVelocity ANGLE_VELOCITY_TOLERANCE = RotationsPerSecond.of(0.01); public static final AngularVelocity CRUISE_VELOCITY = RotationsPerSecond.of(204); @@ -119,9 +119,10 @@ public final class ShooterConstants { public static final Angle MAX_ANGLE = Rotations.of(10.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); - public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + public static final RotaryMechCharacteristics CONSTANTS = + new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } - + public static final int CANDLE_ID = 50; public class IntakeConstants { @@ -138,6 +139,7 @@ public class IntakeConstants { public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } + public class ClimberConstants { public static final Distance TOLERANCE = Inches.of(0.1); public static final double GEARING = (5.0 / 1.0); @@ -146,6 +148,12 @@ public class ClimberConstants { public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); - public static final LinearMechCharacteristics CHARACTERISTICS = new LinearMechCharacteristics(new Translation3d(0.0, 0.0, 0.0), MIN_DISTANCE, MAX_DISTANCE, STARTING_DISTANCE, CONVERTER); + public static final LinearMechCharacteristics CHARACTERISTICS = + new LinearMechCharacteristics( + new Translation3d(0.0, 0.0, 0.0), + MIN_DISTANCE, + MAX_DISTANCE, + STARTING_DISTANCE, + CONVERTER); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a96825d..52d9e86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,14 +13,13 @@ package frc.robot; -import com.ctre.phoenix6.hardware.TalonFX; -import com.pathplanner.lib.auto.AutoBuilder; -import com.revrobotics.spark.config.SparkBaseConfig; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import com.pathplanner.lib.auto.AutoBuilder; 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.units.measure.MomentOfInertia; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -40,149 +39,160 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.hopper.Hopper; - -import static edu.wpi.first.units.Units.Kilogram; -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** -* This class is where the bulk of the robot should be declared. Since Command-based is a -* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} -* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including -* subsystems, commands, and button mappings) should be declared here. -*/ + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. + */ public class RobotContainer { -// Subsystems -private final Drive drive; -private final Hopper hopper; - -// Controller -private final CommandXboxController controller = new CommandXboxController(0); - -// Dashboard inputs -private final LoggedDashboardChooser autoChooser; - -/** The container for the robot. Contains subsystems, OI devices, and commands. */ -public RobotContainer() { -switch (Constants.currentMode) { -case REAL: -// Real robot, instantiate hardware IO implementations -drive = -new Drive( -new GyroIOPigeon2(), -new ModuleIOTalonFX(TunerConstants.FrontLeft), -new ModuleIOTalonFX(TunerConstants.FrontRight), -new ModuleIOTalonFX(TunerConstants.BackLeft), -new ModuleIOTalonFX(TunerConstants.BackRight)); - -hopper = -new Hopper(new FlywheelMechanismReal(new MotorIORev("HopperMotor", new CAN(HopperConstants.CAN_ID, "rio"), false, HopperConstants.MOTOR_CONFIG))); -break; - -case SIM: -// Sim robot, instantiate physics sim IO implementations -drive = -new Drive( -new GyroIO() {}, -new ModuleIOSim(TunerConstants.FrontLeft), -new ModuleIOSim(TunerConstants.FrontRight), -new ModuleIOSim(TunerConstants.BackLeft), -new ModuleIOSim(TunerConstants.BackRight)); - -hopper = -new Hopper(new FlywheelMechanismSim(new MotorIORevSim("Simulated Rev Motor", new CAN(HopperConstants.CAN_ID, "rio"), false, 1, 1, DCMotor.getNEO(1), HopperConstants.MOTOR_CONFIG), DCMotor.getNEO(1), KilogramSquareMeters.of(0.5), RotationsPerSecond.of(0.1))); -break; - -default: -// Replayed robot, disable IO implementations -drive = -new Drive( -new GyroIO() {}, -new ModuleIO() {}, -new ModuleIO() {}, -new ModuleIO() {}, -new ModuleIO() {}); -hopper = -new Hopper(new FlywheelMechanism() {}); -break; + // Subsystems + private final Drive drive; + private final Hopper hopper; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + drive = + new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight)); + + hopper = + new Hopper( + new FlywheelMechanismReal( + new MotorIORev( + "HopperMotor", + new CAN(HopperConstants.CAN_ID, "rio"), + false, + HopperConstants.MOTOR_CONFIG))); + break; + + case SIM: + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(TunerConstants.FrontLeft), + new ModuleIOSim(TunerConstants.FrontRight), + new ModuleIOSim(TunerConstants.BackLeft), + new ModuleIOSim(TunerConstants.BackRight)); + + hopper = + new Hopper( + new FlywheelMechanismSim( + new MotorIORevSim( + "Simulated Rev Motor", + new CAN(HopperConstants.CAN_ID, "rio"), + false, + 1, + 1, + DCMotor.getNEO(1), + HopperConstants.MOTOR_CONFIG), + DCMotor.getNEO(1), + KilogramSquareMeters.of(0.5), + RotationsPerSecond.of(0.1))); + break; + + default: + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + hopper = new Hopper(new FlywheelMechanism() {}); + break; + } + + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + + // Set up SysId routines + autoChooser.addOption( + "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addOption( + "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); + autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Default command, normal field-relative drive + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> controller.getLeftY(), + () -> controller.getLeftX(), + () -> -controller.getRightX())); + + // Lock to 0° when A button is held + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> new Rotation2d())); + + // Switch to X pattern when X button is pressed + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + + // Reset gyro to 0° when B button is pressed + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); + + controller + .x() + .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } - -// Set up auto routines -autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - -// Set up SysId routines -autoChooser.addOption( -"Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); -autoChooser.addOption( -"Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive)); -autoChooser.addOption( -"Drive SysId (Quasistatic Forward)", -drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); -autoChooser.addOption( -"Drive SysId (Quasistatic Reverse)", -drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); -autoChooser.addOption( -"Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); -autoChooser.addOption( -"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - -// Configure the button bindings -configureButtonBindings(); -} - -/** -* Use this method to define your button->command mappings. Buttons can be created by -* instantiating a {@link GenericHID} or one of its subclasses ({@link -* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link -* edu.wpi.first.wpilibj2.command.button.JoystickButton}. -*/ -private void configureButtonBindings() { -// Default command, normal field-relative drive -drive.setDefaultCommand( -DriveCommands.joystickDrive( -drive, -() -> controller.getLeftY(), -() -> controller.getLeftX(), -() -> -controller.getRightX())); - -// Lock to 0° when A button is held -controller -.a() -.whileTrue( -DriveCommands.joystickDriveAtAngle( -drive, -() -> -controller.getLeftY(), -() -> -controller.getLeftX(), -() -> new Rotation2d())); - -// Switch to X pattern when X button is pressed -controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - -// Reset gyro to 0° when B button is pressed -controller -.b() -.onTrue( -Commands.runOnce( -() -> -drive.setPose( -new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), -drive) -.ignoringDisable(true)); - -// Followed example on line 161 -controller.x().onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); -} - -/** -* Use this to pass the autonomous command to the main {@link Robot} class. -* -* @return the command to run in autonomous -*/ -public Command getAutonomousCommand() { -return autoChooser.get(); -} - -TalonFX billyBob = new TalonFX(30, TunerConstants.DrivetrainConstants.CANBusName); -} \ 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 95f1b8e..08d7799 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -6,23 +6,26 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; -import frc.robot.Constants; import frc.robot.Constants.HopperConstants; public class Hopper extends SubsystemBase { -private FlywheelMechanism _io; + private FlywheelMechanism _io; -public Hopper(FlywheelMechanism io) { -_io = io; -} + public Hopper(FlywheelMechanism io) { + _io = io; + } -public void setGoal(double position) { - Distance positionInches = Inches.of(position); - _io.runPosition(HopperConstants.CONVERTER.toAngle(positionInches), HopperConstants.ANGULAR_VELOCITY, HopperConstants.ANGULAR_ACCELERATION, null, PIDSlot.SLOT_0); -} + public void setGoal(double position) { + Distance positionInches = Inches.of(position); + _io.runPosition( + HopperConstants.CONVERTER.toAngle(positionInches), + HopperConstants.ANGULAR_VELOCITY, + HopperConstants.ANGULAR_ACCELERATION, + null, + PIDSlot.SLOT_0); + } @Override public void periodic() {} - -} \ No newline at end of file +}