Merge pull request #224 from strykeforce/rabsamu/vision-tuning

Rabsamu/vision tuning

Author: mwitcpalek

src/main/java/frc/team2767/deepspace/command/vision/VisionTuneCommand.java

@@ -0,0 +1,37 @@
+package frc.team2767.deepspace.command.vision;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.subsystem.VisionSubsystem;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class VisionTuneCommand extends InstantCommand {
+
+  private static final VisionSubsystem VISION = Robot.VISION;
+  private final Logger logger = LoggerFactory.getLogger(this.getClass());
+  private int tuningID;
+
+  public VisionTuneCommand(int tuningID) {
+    this.tuningID = tuningID;
+
+    setRunWhenDisabled(true);
+    requires(VISION);
+  }
+
+  @Override
+  protected void initialize() {
+    VISION.enableLights(true);
+    VISION.runTuning(tuningID);
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return VISION.tuneFinished();
+  }
+
+  @Override
+  protected void end() {
+    VISION.enableLights(false);
+  }
+}

src/main/java/frc/team2767/deepspace/control/Controls.java

@@ -5,6 +5,7 @@ public class Controls {
   private final DriverControls driverControls = new DriverControls(0);
   private final XboxControls xboxControls = new XboxControls(1);
   private final SmartDashboardControls smartDashboardControls = new SmartDashboardControls();
+  private final RoborioControls roborioControls = new RoborioControls();
 
   public Controls() {
     //    new SmartDashboardControls();
@@ -21,4 +22,8 @@ public XboxControls getXboxControls() {
   public SmartDashboardControls getSmartDashboardControls() {
     return smartDashboardControls;
   }
+
+  public RoborioControls getRoborioControls() {
+    return roborioControls;
+  }
 }

src/main/java/frc/team2767/deepspace/control/RoborioControls.java

@@ -0,0 +1,30 @@
+package frc.team2767.deepspace.control;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.buttons.Trigger;
+import frc.team2767.deepspace.command.vision.VisionTuneCommand;
+
+public class RoborioControls {
+
+  public RoborioControls() {
+    DigitalInput tuneTrigger = new DigitalInput(9);
+    Trigger visionButton =
+        new Trigger() {
+          @Override
+          public boolean get() {
+            return tuneTrigger.get();
+          }
+        };
+    Trigger userButton =
+        new Trigger() {
+          @Override
+          public boolean get() {
+            return RobotController.getUserButton();
+          }
+        };
+
+    userButton.whenActive(new VisionTuneCommand(0));
+    visionButton.whenActive(new VisionTuneCommand(1));
+  }
+}

src/main/java/frc/team2767/deepspace/subsystem/VisionSubsystem.java

@@ -51,6 +51,8 @@ public class VisionSubsystem extends Subsystem implements Item {
   private NetworkTableEntry rangeEntry;
   private NetworkTableEntry cameraIDEntry;
   private NetworkTableEntry targetYawEntry;
+  private NetworkTableEntry tuningEntry;
+  private NetworkTableEntry tuningFinished;
   private double rawRange;
   private double rawBearing;
   private double correctedRange;
@@ -70,9 +72,12 @@ public VisionSubsystem() {
     rangeEntry = table.getEntry("camera_range");
     cameraIDEntry = table.getEntry("camera_id");
     targetYawEntry = table.getEntry("target_yaw");
+    tuningEntry = table.getEntry("tuning_id");
+    tuningFinished = table.getEntry("tuning_finished");
     targetYawEntry.setNumber(0.0);
     bearingEntry.setNumber(0.0);
     rangeEntry.setNumber(-1.0);
+    tuningEntry.setNumber(-1.0);
 
     UsbCamera usbCamera = cameraServer.startAutomaticCapture();
     logger.info("camera is connected = {}", usbCamera.isConnected());
@@ -141,6 +146,14 @@ public void setFieldDirection(FieldDirection direction) {
     logger.info("set direction to {}", direction);
   }
 
+  public void runTuning(int camID) {
+    tuningEntry.setNumber(camID);
+  }
+
+  public boolean tuneFinished() {
+    return ((int) tuningFinished.getNumber(0) == 1);
+  }
+
   public void selectCamera() {
     VisionSubsystem.Camera camera;
     camera = Camera.LEFT;