Merge pull request #54 from 2491-NoMythic/lights-)

Lights trying to merge

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

@@ -33,6 +33,7 @@
 
 import frc.robot.settings.Constants.Field;
 import frc.robot.commands.IndexCommand;
+import frc.robot.commands.IndicatorLights;
 import frc.robot.settings.Constants;
 import frc.robot.settings.Constants.ClimberConstants;
 import frc.robot.settings.Constants.DriveConstants;
@@ -56,6 +57,7 @@
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.RobotState;
 import frc.robot.subsystems.ShooterSubsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -168,7 +170,7 @@ private void climbSpotChooserInit() {
     SmartDashboard.putData(climbSpotChooser);
   }
   private void driveTrainInst() {
-    driveTrain = new DrivetrainSubsystem(lights, lightsExist);
+    driveTrain = new DrivetrainSubsystem();
     defaultDriveCommand = new Drive(
       driveTrain, 
       () -> driverController.getL1Button(),
@@ -208,6 +210,7 @@ private void limelightInit() {
   }
   private void lightsInst() {
     lights = new Lights(Constants.LED_COUNT-1);
+    lights.setDefaultCommand(new IndicatorLights(lights));
   }
 
 
@@ -338,7 +341,11 @@ public void teleopPeriodic() {
     driveTrain.calculateSpeakerAngle();
     if(useDetectorLimelight) {
       SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta);
+      RobotState.getInstance().IsNoteSeen = limelight.getNeuralDetectorValues().isResultValid;
     }
+    SmartDashboard.putBoolean("is note seen", RobotState.getInstance().IsNoteSeen);
+		SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange);
+		SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady);
   }
 
   public void disabledPeriodic() {

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

@@ -15,6 +15,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RobotState;
 import frc.robot.subsystems.ShooterSubsystem;
 
 
@@ -72,20 +73,30 @@ public void execute() {
         shooter.shootThing(ShooterConstants.SHOOTER_AMP_POWER);
       }
     }
-    if (shootIfReadySupplier.getAsBoolean()) {
-      if((angleShooterSubsytem.calculateSpeakerAngleDifference()<ShooterConstants.ALLOWED_ERROR)
-        && drivetrain.getSpeakerAngleDifference()<DriveConstants.ALLOWED_ERROR
-        && Math.abs(shooter.getError())<ShooterConstants.ALLOWED_SPEED_ERROR) {
-          m_Indexer.on();
-      } else {
-        m_Indexer.off();
+    boolean indexer = false;
+    if((angleShooterSubsytem.calculateSpeakerAngleDifference()<ShooterConstants.ALLOWED_ERROR)
+      && drivetrain.getSpeakerAngleDifference()<DriveConstants.ALLOWED_ERROR
+      && Math.abs(shooter.getError())<ShooterConstants.ALLOWED_SPEED_ERROR)
+    {
+      RobotState.getInstance().ShooterReady = true;
+      if (shootIfReadySupplier.getAsBoolean()) {
+        indexer = true;
       }
+    } else {
+      RobotState.getInstance().ShooterReady = false;
+    }
+    if (indexer) {
+      m_Indexer.on();
+    } else {
+      m_Indexer.off();
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    RobotState.getInstance().ShooterReady = false;
+  }
 
   // Returns true when the command should end.
   @Override

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

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Lights;
+import frc.robot.subsystems.RobotState;
+public class IndicatorLights extends Command {
+
+  Lights lights;
+
+  public IndicatorLights(Lights lights) {
+    addRequirements(lights);
+    this.lights = lights;
+  }
+
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    if (RobotState.getInstance().IsNoteSeen) {
+    lights.setSectionOne(50,40,0);
+    } else {
+      lights.setSectionOne(50, 0, 50);
+    }
+    if (RobotState.getInstance().ShooterInRange && RobotState.getInstance().ShooterReady){
+      lights.setSectionTwo(0,50,0);
+    } else if (RobotState.getInstance().ShooterInRange){
+      lights.setSectionTwo(50, 50, 0);
+    } else {
+      lights.setSectionTwo(50, 0, 50);
+    }
+    lights.dataSetter();
+  }
+
+
+  @Override
+  public void end(boolean interrupted) {
+    lights.lightsOut();
+    lights.dataSetter();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/settings/Constants.java

@@ -33,7 +33,7 @@ public final class Constants {
 
   private Constants () {}
 
-  public static final int LED_COUNT = 52;
+  public static final int LED_COUNT = 24;
 
   public static final class DriveConstants {
     public static final double ALLOWED_ERROR = 1;

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

@@ -80,7 +80,6 @@ public class DrivetrainSubsystem extends SubsystemBase {
 
 	private final SwerveDrivePoseEstimator odometer;
 	private final Field2d m_field = new Field2d();
-	Lights lights;
 
 	//speaker angle calculating variables:
 	double m_desiredRobotAngle;
@@ -106,16 +105,12 @@ public class DrivetrainSubsystem extends SubsystemBase {
 	Translation2d adjustedTarget;
 	double offsetSpeakerdist;
 	public double speakerDist;
-
-	Boolean lightsExist;
 	Limelight limelight;
 
 	double MathRanNumber;
 
-	public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
+	public DrivetrainSubsystem() {
 		MathRanNumber = 0;
-		this.lights = lights;
-		this.lightsExist = lightsExist;
 		this.limelight=Limelight.getInstance();
 
 		Preferences.initDouble("FL offset", 0);
@@ -292,11 +287,8 @@ public double calculateSpeakerAngle(){
 		}
 		speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
 		SmartDashboard.putNumber("dist to speakre", speakerDist);
-		if(speakerDist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
-			lights.setLights(0, Constants.LED_COUNT, 0, 100, 0);
-		} else {if(lights != null) {
-			lights.lightsOut();
-		} }
+
+		// RobotState.getInstance().ShooterInRange = speakerDist<Field.MAX_SHOOTING_DISTANCE;
 
 		//getting desired robot angle
 		// if (alliance.isPresent() && alliance.get() == Alliance.Blue) {
@@ -365,11 +357,7 @@ public double calculateSpeakerAngleMoving(){
 		adjustedTarget = new Translation2d(offsetSpeakerX, offsetSpeakerY);
 		offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaY, 2) + Math.pow(offsetDeltaX, 2));
 		SmartDashboard.putNumber("offsetSpeakerDis", offsetSpeakerdist);
-		if(offsetSpeakerdist<Field.MAX_SHOOTING_DISTANCE && lightsExist) {
-			lights.setLights(0, Constants.LED_COUNT, 0, 100, 0);
-		} else {if(lightsExist) {
-			lights.lightsOut();
-		}}
+		RobotState.getInstance().ShooterInRange = offsetSpeakerdist<Field.MAX_SHOOTING_DISTANCE;
 		SmartDashboard.putString("offset amount", targetOffset.toString());
 		SmartDashboard.putString("offset speaker location", new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
 		//getting desired robot angle

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

@@ -6,6 +6,8 @@
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
 
 public class Lights extends SubsystemBase {
   /** Creates a new SubsystemLights. */
@@ -28,21 +30,16 @@ public void setLights(int start, int end, int R, int G, int B){
     for(int i = start; i < end; i++){
       setOneLightRGB(i, R, G, B);
     }
-  } 
-  public void setLightsCone() {
-    for(int i = 0; i < 52; i++){
-      setOneLightRGB(i, 100, 64, 0);
   }
-}
-  public void setLightsCube() {
-    for(int i = 0; i < 52; i++){
-      setOneLightRGB(i, 0, 0, 100);
+  public void setSectionOne(int R, int G, int B){
+    setLights(0, LEDBuffer.getLength()/2, R, G, B);
   }
+  public void setSectionTwo(int R, int G, int B){
+    setLights(LEDBuffer.getLength()/2, LEDBuffer.getLength(), R, G, B);
   }
+
   public void lightsOut() {
-    for(int i = 0; i < LEDBuffer.getLength(); i++) {
-      setOneLightRGB(i, 0, 0, 0);
-    }
+    setLights(0, LEDBuffer.getLength(), 0, 0, 0);
   }
   @Override
   public void periodic() {

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

@@ -0,0 +1,19 @@
+package frc.robot.subsystems;
+
+public class RobotState {
+	private static RobotState instance;
+	public boolean IsNoteSeen;
+	public boolean ShooterInRange;
+	public boolean ShooterReady;
+
+	private RobotState() {
+
+	}
+
+	public static RobotState getInstance() {
+		if (instance == null) {
+			instance = new RobotState();
+		}
+		return instance;
+	}
+}