diff --git a/.vscode/settings.json b/.vscode/settings.json index f3d43d9..930a088 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -46,6 +46,7 @@ "AHRS", "Brushless", "Deadband", + "Feedforward", "intaking", "Odometry", "Roborio", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ef92ef..9decf4e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -118,7 +118,7 @@ public static final class ModuleConstants { public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; public static final int kDrivingMotorCurrentLimit = 50; // amps - public static final int kTurningMotorCurrentLimit = 20; // amps + public static final int kTurningMotorCurrentLimit = 30; // amps } public static final class OIConstants { @@ -152,6 +152,14 @@ public static final class ArmPIDConstants { public static final double kI = 0.0; public static final double kD = 0.0; public static final double kFF = 0.0; + + // Feedforward gains + // ! TODO: TUNE + public static final double kS = 0.0; + public static final double kG = 0.0; + public static final double kV = 0.0; + + public static final double kAllowedError = 0.05; } } @@ -178,6 +186,11 @@ public static final class PiCamera extends CameraDetails { } } + public static final class LEDConstants { + public static final int kLedPin = 9; + public static final int kLedCount = 50; + } + public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9df67f3..60826d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,5 +1,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; @@ -8,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.commands.SmartIntakeCommand; import frc.robot.commands.SmartShootCommand; import frc.robot.commands.TestAutoCommand; import frc.robot.simulationSystems.PhotonSim; @@ -30,12 +33,24 @@ public RobotContainer() { configureBindings(); setDefaultCommands(); PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable. + if (RobotBase.isSimulation()) { + simInit(); + } } - public void simPeriodic() { - PhotonSim.update(driveSubsystem.getPose()); + public void simInit() { + new Thread( + () -> { + for (; ; ) { + PhotonSim.update(driveSubsystem.getPose()); + Timer.delay(0.02); + } + }) + .start(); } + public void simPeriodic() {} + private void setDefaultCommands() { driveSubsystem.setDefaultCommand(driveSubsystem.defaultDriveCommand(controller)); intakeSubsystem.setDefaultCommand( @@ -52,11 +67,7 @@ private void configureBindings() { .whileTrue(new RunCommand(driveSubsystem::setX, driveSubsystem)); new JoystickButton(controller, Button.kB.value) // Intake - .whileTrue( - new RunCommand( - () -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed), - intakeSubsystem) - .alongWith(intakeSubsystem.vibrateControllerOnNoteCommand(controller))); + .whileTrue(new SmartIntakeCommand(intakeSubsystem, armSubsystem, controller)); // TODO: DEPRECATED, REMOVE new JoystickButton(controller, Button.kX.value) // Shoot, basic (Run Shooter) diff --git a/src/main/java/frc/robot/commands/SmartIntakeCommand.java b/src/main/java/frc/robot/commands/SmartIntakeCommand.java new file mode 100644 index 0000000..9991ac0 --- /dev/null +++ b/src/main/java/frc/robot/commands/SmartIntakeCommand.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.IntakeSubsystem; + +public class SmartIntakeCommand extends SequentialCommandGroup { + public SmartIntakeCommand( + IntakeSubsystem intakeSubsystem, ArmSubsystem armSubsystem, XboxController controller) { + super( + armSubsystem + .setArmToPositionCommand(0) + .onlyIf(() -> !intakeSubsystem.hasNote()) + .beforeStarting(new PrintCommand("Setting Arm to 0")) + .andThen( + intakeSubsystem.run( + () -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed))) + .alongWith(intakeSubsystem.vibrateControllerOnNoteCommand(controller))); + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 87eeb73..4395425 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -5,10 +5,13 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.IntakeConstants.ArmPIDConstants; import frc.utils.ExtraFunctions; import frc.utils.sim_utils.CANSparkMAXWrapped; @@ -18,6 +21,7 @@ public class ArmSubsystem extends SubsystemBase { private final CANSparkMAXWrapped armFollower; private final RelativeEncoder encoder; private final SparkPIDController pidController; + private final ArmFeedforward feedforward; public ArmSubsystem() { arm = new CANSparkMAXWrapped(IntakeConstants.kArmMotorCanID, MotorType.kBrushed); @@ -26,6 +30,7 @@ public ArmSubsystem() { armFollower.follow(arm, true); // Inverted encoder = arm.getEncoder(SparkRelativeEncoder.Type.kQuadrature, IntakeConstants.kArmEncoderCPR); pidController = arm.getPIDController(); + feedforward = new ArmFeedforward(ArmPIDConstants.kS, ArmPIDConstants.kG, ArmPIDConstants.kV); setupSparkMax(); } @@ -67,10 +72,14 @@ public void setArmSpeed(double speed) { * * @apiNote YOU SHOULD NOT USE THIS METHOD UNLESS NECESSARY! USE THE PID CONTROLLER INSTEAD. */ - public void setArmToZero() { + public void stopArm() { arm.set(0); } + public boolean isArmAtPosition(double position) { + return Math.abs(encoder.getPosition() - position) < ArmPIDConstants.kAllowedError; + } + /** * Sets the arm to a given position. * @@ -78,7 +87,8 @@ public void setArmToZero() { * @see #setArmToPosition(int) */ public void setArmToPosition(double position) { - pidController.setReference(position, ControlType.kPosition); + pidController.setReference( + position, ControlType.kPosition, 0, feedforward.calculate(position, encoder.getVelocity())); } /** @@ -91,6 +101,21 @@ public void setArmToPosition(int positionDegrees) { setArmToPosition(positionDegrees / 360.0); } + public Command setArmToPositionCommand(double position) { + return this.run(() -> this.setArmToPosition(position)) + .onlyWhile(() -> !this.isArmAtPosition(position)); + } + + /** + * Sets the arm to a given position. + * + * @param positionDegrees The rotation the arm should be at. (from 0 to 360) + * @return A command that will set the arm to the given position. + */ + public Command setArmToPositionCommand(int positionDegrees) { + return this.setArmToPositionCommand(positionDegrees / 360.0); + } + public void setArmToAprilTag() { var currentAlliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue); boolean isBlueAlliance = currentAlliance == DriverStation.Alliance.Blue; diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 246de9f..2dc3b6e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,8 +4,10 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakeConstants.ColorSensorConstants; import frc.robot.commands.VibrateControllerCommand; @@ -35,6 +37,7 @@ private void setupIntakeMotor() { @Override public void close() { intake.close(); + colorSensor.close(); } public boolean hasNote() { @@ -73,7 +76,9 @@ public Command loadToShooterCommand() { } public Command vibrateControllerOnNoteCommand(XboxController controller) { - return new VibrateControllerCommand(controller, 2, 1, 0.2); + return new VibrateControllerCommand(controller, 2, 1, 0.1) + .alongWith(new PrintCommand("Note!")) + .beforeStarting(new WaitUntilCommand(this::hasNote)); } @Override diff --git a/src/main/java/frc/robot/subsystems/LEDSystem.java b/src/main/java/frc/robot/subsystems/LEDSystem.java new file mode 100644 index 0000000..3795a6c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSystem.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.Constants.LEDConstants; +import frc.utils.BetterLED; + +public final class LEDSystem { + private static final BetterLED strip = + new BetterLED(LEDConstants.kLedPin, LEDConstants.kLedCount); + + public static void setLEDColorRGB(int r, int g, int b) { + strip.fill(new Color(r, g, b)); + } +} diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java new file mode 100644 index 0000000..43bf9e8 --- /dev/null +++ b/src/main/java/frc/utils/BetterLED.java @@ -0,0 +1,191 @@ +package frc.utils; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import java.util.ArrayList; + +public class BetterLED extends AddressableLED { + public enum AnimationType { // Is already static. + LEFT_TO_RIGHT, + RIGHT_TO_LEFT + } + + private AddressableLEDBuffer mainBuffer; + private int ledCount; + private int rainbowFirstPixelHue = 0; + private int currentAnimationIndex = 0; + + // The call list will have all animation calls in it, + // This will run like a scheduler, just on a smaller scale. + // it will probably have only a single function, but is a an Arraylist just in case. + private ArrayList callList; + + // The Thread will handle calling the callList and all other requirements for animations. + private Thread ledUpdateThread; + + public BetterLED(int port, int ledCount) { + super(port); + mainBuffer = new AddressableLEDBuffer(ledCount); + this.setLength(mainBuffer.getLength()); + this.start(); + callList = new ArrayList<>(3); // 3 should be enough on most cases. + // Create an update thread that handles animations + ledUpdateThread = + new Thread( + () -> { + for (; ; ) { // Infinity call periodic, + periodic(); + // Maybe add a delay? + // or should the delay be handled by the respective functions? + } + }, + "LED Control Thread"); + ledUpdateThread.start(); + } + + public void fill(Color color) { + for (int i = 0; i < ledCount; i++) { + mainBuffer.setLED(i, color); + } + this.setData(mainBuffer); + } + + public void startRainbow() { + changeLoopTo(this::rainbowLoop); + } + + public void animateColor(Color color, AnimationType animation) { + animateColor(color, animation, (ledCount * 2) / 3); + } + + public void animateColor(Color color, AnimationType animation, int onLEDCount) { + switch (animation) { + case LEFT_TO_RIGHT: + changeLoopTo(createLeftToRightAnimationLoop(color, onLEDCount)); + break; + case RIGHT_TO_LEFT: + changeLoopTo(createRightToLeftAnimationLoop(color, onLEDCount)); + break; + } + } + + public void stopAnimation() { + changeLoopTo(() -> {}); // Empty runnable + } + + public void blink(Color color, int blinkCount) { + changeLoopTo( + () -> { + for (int i = 0; i < blinkCount; i++) { + fill(color); + Timer.delay(0.5); + fill(Color.kBlack); + Timer.delay(0.5); + } + }); + } + + /** + * Breathe the led, this will light up the led and then light it down. This will repeat forever. + * + * @param color The color to breathe. see: WPILIB {@link Color} + * @param delayTime The time delay between all the steps in seconds; NOT the total time! + * @param max The max value of the led, normally the maximum is 255. + * @param min The min value of the led, normally the minimum is 0. + */ + public void breathe(Color color, double delayTime, int max, int min) { + changeLoopTo( + () -> { + // Light UP The led + for (int i = min; i < max; i++) { + fill(new Color(color.red * i, color.green * i, color.blue * i)); + Timer.delay(delayTime); + } + // Light DOWN the led + for (int i = max; i > min; i--) { + fill(new Color(color.red * i, color.green * i, color.blue * i)); + Timer.delay(delayTime); + } + }); + } + + public void breathe(Color color, double delayTime) { + breathe(color, delayTime, 255, 0); + } + + public void breathe(Color color) { + breathe(color, 0.01); + } + + /** + * Adds a runnable to the call list, this will be called every loop. You probably don't need to + * use this, but it's here if you need it. + * + * @param func The runnable to add to the call list. + */ + public void addRunnableToLoop(Runnable func) { + callList.add(func); + } + + @Override + public void setLength(int length) { + mainBuffer = new AddressableLEDBuffer(length); + ledCount = length; + super.setLength(length); + } + + private void changeLoopTo(Runnable func) { + callList.clear(); // Clear the list first + callList.add(func); // then add the runnable + } + + private void periodic() { + // Call any functions in the callList + for (var callable : callList) { + callable.run(); + } + } + + private void rainbowLoop() { + for (int i = 0; i < ledCount; i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to precess + final var hue = (rainbowFirstPixelHue + (i * 180 / ledCount)) % 80; + // Set the value + mainBuffer.setHSV(i, hue, 255, 255); + } + rainbowFirstPixelHue += 3; + rainbowFirstPixelHue %= 180; + this.setData(mainBuffer); + } + + private Runnable createLeftToRightAnimationLoop(Color color, int onLEDCount) { + return () -> { + final int start = currentAnimationIndex > onLEDCount ? currentAnimationIndex - onLEDCount : 0; + for (int i = start; i < ledCount && i < currentAnimationIndex; i++) { + mainBuffer.setLED(i, color); + } + currentAnimationIndex++; + currentAnimationIndex %= ledCount; + this.setData(mainBuffer); + Timer.delay(0.1); + }; + } + + private Runnable createRightToLeftAnimationLoop(Color color, int onLEDCount) { + return () -> { + final int start = + currentAnimationIndex < onLEDCount ? currentAnimationIndex + onLEDCount : ledCount; + for (int i = start; i > 0 && i > currentAnimationIndex; i--) { + mainBuffer.setLED(i, color); + } + currentAnimationIndex--; + currentAnimationIndex = + Math.floorMod(currentAnimationIndex, ledCount); // ! DO NOT USE % WITH NEGATIVE NUMBERS. + this.setData(mainBuffer); + Timer.delay(0.1); + }; + } +} diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java index 1e4c233..89480c7 100644 --- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java +++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java @@ -23,6 +23,11 @@ public void close() { return; } colorSensor = null; + + // Clear the RGBD values + for (int i = 0; i < rgbd.length; i++) { + rgbd[i] = 0; + } } /** @@ -50,7 +55,9 @@ public static void setRGBD(int red, int green, int blue, int distance) { @Override public int getRed() { - if (RobotBase.isReal()) { + // we check if getRed() is 0 because the color sensor returns 0 when its not being "simulated" + // as this is only used for unit tests. + if (RobotBase.isReal() || colorSensor == null || super.getRed() != 0) { return super.getRed(); } return rgbd[0]; @@ -58,7 +65,7 @@ public int getRed() { @Override public int getGreen() { - if (RobotBase.isReal()) { + if (RobotBase.isReal() || colorSensor == null || super.getGreen() != 0) { return super.getGreen(); } return rgbd[1]; @@ -66,7 +73,7 @@ public int getGreen() { @Override public int getBlue() { - if (RobotBase.isReal()) { + if (RobotBase.isReal() || colorSensor == null || super.getBlue() != 0) { return super.getBlue(); } return rgbd[2]; @@ -74,7 +81,7 @@ public int getBlue() { @Override public int getProximity() { - if (RobotBase.isReal()) { + if (RobotBase.isReal() || colorSensor == null || super.getProximity() != 0) { return super.getProximity(); } return rgbd[3]; diff --git a/src/test/java/IntakeTests.java b/src/test/java/IntakeTests.java index 5e5164e..d9c7fd5 100644 --- a/src/test/java/IntakeTests.java +++ b/src/test/java/IntakeTests.java @@ -32,6 +32,15 @@ void testIntakeSubsystem() { assertEquals(0.5, intakeMotor.get(), 0.001); } + @Test + void testIntakeNoteDetected() { + ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + assertEquals(true, intakeSubsystem.hasNote(), "Intake should detect a note"); + ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0); + assertEquals(false, intakeSubsystem.hasNote(), "Intake should not detect a note"); + // TODO: Add more tests for different RGBD values + } + @Test void testIntakeSubsystemWithNote() { ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900);