LEDS checking

Team2470 · Jan 6, 2024 · 6855946 · 6855946
1 parent e7738c0
commit 6855946
Show file tree

Hide file tree

Showing 6 changed files with 134 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -7,8 +7,10 @@
 import com.pathplanner.lib.auto.PIDConstants;
 import com.swervedrivespecialties.swervelib.SdsModuleConfigurations;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
@@ -144,7 +146,13 @@ public static class AutoConstants {
   }
 
   public static class VisionConstants {
-    public static final Transform3d kBackLeftCamera = new Transform3d(); // TODO
-    public static final Transform3d kBackRightCamera = new Transform3d(); // TODO
+    public static final Transform3d kBackLeftCamera = new Transform3d(
+      new Translation3d(Units.inchesToMeters(-5.89), Units.inchesToMeters(6.30), Units.inchesToMeters(9.66)),
+      new Rotation3d(0,Units.degreesToRadians(-15.0),Units.degreesToRadians(150))
+    );
+    public static final Transform3d kBackRightCamera = new Transform3d(
+      new Translation3d(Units.inchesToMeters(-5.89), -Units.inchesToMeters(6.30), Units.inchesToMeters(9.66)),
+      new Rotation3d(0,Units.degreesToRadians(-15.0),Units.degreesToRadians(-150))
+    );
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,8 +17,10 @@
 import frc.robot.Constants.VisionConstants;
 import frc.robot.commands.DriveWithController;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.PivotLEDs;
 import frc.robot.subsystems.PivotTemp;
 import frc.robot.subsystems.VisionSubsystem;
+import frc.robot.subsystems.PivotLEDs.Side;
 import frc.robot.vision.VisionIOPhoton;
 
 /**
@@ -35,6 +37,8 @@ public class RobotContainer {
       new VisionSubsystem(
           new VisionIOPhoton("Back-Left", VisionConstants.kBackLeftCamera),
           new VisionIOPhoton("Back-Right", VisionConstants.kBackRightCamera));
+  private final PivotLEDs m_pivotLEDsLeft = new PivotLEDs(Side.kLeft, m_vision.getCameras()[0]);
+  private final PivotLEDs m_pivotLEDsRight = new PivotLEDs(Side.kRight, m_vision.getCameras()[1]);
   private final Drivetrain m_drivetrain = new Drivetrain(m_vision);
   private final PivotTemp m_pivotTemp = new PivotTemp();
 

diff --git a/src/main/java/frc/robot/subsystems/PivotLEDs.java b/src/main/java/frc/robot/subsystems/PivotLEDs.java
@@ -0,0 +1,57 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.vision.VisionIO;
+
+public class PivotLEDs extends SubsystemBase {
+    public enum Side {
+        kLeft("Left"),
+        kRight("Right");
+
+        public final String name;
+        private Side(String name) {
+            this.name = name;
+        }
+    } 
+
+    public enum PivotLEDState {
+        kCameraDisconnected,
+        kCameraConnected,
+        kCamera1TagDetected,
+        kCamera2TagDetected,
+        kCamera3OrMoreTagDetected,
+    }
+
+    private final Side m_side;
+    private final String m_name;
+    private final VisionIO m_camera;
+
+    public PivotLEDs(Side side, VisionIO camera) {
+        m_side = side;
+        m_name = "PivotLED-"+m_side.name;
+        m_camera = camera;
+    }
+
+    @Override
+    public void periodic() {
+        // Determine the led state
+        PivotLEDState state = PivotLEDState.kCameraDisconnected;
+        int detectedTags = m_camera.detectedTagCount(); 
+        if (m_camera.isConnected()) {
+            state = PivotLEDState.kCameraConnected;
+            if (detectedTags == 1) {
+                state = PivotLEDState.kCamera1TagDetected;
+            } else if (detectedTags == 2) {
+                state = PivotLEDState.kCamera2TagDetected;
+            } else if (detectedTags >=3) {
+                state = PivotLEDState.kCamera3OrMoreTagDetected;
+            }
+        }
+
+        // Set LEDS
+        // TODO
+
+        SmartDashboard.putString(m_name, state.toString());
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -4,8 +4,13 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 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.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.vision.VisionIO;
+
+import java.sql.Driver;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -32,6 +37,8 @@ public VisionSubsystem(VisionIO... cameras) {
     for (int i = 0; i < cameras.length; i++) {
       inputs[i] = new VisionIO.VisionIOInputs();
     }
+
+    // TODO configure a vision tab in ShuffleBoard
   }
 
   @Override
@@ -47,17 +54,23 @@ public void periodic() {
       // Logger.getInstance().processInputs("Vision/" + cameras[i].getName() + "/Inputs",
       // inputs[i]);
 
-      if (inputs[i].hasTarget
-          && inputs[i].isNew
-          && !DriverStation.isAutonomous()
-          && inputs[i].maxDistance < LOWEST_DISTANCE) {
-        if (useSingleTag) {
-          if (inputs[i].singleIDUsed == acceptableTagID) {
+      // TODO figure out what the FMS sends, before robot match starts.
+      // 2910 had: !DriverStation.isAutonomous()
+      // Going to try to accept updates all the time. This should maybe settable/toggle by the driver or elsewhere to control
+      // when to accept updates
+      if (true) {
+        SmartDashboard.putBoolean("Vision accepting updates", true);
+        if (inputs[i].hasTarget && inputs[i].isNew && inputs[i].maxDistance < LOWEST_DISTANCE) {
+          if (useSingleTag) {
+            if (inputs[i].singleIDUsed == acceptableTagID) {
+              processVision(i);
+            }
+          } else {
             processVision(i);
           }
-        } else {
-          processVision(i);
-        }
+        }  
+      } else {
+        SmartDashboard.putBoolean("Vision accepting updates", false);
       }
     }
 
@@ -80,6 +93,11 @@ public List<VisionSubsystem.PoseAndTimestamp> getVisionOdometry() {
     return results;
   }
 
+  /** Returns the array of known cameras */
+  public VisionIO[] getCameras() {
+    return cameras;
+  }
+
   /** Inner class to record a pose and its timestamp */
   public static class PoseAndTimestamp {
     Pose2d pose;
@@ -117,4 +135,14 @@ public void setReferencePose(Pose2d pose) {
   public double getMinDistance(int camera) {
     return inputs[camera].minDistance;
   }
+
+  public void takeSnapshot() {
+    for (VisionIO io : cameras) {
+      io.takeSnapshot();
+    }
+  }
+
+  public Command takeSnapshotCommand() {
+    return Commands.runOnce(this::takeSnapshot);
+  }
 }
diff --git a/src/main/java/frc/robot/vision/VisionIO.java b/src/main/java/frc/robot/vision/VisionIO.java
@@ -29,4 +29,10 @@ default String getName() {
   }
 
   default void setReferencePose(Pose2d pose) {}
+
+  default boolean isConnected() { return false; }
+
+  default int detectedTagCount() {return 0;}
+
+  default void takeSnapshot() {} 
 }
diff --git a/src/main/java/frc/robot/vision/VisionIOPhoton.java b/src/main/java/frc/robot/vision/VisionIOPhoton.java
@@ -1,7 +1,9 @@
 package frc.robot.vision;
 
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.util.FieldConstants;
+
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
@@ -16,6 +18,7 @@ public class VisionIOPhoton implements VisionIO {
   private final PhotonPoseEstimator odometry;
   private double pastTimestamp;
   public List<PhotonTrackedTarget> targets;
+  private int lastDetectedTargetCount = 0;
 
   /**
    * Implements PhotonVision camera
@@ -52,7 +55,8 @@ public void updateInputs(VisionIOInputs inputs) {
     }
 
     inputs.isNew = false;
-
+    lastDetectedTargetCount = result.targets.size();
+    SmartDashboard.putNumber(name + " detected tags", lastDetectedTargetCount);
     if (currentPose.isPresent() && targets != null) {
       if (targets.size() > 1) {
         double minDistance = Double.MAX_VALUE;
@@ -112,6 +116,21 @@ public void setReferencePose(Pose2d pose) {
     odometry.setReferencePose(pose);
   }
 
+  @Override
+  public boolean isConnected() {
+      return camera.isConnected();
+  }
+
+  @Override
+  public int detectedTagCount() {
+    return lastDetectedTargetCount;
+  }
+
+  @Override
+  public void takeSnapshot() {
+      camera.takeOutputSnapshot();
+  }
+
   private boolean checkValidResult(List<PhotonTrackedTarget> result) {
     for (PhotonTrackedTarget target : result) {
       if (target.getFiducialId() > 8) {