From fe5a07bd17e96db246df6cedaf395eaf2623314d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 19:10:26 -0600 Subject: [PATCH 01/19] target alignment basically completed --- .../java/frc4388/robot/RobotContainer.java | 42 +++++++++++-------- .../frc4388/robot/commands/LimeAlign.java | 9 +++- .../frc4388/robot/commands/RotateToAngle.java | 34 +++++++++++++++ .../java/frc4388/robot/subsystems/Arm.java | 10 +---- .../frc4388/robot/subsystems/Limelight.java | 25 ++++++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 16 ++++++- 6 files changed, 105 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RotateToAngle.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 77ca04b..488520d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,8 +9,10 @@ package frc4388.robot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; @@ -22,6 +24,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.LimeAlign; import frc4388.robot.commands.PlaybackChooser; +import frc4388.robot.commands.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -47,7 +50,7 @@ public class RobotContainer { public final Claw m_robotClaw = new Claw(m_robotMap.servo); - public final Limelight m_limeLight = new Limelight(); + public final Limelight m_robotLimeLight = new Limelight(); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -57,7 +60,22 @@ public class RobotContainer { public SendableChooser chooser = new SendableChooser<>(); private Command noAuto = new InstantCommand(); + + // private SequentialCommandGroup alignToTarget = new SequentialCommandGroup( + // new RotateToAngle(m_robotSwerveDrive, m_robotLimeLight, 0.0, true), + // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight) + // ); + + private ConditionalCommand alignToTarget = new ConditionalCommand( + new SequentialCommandGroup( + new RotateToAngle(m_robotSwerveDrive, 0.0), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight) + ), + noAuto, + () -> m_robotLimeLight.numTargets() <= 2 + ); + // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); @@ -146,31 +164,21 @@ public class RobotContainer { // .onFalse(new InstantCommand()); // * Operator Buttons - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); - // .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight)); - - + .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight)); + .onTrue(alignToTarget); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); } /** diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java index 533b6f0..c309b4f 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -17,7 +17,7 @@ public class LimeAlign extends PelvicInflammatoryDisease { Limelight lime; public LimeAlign(SwerveDrive drive, Limelight lime) { - super(0.7, 0.1, 0.0, 0.0, 0); + super(0.7, 0.4, 0.0, 0.0, 0.04); this.drive = drive; this.lime = lime; @@ -27,10 +27,15 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override public double getError() { + + if (lime.numTargets() > 2) { + return 0.0; + } + double err = 0.0; try { - err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); } catch (NullPointerException ex) {} return err; diff --git a/src/main/java/frc4388/robot/commands/RotateToAngle.java b/src/main/java/frc4388/robot/commands/RotateToAngle.java new file mode 100644 index 0000000..bf19c1e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RotateToAngle.java @@ -0,0 +1,34 @@ +// 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 edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.subsystems.SwerveDrive; + +public class RotateToAngle extends PelvicInflammatoryDisease { + + SwerveDrive drive; + double targetAngle; + + /** Creates a new RotateToAngle. */ + public RotateToAngle(SwerveDrive drive, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.targetAngle = targetAngle; + + addRequirements(drive); + } + + @Override + public double getError() { + return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 06fab10..db605ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -156,15 +156,7 @@ public class Arm extends SubsystemBase { // double x = Math.cos(Math.toRadians(degrees)); } - boolean soft_limits = true; public void killSoftLimits() { resetTeleSoftLimit(); - var pivot_soft = m_pivot.getSelectedSensorPosition(); - var tele_soft = m_tele.getSelectedSensorPosition(); - - m_pivot.configForwardSoftLimitEnable(!soft_limits); - m_pivot.configReverseSoftLimitEnable(!soft_limits); - - soft_limits = !soft_limits; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index c5f3c50..c53a62b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -95,8 +95,29 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } - private double getPointAngle(Point point) { - return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS); + public PhotonTrackedTarget getLowestTargetPoint() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + ArrayList points = (ArrayList) result.getTargets(); + + PhotonTrackedTarget lowest = points.get(0); + for (PhotonTrackedTarget point : points) { + if (point.getPitch() < lowest.getPitch()) { + lowest = point; + } + } + + return lowest; + } + + public int numTargets() { + PhotonPipelineResult result = cam.getLatestResult(); + + return result.getTargets().size(); } public double getHorizontalDistanceToTarget(boolean high) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f65dd00..0b24778 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -101,6 +101,19 @@ public class SwerveDrive extends SubsystemBase { } } + public boolean rotateToTarget(double angle) { + double currentAngle = getGyroAngle(); + double error = angle - currentAngle; + + driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + public double getGyroAngle() { return gyro.getAngle(); } @@ -122,7 +135,8 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { - // This method will be called once per scheduler run + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); } public void shiftDown() { From 2ee2826eb69cd05d176f26f63ad0aff86e5385f2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 19:14:08 -0600 Subject: [PATCH 02/19] command reorganization --- .../java/frc4388/robot/RobotContainer.java | 12 +++---- .../commands/{ => Arm}/PivotCommand.java | 3 +- .../robot/commands/{ => Arm}/TeleCommand.java | 3 +- .../commands/{ => Autos}/AutoBalance.java | 3 +- .../robot/commands/{ => Autos}/Blue1Path.txt | 0 .../commands/{ => Autos}/PlaybackChooser.java | 3 +- .../robot/commands/{ => Autos}/Taxi.txt | 0 .../Placement/DriveToLimeDistance.java | 32 +++++++++++++++++++ .../commands/{ => Placement}/LimeAlign.java | 3 +- .../{ => Swerve}/JoystickPlayback.java | 2 +- .../{ => Swerve}/JoystickRecorder.java | 2 +- .../commands/{ => Swerve}/RotateToAngle.java | 3 +- 12 files changed, 52 insertions(+), 14 deletions(-) rename src/main/java/frc4388/robot/commands/{ => Arm}/PivotCommand.java (90%) rename src/main/java/frc4388/robot/commands/{ => Arm}/TeleCommand.java (90%) rename src/main/java/frc4388/robot/commands/{ => Autos}/AutoBalance.java (91%) rename src/main/java/frc4388/robot/commands/{ => Autos}/Blue1Path.txt (100%) rename src/main/java/frc4388/robot/commands/{ => Autos}/PlaybackChooser.java (97%) rename src/main/java/frc4388/robot/commands/{ => Autos}/Taxi.txt (100%) create mode 100644 src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java rename src/main/java/frc4388/robot/commands/{ => Placement}/LimeAlign.java (92%) rename src/main/java/frc4388/robot/commands/{ => Swerve}/JoystickPlayback.java (99%) rename src/main/java/frc4388/robot/commands/{ => Swerve}/JoystickRecorder.java (98%) rename src/main/java/frc4388/robot/commands/{ => Swerve}/RotateToAngle.java (90%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 488520d..556f170 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,16 +15,16 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.AutoBalance; -import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.commands.JoystickRecorder; -import frc4388.robot.commands.LimeAlign; -import frc4388.robot.commands.PlaybackChooser; -import frc4388.robot.commands.RotateToAngle; +import frc4388.robot.commands.Autos.AutoBalance; +import frc4388.robot.commands.Autos.PlaybackChooser; +import frc4388.robot.commands.Placement.LimeAlign; +import frc4388.robot.commands.Swerve.JoystickPlayback; +import frc4388.robot.commands.Swerve.JoystickRecorder; +import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java similarity index 90% rename from src/main/java/frc4388/robot/commands/PivotCommand.java rename to src/main/java/frc4388/robot/commands/Arm/PivotCommand.java index a84690d..279e1b0 100644 --- a/src/main/java/frc4388/robot/commands/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java @@ -2,9 +2,10 @@ // 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; +package frc4388.robot.commands.Arm; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.Arm; public class PivotCommand extends PelvicInflammatoryDisease { diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java similarity index 90% rename from src/main/java/frc4388/robot/commands/TeleCommand.java rename to src/main/java/frc4388/robot/commands/Arm/TeleCommand.java index 02c8c27..356b2d6 100644 --- a/src/main/java/frc4388/robot/commands/TeleCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java @@ -2,9 +2,10 @@ // 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; +package frc4388.robot.commands.Arm; import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.Arm; public class TeleCommand extends PelvicInflammatoryDisease { diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java similarity index 91% rename from src/main/java/frc4388/robot/commands/AutoBalance.java rename to src/main/java/frc4388/robot/commands/Autos/AutoBalance.java index 629c903..4d18c99 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java @@ -2,11 +2,12 @@ // 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; +package frc4388.robot.commands.Autos; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.RobotGyro; diff --git a/src/main/java/frc4388/robot/commands/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt similarity index 100% rename from src/main/java/frc4388/robot/commands/Blue1Path.txt rename to src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java similarity index 97% rename from src/main/java/frc4388/robot/commands/PlaybackChooser.java rename to src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index c2b32e4..2438a38 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands; +package frc4388.robot.commands.Autos; import java.io.File; import java.util.ArrayList; @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.subsystems.SwerveDrive; public class PlaybackChooser { diff --git a/src/main/java/frc4388/robot/commands/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt similarity index 100% rename from src/main/java/frc4388/robot/commands/Taxi.txt rename to src/main/java/frc4388/robot/commands/Autos/Taxi.txt diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java new file mode 100644 index 0000000..cdac753 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -0,0 +1,32 @@ +// 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.Placement; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveToLimeDistance extends CommandBase { + /** Creates a new DriveToLimeDistance. */ + public DriveToLimeDistance() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java similarity index 92% rename from src/main/java/frc4388/robot/commands/LimeAlign.java rename to src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index c309b4f..f951aca 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -2,12 +2,13 @@ // 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; +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; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java similarity index 99% rename from src/main/java/frc4388/robot/commands/JoystickPlayback.java rename to src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 0f496a9..56e8d43 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.Swerve; import java.io.File; import java.io.FileNotFoundException; diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java similarity index 98% rename from src/main/java/frc4388/robot/commands/JoystickRecorder.java rename to src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index eea72b4..84608cc 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.Swerve; import java.io.File; import java.io.IOException; diff --git a/src/main/java/frc4388/robot/commands/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java similarity index 90% rename from src/main/java/frc4388/robot/commands/RotateToAngle.java rename to src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java index bf19c1e..8cf630f 100644 --- a/src/main/java/frc4388/robot/commands/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -2,9 +2,10 @@ // 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; +package frc4388.robot.commands.Swerve; import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.SwerveDrive; public class RotateToAngle extends PelvicInflammatoryDisease { From 75951c88fc644fff63fe1c1c85a8ac59a94cbfcc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 19:48:12 -0600 Subject: [PATCH 03/19] 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() { From 2ff3597930af6d5c5c2257a19ba49f0900e66d4e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 22:20:23 -0600 Subject: [PATCH 04/19] finished command stuff but shits complicated --- .../java/frc4388/robot/RobotContainer.java | 128 ++++++++++++------ 1 file changed, 90 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 182c5d9..2bdef5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,9 @@ package frc4388.robot; +import java.util.function.Consumer; +import java.util.function.UnaryOperator; + import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -15,11 +18,14 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.commands.Arm.PivotCommand; +import frc4388.robot.commands.Arm.TeleCommand; import frc4388.robot.commands.Autos.AutoBalance; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Placement.DriveToLimeDistance; @@ -60,36 +66,6 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - - private Command noAuto = new InstantCommand(); - - // private SequentialCommandGroup alignToTarget = new SequentialCommandGroup( - // new RotateToAngle(m_robotSwerveDrive, m_robotLimeLight, 0.0, true), - // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight) - // ); - - private ConditionalCommand alignToTarget = new ConditionalCommand( - new SequentialCommandGroup( - new RotateToAngle(m_robotSwerveDrive, 0.0), - 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); @@ -102,6 +78,49 @@ public class RobotContainer { // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private PlaybackChooser playbackChooser; + + /* Commands */ + private Command emptyCommand = new InstantCommand(); + + private Boolean isPole = null; + + private ConditionalCommand alignToPole = new ConditionalCommand( + new SequentialCommandGroup( + new RotateToAngle(m_robotSwerveDrive, 0.0), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight), + new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30) + ).andThen(new InstantCommand(() -> isPole = true)), + emptyCommand.asProxy(), + () -> m_robotLimeLight.numTargets() <= 2 + ); + + // TODO: make + private ConditionalCommand alignToShelf = new ConditionalCommand( + new SequentialCommandGroup( + + ).andThen(new InstantCommand(() -> isPole = false)), + emptyCommand.asProxy(), + () -> true + ); + + public SequentialCommandGroup place = null; + + private Consumer queuePlacement = (scg) -> { + place = scg.andThen(new InstantCommand(() -> m_robotLimeLight.readyForPlacement = false), new InstantCommand(() -> isPole = null)); + }; + + // TODO: find actual values + private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + + private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + + private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + + private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + + private SequentialCommandGroup placeLow = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -175,22 +194,55 @@ public class RobotContainer { // * Operator Buttons - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); + // align (pole) new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(alignToTarget); + .onTrue(alignToPole); + // align (shelf) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(alignToPole); + + // toggle claw new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); - + + // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); - + // toggle limelight + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); + + // interrupt button + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight)); + + // place high + new POVButton(getDeadbandedOperatorController(), 0) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), + emptyCommand.asProxy(), + () -> m_robotLimeLight.readyForPlacement == true) + ); + + // place mid + new POVButton(getDeadbandedOperatorController(), 270) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), + emptyCommand.asProxy(), + () -> m_robotLimeLight.readyForPlacement == true) + ); + + // place low + new POVButton(getDeadbandedOperatorController(), 180) + .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> m_robotLimeLight.readyForPlacement == true)); + + // confirm + new POVButton(getDeadbandedOperatorController(), 90) + .onTrue(new ConditionalCommand(place, emptyCommand.asProxy(), () -> place != null)); + } /** From 94a092b7b5954e099a06893b8184d4acf747b30a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 22:32:49 -0600 Subject: [PATCH 05/19] finished boilerplate for place commands --- .../java/frc4388/robot/RobotContainer.java | 58 ++++++++++++++----- .../Placement/DriveToLimeDistance.java | 6 -- .../frc4388/robot/subsystems/Limelight.java | 1 - 3 files changed, 44 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2bdef5b..64bb18e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -82,6 +82,11 @@ public class RobotContainer { /* Commands */ private Command emptyCommand = new InstantCommand(); + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 0)); + + private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); + + private boolean readyForPlacement = false; private Boolean isPole = null; private ConditionalCommand alignToPole = new ConditionalCommand( @@ -89,7 +94,7 @@ public class RobotContainer { new RotateToAngle(m_robotSwerveDrive, 0.0), new LimeAlign(m_robotSwerveDrive, m_robotLimeLight), new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30) - ).andThen(new InstantCommand(() -> isPole = true)), + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)), emptyCommand.asProxy(), () -> m_robotLimeLight.numTargets() <= 2 ); @@ -98,7 +103,7 @@ public class RobotContainer { private ConditionalCommand alignToShelf = new ConditionalCommand( new SequentialCommandGroup( - ).andThen(new InstantCommand(() -> isPole = false)), + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)), emptyCommand.asProxy(), () -> true ); @@ -106,19 +111,44 @@ public class RobotContainer { public SequentialCommandGroup place = null; private Consumer queuePlacement = (scg) -> { - place = scg.andThen(new InstantCommand(() -> m_robotLimeLight.readyForPlacement = false), new InstantCommand(() -> isPole = null)); + place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null)); }; // TODO: find actual values - private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); - private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); - private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); - private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); - private SequentialCommandGroup placeLow = new SequentialCommandGroup(new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0)); + private SequentialCommandGroup placeLow = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); /** @@ -201,11 +231,11 @@ public class RobotContainer { // align (shelf) new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(alignToPole); + .onTrue(alignToShelf); // toggle claw new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + .onTrue(toggleClaw.asProxy()); // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) @@ -224,7 +254,7 @@ public class RobotContainer { .onTrue(new ConditionalCommand( new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), emptyCommand.asProxy(), - () -> m_robotLimeLight.readyForPlacement == true) + () -> readyForPlacement == true) ); // place mid @@ -232,12 +262,12 @@ public class RobotContainer { .onTrue(new ConditionalCommand( new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), emptyCommand.asProxy(), - () -> m_robotLimeLight.readyForPlacement == true) + () -> readyForPlacement == true) ); - + // place low new POVButton(getDeadbandedOperatorController(), 180) - .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> m_robotLimeLight.readyForPlacement == true)); + .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true)); // confirm new POVButton(getDeadbandedOperatorController(), 90) diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index 79d376b..b74a395 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -29,12 +29,6 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease { addRequirements(drive, lime); } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - lime.readyForPlacement = true; - } - @Override public double getError() { return lime.getHorizontalDistanceToTarget(false) - targetDistance; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index dab5828..c53a62b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -23,7 +23,6 @@ public class Limelight extends SubsystemBase { private PhotonCamera cam; private boolean lightOn; - public boolean readyForPlacement = false; /** Creates a new Limelight. */ public Limelight() { From 5ae5db7847f7c8fe1c5934b2cca951ed3eadccf2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:08:08 -0600 Subject: [PATCH 06/19] updated limealign and drivetolimedistance --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 19 ++--- .../Placement/DriveToLimeDistance.java | 8 +- .../robot/commands/Placement/LimeAlign.java | 11 ++- .../frc4388/robot/subsystems/Limelight.java | 76 +++++++------------ 5 files changed, 50 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9fa45e3..fc518b1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -168,5 +168,7 @@ public final class Constants { // public static final double MID_TARGET_HEIGHT = 34.0; public static final double MID_TAPE_HEIGHT = 24.0; + + public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 64bb18e..50fcd85 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,21 +92,18 @@ public class RobotContainer { private ConditionalCommand alignToPole = new ConditionalCommand( new SequentialCommandGroup( new RotateToAngle(m_robotSwerveDrive, 0.0), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight), - new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30) + 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.numTargets() <= 2 + () -> m_robotLimeLight.getNumTapes() <= 2 ); - // TODO: make - private ConditionalCommand alignToShelf = new ConditionalCommand( - new SequentialCommandGroup( - - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)), - emptyCommand.asProxy(), - () -> true - ); + 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()) // TODO: find distance + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); public SequentialCommandGroup place = null; diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index b74a395..ac49334 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -4,6 +4,8 @@ package frc4388.robot.commands.Placement; +import java.util.function.DoubleSupplier; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.RobotContainer; @@ -17,21 +19,23 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease { Limelight lime; double targetDistance; + DoubleSupplier ds; /** Creates a new DriveToLimeDistance. */ - public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance) { + public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { super(0.2, 0.0, 0.0, 0.0, 1); this.drive = drive; this.lime = lime; this.targetDistance = targetDistance; + this.ds = ds; addRequirements(drive, lime); } @Override public double getError() { - return lime.getHorizontalDistanceToTarget(false) - targetDistance; + return ds.getAsDouble() - targetDistance; } @Override diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index e707459..1079c6d 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -4,6 +4,8 @@ package frc4388.robot.commands.Placement; +import java.util.function.DoubleSupplier; + import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.commands.PelvicInflammatoryDisease; @@ -15,11 +17,14 @@ public class LimeAlign extends PelvicInflammatoryDisease { SwerveDrive drive; Limelight lime; - public LimeAlign(SwerveDrive drive, Limelight lime) { + DoubleSupplier ds; + + public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds) { super(0.7, 0.4, 0.0, 0.0, 0.04); this.drive = drive; this.lime = lime; + this.ds = ds; addRequirements(drive, lime); } @@ -27,14 +32,14 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override public double getError() { - if (lime.numTargets() > 2) { + if (lime.getNumTapes() > 2) { return 0.0; } double err = 0.0; try { - err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); + err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); } catch (NullPointerException ex) {} return err; diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index c53a62b..8fbe5a9 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -28,8 +28,6 @@ public class Limelight extends SubsystemBase { public Limelight() { cam = new PhotonCamera(VisionConstants.NAME); cam.setDriverMode(false); - - setToLimePipeline(); } public void setLEDs(boolean on) { @@ -54,38 +52,9 @@ public class Limelight extends SubsystemBase { cam.setPipelineIndex(0); } - public ArrayList getTargetPoints() { - if (!cam.isConnected()) return null; + public PhotonTrackedTarget getAprilPoint() { + setToAprilPipeline(); - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = new ArrayList<>(2); - - for(PhotonTrackedTarget target : result.getTargets()) { - List corners = target.getDetectedCorners(); - - double sumX = 0.0; - double sumY = 0.0; - double mx = 0.0; - double my = 0.0; - - for (TargetCorner c : corners) { - sumX += c.x; - sumY += c.y; - } - - mx = sumX / 4.0; - my = sumY / 4.0; - - points.add(new Point(mx, my)); - } - - return points; - } - - public PhotonTrackedTarget getFirstTargetPoint() { if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); @@ -95,7 +64,20 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } - public PhotonTrackedTarget getLowestTargetPoint() { + public double getDistanceToApril() { + PhotonTrackedTarget aprilPoint = getAprilPoint(); + if (aprilPoint == null) return -1; + + double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + aprilPoint.getPitch(); + + double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); + return distanceToApril; + } + + public PhotonTrackedTarget getLowestTape() { + setToLimePipeline(); + if (!cam.isConnected()) return null; PhotonPipelineResult result = cam.getLatestResult(); @@ -114,29 +96,23 @@ public class Limelight extends SubsystemBase { return lowest; } - public int numTargets() { + public int getNumTapes() { + setToLimePipeline(); + PhotonPipelineResult result = cam.getLatestResult(); return result.getTargets().size(); } - public double getHorizontalDistanceToTarget(boolean high) { - ArrayList targetPoints = getTargetPoints(); - if (targetPoints == null) return -1; - - // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); - // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1); - - PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint; - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT; + public double getDistanceToTape() { + PhotonTrackedTarget tapePoint = getLowestTape(); + if (tapePoint == null) return -1; + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; double theta = 35.0 + tapePoint.getPitch(); - double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT; - - double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta)); - - return horizontalDistanceToTarget; + double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + return distanceToTape; } int ctr = 0; @@ -146,7 +122,7 @@ public class Limelight extends SubsystemBase { // This method will be called once per scheduler run if (ctr % 50 == 0) { - SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false)); + SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape()); } ctr++; From c14f8125caa6c9b02b93caec2738f130e57ea1d7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:08:56 -0600 Subject: [PATCH 07/19] alignToShelf READY TO TEST --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50fcd85..54a7afd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -99,10 +99,11 @@ public class RobotContainer { () -> m_robotLimeLight.getNumTapes() <= 2 ); + // 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()) // TODO: find distance + new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); public SequentialCommandGroup place = null; From f28c382da7a98950602d2c8f87e87bb0bb44b385 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:10:06 -0600 Subject: [PATCH 08/19] april alignment READY TO TEST --- src/main/java/frc4388/robot/subsystems/Limelight.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 8fbe5a9..9670b33 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -52,6 +52,7 @@ public class Limelight extends SubsystemBase { cam.setPipelineIndex(0); } + // ! might need to find midpoint instead of entire target public PhotonTrackedTarget getAprilPoint() { setToAprilPipeline(); From 04c7386b67db17aaa7ff908f0d2e9e32ebb0d45a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:13:10 -0600 Subject: [PATCH 09/19] import cleanup --- src/main/java/frc4388/robot/Constants.java | 2 -- src/main/java/frc4388/robot/Robot.java | 18 ------------------ .../java/frc4388/robot/RobotContainer.java | 4 ---- .../Placement/DriveToLimeDistance.java | 2 -- .../java/frc4388/robot/subsystems/Arm.java | 9 --------- .../java/frc4388/robot/subsystems/Claw.java | 2 -- .../frc4388/robot/subsystems/Limelight.java | 4 ---- .../frc4388/robot/subsystems/SwerveDrive.java | 4 ---- .../java/frc4388/robot/subsystems/Vision.java | 1 - .../controller/DeadbandedXboxController.java | 6 ------ .../utility/controller/XboxTriggerButton.java | 1 - 11 files changed, 53 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index fc518b1..f771c80 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,8 +7,6 @@ package frc4388.robot; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index fbdf90c..5978788 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,20 +7,6 @@ package frc4388.robot; -import java.lang.System; -import java.lang.reflect.Array; -import java.util.Arrays; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.can.MotControllerJNI; - -import java.io.File; -import java.io.IOException; -import java.io.PrintWriter; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -28,10 +14,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; -import frc4388.robot.subsystems.Location; -import frc4388.robot.subsystems.Apriltags.Tag; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 54a7afd..e62c452 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,9 +8,7 @@ package frc4388.robot; import java.util.function.Consumer; -import java.util.function.UnaryOperator; -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; @@ -30,8 +28,6 @@ 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; import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index ac49334..613ec99 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -7,8 +7,6 @@ package frc4388.robot.commands.Placement; import java.util.function.DoubleSupplier; 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; diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index db605ec..47beeb2 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,28 +1,20 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.DeferredBlock; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; - private boolean m_debug; // Moves arm to distance [dist] then returns new ang public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) { @@ -140,7 +132,6 @@ public class Arm extends SubsystemBase { @Override public void periodic() { - double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135); if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { var tele_soft = m_tele.getSelectedSensorPosition(); diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 7d6d12c..8e87ada 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,12 +1,10 @@ package frc4388.robot.subsystems; -import edu.wpi.first.hal.PWMJNI; import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Claw extends SubsystemBase { private PWM m_clawMotor; private boolean m_open = false; - private boolean m_disabled = false; // Opens claw public Claw(PWM m_clawMotor) { diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9670b33..467aa51 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -5,19 +5,15 @@ package frc4388.robot.subsystems; import java.util.ArrayList; -import java.util.List; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.AbhiIsADumbass; public class Limelight extends SubsystemBase { private PhotonCamera cam; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0b24778..bbe2fa8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,16 +4,12 @@ package frc4388.robot.subsystems; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 3731849..371f621 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,7 +3,6 @@ package frc4388.robot.subsystems; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index d339841..4577a2e 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -4,7 +4,6 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; -import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } @@ -21,11 +20,6 @@ public class DeadbandedXboxController extends XboxController { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - // if (OIConstants.SKEW_STICKS && magnitude >= 1) { - // System.out.println("if statement running"); - // return translation2d.div(magnitude); - // } - if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index e263633..8c2fe88 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,7 +1,6 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj2.command.button.Button; -import frc4388.utility.controller.XboxController; /** * Mapping for the Xbox controller triggers to allow triggers to be defined as From b16c9e2194140c5a910c90c038725f3f33bdb87c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 23:24:30 -0600 Subject: [PATCH 10/19] reset place after place --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e62c452..68fc2b5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { public SequentialCommandGroup place = null; private Consumer queuePlacement = (scg) -> { - place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null)); + place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null)); }; // TODO: find actual values @@ -217,7 +217,6 @@ public class RobotContainer { // .onFalse(new InstantCommand()); // * Operator Buttons - // align (pole) new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) From cce3e52412e0ac149ba8eb57630161602d465f02 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 17 Mar 2023 15:14:53 -0600 Subject: [PATCH 11/19] 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); From 035a76a32bf35b21fcbdee5ec74602c05146cf87 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Fri, 17 Mar 2023 17:00:34 -0600 Subject: [PATCH 12/19] tele no PID, cube high/mid needs further testing --- .../java/frc4388/robot/RobotContainer.java | 22 ++++++++++------- .../robot/commands/Arm/TeleCommand.java | 24 ++++++++++++------- .../java/frc4388/robot/subsystems/Arm.java | 10 ++++++-- 3 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aaed46a..f6d8239 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -114,16 +114,19 @@ public class RobotContainer { }; // TODO: find actual values - private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); + private SequentialCommandGroup placeConeHigh = + new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println("Placing cone high")), + new PivotCommand(m_robotArm, 64 + 135), + new InstantCommand(() -> m_robotArm.setRotVel(0)), + new TeleCommand(m_robotArm, 95642), + toggleClaw.asProxy(), + armToHome.asProxy() + ); private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), + new PivotCommand(m_robotArm, 70 + 135), + new TeleCommand(m_robotArm, 32866), toggleClaw.asProxy(), armToHome.asProxy() ); @@ -247,7 +250,8 @@ public class RobotContainer { // interrupt button new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(interruptCommand.asProxy()); + .onTrue(placeConeHigh.asProxy()); + // .onTrue(interruptCommand.asProxy()); // place high new POVButton(getDeadbandedOperatorController(), 0) diff --git a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java index 356b2d6..a5a6e9f 100644 --- a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java @@ -4,30 +4,36 @@ package frc4388.robot.commands.Arm; +import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.commands.PelvicInflammatoryDisease; import frc4388.robot.subsystems.Arm; -public class TeleCommand extends PelvicInflammatoryDisease { - private final Arm arm; - private final double target; +public class TeleCommand extends CommandBase { + private final Arm arm; + private final double target; + private boolean goIn; /** Creates a new ArmCommand. */ public TeleCommand(Arm arm, double target) { - super(0.6, 0, 0, 0, 0); this.arm = arm; this.target = target; addRequirements(arm); } @Override - public double getError() { - return (arm.getArmLength() - target) / - (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + public void initialize() { + this.goIn = target < arm.getArmLength(); } @Override - public void runWithOutput(double output) { - arm.setTeleVel(output); + public void execute() { + arm.setTeleVel(goIn ? 1 : -1); + } + + @Override + public boolean isFinished() { + if (goIn) return arm.getArmLength() < target; + else return arm.getArmLength() > target; } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 47beeb2..94081df 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -40,6 +40,8 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { + if (vel > 1) vel = 1; + var degrees = Math.abs(getArmRotation()) - 135; SmartDashboard.putNumber("arm degrees", degrees); SmartDashboard.putNumber("arm rot vel", vel); @@ -48,6 +50,8 @@ public class Arm extends SubsystemBase { m_pivot.set(ControlMode.PercentOutput, 0); } else if (degrees > 90 && vel > 0) { m_pivot.set(ControlMode.PercentOutput, .15 * vel); + } else if (degrees < 25 && vel < 0) { + m_pivot.set(ControlMode.PercentOutput, .15 * vel); } else { m_pivot.set(ControlMode.PercentOutput, .3 * vel); } @@ -78,7 +82,7 @@ public class Arm extends SubsystemBase { } public double getArmLength() { - return m_tele.getSelectedSensorPosition(); + return m_tele.getSelectedSensorPosition() - tele_soft; } public double getArmRotation() { @@ -112,9 +116,10 @@ public class Arm extends SubsystemBase { } boolean tele_softLimit = false; + double tele_soft = 0; public void resetTeleSoftLimit() { if (!tele_softLimit) { - var tele_soft = m_tele.getSelectedSensorPosition(); + tele_soft = m_tele.getSelectedSensorPosition(); m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); m_tele.configReverseSoftLimitThreshold(tele_soft); m_tele.configForwardSoftLimitEnable(true); @@ -145,6 +150,7 @@ public class Arm extends SubsystemBase { } // double x = Math.cos(Math.toRadians(degrees)); + SmartDashboard.putNumber("arm length", getArmLength()); } public void killSoftLimits() { From 04777e3062a23f0d61fad65221c7c54f440507ed Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Sat, 18 Mar 2023 13:21:26 -0600 Subject: [PATCH 13/19] Controller maps --- .../java/frc4388/robot/RobotContainer.java | 56 ++++++++++++++----- 1 file changed, 41 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f6d8239..e4f93d2 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 java.lang.StackWalker.Option; import java.util.function.Consumer; import edu.wpi.first.math.geometry.Translation2d; @@ -189,6 +190,11 @@ public class RobotContainer { .buildDisplay(); } + // here be dragons - enter if you dare + private static class TapState { + public boolean gearTapped = true; + public long gearTime = 0; + } /** * Use this method to define your button->command mappings. Buttons can be @@ -199,18 +205,38 @@ public class RobotContainer { private void configureButtonBindings() { // * Driver Buttons new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + // because closure reasons - ask Daniel Thomas + // final TapState tap = new TapState(); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // .onTrue(new InstantCommand(() -> { + // tap.gearTapped = true; + // tap.gearTime = System.currentTimeMillis(); + // })) + // .whileTrue(new RunCommand(() -> { + // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { + // m_robotSwerveDrive.setToTurbo(); + // tap.gearTapped = false; + // } + // })) + // .onFalse(new InstantCommand(() -> { + // if (tap.gearTapped) + // m_robotSwerveDrive.setToFast(); + // else + // m_robotSwerveDrive.setToSlow(); + // })); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(interruptCommand.asProxy()); // final // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, // () -> getDeadbandedDriverController().getLeftX(), @@ -222,36 +248,36 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); + // .onFalse(new InstantCommand()); // * Operator Buttons // align (pole) - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(alignToPole) .onFalse(interruptCommand.asProxy()); // align (shelf) - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(alignToShelf) .onFalse(interruptCommand.asProxy()); // toggle claw - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final .onTrue(toggleClaw.asProxy()); // kill soft limits - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); // toggle limelight - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); // final? // interrupt button - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(placeConeHigh.asProxy()); - // .onTrue(interruptCommand.asProxy()); + //new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(placeConeHigh.asProxy()); + // .onTrue(interruptCommand.asProxy()); // place high new POVButton(getDeadbandedOperatorController(), 0) From a3bc7b56b953099d6f4d80505680b74d7075a648 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 14:41:01 -0600 Subject: [PATCH 14/19] aprilrotalign works --- .../java/frc4388/robot/RobotContainer.java | 7 ++- .../commands/Placement/AprilRotAlign.java | 42 +++++++++++++ .../robot/commands/Placement/LimeAlign.java | 7 --- .../frc4388/robot/subsystems/Limelight.java | 59 +++++++++++++++---- 4 files changed, 94 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e4f93d2..8da0eab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,7 +7,6 @@ package frc4388.robot; -import java.lang.StackWalker.Option; import java.util.function.Consumer; import edu.wpi.first.math.geometry.Translation2d; @@ -29,7 +28,7 @@ import frc4388.robot.commands.Arm.PivotCommand; import frc4388.robot.commands.Arm.TeleCommand; import frc4388.robot.commands.Autos.AutoBalance; import frc4388.robot.commands.Autos.PlaybackChooser; -import frc4388.robot.commands.Placement.DriveToLimeDistance; +import frc4388.robot.commands.Placement.AprilRotAlign; import frc4388.robot.commands.Placement.LimeAlign; import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; @@ -263,8 +262,10 @@ public class RobotContainer { .onFalse(interruptCommand.asProxy()); // toggle claw + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final + // .onTrue(toggleClaw.asProxy()); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .onTrue(toggleClaw.asProxy()); + .whileTrue(new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight)); // kill soft limits new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java new file mode 100644 index 0000000..212d979 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -0,0 +1,42 @@ +// 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.Placement; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class AprilRotAlign extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + /** Creates a new AprilRotAlign. */ + public AprilRotAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.2, 0.0, 0.0, 0.0); + + this.drive = drive; + this.lime = lime; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + double err = 0.0; + + try { + err = lime.getAprilSkew(); + } catch (NullPointerException ex) {} + + return err; + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 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 578c778..5b2e87f 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -43,13 +43,6 @@ public class LimeAlign extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { - - // 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 a54488d..262b0e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -5,11 +5,13 @@ package frc4388.robot.subsystems; import java.util.ArrayList; +import java.util.List; import org.photonvision.PhotonCamera; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -50,7 +52,6 @@ public class Limelight extends SubsystemBase { setLEDs(false); } - // ! might need to find midpoint instead of entire target public PhotonTrackedTarget getAprilPoint() { if (!cam.isConnected()) return null; @@ -61,6 +62,51 @@ public class Limelight extends SubsystemBase { return result.getBestTarget(); } + private List getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); + } + + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList bottomSide = getAprilBottomSide(corners); + + if (bottomSide == null) return 0; + + TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); + TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); + + return bottomLeft.y - bottomRight.y; + } + + private ArrayList getAprilBottomSide(List box) { + if (box == null) return null; + + ArrayList bottomSide = new ArrayList<>(); + + TargetCorner l1 = new TargetCorner(-1, -1); + TargetCorner l2 = new TargetCorner(-1, -1); + + for (TargetCorner c : box) { + if (c.y > l1.y) l1 = c; + } + + for (TargetCorner c : box) { + if (c.y == l1.y) continue; + if (c.y > l2.y) l2 = c; + } + + bottomSide.add(l1); + bottomSide.add(l2); + + return bottomSide; + } + public double getDistanceToApril() { PhotonTrackedTarget aprilPoint = getAprilPoint(); if (aprilPoint == null) return -1; @@ -102,17 +148,8 @@ public class Limelight extends SubsystemBase { return distanceToTape; } - int ctr = 0; - @Override public void periodic() { - // This method will be called once per scheduler run - - if (ctr % 50 == 0) { - SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape()); - } - - ctr++; - + SmartDashboard.putNumber("April Skew", getAprilSkew()); } } From c18f93be82b2107c6a7d01ca2173c023cbba6f73 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 14:48:09 -0600 Subject: [PATCH 15/19] dpad queueing WORKS!!! --- src/main/java/frc4388/robot/RobotContainer.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8da0eab..dba8ee9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -98,12 +98,21 @@ public class RobotContainer { // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); + // 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)); + private SequentialCommandGroup alignToShelf = new SequentialCommandGroup( - new RotateToAngle(m_robotSwerveDrive, 0.0), + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), 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 AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), 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)); From bf8a271df23b49c28262032ee1a8495c82816e9e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 14:49:50 -0600 Subject: [PATCH 16/19] cube high and cube mid DONE --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dba8ee9..81ac35f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -123,7 +123,7 @@ public class RobotContainer { }; // TODO: find actual values - private SequentialCommandGroup placeConeHigh = + private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup( // new InstantCommand(() -> System.out.println("Placing cone high")), new PivotCommand(m_robotArm, 64 + 135), @@ -133,21 +133,21 @@ public class RobotContainer { armToHome.asProxy() ); - private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( + private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( new PivotCommand(m_robotArm, 70 + 135), new TeleCommand(m_robotArm, 32866), toggleClaw.asProxy(), armToHome.asProxy() ); - private SequentialCommandGroup placeCubeHigh = new SequentialCommandGroup( + private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0), toggleClaw.asProxy(), armToHome.asProxy() ); - private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( + private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( new PivotCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0), toggleClaw.asProxy(), From daae169d515f2c711f47ec8695ba671c0d4fec15 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 18 Mar 2023 16:28:41 -0600 Subject: [PATCH 17/19] auto chooser --- src/main/java/frc4388/robot/RobotContainer.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 81ac35f..daae3df 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -183,15 +183,21 @@ public class RobotContainer { .withName("Arm DefaultCommand")); // * Auto Commands - // chooser.setDefaultOption("NoAuto", noAuto); + chooser.setDefaultOption("NoAuto", emptyCommand); + chooser.addOption("alignToPole", alignToPole); + chooser.addOption("alignToShelf", alignToShelf); + + chooser.addOption("placeConeHigh", placeConeHigh); + chooser.addOption("placeConeMid", placeConeMid); + chooser.addOption("placeCubeHigh", placeCubeHigh); + chooser.addOption("placeCubeMid", placeCubeMid); + chooser.addOption("placeLow", placeLow); // chooser.addOption("Blue1Path", blue1Path); // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); // chooser.addOption("Red1Path", red1Path); // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - - // chooser.addOption("Taxi", taxi); playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) From cd7c53bf832472bbf90ba15ad7562a98c23136bd Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 18 Mar 2023 17:23:38 -0600 Subject: [PATCH 18/19] sum change --- src/main/java/frc4388/robot/Constants.java | 6 ++++++ .../java/frc4388/robot/subsystems/Limelight.java | 16 +++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f771c80..af39c6a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,9 +7,14 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotUnits; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -168,5 +173,6 @@ public final class Constants { public static final double MID_TAPE_HEIGHT = 24.0; public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 262b0e7..9d1289b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -4,21 +4,33 @@ package frc4388.robot.subsystems; +import java.io.IOException; import java.util.ArrayList; import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.VisionConstants; public class Limelight extends SubsystemBase { + private PhotonCamera cam; + private PhotonPoseEstimator photonPoseEstimator; private boolean lightOn; @@ -149,7 +161,5 @@ public class Limelight extends SubsystemBase { } @Override - public void periodic() { - SmartDashboard.putNumber("April Skew", getAprilSkew()); - } + public void periodic() {} } From be1689894350c193cecacd824fa64bb394356297 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Tue, 21 Mar 2023 22:20:19 -0600 Subject: [PATCH 19/19] buttons final --- .../java/frc4388/robot/RobotContainer.java | 30 +++++++++---------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index daae3df..a8ce228 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -81,7 +81,7 @@ public class RobotContainer { 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)); + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); @@ -267,33 +267,31 @@ public class RobotContainer { // * Operator Buttons // align (pole) - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final .onTrue(alignToPole) .onFalse(interruptCommand.asProxy()); // align (shelf) - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final .onTrue(alignToShelf) .onFalse(interruptCommand.asProxy()); // toggle claw - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - // .onTrue(toggleClaw.asProxy()); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .whileTrue(new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight)); - + .onTrue(toggleClaw.asProxy()); + // kill soft limits - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - // toggle limelight - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight)); // final? - - // interrupt button - //new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(placeConeHigh.asProxy()); - // .onTrue(interruptCommand.asProxy()); + //Arm to Home + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(armToHome.asProxy()); + + //Interupt Button + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(interruptCommand.asProxy()); + // place high new POVButton(getDeadbandedOperatorController(), 0)