Merge pull request #144 from 2491-NoMythic/reorganizeSensors

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

@@ -221,7 +221,6 @@ public RobotContainer() {
     if(lightsExist) {lightsInst();}
     if(indexerExists) {indexInit();}
     if(intakeExists && shooterExists && indexerExists && angleShooterExists) {indexCommandInst();}
-    Limelight.useDetectorLimelight(useDetectorLimelight);
     configureDriveTrain();
     configureBindings();
     autoInit();
@@ -273,7 +272,7 @@ private void climberInst() {
       ClimberDownSup));
   }
   private void indexInit() {
-    indexer = new IndexerSubsystem(intakeExists ? intake::isNoteSeen : () -> false);
+    indexer = new IndexerSubsystem(intakeExists ? RobotState.getInstance()::isNoteSeen : () -> false);
   }
   private void indexCommandInst() {
     defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorRevToZero, intakeReverse, OverStagePassSup, OppositeStageShotSup);
@@ -341,13 +340,13 @@ public boolean runsWhenDisabled() {
     if(Preferences.getBoolean("Detector Limelight", false)) {
       Command AutoGroundIntake = new SequentialCommandGroup(
         new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem),
-        new InstantCommand(()->intake.setNoteHeld(true))
+        new InstantCommand(()->RobotState.getInstance().IsNoteHeld = true)
       );
       autoPickup = new ParallelRaceGroup(
         new SequentialCommandGroup(
           new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(30), angleShooterSubsystem),
           new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem),
-          new InstantCommand(()->intake.setNoteHeld(true))),
+          new InstantCommand(()->RobotState.getInstance().setNoteHeld(true))),
         new SequentialCommandGroup(
           new CollectNote(driveTrain, limelight),
           new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain),
@@ -358,7 +357,7 @@ public boolean runsWhenDisabled() {
       podiumAutoPickup = new ParallelRaceGroup(
           new SequentialCommandGroup(
             new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem),
-            new InstantCommand(()->intake.setNoteHeld(true))),
+            new InstantCommand(()->RobotState.getInstance().setNoteHeld(true))),
           new SequentialCommandGroup(
             new PodiumCollectNote(driveTrain, limelight),
             new DriveTimeCommand(-1, 0, 0, 0.5, driveTrain),
@@ -387,7 +386,7 @@ public boolean runsWhenDisabled() {
       new Trigger(GroundIntakeSup).whileTrue(new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem));
     }
     if(intakeExists&&indexerExists) {
-      new Trigger(intake::isNoteSeen).and(()->!intake.isNoteHeld()).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5));
+      new Trigger(()->RobotState.getInstance().isNoteSeen()).and(()->!RobotState.getInstance().IsNoteHeld).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5));
     }
 
     SmartDashboard.putData("move 1 meter", new MoveMeters(driveTrain, 1, 0.2, 0.2, 0.2));
@@ -579,7 +578,7 @@ public void execute() {
       if(sideWheelsExists){
         NamedCommands.registerCommand("intakeSideWheels", new InstantCommand(()-> intake.intakeSideWheels(1)));
       }
-      NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!intake.isNoteSeen()));
+      NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!RobotState.getInstance().isNoteSeen()));
     }
     if(indexerExists&&shooterExists) {
       NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.9, 0.1, angleShooterSubsystem));
@@ -624,7 +623,7 @@ public void ampShotInit() {
         new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)),
         new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high
         new WaitCommand(0.5),
-        new InstantCommand(()->intake.setNoteHeld(false))
+        new InstantCommand(()->RobotState.getInstance().setNoteHeld(false))
         );
         SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed);
         /**
@@ -646,7 +645,7 @@ public void ampShotInit() {
         new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)),
         new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer),
         new WaitCommand(0.5),
-        new InstantCommand(()->intake.setNoteHeld(false))
+        new InstantCommand(()->RobotState.getInstance().setNoteHeld(false))
       );
         new Trigger(AmpAngleSup).whileTrue(orbitAmpShot);
         SmartDashboard.putData("amp shot", scoreAmp);
@@ -688,6 +687,13 @@ public void robotPeriodic() {
       currentAlliance = DriverStation.getAlliance().get();
       SmartDashboard.putBoolean("RobotPeriodicRan", true);
       SmartDashboard.putString("AlliancePeriodic", currentAlliance == null? "null" : currentAlliance == Alliance.Red? "Red": "Blue" );
+
+      if(Preferences.getBoolean("Use Limelight", false)) {
+        limelight.updateLoggingWithPoses();
+      }
+
+      SmartDashboard.putBoolean("is note in", RobotState.getInstance().isNoteSeen());
+      SmartDashboard.putBoolean("is note held", RobotState.getInstance().IsNoteHeld);
     // logPower();
   }
   public void disabledPeriodic() {

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

@@ -34,7 +34,7 @@ public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer, Drivetrain
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    if (intake.isNoteSeen()){
+    if (RobotState.getInstance().isNoteSeen()){
       RobotState.getInstance().lightsReset = true;
     }
     angleShooter.setDesiredShooterAngle(30);
@@ -75,6 +75,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return intake.isNoteSeen();
+    return RobotState.getInstance().isNoteSeen();
   }
 }

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

@@ -116,12 +116,12 @@ public void execute() {
     } else {
       auto = false;
     }
-    if (!intake.isNoteSeen()) {
+    if (!RobotState.getInstance().isNoteSeen()) {
       // intake.intakeYes(IntakeConstants.INTAKE_SPEED); // only code that runs the intake
       if(runsEmpty<21) {runsEmpty++;}
       if(runsEmpty>=20) {
         idleReving = false;
-        intake.setNoteHeld(false);
+        RobotState.getInstance().IsNoteHeld = false;
         if(humanPlayerSupplier.getAsBoolean()) {
           m_Indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED);
           shooter.setTargetVelocity(HUMAN_PLAYER_RPS, HUMAN_PLAYER_RPS, 40, 40);
@@ -168,8 +168,8 @@ public void execute() {
       }
       if (indexer) {
           m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_POWER);
-          if(!intake.isNoteSeen()) {
-            intake.setNoteHeld(false);
+          if(!RobotState.getInstance().isNoteSeen()) {
+            RobotState.getInstance().IsNoteHeld = false;
           }
        } else {
           m_Indexer.off();

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

@@ -11,6 +11,7 @@
 import frc.robot.settings.Constants.IndexerConstants;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RobotState;
 import frc.robot.subsystems.ShooterSubsystem;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -35,9 +36,9 @@ public IndexerNoteAlign(IndexerSubsystem indexer, IntakeSubsystem intake) {
       new WaitCommand(()->0.6),
       new InstantCommand(()->indexer.magicRPS(10), indexer),
       new WaitCommand(()->0.3),
-      new WaitUntil(()->intake.isNoteSeen()),
+      new WaitUntil(()->RobotState.getInstance().isNoteSeen()),
       new WaitCommand(()->0.1),
-      new InstantCommand(()->intake.setNoteHeld(true))
+      new InstantCommand(()->RobotState.getInstance().IsNoteHeld = true)
     );
   }
 }

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

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.commands;
+import frc.robot.subsystems.RobotState;
 import frc.robot.settings.Constants.IndexerConstants;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -45,7 +46,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     indexer.off();
-    intake.setNoteHeld(false);
+    RobotState.getInstance().IsNoteHeld = false;
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/NamedCommands/AutoGroundIntake.java

@@ -14,6 +14,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.RobotState;
 public class AutoGroundIntake extends Command {
   /** Creates a new ConditionalIndexer. */
   IndexerSubsystem indexer;
@@ -55,12 +56,12 @@ public void execute() {
   public void end(boolean interrupted) {
     intake.intakeOff();
     indexer.off();
-    intake.setNoteHeld(true);
+    RobotState.getInstance().IsNoteHeld = true;
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return intake.isNoteSeen();
+    return RobotState.getInstance().isNoteSeen();
   }
 }

src/main/java/frc/robot/commands/NamedCommands/ShootNote.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands.NamedCommands;
 
+import frc.robot.subsystems.RobotState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -54,7 +55,7 @@ public void end(boolean interrupted) {
     timer.stop();
     indexer.off();
     timer.reset();
-    intake.setNoteHeld(false);
+    RobotState.getInstance().IsNoteHeld = false;
   }
 
   // Returns true when the command should end.

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

@@ -220,6 +220,10 @@ public double getHeadingLooped() {
 	public Rotation2d getGyroscopeRotation() {
 		return pigeon.getRotation2d();
 	}
+	/**
+	 * 
+	 * @return a rotation2D of the angle according to the odometer
+	 */
 	public Rotation2d getOdometryRotation() {
 		return odometer.getEstimatedPosition().getRotation();
 	}
@@ -313,7 +317,7 @@ public void updateOdometry() {
 	 * larger pose shifts will take multiple calls to complete.
 	 */
 	public void updateOdometryWithVision() {
-		PoseEstimate estimate = limelight.getTrustedPose(getPose(), getLLFOM(APRILTAG_LIMELIGHT2_NAME), getLLFOM(APRILTAG_LIMELIGHT3_NAME));
+		PoseEstimate estimate = limelight.getTrustedPose();
 		if (estimate != null) {
 			boolean doRejectUpdate = false;
 			if(Math.abs(pigeon.getRate()) > 720) {
@@ -330,16 +334,14 @@ public void updateOdometryWithVision() {
 		} else {
 			RobotState.getInstance().LimelightsUpdated = false;
 		}
-
-		limelight.updateLoggingWithPoses();
 	}
 	/**
 	 * Set the odometry using the current apriltag estimate, disregarding the pose trustworthyness.
 	 * <p>
 	 * You only need to run this once for it to take effect.
 	 */
 	public void forceUpdateOdometryWithVision() {
-		PoseEstimate estimate = limelight.getValidPose();
+		PoseEstimate estimate = limelight.getTrustedPose();
 		if (estimate != null) {
 			resetOdometry(estimate.pose);
 		} else {
@@ -574,6 +576,7 @@ public void periodic() {
 
 		m_field.setRobotPose(odometer.getEstimatedPosition());
         SmartDashboard.putNumber("Robot Angle", getOdometryRotation().getDegrees());
+		RobotState.getInstance().odometerOrientation = getOdometryRotation().getDegrees();
         SmartDashboard.putString("Robot Location", getPose().getTranslation().toString());
 		SmartDashboard.putNumber("calculated speaker angle", calculateSpeakerAngleMoving());
 		SmartDashboard.putNumber("TESTING robot angle difference", getSpeakerAngleDifference());
@@ -622,37 +625,4 @@ private Pose3d robotToFieldCoordinates(Pose3d robotCoordinates)
 
 		return new Pose3d(robotCoordinatesX+odometrPoseX,robotCoordinatesY+odometrPosey,robotCoordinatesZ, robotCoordinatesAngle);
 	}
-
-	public double getLLFOM(String limelightName) //larger fom is BAD, and is less trustworthy. 
-    {
-		//the value we place on each variable in the FOM. Higher value means it will get weighted more in the final FOM
-		/*These values should be tuned based on how heavily you want a contributer to be favored. Right now, we want the # of tags to be the most important 
-		 * with the distance from the tags also being immportant. and the tx and ty should only factor in a little bit, so they have the smallest number. Test this by making sure the two 
-		 * limelights give very different robot positions, and see where it decides to put the real robot pose.
-		*/
-		double distValue = 6;
-		double tagCountValue = 7;
-		double xyValue = 1;
-
-		//numTagsContributer is better when smaller, and is based off of how many april tags the Limelight identifies
-		double numTagsContributer;
-		if(limelight.getLLTagCount(limelightName) <= 0){
-			numTagsContributer = 0;
-		}else{
-			numTagsContributer = 1/limelight.getLLTagCount(limelightName);
-		}
-		//tx and ty contributers are based off where on the limelights screen the april tag is. Closer to the center means the contributer will bea smaller number, which is better.
-		double centeredTxContributer = Math.abs((limelight.getAprilValues(limelightName).tx))/29.8; //tx gets up to 29.8, the closer to 0 tx is, the closer to the center it is.
-		double centeredTyContributer = Math.abs((limelight.getAprilValues(limelightName).ty))/20.5; //ty gets up to 20.5 for LL2's and down. LL3's go to 24.85. The closer to 0 ty is, the closer to the center it is.
-		//the distance contributer gets smaller when the distance is closer, and is based off of how far away the closest tag is
-		double distanceContributer = (limelight.getClosestTagDist(limelightName)/5);
-
-		// calculates the final FOM by taking the contributors and multiplying them by their values, adding them all together and then dividing by the sum of the values.
-		double LLFOM = (
-			(distValue*distanceContributer)+(tagCountValue*numTagsContributer)+(centeredTxContributer*xyValue)+(centeredTyContributer)
-			)/distValue+tagCountValue+xyValue+xyValue;
-		Logger.recordOutput("Vision/LLFOM" + limelightName, LLFOM);
-		return LLFOM;
-    }
-
 }

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

@@ -83,12 +83,7 @@ public IntakeSubsystem() {
       intakeSideRight.burnFlash();
       //intakeSideRight.getPIDController().setReference(intakeRunSpeed, CANSparkMax.ControlType.kVelocity)
     }
-
-    if (Preferences.getBoolean("CompBot", true)) {
-      m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute);
-    } else {
-      m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute);
-    }
+    m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute);
 
     DataLog log = DataLogManager.getLog();
     motorLogger1 = new MotorLogger(log, "/intake/motor1");
@@ -152,24 +147,6 @@ public void intakeOff() {
     }
   }
 
-  /**
-   * uses the distance sensor inside the indexer to tell if there is a note fully
-   * inside the indexer
-   * 
-   * @return if the sensor sees something within it's range in front of it
-   */
-  public boolean isNoteSeen() {
-    return m_DistanceSensor.getVoltage() < 2;
-  }
-
-  public boolean isNoteHeld() {
-    return isNoteHeld;
-  }
-
-  public void setNoteHeld(boolean held) {
-    isNoteHeld = held;
-  }
-
   @Override
   public void periodic() {
     SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage());
@@ -178,9 +155,7 @@ public void periodic() {
     motorLogger1.log(intake1);
     motorLogger2.log(intake2);
     logDistance.append(m_DistanceSensor.getVoltage());
-    logNoteIn.append(isNoteSeen());
-    SmartDashboard.putBoolean("is note in", isNoteSeen());
-    SmartDashboard.putBoolean("is note held", isNoteHeld());
-    RobotState.getInstance().IsNoteHeld = isNoteSeen();
+    logNoteIn.append(RobotState.getInstance().isNoteSeen());
+    RobotState.getInstance().intakeSensorVoltage = m_DistanceSensor.getVoltage();
   }
 }