Internals color sensor thread

grt192 · Feb 13, 2022 · 594df6f · 594df6f
1 parent 236f2aa
commit 594df6f
Show file tree

Hide file tree

Showing 3 changed files with 95 additions and 38 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,29 +4,23 @@
 
 package frc.robot;
 
-import java.io.File;
-import java.io.FileInputStream;
-import java.io.IOException;
 import java.util.List;
-import java.util.Properties;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import frc.robot.brownout.PowerController;
 import frc.robot.commands.tank.FollowPathCommand;
 import frc.robot.jetson.JetsonConnection;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.InternalSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.subsystems.internals.InternalSubsystem;
 import frc.robot.subsystems.tank.TankSubsystem;
 
 /**
@@ -77,25 +71,24 @@ public RobotContainer() {
 
         // Instantiate power controller
         powerController = new PowerController(
-            tankSubsystem, 
-            turretSubsystem, 
-            internalSubsystem, 
-            intakeSubsystem
-            //, climbSubsystem
+                tankSubsystem,
+                turretSubsystem,
+                internalSubsystem,
+                intakeSubsystem
+        // , climbSubsystem
         );
 
         // Instantiate commands
         // Drive an S-shaped curve from the origin to 3 meters in front through 2
         // waypoints
         if (tankSubsystem != null) {
             tankCommand = new FollowPathCommand(
-                tankSubsystem,
-                new Pose2d(),
-                List.of(
-                    new Translation2d(1, 1),
-                    new Translation2d(2, -1)),
-                new Pose2d(3, 0, new Rotation2d())
-            );
+                    tankSubsystem,
+                    new Pose2d(),
+                    List.of(
+                            new Translation2d(1, 1),
+                            new Translation2d(2, -1)),
+                    new Pose2d(3, 0, new Rotation2d()));
         } else {
             tankCommand = new InstantCommand();
         }
@@ -135,14 +128,18 @@ private void controllerBindings() {
         // mechXButton.whenPressed(climbSubsystem.climb());
 
         if (tankSubsystem != null) {
-            Runnable tank = () -> tankSubsystem.setCarDrivePowers(-driveController.getLeftY(), driveController.getRightX());
+            Runnable tank = () -> tankSubsystem.setCarDrivePowers(-driveController.getLeftY(),
+                    driveController.getRightX());
             tankSubsystem.setDefaultCommand(new RunCommand(tank, tankSubsystem));
         }
 
         if (intakeSubsystem != null) {
             // TODO: tune deadband
             Runnable intake = () -> {
-                intakeSubsystem.setIntakePower((driveController.getRightTriggerAxis() * 0.5)); // - driveController.getLeftTriggerAxis()) * 0.4);
+                intakeSubsystem
+                        .setIntakePower(driveController.getRightTriggerAxis() - driveController.getLeftTriggerAxis()); // -
+                // driveController.getLeftTriggerAxis())
+                // * 0.4);
                 // System.out.println(driveController.getRightTriggerAxis() * 0.5);
                 double deployPow = 0;
                 if (driveController.getPOV() == 90) {
@@ -154,7 +151,7 @@ private void controllerBindings() {
             };
             intakeSubsystem.setDefaultCommand(new RunCommand(intake, intakeSubsystem));
         }
-/*
+
         if (internalSubsystem != null) {
             Runnable internals = () -> {
                 double pow = 0;
@@ -175,11 +172,11 @@ private void controllerBindings() {
             };
             turretSubsystem.setDefaultCommand(new RunCommand(turret, turretSubsystem));
         }
-        */
     }
 
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
+     * 
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
@@ -188,6 +185,7 @@ public Command getAutonomousCommand() {
 
     /**
      * Gets the PowerController instance.
+     * 
      * @return The PowerController instance.
      */
     public PowerController getPowerController() {

diff --git a/src/main/java/frc/robot/subsystems/internals/ColorSensorThread.java b/src/main/java/frc/robot/subsystems/internals/ColorSensorThread.java
@@ -0,0 +1,47 @@
+package frc.robot.subsystems.internals;
+
+import com.revrobotics.ColorSensorV3;
+
+import edu.wpi.first.wpilibj.util.Color;
+
+public class ColorSensorThread {
+    private ColorSensorRunnable runnable;
+
+    public ColorSensorThread(ColorSensorV3 storage, ColorSensorV3 staging) {
+        this.runnable = new ColorSensorRunnable(storage, staging);
+
+        Thread thread = new Thread(runnable);
+        thread.setDaemon(true);
+        thread.start();
+    }
+
+    public Color getLastStorage() {
+        return runnable.lastStorage;
+    }
+
+    public Color getLastStaging() {
+        return runnable.lastStaging;
+    }
+
+    class ColorSensorRunnable implements Runnable {
+        ColorSensorV3 storage;
+        ColorSensorV3 staging;
+
+        Color lastStorage;
+        Color lastStaging;
+
+        public ColorSensorRunnable(ColorSensorV3 storage, ColorSensorV3 staging) {
+            this.storage = storage;
+            this.staging = staging;
+        }
+
+        @Override
+        public void run() {
+            // TODO Auto-generated method stub
+            while (true) {
+                this.lastStorage = storage.getColor();
+                this.lastStaging = staging.getColor();
+            }
+        }
+    }
+}
diff --git a/...c/robot/subsystems/InternalSubsystem.java → ...bsystems/internals/InternalSubsystem.java b/...c/robot/subsystems/InternalSubsystem.java → ...bsystems/internals/InternalSubsystem.java
@@ -1,21 +1,25 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems.internals;
+
+import static frc.robot.Constants.InternalConstants.BLUE;
+import static frc.robot.Constants.InternalConstants.RED;
+import static frc.robot.Constants.InternalConstants.allianceColor;
+import static frc.robot.Constants.InternalConstants.entranceIRPort;
+import static frc.robot.Constants.InternalConstants.motorPortBottom;
+import static frc.robot.Constants.InternalConstants.motorPortTop;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
 import com.revrobotics.ColorMatch;
 import com.revrobotics.ColorMatchResult;
 import com.revrobotics.ColorSensorV3;
 
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.util.Color;
-
 import frc.robot.GRTSubsystem;
 import frc.robot.brownout.PowerController;
-
-import static frc.robot.Constants.InternalConstants.*;
+import frc.robot.subsystems.TurretSubsystem;
 
 public class InternalSubsystem extends GRTSubsystem {
 
@@ -27,7 +31,7 @@ public class InternalSubsystem extends GRTSubsystem {
     private final AnalogPotentiometer entrance;
     private final ColorSensorV3 staging;
     private final ColorSensorV3 storage;
-    //private final AnalogPotentiometer exit;
+    // private final AnalogPotentiometer exit;
 
     private final ColorMatch colorMatcher;
 
@@ -59,12 +63,12 @@ public InternalSubsystem(TurretSubsystem turretSubsystem) {
         entrance = new AnalogPotentiometer(entranceIRPort);
         staging = new ColorSensorV3(I2C.Port.kOnboard);
         storage = new ColorSensorV3(I2C.Port.kMXP);
-        //exit = new AnalogPotentiometer(exitIRPort);
+        // exit = new AnalogPotentiometer(exitIRPort);
 
         colorMatcher = new ColorMatch();
         colorMatcher.addColorMatch(RED);
         colorMatcher.addColorMatch(BLUE);
-        //colorMatcher.addColorMatch(EMPTY);
+        // colorMatcher.addColorMatch(EMPTY);
     }
 
     @Override
@@ -73,12 +77,14 @@ public void periodic() {
         boolean reject = false;
 
         // If a ball has entered internals, start the bottom motor
-        if (ballDetected(entrance)) motorBottom.set(0.5);
+        if (ballDetected(entrance))
+            motorBottom.set(0.5);
 
         // If there is a ball in storage, stop the bottom motor and start the top motor
         if (ballDetected(storage)) {
             // Reject the ball if it doesn't match alliance color
-            // Call this in storage *and* staging to give the turret more time to aim in a one-ball scenario;
+            // Call this in storage *and* staging to give the turret more time to aim in a
+            // one-ball scenario;
             // a second ball in staging will override this call and not have any effect.
             reject = getColor(storage) != allianceColor;
             motorBottom.set(0);
@@ -94,15 +100,16 @@ public void periodic() {
 
         if (turretSubsystem != null) {
             // If a shot was requested and the turret is ready, load a ball into the turret.
-            // If rejecting, the turret can be in a semi-aligned state; otherwise, require it to be fully lined up.
+            // If rejecting, the turret can be in a semi-aligned state; otherwise, require
+            // it to be fully lined up.
             turretSubsystem.setReject(reject);
             if (shotRequested && turretSubsystem.getState() == TurretSubsystem.ModuleState.READY
-                || reject && turretSubsystem.getState() == TurretSubsystem.ModuleState.ALMOST) { 
+                    || reject && turretSubsystem.getState() == TurretSubsystem.ModuleState.ALMOST) {
                 // If the ball hasn't left staging, spin the top motor
                 if (ballDetected(staging)) {
                     motorTop.set(0.5);
                 } else {
-                    // Otherwise, the ball has left internals; 
+                    // Otherwise, the ball has left internals;
                     // Mark the shot as finished and decrement the ball count.
                     shotRequested = false;
                     ballCount--;
@@ -137,14 +144,17 @@ public void updateBallCount() {
 
     /**
      * Gets how many balls are inside internals.
+     * 
      * @return The current ball count.
      */
     public int getBallCount() {
         return ballCount;
     }
 
     /**
-     * Gets the detected color from a color sensor, normalizing it to the closest stored color in colorMatcher.
+     * Gets the detected color from a color sensor, normalizing it to the closest
+     * stored color in colorMatcher.
+     * 
      * @param s The color sensor to get.
      * @return The normalized detected color.
      */
@@ -156,6 +166,7 @@ public Color getColor(ColorSensorV3 s) {
 
     /**
      * Checks whether a ball has been detected by a color sensor.
+     * 
      * @param s The color sensor to check.
      * @return Whether a ball has been detected by the sensor.
      */
@@ -166,9 +177,10 @@ public boolean ballDetected(ColorSensorV3 s) {
 
     /**
      * Checks whether a ball has been detected by an IR sensor.
+     * 
      * @param s The IR sensor to check.
      * @return Whether a ball has been detected by the sensor.
-     * TODO: test this and make sure it works
+     *         TODO: test this and make sure it works
      */
     public boolean ballDetected(AnalogPotentiometer s) {
         // TODO: measure the resting value of the IR sensor on the wall