Added_led_subsystem_gb

src/main/java/frc/robot/OI.java

@@ -90,9 +90,12 @@ private static class operatorBindings {
         public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON);
 
         // Trigger controls for coral claw
+    public static DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();};
         public static final Trigger operatorRightTrigger = new Trigger(() -> 
             operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
 
+    public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();};
+
         public static final Trigger operatorLeftTrigger = new Trigger(() -> 
             operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD);
 

src/main/java/frc/robot/RobotContainer.java

@@ -30,6 +30,7 @@ public class RobotContainer {
 
     public Camera camera = new Camera();
     public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro);
+    public LEDStrip ledStrip = new LEDStrip(RobotMap.PWM.LED_STRIP_PORT, RobotMap.LED_STRIP_LENGTH);
 
   /**
    * The RobotContainer class is where the bulk of the robot should be declared. 
@@ -66,10 +67,17 @@ private void configureBindings() {
       // Elevator level controls
       // OPERATOR B : INTAKE
       OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw));
+      OI.operatorControllerB.onTrue(Commands.runOnce(() -> {
+          ledStrip.indicateIntake();
+      }));
       // OPERATOR RB : TROUGH(L1)
       OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.LOW)); 
+      OI.operatorControllerRightBumper.onTrue(Commands.runOnce(() -> {
+          ledStrip.indicateScoring();
+      }));
       // OPERATOR A : L2 (lowest pole)
-      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED));
+      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED).alongWith(
+          Commands.runOnce(() -> ledStrip.indicateScoring())));
       // OPERATOR X : L3 (2nd pole)
       OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.HIGH));
       // OPERATOR Y : L4 (last pole)
@@ -84,17 +92,17 @@ private void configureBindings() {
       OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175)));
 
       // OPERATOR RT : INTAKE
-      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1));
+      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1));
       // OPERATOR LT : OUTTAKE
-      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0));
+      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0));
 
       // Manual control with right stick for testing in simulation
       // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
           // elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator));
   }
 
   public Command getAutonomousCommand() {
-      return new ScoreTest(drivetrain, gyro, camera);
+      return new ScoreTest(drivetrain, gyro, camera, elevator, wrist, coralClaw);
       // return new DriveDistance(drivetrain, gyro, 7.0, 0.0, 0.0).andThen(new DriveDistance(drivetrain, gyro, 0.0, 3.0, 0.0));
     // return new DriveWithHeadingCommand(drivetrain, gyro, OI.xboxLeftStickXSupplier, OI.xboxRightStickYSupplier, new Rotation2d(0.0));
     // return new DriveMeters(drivetrain, 0.0, 0.0, 0.0);

src/main/java/frc/robot/RobotMap.java

@@ -69,6 +69,11 @@ private class DIO {
         private static final int CORAL_LIMIT_SWITCH = 0;  // Digital I/O port for coral limit switch
     }
 
+    // PWM port configuration
+    public class PWM {
+        public static final int LED_STRIP_PORT = 0;  // PWM port for LED strip
+    }
+
     // Pneumatic port configuration
     private class Pneumatics {
         private static final int WRIST_FORWARD_CHANNEL = 0;
@@ -113,6 +118,9 @@ private class Pneumatics {
     // Coral claw limit switch
     public static final DigitalInput coralLimitSwitch = new DigitalInput(DIO.CORAL_LIMIT_SWITCH);
 
+    // LED strip
+    public static final int LED_STRIP_LENGTH = 60;  // Number of LEDs in the strip
+
     // Remove the subsystem instances from here
     // public static final Elevator elevator = new Elevator();
     // public static final Wrist wrist = new Wrist(wristSolenoid);

src/main/java/frc/robot/commands/ScoreCommandGroup.java

@@ -11,6 +11,11 @@
 
 public class ScoreCommandGroup extends ParallelCommandGroup {
     public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, ElevatorLevel level) {
+	if(level == ElevatorLevel.LOW) {
+	    addCommands(new InstantCommand(() -> claw.setMotorsOn(false, true)));
+	} else {
+	    addCommands(new InstantCommand(() -> claw.setMotorsOn(true, true)));
+	}
         addCommands(
             new InstantCommand(() -> elevator.setTarget(level)),
             new InstantCommand(() -> wrist.extend())

src/main/java/frc/robot/commands/auto/DriveDistance.java

@@ -21,21 +21,23 @@ public class DriveDistance extends Command {
 	double angleDisplacement;
 
 	double xyThreshold = 0.6;
-	double xyMaxSpeed = 0.65;
+	double xyMaxSpeed;
 	double rotateMaxSpeed = 1.5;
 
-	public DriveDistance(Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) {
+    public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) {
 		this.drivetrain = drivetrain;
 		this.gyro = gyro;
 		this.xDistance = xDistance;
 		this.yDistance = yDistance;
 		this.theta = theta;
+		this.xyMaxSpeed = xyMaxSpeed;
 		addRequirements(drivetrain);
 
 	}
 
 	@Override
 	public void initialize() {
+	    System.out.println("drivedistance initialized");
 		Pose2d start = drivetrain.getPose2d();
 		this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta));
 	}
@@ -84,7 +86,7 @@ public void execute() {
 
 	@Override
 	public void end(boolean interrupted) {
-		drivetrain.driveRobotRelative(0.0, 0.0, 0.0);
+		drivetrain.drive(0.0, 0.0, 0.0);
 	}
 
     @Override

src/main/java/frc/robot/commands/auto/ScoreTest.java

@@ -2,9 +2,17 @@
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Gyro;
 import frc.robot.subsystems.Camera;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Wrist;
+import frc.robot.subsystems.CoralClaw;
 import frc.robot.commands.auto.DriveDistance;
+import frc.robot.commands.IntakeCommandGroup;
+import frc.robot.commands.ScoreCommandGroup;
+import frc.robot.ElevatorLevel;
+
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 /**
  * The ScoreTest command is a SequentialCommandGroup that performs a series of driving maneuvers
@@ -37,14 +45,52 @@ public class ScoreTest extends SequentialCommandGroup {
 	 * @param gyro The gyro sensor used for orientation and heading.
 	 * @param camera The camera used for vision processing (currently commented out).
 	 */
-    public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera) {
-	// ID7 from outer
+    public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) {
 	addCommands(
 		    new InstantCommand(() -> drivetrain.resetPosition()),
-		    new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall
-		    new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7
-		    new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command
-		    new DriveTowardsThing(drivetrain, camera)
+		    new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472),
+			new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.1, 1.0472),
+			new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+			new WaitCommand(0.5),
+			new InstantCommand(() -> claw.setOutput(-0.4)),
+			new WaitCommand(1.0),
+		    new InstantCommand(() -> claw.setOutput(0.0))
 		    );
+
+	// SIDE 2
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599),
+		    // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599),
+		    // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+		    // new WaitCommand(500),
+		    // new InstantCommand(() -> claw.setOutput(-0.4)),
+		    // new WaitCommand(1000.0),
+		    // new InstantCommand(() -> claw.setOutput(0.0))
+		    // );
+
+
+		    // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0))
+
+	// SIDE 1 L1
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall
+		    // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW),
+		    // new WaitCommand(500.0),
+		    // new InstantCommand(() -> claw.setOutput(-0.4)),
+		    // new WaitCommand(1000.0),
+		    // new InstantCommand(() -> claw.setOutput(0.0))
+		    // );
+
+
+	// ID7 from outer
+	// addCommands(
+		    // new InstantCommand(() -> drivetrain.resetPosition()),
+		    // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall
+		    // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7
+		    // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command
+		    // new DriveTowardsThing(drivetrain, camera)
+		    // );
     }
 }

src/main/java/frc/robot/commands/instant/ClawCommand.java

@@ -3,17 +3,20 @@
 import frc.robot.subsystems.CoralClaw;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.DoubleSupplier;
 
 public class ClawCommand extends Command {
     CoralClaw claw;
     double speed;
     int outin;
+    DoubleSupplier triggerSupplier;
 
-    public ClawCommand(CoralClaw claw, int outin) {
+    public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) {
 	addRequirements(claw);
 	this.claw = claw;
 	this.speed = claw.INTAKE_SPEED;
 	this.outin = outin;
+	this.triggerSupplier = triggerSupplier;
     }
 
     @Override
@@ -22,13 +25,9 @@ public ClawCommand(CoralClaw claw, int outin) {
     @Override
     public void execute() {
 	if(outin == 0) { // OUT
-	    System.out.println("CLAWCOMMAND OUTTAKE");
-	    claw.outtake();
+	    claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.2));
 	} else { // INTAKE
-	    System.out.println("CLAWCOMMAND INTAKE");
-	    claw.intake();
-	    // if(!claw.isCoralDetected()) {
-	// }
+	    claw.setOutput(triggerSupplier.getAsDouble() * 0.5 + 0.2);
 	}
 
 

src/main/java/frc/robot/subsystems/CoralClaw.java

@@ -84,7 +84,8 @@ public class CoralClaw extends SubsystemBase {
 
     // Track previous coral state to detect transitions
     private boolean previousCoralDetected = false;
-
+
+    private boolean[] motorsOn = {true, true};
     /**
      * Creates a new CoralClaw subsystem.
      * 
@@ -101,8 +102,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li
         // Configure the secondary motor to follow the primary motor with inversion
         SparkMaxConfig config = new SparkMaxConfig();
 	primaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        config.follow(primaryMotor, true); // true makes it follow with inversion
         secondaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        // config.follow(primaryMotor, true); // true makes it follow with inversion
 
         // Initialize simulation-specific components if in simulation
         isSimulation = RobotBase.isSimulation();
@@ -118,8 +119,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li
      * Will run until a coral is detected or manually stopped
      */
     public void intake() {
-	System.out.println("CLAW INTAKE");
-        primaryMotor.set(INTAKE_SPEED);
+	if(motorsOn[0]) { primaryMotor.set(INTAKE_SPEED); }
+	if(motorsOn[1]) { secondaryMotor.set(INTAKE_SPEED); }
         // Secondary motor automatically follows with inversion
         // autoIntakeMode = true;
         SmartDashboard.putString("Coral Claw State", "Intaking (Auto)");
@@ -129,18 +130,26 @@ public void intake() {
      * Spins the wheels to outtake/eject a coral game piece
      */
     public void outtake() {
-	System.out.println("CLAW OUTTAKE");
-        primaryMotor.set(OUTTAKE_SPEED);
+	if(motorsOn[0]) { primaryMotor.set(OUTTAKE_SPEED); }
+	if(motorsOn[1]) { secondaryMotor.set(OUTTAKE_SPEED); }
+
         // Secondary motor automatically follows with inversion
         // autoIntakeMode = false;
         SmartDashboard.putString("Coral Claw State", "Outtaking");
     }
+
+    public void setMotorsOn(boolean left, boolean right) {
+	motorsOn[0] = left;
+	motorsOn[1] = right;
+    }
+
 
     /**
      * Stops the intake/outtake wheels
      */
     public void stop() {
         primaryMotor.set(0);
+	secondaryMotor.set(0);
         // Secondary motor automatically follows with inversion
         // autoIntakeMode = false;
         SmartDashboard.putString("Coral Claw State", "Stopped");
@@ -192,9 +201,9 @@ public void setSimulatedCoralDetected(boolean detected) {
      * set claw output to @param out
      **/
     public void setOutput(double out) {
-	System.out.println("claw set output; " + out);
-        primaryMotor.set(INTAKE_SPEED);
-	
+	if(motorsOn[0]) { primaryMotor.set(out); }
+	if(motorsOn[1]) { secondaryMotor.set(out); }
+        primaryMotor.set(out);
     }
 
     @Override

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -71,7 +71,6 @@ public Drivetrain(SwerveModule leftBackSwerve,
     }
 
     public void drive(double x, double y, double omega) {
-	System.out.println("TODO:REMOVETHIS DRIVE(" + x + ", " + y + ", " + omega + ")");
         // x: x meters / second , y: y meters per second, omega: angular velocity
         // (radians per second)
         ChassisSpeeds chassisSpeed = ChassisSpeeds

src/main/java/frc/robot/subsystems/Elevator.java

@@ -153,7 +153,7 @@ public void setTarget(ElevatorLevel level) {
         // Tune these values to match the actual robot requirmenets to seed the coral
         switch (level) {
             case STATION:
-                groundHeight = Units.inchesToMeters(28.0); // all these measurements are from measuring the field components
+                groundHeight = Units.inchesToMeters(27.5); // all these measurements are from measuring the field components
                 break;
             case LOW:
                 groundHeight = Units.inchesToMeters(18.0); 
@@ -162,10 +162,10 @@ public void setTarget(ElevatorLevel level) {
                 groundHeight = Units.inchesToMeters(29.75);
                 break;
             case HIGH:
-                groundHeight = Units.inchesToMeters(42.5);
+                groundHeight = Units.inchesToMeters(42.25);
                 break;
             case EXTRA_HIGH:
-                groundHeight = Units.inchesToMeters(64.75);
+                groundHeight = Units.inchesToMeters(63.75);
                 break;
             default:
                 groundHeight = 0.0;