diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4133372..684137d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,7 @@ 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; @@ -89,7 +90,11 @@ 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; } public class Ports { @@ -101,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); @@ -114,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 { @@ -133,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); @@ -141,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 4372b8d..52d9e86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,16 +13,23 @@ package frc.robot; -import com.ctre.phoenix6.hardware.TalonFX; +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.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.Constants.HopperConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; @@ -31,6 +38,7 @@ 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; /** @@ -42,6 +50,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Hopper hopper; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -61,6 +70,15 @@ public RobotContainer() { 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: @@ -72,6 +90,21 @@ public RobotContainer() { 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: @@ -83,6 +116,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + hopper = new Hopper(new FlywheelMechanism() {}); break; } @@ -147,6 +181,10 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); + + controller + .x() + .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); } /** @@ -157,6 +195,4 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } - - TalonFX billyBob = new TalonFX(30, TunerConstants.DrivetrainConstants.CANBusName); } 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 +}