diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dfbdfde..83ddf0e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -332,9 +332,10 @@ public class ShooterRotaryConstants { public static final Angle TOLERANCE = Degrees.of(2.0); - public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(1); - public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); - public static final Velocity JERK = ACCELERATION.per(Second); + // public static final AngularVelocity CRUISE_VELOCITY = Units.RadiansPerSecond.of(1); + // public static final AngularAcceleration ACCELERATION = + // CRUISE_VELOCITY.div(0.1).per(Units.Second); + // public static final Velocity JERK = ACCELERATION.per(Second); private static final double ROTOR_TO_SENSOR = (2.0 / 1.0); private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0); @@ -396,10 +397,10 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Rotations); + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Units.Rotaitons); config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Rotations); + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Units.Rotations); config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; @@ -656,6 +657,9 @@ public class ClimberConstants { 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 Distance BOTTOM = Inches.of(0.0); + public static final Distance MIDDLE = Inches.of(15.0); + public static final Distance TOP = Inches.of(30.0); public static final LinearMechCharacteristics CHARACTERISTICS = new LinearMechCharacteristics( @@ -708,5 +712,6 @@ public static TalonFXConfiguration getFXConfig() { public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); public static final double CLIMB_SPEED = 1.0; + public static final double HARD_STOP_CURRENT_LIMIT = 50.0; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ffed28..b40f89d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,9 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; @@ -41,6 +44,7 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; + private Alert lowBatteryAlert = new Alert("The robot is low on battery!", AlertType.kWarning); public FuelSim fuelSim = new FuelSim(); public Robot() { @@ -122,7 +126,10 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - + if (0.0 <= RobotController.getBatteryVoltage() + && RobotController.getBatteryVoltage() <= 11.01) { + lowBatteryAlert.set(true); + } // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1895688..fcebeca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,13 +27,9 @@ import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; -import frc.lib.W8.mechanisms.linear.LinearMechanism; -import frc.lib.W8.mechanisms.linear.LinearMechanismReal; -import frc.lib.W8.mechanisms.linear.LinearMechanismSim; 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.ClimberConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; @@ -44,7 +40,6 @@ import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.BallCounter; -import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -71,7 +66,6 @@ public class RobotContainer { public final Drive drive; private final Hopper hopper; private final Shooter shooter; - private final Climber climber; private final Intake intake; private final BallCounter ballCounter; private final Vision vision; @@ -124,14 +118,6 @@ public RobotContainer() { Ports.ShooterRoller), Constants.ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); - climber = - new Climber( - new LinearMechanismReal( - new MotorIOTalonFX( - ClimberConstants.MOTOR_NAME, - ClimberConstants.getFXConfig(), - Ports.ClimberLinearMechanism), - ClimberConstants.CHARACTERISTICS)); intake = new Intake( @@ -204,17 +190,6 @@ public RobotContainer() { true, ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); - climber = - new Climber( - new LinearMechanismSim( - new MotorIOTalonFXSim( - ClimberConstants.MOTOR_NAME, - ClimberConstants.getFXConfig(), - Ports.ClimberLinearMechanism), - ClimberConstants.DCMOTOR, - ClimberConstants.CARRIAGE_MASS, - ClimberConstants.CHARACTERISTICS, - true)); intake = new Intake( @@ -258,10 +233,6 @@ public RobotContainer() { new ModuleIO() {}); hopper = new Hopper(new FlywheelMechanism() {}); - climber = - new Climber( - new LinearMechanism( - ClimberConstants.MOTOR_NAME, ClimberConstants.CHARACTERISTICS) {}); shooter = new Shooter( diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 286256f..4aade98 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -1,15 +1,13 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -26,37 +24,22 @@ public Climber(LinearMechanism io) { _io = io; } - public void Position(double position) { - Distance positionInches = Inches.of(position); - _io.runPosition( - ClimberConstants.CONVERTER.toAngle(positionInches), - ClimberConstants.ANGULAR_VELOCITY, - ClimberConstants.ANGULAR_ACCELERATION, - null, - PIDSlot.SLOT_0); - } - - public Command runClimber(double Position) { - Distance positionInches = Inches.of(Position); - return this.runOnce( - () -> - _io.runPosition( - ClimberConstants.CONVERTER.toAngle(positionInches), - ClimberConstants.ANGULAR_VELOCITY, - ClimberConstants.ANGULAR_ACCELERATION, - ClimberConstants.JERK, - PIDSlot.SLOT_0)); - } - - public enum State { - IDLE(Units.MetersPerSecond.of(0.0)), - ASCENDING(Units.MetersPerSecond.of(ClimberConstants.CLIMB_SPEED)), - DESCENDING(Units.MetersPerSecond.of(-ClimberConstants.CLIMB_SPEED)); + // public void Position(double position) { - private final LinearVelocity velocity; + // Distance positionInches = Inches.of(position); + // _io.runPosition( + // ClimberConstants.CONVERTER.toAngle(positionInches), + // ClimberConstants.ANGULAR_VELOCITY, + // ClimberConstants.ANGULAR_ACCELERATION, + // null, + // PIDSlot.SLOT_0); + // } - private State(LinearVelocity velocity) { - this.velocity = velocity; + public boolean isAboveCurrentLimit() { + if (_io.getSupplyCurrent().in(Amps) > ClimberConstants.HARD_STOP_CURRENT_LIMIT) { + return true; + } else { + return false; } } @@ -75,8 +58,15 @@ public void periodic() { _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } - public void runClimber() { - runClimber(); + public Command runClimber() { + return this.runOnce( + () -> + _io.runPosition( + ClimberConstants.CONVERTER.toAngle(ClimberConstants.TOP), + ClimberConstants.CRUISE_VELOCITY, + ClimberConstants.ACCELERATION, + ClimberConstants.JERK, + PIDSlot.SLOT_0)); } public boolean nearGoalposition() {