Limelight push vv2

src/main/java/frc/robot/Constants.java

@@ -29,6 +29,8 @@ public final class ControllerConstants {
 
         public final static int kThrottleButton = 2;
         public final static int kBalanceButton = 1; //Second joystick
+
+        public final static int kLineUpButton = 3;
 
         // For Xbox
         public final static int kXboxControllerPort = 2;
@@ -58,6 +60,11 @@ public final static class SensorConstants {
 
     }
 
+    public final class LimelightConstants {
+
+        public final static double kLineUpMaxSpeed = .5;
+    }
+
     public final static class MovementConstants {
 
         public final static String kStationary = "Stationary";

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

@@ -18,6 +18,8 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.NavXGyroSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
+import frc.robot.commands.ConeLineUpCommand;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -34,6 +36,9 @@ public class RobotContainer {
   private final BalanceCommand m_balancecommand;
   private DriveCommand teleopDriveCmd;
 
+  private LimelightSubsystem m_limelightSubsystem;
+  private ConeLineUpCommand m_coneLineUpCommand;
+
   private DrivetrainSubsystem drivetrainSubsystem;
 
   private Joystick rightJoystick;
@@ -80,6 +85,9 @@ public RobotContainer() {
     this.m_balancecommand = new BalanceCommand(navxGyroSubsystem, m_blinkinSubsystem, drivetrainSubsystem);
     // this.colorSensorSubsystem.setDefaultCommand(colorSensorCommand);   <--- Causes an error right now
 
+    this.m_limelightSubsystem = new LimelightSubsystem();
+    this.m_coneLineUpCommand = new ConeLineUpCommand(m_limelightSubsystem, drivetrainSubsystem);
+
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -99,6 +107,9 @@ private void configureButtonBindings() {
       JoystickButton balanceButton = new JoystickButton(this.xboxController, XboxController.Button.kLeftBumper.value);
       balanceButton.whileTrue(this.m_balancecommand);
     }
+
+    JoystickButton lineUpButton = new JoystickButton(this.rightJoystick, ControllerConstants.kLineUpButton);
+    lineUpButton.whileTrue(this.m_coneLineUpCommand);
   }
 
   /**

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

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import frc.robot.Constants;
+import frc.robot.Constants.ControllerConstants;
+import frc.robot.Constants.LimelightConstants;
+import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/** An example command that uses an example subsystem. */
+public class ConeLineUpCommand extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final LimelightSubsystem m_subsystem_vision;
+  private final DrivetrainSubsystem m_subsystem_drivetrain;
+  private final Constants constants = new Constants();
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ConeLineUpCommand(LimelightSubsystem l_subsystem, DrivetrainSubsystem d_subsystem) {
+    m_subsystem_vision = l_subsystem;
+    m_subsystem_drivetrain = d_subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(l_subsystem, d_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // Alligning to the target
+    //double angle = m_subsystem_vision.getTableX();
+    //double speed = angle/30.0 * LimelightConstants.kLineUpMaxSpeed;
+    //m_subsystem_drivetrain.tankDrive(speed, -1 * speed);
+
+    // Staying close to the target
+    //double area_speed = -1 * (m_subsystem_vision.getTableA() - 10)/10 * (LimelightConstants.kLineUpMaxSpeed/2);
+    //m_subsystem_drivetrain.tankDrive(area_speed, area_speed);
+
+    // Outline:
+    // if not lined up - line up
+    // else - drive to the cone
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class LimelightSubsystem extends SubsystemBase{
+    private final ShuffleboardTab m_tab = Shuffleboard.getTab("Drivetrain");
+
+
+    private NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+    private NetworkTableEntry tx = table.getEntry("tx");
+    private NetworkTableEntry ty = table.getEntry("ty");
+    private NetworkTableEntry ta = table.getEntry("ta");
+
+      /** Creates a new ExampleSubsystem. */
+    public LimelightSubsystem() {
+        tx.getDouble(0.0);
+
+        m_tab.addNumber("LimeLight X", () -> getTableX());
+        m_tab.addNumber("LimeLight Y", () -> getTableY());
+    }
+
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
+
+    public void setPipeline(int pipelineNum) {
+        table.getEntry("pipeline").setNumber(pipelineNum);
+    }
+
+    public double getTableX() {
+        return tx.getDouble(0.0);
+    }
+
+    public double getTableY() {
+        return ty.getDouble(0.0);
+    }
+
+    public double getTableA() {
+        return ta.getDouble(0.0);
+    }
+}