From 670de7dd9097599ec6d217207b697bde1825bbbc Mon Sep 17 00:00:00 2001 From: Mehooliu Date: Tue, 3 Mar 2026 10:27:14 -0500 Subject: [PATCH] Added Trapezoidal Profile --- src/main/java/frc/robot/Constants.java | 14 +- src/main/java/frc/robot/Robot.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 240 +++++++++--------- .../frc/robot/subsystems/IntakeSubsystem.java | 112 +++++--- .../frc/robot/subsystems/LEDSubsystem.java | 61 ++++- 5 files changed, 275 insertions(+), 159 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d37b250..b0f7ced 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,9 +128,9 @@ public final class Constants { public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = 0.03; + public static final double INTAKE_ROTATOR_P = 10; public static final double INTAKE_ROTATOR_I = 0; - public static final double INTAKE_ROTATOR_D = 0; + public static final double INTAKE_ROTATOR_D = 2; } public static final double INTAKE_MOTOR_P = 0.0001; @@ -138,8 +138,13 @@ public final class Constants { public static final double INTAKE_MOTOR_D = 0; public static final double INTAKE_COLLECT_ENCODER_VALUE = 0; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; public static final double INTAKE_RETRACT_ENCODER_VALUE = -5; + + public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .26; + public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .64; + public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .54; + } // create object and a new widget under programming tab in Elastic where object @@ -195,7 +200,8 @@ public final class Constants { } public static class LEDConstants { - public static final int LED_DIO_PORT = 0; + public static final int LED_PWM_PORT = 6; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2d80edc..e330ec4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.IntakeConstants; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.TargetingSubsystems; /** @@ -35,6 +36,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private LEDSubsystem m_LedSubsystem; private Timer disabledTimer; private static NetworkTable table; @@ -58,6 +60,8 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our // autonomous chooser on the dashboard. + + m_LedSubsystem = new LEDSubsystem(); m_robotContainer = new RobotContainer(); // Create a timer to disable motor brake a few seconds after disable. This will @@ -70,6 +74,7 @@ public class Robot extends TimedRobot { } m_robotContainer.getSwerveDrive().zeroGyroWithAlliance(); + IntakeSubsystem.resetIntakeRotationEncoder(); } /** @@ -126,7 +131,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { - IntakeSubsystem.resetIntakeRotationEncoder(); + // IntakeSubsystem.resetIntakeRotationEncoder(); m_robotContainer.setMotorBrake(true); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a81b947..c50ddcb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -52,112 +53,117 @@ import frc.robot.subsystems.swervedrive.Vision; */ public class RobotContainer { - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - final CommandXboxController operatorXbox = new CommandXboxController(1); + // Replace with CommandPS4Controller or CommandJoystick if needed + final CommandXboxController driverXbox = new CommandXboxController(0); + final CommandXboxController operatorXbox = new CommandXboxController(1); - // The robot's subsystems and commands are defined here... - private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), - "swerve/neo")); + // The robot's subsystems and commands are defined here... + private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), + "swerve/neo")); - // Establish a Sendable Chooser that will be able to be sent to the - // SmartDashboard, allowing selection of desired auto - private final SendableChooser autoChooser; + // Establish a Sendable Chooser that will be able to be sent to the + // SmartDashboard, allowing selection of desired auto + private final SendableChooser autoChooser; - private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - //private final TargetingSubsystems m_TargetingSubsystems = new - // TargetingSubsystems(); - private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); - private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + // private final TargetingSubsystems m_TargetingSubsystems = new + // TargetingSubsystems(); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); + /** + * Converts driver input into a field-relative ChassisSpeeds that is controlled + * by angular velocity. + */ + SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + /** + * Clone's the angular velocity input stream and converts it to a fieldRelative + * input stream. + */ + SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() + .withControllerHeadingAxis(driverXbox::getRightX, + driverXbox::getRightY) + .headingWhile(true); - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() - .withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); + /** + * Clone's the angular velocity input stream and converts it to a robotRelative + * input stream. + */ + SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) + .allianceRelativeControl(true); - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) - .allianceRelativeControl(true); + SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis( + 2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + // Derive the heading axis with math! + SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + .withControllerHeadingAxis(() -> Math.sin( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2), + () -> Math.cos( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2)) + .headingWhile(true) + .translationHeadingOffset(true) + .translationHeadingOffset(Rotation2d.fromDegrees( + 0)); - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( - 2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2), - () -> Math.cos( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2)) - .headingWhile(true) - .translationHeadingOffset(true) - .translationHeadingOffset(Rotation2d.fromDegrees( - 0)); + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + DriverStation.silenceJoystickConnectionWarning(true); - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the trigger bindings - configureBindings(); - DriverStation.silenceJoystickConnectionWarning(true); + // Create the NamedCommands that will be used in PathPlanner + NamedCommands.registerCommand("test", Commands.print("I EXIST")); + NamedCommands.registerCommand("Shoot_Fuel_Command", + m_ShooterSubsystem.shootFuelCommand() + .andThen(m_IntakeSubsystem.startIntakeMotorCommand()) + .andThen(m_IntakeSubsystem.assistFuelIntakeCommand( + Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, + Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE) + .repeatedly() + .withTimeout(2))); + NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem + .goToPositionCommand(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE)); + NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()); + NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand()); - // Create the NamedCommands that will be used in PathPlanner - NamedCommands.registerCommand("test", Commands.print("I EXIST")); - NamedCommands.registerCommand("Shoot_Fuel_Command", m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.startIntakeMotorCommand()).andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(2))); - NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployintakeCommand()); - NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()); - NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand()); + // Have the autoChooser pull in all PathPlanner autos as options + autoChooser = AutoBuilder.buildAutoChooser(); - // Have the autoChooser pull in all PathPlanner autos as options - autoChooser = AutoBuilder.buildAutoChooser(); + // Set the default auto (do nothing) + autoChooser.setDefaultOption("Do Nothing", Commands.none()); - // Set the default auto (do nothing) - autoChooser.setDefaultOption("Do Nothing", Commands.none()); + // Add a simple auto option to have the robot drive forward for 1 second then + // stop + autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); - // Add a simple auto option to have the robot drive forward for 1 second then - // stop - autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); + // Put the autoChooser on the SmartDashboard + SmartDashboard.putData("Auto Chooser", autoChooser); - // Put the autoChooser on the SmartDashboard - SmartDashboard.putData("Auto Chooser", autoChooser); - - - - } + } /** * Use this method to define your trigger->command mappings. Triggers can be @@ -192,17 +198,21 @@ public class RobotContainer { // full shooting system including linear actuators driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); - driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly()); + driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()); driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()); - driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); - driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); + driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); + driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT)); + + //driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand()); + //driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand()); //driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() - .andThen(m_IntakeSubsystem.deployintakeCommand())); + .andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE))); // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), // () -> -driverXbox.getLeftX())); @@ -272,25 +282,23 @@ public class RobotContainer { } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // Pass in the selected auto from the SmartDashboard as our desired autnomous - // commmand - return autoChooser.getSelected(); - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Pass in the selected auto from the SmartDashboard as our desired autnomous + // commmand + return autoChooser.getSelected(); + } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); + } - public SwerveSubsystem getSwerveDrive() { - return drivebase; - } + public SwerveSubsystem getSwerveDrive() { + return drivebase; + } - - -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 96dfbaa..c160024 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,5 +1,9 @@ package frc.robot.subsystems; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -8,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; @@ -24,25 +29,38 @@ public class IntakeSubsystem extends SubsystemBase { private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless); + + private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(10, 5); + private final ProfiledPIDController intakeRotatorProfiledPIDController; + private static SparkClosedLoopController intakeRotatorPIDController; public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); private static SparkClosedLoopController intakeWheelsMotorPIDController; public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig(); - public IntakeSubsystem() { - intakeRotatorConfig.closedLoop - .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) - .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) - .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) + public static DutyCycleEncoder intakeRotatorEncoder = new DutyCycleEncoder(1); - .p(.13, ClosedLoopSlot.kSlot1) - .i(0, ClosedLoopSlot.kSlot1) - .d(0, ClosedLoopSlot.kSlot1); + private static double encoderValue = intakeRotatorEncoder.get(); + + public IntakeSubsystem() { + intakeRotatorProfiledPIDController = new ProfiledPIDController( + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D, m_Constraints); + + intakeRotatorConfig.closedLoop + .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) + .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) + .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) + .p(.13, ClosedLoopSlot.kSlot1) + .i(0, ClosedLoopSlot.kSlot1) + .d(0, ClosedLoopSlot.kSlot1); intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); intakeRotatorConfig.smartCurrentLimit(40); - intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + //intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P, Constants.IntakeConstants.INTAKE_MOTOR_I, @@ -52,17 +70,20 @@ public class IntakeSubsystem extends SubsystemBase { intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); } - public static void resetIntakeRotationEncoder() - { - intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE); + public void goToPosition(double goalPosition) { + double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition); + intakeRotatorMotor.setVoltage(-pidVal); } + public void startIntakeMotor() { - intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity); + intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, + ControlType.kVelocity); } public void reverseIntakeMotor() { - intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity); + intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, + ControlType.kVelocity); } public void stopIntakeMotor() { @@ -81,34 +102,57 @@ public class IntakeSubsystem extends SubsystemBase { return runOnce(() -> stopIntakeMotor()); } - public void deployIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0); + public Command goToPositionCommand(double goalPosition) { + return runOnce(() -> goToPosition(goalPosition)); } - public Command deployintakeCommand() { - return runOnce(() -> deployIntake()); + public Command assistFuelIntakeCommand(double deployedPosition, double retractedPosition) { + return runOnce(() -> goToPositionCommand(retractedPosition).andThen(new WaitCommand(1.5)) + .andThen(goToPositionCommand(deployedPosition)).andThen(new WaitCommand(1.5))); } - public void retractIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1); - } - - public Command retractIntakeCommand() { - return runOnce(() -> retractIntake()); - } - - public void assistFuelIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, - ControlType.kPosition, ClosedLoopSlot.kSlot1); - } - - public Command assistFuelIntakeCommand() { - return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(1.5)).andThen(deployintakeCommand()) - .andThen(new WaitCommand(1.5)); + public static void resetIntakeRotationEncoder() { + intakeRotatorMotor.getEncoder().setPosition(-5); } + + public void deployIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants. + INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0); + } + + public Command deployIntakeCommand() { + return runOnce(() -> deployIntake()); + } + + public void retractIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants. + INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1); + } + + public Command retractIntakeCommand() { + return runOnce(() -> retractIntake()); + } + + public void assistFuelIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants. + INTAKE_MIDDLE_ENCODER_VALUE, + ControlType.kPosition, ClosedLoopSlot.kSlot1); + } + + public Command assistFuelIntakeCommand() { + return runOnce(() -> assistFuelIntake()).andThen(new + WaitCommand(1.5)).andThen(deployIntakeCommand()) + .andThen(new WaitCommand(1.5)); + } + @Override public void periodic() { + encoderValue = intakeRotatorEncoder.get(); SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); + SmartDashboard.putNumber("Intake Rotator Encoder Value", intakeRotatorEncoder.get()); + SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); + SmartDashboard.putNumber("Voltage Retract", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT)); + } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 6fa485b..5657af5 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -4,37 +4,87 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Percent; +import static edu.wpi.first.units.Units.Second; + import java.util.Optional; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.AddressableLEDBufferView; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class LEDSubsystem extends SubsystemBase { - /** Creates a new LEDSubsystem. */ + + AddressableLED m_LED; + + AddressableLEDBuffer m_Buffer; + AddressableLEDBufferView m_Left; + AddressableLEDBufferView m_Right; + + LEDPattern hubActiveColor = LEDPattern.solid(Color.kGreen); + LEDPattern hubActiveBlinkPattern = hubActiveColor.blink(Second.of(0.5)); + + LEDPattern hubInactiveColor = LEDPattern.solid(Color.kRed); + LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.5)); + + LEDPattern allianceShiftColor = LEDPattern.gradient(LEDPattern.GradientType.kContinuous, Color.kTeal, Color.kMagenta); + LEDPattern allianceShiftPattern = allianceShiftColor.scrollAtRelativeSpeed(Percent.per(Second).of(100)); + + LEDPattern transitionColor = LEDPattern.solid(Color.kYellow); + LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.5)); + + LEDPattern endGameColor = LEDPattern.solid(Color.kOrangeRed); + LEDPattern endGameBlinkPattern = endGameColor.blink(Second.of(0.2)); + + LEDPattern rainbow = LEDPattern.rainbow(255, 128); + LEDPattern rainbowScroll = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(100)); public LEDSubsystem() { + m_LED = new AddressableLED(Constants.LEDConstants.LED_PWM_PORT); + m_Buffer = new AddressableLEDBuffer(44); + m_LED.setLength(m_Buffer.getLength()); + m_Left = m_Buffer.createView(0, 21); + m_Right = m_Buffer.createView(22, 43); + + hubInactiveBlinkPattern.applyTo(m_Left); + hubActiveBlinkPattern.applyTo(m_Right); + + m_LED.setData(m_Buffer); + m_LED.start(); } private double matchTime; private boolean isHubActive; public void setLEDPeriod() { + if(DriverStation.isAutonomous()) + { + allianceShiftPattern.applyTo(m_Buffer); + } + if (matchTime <= 140 && matchTime > 130 && !DriverStation.isAutonomous()) // transition { - + transitionBlinkPattern.applyTo(m_Left); } if (matchTime <= 130 && matchTime > 30 && !DriverStation.isAutonomous()) // shifts { - + allianceShiftPattern.applyTo(m_Left); } if (matchTime <= 30 && !DriverStation.isAutonomous()) // endgame { - + endGameBlinkPattern.applyTo(m_Left); } + + allianceShiftPattern.applyTo(m_Buffer); } public void setLEDHubActive() { @@ -113,5 +163,8 @@ public class LEDSubsystem extends SubsystemBase { // This method will be called once per scheduler run matchTime = DriverStation.getMatchTime(); isHubActive = isHubActive(); + setLEDPeriod(); + + m_LED.setData(m_Buffer); } }