drive to lime distance (untested)

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
@@ -21,6 +22,7 @@ import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.Autos.AutoBalance;
 import frc4388.robot.commands.Autos.PlaybackChooser;
+import frc4388.robot.commands.Placement.DriveToLimeDistance;
 import frc4388.robot.commands.Placement.LimeAlign;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
@@ -69,12 +71,25 @@ public class RobotContainer {
     private ConditionalCommand alignToTarget = new ConditionalCommand(
         new SequentialCommandGroup(
             new RotateToAngle(m_robotSwerveDrive, 0.0),
-            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight)
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight),
+            new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30)
         ),
         noAuto,
         () -> m_robotLimeLight.numTargets() <= 2
     );
-    
+
+    public SequentialCommandGroup place = null;
+    private ConditionalCommand queuePlacement = new ConditionalCommand(
+        new InstantCommand(() -> {}),
+        noAuto,
+        () -> m_robotLimeLight.readyForPlacement
+    );
+
+    private SequentialCommandGroup placeConeHigh = null;
+    private SequentialCommandGroup placeConeMid = null;
+    private SequentialCommandGroup placeCubeHigh = null;
+    private SequentialCommandGroup placeCubeMid = null;
+    private SequentialCommandGroup placeLow = null;
 
     // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
     
@@ -138,15 +153,10 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
-
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //     .whileTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
 
-        //     // .onFalse()
-
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
             .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
 
@@ -178,7 +188,9 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
             
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
+            .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));      
+
+        
     }
 
     /**

src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java

@@ -4,29 +4,44 @@
 
 package frc4388.robot.commands.Placement;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveToLimeDistance extends PelvicInflammatoryDisease {
+
+  SwerveDrive drive;
+  Limelight lime;
+
+  double targetDistance;
 
-public class DriveToLimeDistance extends CommandBase {
   /** Creates a new DriveToLimeDistance. */
-  public DriveToLimeDistance() {
-    // Use addRequirements() here to declare subsystem dependencies.
+  public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance) {
+    super(0.2, 0.0, 0.0, 0.0, 1);
+
+    this.drive = drive;
+    this.lime = lime;
+    this.targetDistance = targetDistance;
+
+    addRequirements(drive, lime);
   }
 
-  // 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() {}
-
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    lime.readyForPlacement = true;
+  }
 
-  // Returns true when the command should end.
   @Override
-  public boolean isFinished() {
-    return false;
+  public double getError() {
+    return lime.getHorizontalDistanceToTarget(false) - targetDistance;
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);    
   }
 }

src/main/java/frc4388/robot/commands/Placement/LimeAlign.java

@@ -4,8 +4,6 @@
 
 package frc4388.robot.commands.Placement;
 
-import org.photonvision.targeting.PhotonTrackedTarget;
-
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.commands.PelvicInflammatoryDisease;

src/main/java/frc4388/robot/subsystems/Limelight.java

@@ -23,6 +23,7 @@ public class Limelight extends SubsystemBase {
   private PhotonCamera cam;
 
   private boolean lightOn;
+  public boolean readyForPlacement = false;
 
   /** Creates a new Limelight. */
   public Limelight() {