From 79997fa054b22c7b725eb73b060e5af9a516cb45 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 19:48:12 -0600 Subject: [PATCH] drive to lime distance (untested) --- .../java/frc4388/robot/RobotContainer.java | 28 ++++++++---- .../Placement/DriveToLimeDistance.java | 45 ++++++++++++------- .../robot/commands/Placement/LimeAlign.java | 2 - .../frc4388/robot/subsystems/Limelight.java | 1 + 4 files changed, 51 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 556f170..182c5d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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)); + + } /** diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index cdac753..79d376b 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/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); } } diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index f951aca..e707459 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index c53a62b..dab5828 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/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() {