From cce3e52412e0ac149ba8eb57630161602d465f02 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 17 Mar 2023 15:14:53 -0600 Subject: [PATCH] lime align and april align working mostly --- .../java/frc4388/robot/RobotContainer.java | 41 +++++++----- .../robot/commands/BooleanCommand.java | 67 +++++++++++++++++++ .../Placement/DriveToLimeDistance.java | 5 +- .../robot/commands/Placement/LimeAlign.java | 22 +++--- .../frc4388/robot/subsystems/Limelight.java | 18 ++--- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- 6 files changed, 109 insertions(+), 48 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/BooleanCommand.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 68fc2b5..aaed46a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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)); } diff --git a/src/main/java/frc4388/robot/commands/BooleanCommand.java b/src/main/java/frc4388/robot/commands/BooleanCommand.java new file mode 100644 index 0000000..91e49b3 --- /dev/null +++ b/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 onTrue; + private Supplier onFalse; + + private Supplier condition; + + private Supplier selected; + + + /** Creates a new BooleanCommand. */ + public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier 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; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index 613ec99..e79cffc 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/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); } } diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index 1079c6d..578c778 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 467aa51..a54488d 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bbe2fa8..055230b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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);