Merge pull request #4 from MEFThunders-7035/LedSystem

add an LED System, should be ready to use when wanted

.vscode/settings.json

@@ -46,6 +46,7 @@
     "AHRS",
     "Brushless",
     "Deadband",
+    "Feedforward",
     "intaking",
     "Odometry",
     "Roborio",

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;

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)

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

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,18 +72,23 @@ 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.
    *
    * @param position The rotation the arm should be at. (from 0 to 1)
    * @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;

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

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