Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added Trapezoidal Profile

Browse Source
This commit is contained in:
Mehooliu
2026-03-03 10:27:14 -05:00
parent 1b84701184
commit 670de7dd90
5 changed files with 275 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/main/java/frc/robot/Constants.java
View File

@@ -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;
}
}

7
src/main/java/frc/robot/Robot.java
View File

@@ -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();

240
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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<Command> autoChooser;
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> 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;
}
}
}

112
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -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));
}
}

61
src/main/java/frc/robot/subsystems/LEDSubsystem.java
View File

@@ -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);
}
}