code

src/main/java/frc/robot/RobotContainer.java

@@ -32,6 +32,7 @@ public class RobotContainer {
   private final PID m_PID = new PID();
   private final PDP m_PDP = new PDP();
   private final LineupPID m_LineupPID = new LineupPID();
+  private final Vision m_Vision = new Vision();
 
   // INSTANTIATES ALL COMMANDS
   private final OpenClawCommand m_OpenClawCommand = new OpenClawCommand(m_claw);

src/main/java/frc/robot/commands/LineupCommand.java

@@ -27,10 +27,14 @@ public void execute() {
     @Override
     public void end(boolean interrupted) {
         m_lineupPID.disable();
+        Tank.arcadeDrive(0, 0);
     }
 
     @Override
     public boolean isFinished() {
-        return false;
+        // Return true when the command should end.
+
+        // if the PID controller is at the setpoint, return true
+        return m_lineupPID.atSetpoint();
     }
 }

src/main/java/frc/robot/subsystems/LineupPID.java

@@ -22,6 +22,14 @@ public LineupPID() {
         // add that sendable to the shuffleboard
         Shuffleboard.getTab("Subsystems").add(getController());
         m_controller = getController();
+
+        // create a new NetworkTable table for all the PID stuff
+        NetworkTable table = NetworkTableInstance.getDefault().getTable("lineupPID");
+
+        // add the PID values to the table
+        table.getEntry("kP").setDouble(kP);
+        table.getEntry("kI").setDouble(kI);
+        table.getEntry("kD").setDouble(kD);
 
     }
 
@@ -37,6 +45,7 @@ public double getMeasurement() {
         // Return the distance from the best retroreflective tape pole (determined by the Limelight itself, configured in the Limelight's web interface) to the center of the robot
         return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0);
 
+
 
     }
 
@@ -45,4 +54,9 @@ public void setSetpoint() {
         m_controller.setSetpoint(getMeasurement());
     }
 
+    // check if the PID controller is at the setpoint; if it is, return true
+    public boolean atSetpoint() {
+        return m_controller.atSetpoint();
+    }
+
 }

src/main/java/frc/robot/subsystems/Tank.java

@@ -68,7 +68,7 @@ public class Tank extends SubsystemBase {
 
   private Field2d field2d;
 
-  private int initialCurrentLimit = 50;
+  private int initialCurrentLimit = 30; // TODO tune this
 
   /** */
   public Tank() {

src/main/java/frc/robot/subsystems/Vision.java

@@ -1,22 +1,40 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Vision extends SubsystemBase {
   NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+  NetworkTableEntry ty = table.getEntry("ty");
+
+  double targetOffsetAngle_Vertical = ty.getDouble(0);
   // Pipeline mode.
 
+  private double limelightMountAngleDegrees = 0;
+  private double limelightLensHeightInches = 6.25;
+  private double goalHeightInches = 24.125;
+  private double angleToGoalRadians = limelightMountAngleDegrees + targetOffsetAngle_Vertical;
+  private double angleToGoalDegrees = Units.radiansToDegrees(angleToGoalRadians);
+  private double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)/Math.tan(angleToGoalRadians);
+  public GenericEntry widget;
   public Vision() {
     // Stuff goes here
+    widget = Shuffleboard.getTab("stuff").add("distance", -(goalHeightInches - limelightLensHeightInches)/Math.tan(limelightMountAngleDegrees + ty.getDouble(0))).withWidget(BuiltInWidgets.kGraph).getEntry();
   }
 
   // display camera on shuffleboard
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    widget.setDouble(-(goalHeightInches - limelightLensHeightInches)/Math.tan(limelightMountAngleDegrees + ty.getDouble(0)));
+
   }
 
   @Override

src/main/java/frc/robot/util/limelight/Limelight.java

@@ -11,13 +11,26 @@
 
 public class Limelight {
     private NetworkTable table;
-    private NetworkTableEntry tv, tx, ty, ta, ts, tl, ledMode, pipeline, stream;
+    private NetworkTableEntry tv;
+    private NetworkTableEntry tx;
+    private NetworkTableEntry ty;
+    private NetworkTableEntry ta;
+    private NetworkTableEntry ts;
+    private NetworkTableEntry tl;
+    private NetworkTableEntry ledMode;
+    private NetworkTableEntry pipeline;
+    private NetworkTableEntry stream;
     private List<Pipeline> knownPipelines;
     private Target lastTarget = null;
 
 
+
+    // set the variables here...
+
     public Limelight(Pipeline... knownPipelines) {
         this("limelight", knownPipelines);
+
+
     }
 
     public Limelight(String tableName, Pipeline... knownPipelines) {