Limelight

Robot2019/src/main/java/frc/robot/OI.java

@@ -26,6 +26,7 @@
 import frc.robot.commands.ToggleClimberRails;
 import frc.robot.commands.ToggleHatchEject;
 import frc.robot.commands.ToggleLight;
+import frc.robot.commands.ToggleLimelight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
@@ -44,6 +45,7 @@ public class OI {
   JoystickButton arcadeOrTankBtn;
   JoystickButton normDriveBtn;
   JoystickButton gradDriveBtn;
+  JoystickButton limelightBtn;
   JoystickButton hatchIntakeBtn, hatchEjectBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
@@ -74,6 +76,8 @@ public class OI {
     normDriveBtn.whenPressed(new NormalDrive());
     gradDriveBtn = new JoystickButton(leftJoy, 5);
     gradDriveBtn.whenPressed(new GradualDrive());
+    limelightBtn = new JoystickButton(rightJoy, 2);   // Figure out the correct button
+    limelightBtn.whenPressed(new ToggleLimelight());
 
     hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
     hatchIntakeBtn.whenPressed(new IntakeHatch(hp, dt));
@@ -94,7 +98,6 @@ public class OI {
     manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
     manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator, lights));
 
-
     slowClimbBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
     slowClimbBtn.whileHeld(new SlowClimb());
 

Robot2019/src/main/java/frc/robot/Robot.java

@@ -13,7 +13,8 @@
 import frc.robot.commands.DrivetrainAnalysis;
 import frc.robot.commands.IncreaseVoltageLinear;
 import frc.robot.commands.IncreaseVoltageStepwise;
-import frc.robot.commands.TeleopDrive;
+import frc.robot.commands.Drive;
+import frc.robot.lib.Limelight;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
@@ -25,6 +26,7 @@ public class Robot extends TimedRobot {
   private static Drivetrain dt;
   private static HatchPanel hp;
   private static OI oi;
+  private static Limelight lime;
   private static Cargo cargo;
   private static Climber climber;
   private static Lights lights;
@@ -40,6 +42,7 @@ public void robotInit() {
     climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons);
     lights = new Lights(RobotMap.lights);
     oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
+    lime = new Limelight();
 
     fname1 = "/home/lvuser/drive_char_linear_for.csv";
     fname2 = "/home/lvuser/drive_char_stepwise_for.csv";
@@ -56,10 +59,11 @@ public void robotInit() {
     SmartDashboard.putData("Increase Voltage Stepwise Backward", ivsb);
     SmartDashboard.putData("Drivetrain Characterization Analysis", dca);
 
-    dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
+    dt.setDefaultCommand(new Drive(dt, lime, oi.leftJoy, oi.rightJoy));
     SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
 
     SmartDashboard.putBoolean("Outreach Mode", false);
+    SmartDashboard.putBoolean("Using Limelight", false);
     timey = new Timer();
   }
 

Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java → Robot2019/src/main/java/frc/robot/commands/Drive.java

@@ -1,34 +1,33 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
 package frc.robot.commands;
 
+import frc.robot.lib.Limelight;
+import frc.robot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.subsystems.Drivetrain;
-
-public class TeleopDrive extends Command {
-  Drivetrain dt;
-  Joystick leftJoy, rightJoy;
-
-  double prevSpeed = 0, prevLeft = 0, prevRight = 0;
 
-  double outreachSpeed = 0.3;
-
-  /**
-   * Handles all the teleoperated driving functionality
-   * 
-   * @param dt the Drivetrain object to use, passing it in is useful for testing
-   *           purposes
-   */
-  public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
+public class Drive extends Command {
+  private Drivetrain dt;
+  private Joystick leftJoy, rightJoy;
+  private Limelight lime;
+  private Limelight.Mode limelightMode = Limelight.Mode.TARGET;
+  private double adjustment = 0;
+  private double minError = 0.05;   // TODO: Test minimum values
+  private double prevSpeed = 0, prevLeft = 0, prevRight = 0;
+  private double outreachSpeed = 0.3;
+
+  public Drive(Drivetrain dt, Limelight lime, Joystick leftJoy, Joystick rightJoy) {
     requires(dt);
     this.dt = dt;
+    this.lime = lime;
+    this.dt = dt;
     this.leftJoy = leftJoy;
     this.rightJoy = rightJoy;
 
@@ -45,12 +44,22 @@ public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) {
     }
   }
 
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if (SmartDashboard.getBoolean("Arcade Drive", true)) {
-      arcadeDrive();
+    if (SmartDashboard.getBoolean("Using Limelight", false)) {
+        autoAlign();
     } else {
-      tankDrive();
+        if (SmartDashboard.getBoolean("Arcade Drive", true)) {
+            arcadeDrive();
+        } else {
+            tankDrive();
+        }
     }
   }
 
@@ -230,16 +239,49 @@ private void charDrive(double left, double right) {
     dt.disableVoltageCompensation();
   }
 
+  private void autoAlign() {
+    if (limelightMode == Limelight.Mode.DIST) {
+        adjustment = lime.distanceAssist();
+        dt.drive(adjustment, adjustment);
+        if (Math.abs(adjustment) < minError)  {
+          SmartDashboard.putBoolean("Finished Aligning", true);
+        }
+      }
+      else if (limelightMode == Limelight.Mode.STEER) {
+        adjustment = lime.distanceAssist();
+        dt.drive(adjustment, -adjustment);
+        if (Math.abs(adjustment) < minError)  {
+          SmartDashboard.putBoolean("Finished Aligning", true);
+        }
+      } else {
+        double[] params = lime.autoTarget();
+        dt.drive(params[0], params[1]);
+        double maxInput = Math.max(Math.abs(params[0]), Math.abs(params[1]));
+        if (maxInput < minError)  {
+          SmartDashboard.putBoolean("Finished Aligning", true);
+        }
+      }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+      if (!SmartDashboard.getBoolean("Use Limelight", false) || SmartDashboard.getBoolean("Finished Aligning", false)) {
+        SmartDashboard.putBoolean("Use Limelight", false);
+        SmartDashboard.putBoolean("Finished Aligning", false);
+        return true;
+      } else {
+        return false;
+      }
   }
 
+  // Called once after isFinished returns true
   @Override
   protected void end() {
-    dt.stop();
   }
 
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
   @Override
   protected void interrupted() {
     end();

Robot2019/src/main/java/frc/robot/commands/SetLight.java

@@ -28,4 +28,4 @@ protected void initialize() {
     lights.setLights(state);
   }
 
-}
+}

Robot2019/src/main/java/frc/robot/commands/ToggleLimelight.java

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ToggleLimelight extends InstantCommand {
+  public ToggleLimelight() {
+  }
+
+  @Override
+  protected void initialize() {
+    if (SmartDashboard.getBoolean("Using Limelight", false)) {
+        SmartDashboard.putBoolean("Using Limelight", true);
+    } else {
+        SmartDashboard.putBoolean("Using Limelight", false);
+    }
+  }
+}

Robot2019/src/main/java/frc/robot/lib/Limelight.java

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.lib;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Limelight {
+  public enum Mode {
+    DIST, STEER, TARGET
+  }
+
+  private NetworkTableInstance inst;
+  private NetworkTable table;
+  private double tv, tx, ty, ta;
+
+  public Limelight() {
+    inst = NetworkTableInstance.getDefault();
+    table = inst.getTable("limelight");
+  }
+
+  // Adjusts the distance between a vision target and the robot. Uses basic PID with the ty value from the network table.
+  public double distanceAssist() {
+    tv = table.getEntry("tv").getDouble(0.0);
+    ty = table.getEntry("ty").getDouble(0.0);
+    SmartDashboard.putNumber("Crosshair Vertical Offset", ty);
+    ta = table.getEntry("ta").getDouble(0.0);
+    double adjustment = 0.0;
+    double area_threshold = 0.5;  // TODO: Set the desired area.
+    double Kp = 0.2;   // TODO: Set PID K value.
+
+    if (tv == 1.0) {
+      if (ta < area_threshold) {
+        adjustment += Kp * ty;
+      }
+    }
+    return adjustment;
+  }
+
+  // Adjusts the angle facing a vision target. Uses basic PID with the tx value from the network table.
+  public double steeringAssist() {
+    tv = table.getEntry("tv").getDouble(0.0);
+    tx = -table.getEntry("tx").getDouble(0.0);
+    SmartDashboard.putBoolean("Found Vision Target", tv == 1.0);
+    SmartDashboard.putNumber("Crosshair Horizontal Offset", tx);
+    double adjustment = 0.0;
+    double steering_factor = 0.25;
+    double Kp = 0.2;   // TODO: Set PID K value.
+
+    if (tv == 1.0) {
+      adjustment += Kp * tx;
+    } else {
+      adjustment += steering_factor;
+    }
+    return adjustment;
+  }
+
+  // Combination of distance assist and steering assist
+  public double[] autoTarget() {
+    double dist_assist = distanceAssist();
+    double steer_assist = steeringAssist();
+    double[] params = {dist_assist + steer_assist, dist_assist - steer_assist};
+    return params;
+  }
+}

Robot2019/src/main/java/frc/robot/subsystems/Lights.java

@@ -8,7 +8,6 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.wpilibj.Relay;
-import edu.wpi.first.wpilibj.Relay.Value;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.SetLight;