lime align and april align working mostly

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

@@ -9,6 +9,7 @@ package frc4388.robot;
 
 import java.util.function.Consumer;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
@@ -22,6 +23,7 @@ import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.Claw;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.commands.BooleanCommand;
 import frc4388.robot.commands.Arm.PivotCommand;
 import frc4388.robot.commands.Arm.TeleCommand;
 import frc4388.robot.commands.Autos.AutoBalance;
@@ -77,6 +79,7 @@ public class RobotContainer {
 
     /* Commands */
     private Command emptyCommand = new InstantCommand();
+    private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
 
     private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 0));
 
@@ -85,22 +88,24 @@ public class RobotContainer {
     private boolean readyForPlacement = false;
     private Boolean isPole = null;
 
-    private ConditionalCommand alignToPole = new ConditionalCommand(
+    private SequentialCommandGroup alignToPole =
         new SequentialCommandGroup(
             new RotateToAngle(m_robotSwerveDrive, 0.0),
-            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw()),
-            new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToTape())
-        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)),
-        emptyCommand.asProxy(),
-        () -> m_robotLimeLight.getNumTapes() <= 2
-    );
+            new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+            // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
+        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
 
-    // TODO: find actual distance
-    private SequentialCommandGroup alignToShelf = new SequentialCommandGroup(
-        new RotateToAngle(m_robotSwerveDrive, 0.0),
-        new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw()),
-        new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril())
-    ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
+    private SequentialCommandGroup alignToShelf = 
+        new SequentialCommandGroup(
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
 
     public SequentialCommandGroup place = null;
 
@@ -220,11 +225,13 @@ public class RobotContainer {
         
         // align (pole)
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-            .onTrue(alignToPole);
+            .onTrue(alignToPole)
+            .onFalse(interruptCommand.asProxy());
         
         // align (shelf)
         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(alignToShelf);
+            .onTrue(alignToShelf)
+            .onFalse(interruptCommand.asProxy());
 
         // toggle claw
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
@@ -240,7 +247,7 @@ public class RobotContainer {
         
         // interrupt button
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight));
+            .onTrue(interruptCommand.asProxy());
 
         // place high
         new POVButton(getDeadbandedOperatorController(), 0)
@@ -264,7 +271,7 @@ public class RobotContainer {
 
         // confirm
         new POVButton(getDeadbandedOperatorController(), 90)
-            .onTrue(new ConditionalCommand(place, emptyCommand.asProxy(), () -> place != null));
+            .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
 
     }
 

src/main/java/frc4388/robot/commands/BooleanCommand.java

@@ -0,0 +1,67 @@
+// 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 frc4388.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class BooleanCommand extends CommandBase {
+
+  private Supplier<Command> onTrue;
+  private Supplier<Command> onFalse;
+
+  private Supplier<Boolean> condition;
+
+  private Supplier<Command> selected;
+
+
+  /** Creates a new BooleanCommand. */
+  public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) {
+    this.onTrue = onTrue;
+    this.onFalse = onFalse;
+    this.condition = condition;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (condition.get()) {
+      selected = onTrue;
+    } else {
+      selected = onFalse;
+    }
+    if (selected.get() != null) {
+      selected.get().initialize();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (selected.get() != null) {
+      selected.get().execute();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    if (selected.get() != null) {
+      selected.get().end(interrupted);
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (selected.get() != null) {
+      return selected.get().isFinished();
+    } else {
+      return true;
+    }
+  }
+}

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

@@ -21,7 +21,7 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease {
 
   /** Creates a new DriveToLimeDistance. */
   public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
-    super(0.2, 0.0, 0.0, 0.0, 1);
+    super(0.5, 0.0, 0.0, 0.0, 1);
 
     this.drive = drive;
     this.lime = lime;
@@ -33,11 +33,12 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease {
 
   @Override
   public double getError() {
-    return ds.getAsDouble() - targetDistance;
+    return targetDistance - ds.getAsDouble();
   }
 
   @Override
   public void runWithOutput(double output) {
+    System.out.println(output / Math.abs(getError()));
     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

@@ -19,8 +19,8 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   DoubleSupplier ds;
 
-  public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds) {
-    super(0.7, 0.4, 0.0, 0.0, 0.04);
+  public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
+    super(0.4, 0.4, 0.0, 0.0, tolerance);
 
     this.drive = drive;
     this.lime = lime;
@@ -31,15 +31,11 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   @Override
   public double getError() {
-
-    if (lime.getNumTapes() > 2) {
-      return 0.0;
-    }
-
     double err = 0.0;
 
     try {
-      err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); 
+      System.out.println(ds.getAsDouble());
+      err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
     } catch (NullPointerException ex) {}
     
     return err;
@@ -48,11 +44,11 @@ public class LimeAlign extends PelvicInflammatoryDisease {
   @Override
   public void runWithOutput(double output) {
 
-    if (output > 0) {
-      output += 0.6;
-    } else if (output < 0) {
-      output -= 0.6;
-    }
+    // if (output > 0) {
+    //   output += 0.6;
+    // } else if (output < 0) {
+    //   output -= 0.6;
+    // }
 
     drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
   }

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

@@ -42,16 +42,16 @@ public class Limelight extends SubsystemBase {
 
   public void setToLimePipeline() {
     cam.setPipelineIndex(1);
+    setLEDs(true);
   }
 
   public void setToAprilPipeline() {
     cam.setPipelineIndex(0);
+    setLEDs(false);
   }
 
   // ! might need to find midpoint instead of entire target
   public PhotonTrackedTarget getAprilPoint() {
-    setToAprilPipeline();
-
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
@@ -61,7 +61,7 @@ public class Limelight extends SubsystemBase {
     return result.getBestTarget();
   }
 
-  public double getDistanceToApril() {
+  public double getDistanceToApril() {    
     PhotonTrackedTarget aprilPoint = getAprilPoint();
     if (aprilPoint == null) return -1;
 
@@ -73,8 +73,6 @@ public class Limelight extends SubsystemBase {
   }
 
   public PhotonTrackedTarget getLowestTape() {
-    setToLimePipeline();
-
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
@@ -93,15 +91,7 @@ public class Limelight extends SubsystemBase {
     return lowest;
   }
 
-  public int getNumTapes() {
-    setToLimePipeline();
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    return result.getTargets().size();
-  }
-
-  public double getDistanceToTape() {
+  public double getDistanceToTape() {    
     PhotonTrackedTarget tapePoint = getLowestTape();
     if (tapePoint == null) return -1;
 

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

@@ -73,10 +73,10 @@ public class SwerveDrive extends SubsystemBase {
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-      Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);