diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bdc74e7..d1398d7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,23 +13,38 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.RGBWColor; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.AngularAccelerationUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; @@ -63,11 +78,18 @@ public static enum Mode { REPLAY } + public class LEDConstants { + public static final RainbowAnimation rainbowAnim = new RainbowAnimation(0, 2); + public static final RGBWColor colorPaleBlue = new RGBWColor(165, 180, 208, 0); + public static final RGBWColor colorWheezerBlue = new RGBWColor(24, 155, 204, 0); + } + public class FieldConstants { /** - *     Contains various field dimensions and useful reference points. All units are in meters - * and poses    have a blue alliance origin. + * Contains various field dimensions and useful reference points. All units are in meters and + * poses have a blue alliance origin. */ + // TODO: Update to 2026 Field Constants and add HUB Center public static final AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); @@ -79,36 +101,102 @@ public class FieldConstants { public static final Distance ALGAEDIAMETER = Meters.of(.41); } + public class Ports { + // Constants for Port Values + public static final Device.CAN IntakeRoller = new CAN(1, "rio"); + public static final Device.CAN LEDs = new CAN(2, "rio"); + public static final Device.CAN HopperRoller = new CAN(3, "rio"); + public static final Device.CAN ClimberLinearMechanism = new CAN(4, "rio"); + } + public class HopperConstants { // holds constants for the hopper - public static final Distance TOLERANCE = Inches.of(2.0); - public static final Double GEARING = (5.0 / 1.0); - public static final Distance MIN_DISTANCE = Inches.of(0.0); - public static final Distance MAX_DISTANCE = Inches.of(15.0); - public static final Distance STARTING_DISTANCE = Inches.of(0.0); + + public static final String MOTOR_NAME = "Hopper Roller"; // CHANGE TO PROPER RPMS !!!! public static final double SLOW_SPEED_RPM = 0.0; public static final double FAST_SPEED_RPM = 0.0; public static final double REVERSE_SPEED_RPM = 0.0; public static final Voltage VOLTAGE = Volts.of(12.0); - public 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 int HOPPER_POSITION = 1; - public class Ports { - // Constants for Port Values !!!!!! - public static final Device.CAN IntakeRoller = new CAN(1, "rio"); - public static final Device.CAN LEDs = new CAN(2, "rio"); - public static final Device.CAN HopperRoller = new CAN(3, "rio"); - public static final Device.CAN ClimberLinearMechanism = new CAN(4, "rio"); + public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); + + // Mechanism Constants + public static final AngularVelocity MAX_VELOCITY = RotationsPerSecond.of(3200 / 60); + public static final AngularAcceleration MAX_ACCELERATION = + RotationsPerSecondPerSecond.of(3200 / 60 * 10); + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final AngularVelocity CRUISE_VELOCITY = + RadiansPerSecond.of(2 * Math.PI).times(10.0); + public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); + public static final Velocity JERK = ACCELERATION.per(Second); + + public static final double GEARING = (5.0 / 1.0); + public static final Distance MIN_DISTANCE = Inches.of(0.0); + public static final Distance MAX_DISTANCE = Inches.of(10.0); + public static final Distance STARTING_DISTANCE = Inches.of(0.0); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + + 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 TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + + config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); + config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); + config.MotionMagic.MotionMagicJerk = JERK.in(RotationsPerSecondPerSecond.per(Second)); + + return config; + } // End here } public final class ShooterConstants { - // Constants for the Shooter + // Constants for 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); @@ -121,30 +209,182 @@ 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 double IDLE_SPEED_RPM = (1.0); public static final double HUB_SPEED_RPM = (1.0); public static final double TOWER_SPEED_RPM = (1.0); public static final double DEFAULT_SPEED_RPM = (1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; + + // Hood Constants + public static final double HEIGHT_DIFFERENCE = + 1.295; // Meters between flywheel center and top of hub opening + public static final double EXIT_VELOCITY = 7.4; // m/s from ReCalc Flywheel Calculator + public static final AngularVelocity HOOD_VELOCITY = RotationsPerSecond.of(1.0); + public static final AngularAcceleration HOOD_ACCELERATION = RotationsPerSecondPerSecond.of(1.0); + public static final Velocity HOOD_JERK = HOOD_ACCELERATION.per(Second); + public static final double HOOD_TOLERANCE = 1.0; // In degrees + public static final double GRAVITY = 9.81; // m/s^2 + public static final double IDLE_HOOD_ANGLE = 25.0; // degrees + 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 { - // constants the intake :thumbs_up: :D - public static final AngularVelocity TOLERANCE = RotationsPerSecond.of(0.0); - public static final AngularVelocity CRUISE_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularAcceleration ACCELERATION = RotationsPerSecondPerSecond.of(0.0); - public static final Velocity JERK = ACCELERATION.per(Second); + // Constants for Intake public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + public static final String MOTOR_NAME = "Intake Flywheel"; + + // Mechanism Constants + public static final AngularVelocity MAX_VELOCITY = RotationsPerSecond.of(3200 / 60); + public static final AngularAcceleration MAX_ACCELERATION = + RotationsPerSecondPerSecond.of(3200 / 60 * 10); + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final AngularVelocity CRUISE_VELOCITY = + RadiansPerSecond.of(2 * Math.PI).times(10.0); + public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); + public static final Velocity JERK = ACCELERATION.per(Second); + + public static final double GEARING = (5.0 / 1.0); + public static final Distance MIN_DISTANCE = Inches.of(0.0); + public static final Distance MAX_DISTANCE = Inches.of(10.0); + public static final Distance STARTING_DISTANCE = Inches.of(0.0); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + + public static final Distance DRUM_RADIUS = Inches.of(2.0); + public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); + private static final Angle ENCODER_OFFSET = Rotations.of(0.0); + + public static final LinearMechCharacteristics CHARACTERISTICS = + new LinearMechCharacteristics( + new Translation3d(0.0, 0.0, 0.0), + MIN_DISTANCE, + MAX_DISTANCE, + STARTING_DISTANCE, + CONVERTER); + + public static CANcoderConfiguration getCANcoderConfig(boolean sim) { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.MagnetOffset = sim ? 0.0 : ENCODER_OFFSET.in(Rotations); + + return config; + } + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + + config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); + config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); + config.MotionMagic.MotionMagicJerk = JERK.in(RotationsPerSecondPerSecond.per(Second)); + + return config; + } + } + + public class IntakePivotConstants { + public static final String NAME = "Intake"; + + public static final Angle TOLERANCE = Degrees.of(1.0); + + public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); + public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100); + public static final Velocity JERK = + RadiansPerSecondPerSecond.per(Second).of(0); + + private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = 1.0; + + public static final Angle MIN_ANGLE = Degrees.of(-90.0); + public static final Angle MAX_ANGLE = Degrees.of(90.0); + public static final Angle STARTING_ANGLE = Radians.zero(); + public static final Distance ARM_LENGTH = Foot.one(); + + public static final RotaryMechCharacteristics CONSTANTS = + new RotaryMechCharacteristics( + new Translation3d(), ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25); + + // Positional PID + public static final Slot0Configs SLOT_0_CONFIG = + new Slot0Configs().withKP(10.0).withKI(2.0).withKD(8).withKS(0.07).withKV(0.1); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimit = 120.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Rotations); + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Rotations); + + config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; + config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; + + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; + + config.Slot0 = SLOT_0_CONFIG; + config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); + config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); + config.MotionMagic.MotionMagicJerk = JERK.in(RotationsPerSecondPerSecond.per(Second)); + + return config; + } } public class FeederConstants { @@ -167,5 +407,6 @@ public class ClimberConstants { MAX_DISTANCE, STARTING_DISTANCE, CONVERTER); + public static final double CLIMB_SPEED = 1.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4372b8d..777852a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,6 @@ package frc.robot; -import com.ctre.phoenix6.hardware.TalonFX; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,14 +22,26 @@ 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.absoluteencoder.AbsoluteEncoderIOCANCoderSim; +import frc.lib.W8.io.motor.*; +import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; +import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.HopperConstants; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.Ports; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.Hopper; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -42,6 +53,8 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Hopper hopper; + private final Intake intake; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -61,6 +74,33 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.FrontRight), new ModuleIOTalonFX(TunerConstants.BackLeft), new ModuleIOTalonFX(TunerConstants.BackRight)); + + hopper = + new Hopper( + new FlywheelMechanismReal( + new MotorIOTalonFX( + HopperConstants.MOTOR_NAME, + HopperConstants.getFXConfig(), + Ports.HopperRoller))); + + intake = + new Intake( + new FlywheelMechanismReal( + new MotorIOTalonFX( + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), + Ports.IntakeRoller)), + new RotaryMechanismReal( + new MotorIOTalonFX( + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), + Ports.IntakeRoller), + IntakeConstants.CONSTANTS, + Optional.of( + new AbsoluteEncoderIOCANCoderSim( + Ports.IntakeRoller, + IntakeConstants.MOTOR_NAME + " Encoder", + IntakeConstants.getCANcoderConfig(false))))); break; case SIM: @@ -72,6 +112,42 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); + + hopper = + new Hopper( + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + HopperConstants.MOTOR_NAME, + HopperConstants.getFXConfig(), + Ports.HopperRoller), + HopperConstants.DCMOTOR, + HopperConstants.MOI, + HopperConstants.TOLERANCE)); + + intake = + new Intake( + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), + Ports.IntakeRoller), + IntakeConstants.DCMOTOR, + IntakeConstants.MOI, + IntakeConstants.TOLERANCE), + new RotaryMechanismSim( + new MotorIOTalonFXSim( + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), + Ports.IntakeRoller), + IntakeConstants.DCMOTOR, + IntakeConstants.MOI, + false, + IntakeConstants.CONSTANTS, + Optional.of( + new AbsoluteEncoderIOCANCoderSim( + Ports.IntakeRoller, + IntakeConstants.MOTOR_NAME + " Encoder", + IntakeConstants.getCANcoderConfig(false))))); break; default: @@ -83,6 +159,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + + hopper = new Hopper(new FlywheelMechanism() {}); + + intake = + new Intake( + new FlywheelMechanism() {}, + new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {}); break; } @@ -147,6 +230,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 +244,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/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..6bc1cbb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.robot.Constants.ClimberConstants; + +public class Climber extends SubsystemBase { + private LinearMechanism _io; + + public Climber(LinearMechanism io) { + io = _io; + } + + public enum State { + IDLE(Units.MetersPerSecond.of(0.0)), + ASCENDING(Units.MetersPerSecond.of(ClimberConstants.CLIMB_SPEED)), + DESCENDING(Units.MetersPerSecond.of(-ClimberConstants.CLIMB_SPEED)); + + private final LinearVelocity velocity; + + private State(LinearVelocity velocity) { + this.velocity = velocity; + } + } + + @Override + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java new file mode 100644 index 0000000..a89de2a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Inches; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Distance; +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.HopperConstants; + +public class Hopper extends SubsystemBase { + private FlywheelMechanism _io; + + public enum State { + OFF(RevolutionsPerSecond.of(0.0)), + FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)), + FORWARD_FAST(RevolutionsPerSecond.of(HopperConstants.FAST_SPEED_RPM / 60)), + REVERSE(RevolutionsPerSecond.of(HopperConstants.REVERSE_SPEED_RPM / 60)); + + private final AngularVelocity _stateVelocity; + + private State(AngularVelocity stateVelocity) { + _stateVelocity = stateVelocity; + } + } + + 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); + } + + @Override + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java similarity index 67% rename from src/main/java/frc/robot/subsystems/intake/Intake.java rename to src/main/java/frc/robot/subsystems/Intake.java index 7071705..e5b1386 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -9,10 +9,12 @@ import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.robot.Constants; +import frc.robot.Constants.IntakePivotConstants; public class Intake extends SubsystemBase { private FlywheelMechanism _rollerIO; private RotaryMechanism _pivotIO; + public double desiredAngle; public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; @@ -38,6 +40,21 @@ public void stop() { setVelocity(0); } + public void setAngle(Angle angle) { + _pivotIO.runPosition( + angle, + getVelocity(), + IntakePivotConstants.ACCELERATION, + IntakePivotConstants.JERK, + PIDSlot.SLOT_0); + desiredAngle = angle.magnitude(); + } + + public boolean isIntendedAngle() { + return Math.abs(desiredAngle - _pivotIO.getVelocity().in(RotationsPerSecond)) + <= IntakePivotConstants.TOLERANCE.magnitude(); + } + @Override public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java new file mode 100644 index 0000000..e12cfd7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.SolidColor; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.W8.devices.Lights; +import frc.lib.W8.io.lights.LightsIO; +import frc.robot.Constants.LEDConstants; + +public class LEDs extends SubsystemBase { + private final LightsIO _io; + private final Lights _lights; + + public LEDs(LightsIO io, Lights lights) { + _io = io; + _lights = lights; + } + + public Command runAnimation() { + return this.startEnd( + () -> _lights.setAnimation(new SolidColor(0, 3).withColor(LEDConstants.colorPaleBlue)), + () -> _lights.setAnimation(new SolidColor(0, 3).withColor(LEDConstants.colorWheezerBlue))); + } + + @Override + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/LEDs/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs/LEDs.java deleted file mode 100644 index aba0e14..0000000 --- a/src/main/java/frc/robot/subsystems/LEDs/LEDs.java +++ /dev/null @@ -1,15 +0,0 @@ -package frc.robot.subsystems.LEDs; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.lib.W8.io.lights.LightsIO; - -public class LEDs extends SubsystemBase { - private final LightsIO _io; - - public LEDs(LightsIO io) { - _io = io; - } - - @Override - public void periodic() {} -} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..629e188 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,125 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.W8.io.motor.MotorIO.PIDSlot; +import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; +import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.robot.Constants.FeederConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.ShooterConstants; + +public class Shooter extends SubsystemBase { + + private final FlywheelMechanism _flywheel; + private final FlywheelMechanism _feeder; + private final RotaryMechanism _hood; + + // desired target values + private double desiredVelo; + private double hoodAngle; + + public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + _flywheel = flywheel; + _feeder = feeder; + _hood = hood; + } + + // Sets feeder motor speed + public void runFeeder() { + _feeder.runVelocity( + FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + } + + // Sets the flywheel velocity based on an input. + public void setFlywheelVelocity(double velocity) { + // store the desired velocity then send converted velocity to the mechanism + this.desiredVelo = velocity; + AngularVelocity angVelo = RotationsPerSecond.of(velocity); + + _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + } + + public enum State { + OFF(Units.RevolutionsPerSecond.of(0.0)), + IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), + SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), + SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), + SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), + SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); + + private final AngularVelocity stateVelocity; + + State(AngularVelocity stateVelocity) { + this.stateVelocity = stateVelocity; + } + } + + // Checks if the flywheel is at speed and returns a boolean + public boolean flyAtVelocity() { + return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) + <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; + } + + public double getHoodAngleDegrees(Translation2d robotPos) { + + // TODO: Replace with HUB later once it gets added. + double distance = robotPos.getDistance(FieldConstants.FIELDCENTER); + + double check = + Math.pow(ShooterConstants.EXIT_VELOCITY, 4) + - ShooterConstants.GRAVITY + * (ShooterConstants.GRAVITY * Math.pow(distance, 2) + + 2 + * ShooterConstants.HEIGHT_DIFFERENCE + * Math.pow(ShooterConstants.EXIT_VELOCITY, 2)); + + if (check < 0) { + return ShooterConstants.IDLE_HOOD_ANGLE; // Default angle if the shot is not possible + } + return Math.toDegrees( + Math.atan( + (ShooterConstants.EXIT_VELOCITY * ShooterConstants.EXIT_VELOCITY + Math.sqrt(check)) + / (ShooterConstants.GRAVITY * distance))); + } + + // Sets hood angle + public void setHoodAngle(double angleDegrees) { + hoodAngle = angleDegrees; + _hood.runPosition( + Angle.ofBaseUnits(angleDegrees, Degrees), + ShooterConstants.HOOD_VELOCITY, + ShooterConstants.HOOD_ACCELERATION, + ShooterConstants.HOOD_JERK, + PIDSlot.SLOT_0); + } + + // Checks if hood is at angle + public boolean hoodAtAngle() { + return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; + } + + public Command shoot(double velocity) { + // Prepare targets + return Commands.sequence( + // Set and wait in parallel for both hood and flywheel + Commands.parallel( + Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity), + Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), + // feed once ready + Commands.runOnce(() -> runFeeder()), + // stop flywheel when finished + Commands.runOnce(() -> setFlywheelVelocity(0))); + } + + @Override + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/Vision/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java similarity index 87% rename from src/main/java/frc/robot/subsystems/Vision/Vision.java rename to src/main/java/frc/robot/subsystems/Vision.java index b8c97b6..384ffe3 100644 --- a/src/main/java/frc/robot/subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.Vision; +package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.vision.VisionIO; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 7be04be..07f4a55 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -10,6 +13,18 @@ public Climber(LinearMechanism io) { io = _io; } + public enum State { + IDLE(Units.MetersPerSecond.of(0.0)), + ASCENDING(Units.MetersPerSecond.of(ClimberConstants.CLIMB_SPEED)), + DESCENDING(Units.MetersPerSecond.of(-ClimberConstants.CLIMB_SPEED)); + + private final LinearVelocity velocity; + + private State(LinearVelocity velocity) { + this.velocity = velocity; + } + } + @Override public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index a9f17fe..5698448 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -1,19 +1,25 @@ package frc.robot.subsystems.hopper; -import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RevolutionsPerSecond; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; 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.HopperConstants; -import frc.robot.Constants.HopperConstants.*; public class Hopper extends SubsystemBase { + private FlywheelMechanism _io; + // public enum State { + // OFF(RevolutionsPerSecond.of(0.0)), + // FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)), + // FORWARD_FAST(RevolutionsPerSecond.of(HopperConstants.FAST_SPEED_RPM / 60)), + // REVERSE(RevolutionsPerSecond.of(HopperConstants.REVERSE_SPEED_RPM / 60)); + public enum State { OFF(RevolutionsPerSecond.of(0.0)), FORWARD_SLOW(RevolutionsPerSecond.of(HopperConstants.SLOW_SPEED_RPM / 60)),