Color sensor (#247)

Patribots4738 · Mar 14, 2024 · 5379246 · 5379246
2 parents f2ef2f1 + 0da8e0b
commit 5379246
Show file tree

Hide file tree

Showing 4 changed files with 99 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -6,6 +6,8 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
+import com.revrobotics.ColorSensorV3;
+import com.revrobotics.ColorSensorV3.ColorSensorMeasurementRate;
 
 import edu.wpi.first.math.MathUtil;
 
@@ -41,6 +43,7 @@
 import frc.robot.subsystems.*;
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.Constants.CameraConstants;
+import frc.robot.util.Constants.ColorSensorConstants;
 import frc.robot.util.Constants.DriveConstants;
 import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.NTConstants;
@@ -95,9 +98,13 @@ public class RobotContainer implements Logged {
     public static HDCTuner HDCTuner;
 
     private final LedStrip ledStrip;
+    @IgnoreLogged
     private Indexer indexer;
+    @IgnoreLogged
     private Trapper trapper;
     private ShooterCmds shooterCmds;
+    @IgnoreLogged
+    private ColorSensor colorSensor = new ColorSensor(ColorSensorConstants.I2C_PORT);
 
     @IgnoreLogged
     private PieceControl pieceControl;
@@ -157,6 +164,8 @@ public RobotContainer() {
             limelight2.disableLEDS();
             limelight3.disableLEDS();
         }
+
+
         ledStrip = new LedStrip(swerve::getPose);
         indexer = new Indexer();
 
@@ -184,7 +193,8 @@ public RobotContainer() {
             indexer,
             elevator,
             trapper,
-            shooterCmds);
+            shooterCmds,
+            colorSensor);
 
         calibrationControl = new CalibrationControl(shooterCmds);
 
@@ -677,6 +687,7 @@ private void configureLoggingPaths() {
             Monologue.logObj(limelight2, "Robot/Limelights/limelight2");
             Monologue.logObj(limelight3, "Robot/Limelights/limelight3");
         }
+        Monologue.logObj(colorSensor, "Robot/ColorSensors/colorSensor");
         Monologue.logObj(shooter, "Robot/Subsystems/shooter");
         Monologue.logObj(elevator, "Robot/Subsystems/elevator");
         Monologue.logObj(pivot, "Robot/Subsystems/pivot");

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -13,6 +13,7 @@
 import frc.robot.Robot.GameMode;
 import frc.robot.RobotContainer;
 import frc.robot.commands.logging.NT;
+import frc.robot.subsystems.ColorSensor;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Intake;
@@ -35,6 +36,8 @@ public class PieceControl implements Logged {
 
     private ShooterCmds shooterCmds;
 
+    private ColorSensor colorSensor;
+
     @Log
     private boolean shooterMode = false;
 
@@ -56,12 +59,14 @@ public PieceControl(
             Indexer indexer,
             Elevator elevator,
             Trapper trapper,
-            ShooterCmds shooterCmds) {
+            ShooterCmds shooterCmds,
+            ColorSensor colorSensor) {
         this.intake = intake;
         this.indexer = indexer;
         this.elevator = elevator;
         this.trapper = trapper;
         this.shooterCmds = shooterCmds;
+        this.colorSensor = colorSensor;
     }
 
     public Command stopAllMotors() {
@@ -123,7 +128,7 @@ public Command intakeNote() {
             intake.inCommand(),
             trapper.intakeSlow(0.6),
             indexer.toShooterSlow(),
-            Commands.waitUntil(trapper.getPossessionTrigger()),
+            Commands.waitUntil(colorSensor.hasNoteTrigger()),
             setHasPiece(true),
             NT.getWaitCommand("intakeToTrap1"), // 0.1
             // Use the trap to index it the first time around
@@ -136,7 +141,7 @@ public Command toIndexerAuto() {
             intake.inCommand(),
             trapper.intake(),
             indexer.stopCommand(),
-            Commands.waitUntil(trapper.getPossessionTrigger()),
+            Commands.waitUntil(colorSensor.hasNoteTrigger()),
             setHasPiece(true),
             Commands.waitSeconds(0.15),
             stopIntakeAndIndexer()

diff --git a/src/main/java/frc/robot/subsystems/ColorSensor.java b/src/main/java/frc/robot/subsystems/ColorSensor.java
@@ -0,0 +1,61 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.ColorMatchResult;
+import com.revrobotics.ColorSensorV3;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.util.Constants.ColorSensorConstants;
+import monologue.Logged;
+import monologue.Annotations.Log;
+
+public class ColorSensor extends SubsystemBase implements Logged {
+
+    private ColorSensorV3 colorSensor;
+
+    private Trigger hasNoteTrigger;
+
+    @Log
+    private boolean hasOrange = false;
+
+    private Color detectedColor = new Color(0.0, 0.0, 0.0);
+
+    @Log
+    private String colorString = "";
+
+    private ColorMatchResult match = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColor);
+
+    @Log 
+    private String matchString = "";
+
+    public ColorSensor(I2C.Port i2cPort) {
+        this.colorSensor = new ColorSensorV3(i2cPort);
+    }
+
+    @Override
+    public void periodic() {
+        this.detectedColor = colorSensor.getColor();
+        this.match = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColor);
+
+        colorString = detectedColor.toString();
+        if (this.match.color == ColorSensorConstants.NOTE_ORANGE) {
+            matchString = "Note";
+            hasOrange = true;
+        } else {
+            matchString = "Surely not a note";
+            hasOrange = false;
+        }
+    }
+
+    public boolean hasNote() {
+        return this.hasOrange;
+    }
+
+    public Trigger hasNoteTrigger() {
+        if (hasNoteTrigger == null) {
+            hasNoteTrigger = new Trigger(this::hasNote);
+        }
+        return hasNoteTrigger;
+    }
+}
diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -8,6 +8,8 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
+import com.revrobotics.ColorMatch;
+
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
@@ -26,7 +28,9 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
 import frc.robot.Robot;
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.custom.PatrIDConstants;
@@ -413,6 +417,20 @@ public static final class AutoConstants {
         };
     }
 
+    public static final class ColorSensorConstants {
+
+        public static final I2C.Port I2C_PORT = I2C.Port.kMXP;
+        public static final Color NOTE_ORANGE = new Color(0.957, 0.282, 0.086);
+        public static final ColorMatch COLOR_MATCH = new ColorMatch() 
+        {{
+            // BLACK
+            addColorMatch(Color.kBlack);
+            // NEW COMP READY NOTE ORANGE
+            addColorMatch(NOTE_ORANGE);
+            addColorMatch(Color.kWhite);
+        }};
+
+    }
     public static final class ModuleConstants {
 
         // https://www.revrobotics.com/rev-21-3005/