From bc4efe7f6d70f52a3da9a9c784eade10db8672fe Mon Sep 17 00:00:00 2001 From: Joey456788 Date: Sat, 27 Jan 2024 09:39:08 -0500 Subject: [PATCH 1/8] [#108] - added ontarget check --- .../java/frc/robot/command/chasePieces.java | 115 ++++++++++++++++++ src/main/java/frc/thunder | 2 +- 2 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/command/chasePieces.java diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java new file mode 100644 index 00000000..84cc7dd2 --- /dev/null +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -0,0 +1,115 @@ +package frc.robot.command; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveDriveBrake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ControllerConstants; +import frc.robot.Constants.DrivetrAinConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.vision.Limelight; + +public class chasePieces extends Command { + + private Swerve drivetrain; + private Limelight limelight; + + private XboxController driver; + private FieldCentric slow; + private FieldCentric drive; + + private SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private Limelight[] limelights; + + private int limelightId = 0; + private double pidOutput; + private double lockedOnHeading; + private double targetHeading; + private boolean useLimelights; + private boolean onTarget; + private PIDController headingController = VisionConstants.HEADING_CONTROLLER; + + /** + * Creates a new PointAtTag. + * @param drivetrain to request movement + * @param driver the driver's controller, used for drive input + * @param limelight_name the name of the limelight to use + * @param useLimelights to get if we want to use vision data or not + * @param noteDetection to get if we want to use this for note detection or april tag detection + */ + public chasePieces(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights, boolean noteDetection) { + this.drivetrain = drivetrain; + this.driver = driver; + this.useLimelights = useLimelights; + + //TODO Figure out which of these is the right one to use + for (var l : drivetrain.getLimelights()) { + if (l.getName().equals(limelight_name)) { + limelight = l; + } + } + + limelight = drivetrain.getLimelights()[0]; + + limelightId = limelight.getPipeline(); + + limelight.setPipeline(VisionConstants.NOTE_PIPELINE); + + drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop + slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if(Math.abs(limelight.getTargetX()) < VisionConstants.ALIGNMENT_TOLERANCE){ + + onTarget = true; + } + else{ + onTarget = false; + } + + if (onTarget){ + if (driver.getRightBumper()) { + drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) + .withRotationalRate(-pidOutput) // Drive counterclockwise with negative X (left) + ); + } else { + drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ + .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-pidOutput) // Rotate toward the desired direction + ); // Drive counterclockwise with negative X (left) + } + } else { + + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.setPipeline(limelightId); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index bc17e326..9605b432 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit bc17e3266077838fd43d669470efdb727182e6b6 +Subproject commit 9605b43298b268d6f709309df307e35b3e234261 From a7ec45f2fa2241ef40fe6015f4e50d47d31887a0 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 11:50:04 -0500 Subject: [PATCH 2/8] [#108] - actually chases pieces now (pid needs tuning) --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/command/PointAtTag.java | 3 + .../java/frc/robot/command/chasePieces.java | 71 ++++++++----------- 4 files changed, 35 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62d50248..0ce43e48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,6 +283,7 @@ public class VisionConstants { public static final double COLLISION_DEADZONE = 2d; public static final double ALIGNMENT_TOLERANCE = 4d; // TODO: make this an actual value public static final PIDController HEADING_CONTROLLER = new PIDController(0.05, 0, 0); + public static final PIDController CHASE_CONTROLLER = new PIDController(0.12, 0, 0.05); public static final int TAG_PIPELINE = 0; public static final int NOTE_PIPELINE = 2; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c0931bd8..32754737 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; @@ -102,9 +103,10 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", true, true)); + new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); + // new Trigger(driver::getYButton).whileTrue(new Climb(climber, drivetrain)); } diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index dfa5b0ff..9cb03e7e 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -90,6 +90,9 @@ public void execute() { LightningShuffleboard.setDouble("PointAtTag", "Target Heading", targetHeading); LightningShuffleboard.setDouble("PointAtTag", "Pid Output", pidOutput); + + pidOutput = headingController.calculate(0, targetHeading); + if (driver.getRightBumper()) { drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 84cc7dd2..606c03c5 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -3,67 +3,50 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SwerveDriveBrake; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ControllerConstants; -import frc.robot.Constants.DrivetrAinConstants; + import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Swerve; import frc.thunder.shuffleboard.LightningShuffleboard; import frc.thunder.vision.Limelight; -public class chasePieces extends Command { +public class ChasePieces extends Command { private Swerve drivetrain; private Limelight limelight; - private XboxController driver; - private FieldCentric slow; private FieldCentric drive; - - private SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private Limelight[] limelights; + private RobotCentric noteChase; private int limelightId = 0; private double pidOutput; - private double lockedOnHeading; private double targetHeading; - private boolean useLimelights; private boolean onTarget; - private PIDController headingController = VisionConstants.HEADING_CONTROLLER; + private PIDController headingController = VisionConstants.CHASE_CONTROLLER; /** - * Creates a new PointAtTag. + * Creates a new ChasePieces. * @param drivetrain to request movement * @param driver the driver's controller, used for drive input * @param limelight_name the name of the limelight to use * @param useLimelights to get if we want to use vision data or not * @param noteDetection to get if we want to use this for note detection or april tag detection */ - public chasePieces(Swerve drivetrain, XboxController driver, String limelight_name, boolean useLimelights, boolean noteDetection) { + public ChasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; - this.driver = driver; - this.useLimelights = useLimelights; - - //TODO Figure out which of these is the right one to use - for (var l : drivetrain.getLimelights()) { - if (l.getName().equals(limelight_name)) { - limelight = l; - } - } limelight = drivetrain.getLimelights()[0]; - limelightId = limelight.getPipeline(); limelight.setPipeline(VisionConstants.NOTE_PIPELINE); drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop - slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + } // Called when the command is initially scheduled. @@ -76,29 +59,31 @@ public void initialize() { @Override public void execute() { - if(Math.abs(limelight.getTargetX()) < VisionConstants.ALIGNMENT_TOLERANCE){ + targetHeading = limelight.getTargetX(); + if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ onTarget = true; } else{ onTarget = false; } + + LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); + LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); + LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); + LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); + + headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); + headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); + headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); + + + pidOutput = headingController.calculate(0, targetHeading); - if (onTarget){ - if (driver.getRightBumper()) { - drivetrain.setControl(slow.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SLOW_SPEED_MULT) // Drive left with negative X (left) - .withRotationalRate(-pidOutput) // Drive counterclockwise with negative X (left) - ); - } else { - drivetrain.setControl(drive.withVelocityX(-MathUtil.applyDeadband(driver.getLeftY(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive forward with negative Y (Its worth noting the field Y axis differs from the robot Y axis_ - .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-pidOutput) // Rotate toward the desired direction - ); // Drive counterclockwise with negative X (left) - } - } else { - - } + drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) + .withVelocityX(1.5) + ); + } // Called once the command ends or is interrupted. From 329e743169b3606bffdd347333e7616e8403fb0a Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 11:51:56 -0500 Subject: [PATCH 3/8] [#108] - cleaned up the code a little bit --- src/main/java/frc/robot/command/chasePieces.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 606c03c5..5adc2ac2 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -18,7 +18,6 @@ public class ChasePieces extends Command { private Swerve drivetrain; private Limelight limelight; - private FieldCentric drive; private RobotCentric noteChase; private int limelightId = 0; @@ -30,10 +29,6 @@ public class ChasePieces extends Command { /** * Creates a new ChasePieces. * @param drivetrain to request movement - * @param driver the driver's controller, used for drive input - * @param limelight_name the name of the limelight to use - * @param useLimelights to get if we want to use vision data or not - * @param noteDetection to get if we want to use this for note detection or april tag detection */ public ChasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; @@ -43,8 +38,6 @@ public ChasePieces(Swerve drivetrain) { limelight.setPipeline(VisionConstants.NOTE_PIPELINE); - drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop - noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); } From 886ef1c94fedf62b8e3a7846ad0bb4973ffb8d42 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 12:57:48 -0500 Subject: [PATCH 4/8] [#108] - fixed naming issue --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/command/chasePieces.java | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32754737..e7207efa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Collector; import frc.robot.command.TipDetection; +import frc.robot.command.chasePieces; import frc.robot.command.PointAtTag; import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -103,7 +104,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); + new Trigger(driver::getXButton).whileTrue(new chasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java index 5adc2ac2..3107946d 100644 --- a/src/main/java/frc/robot/command/chasePieces.java +++ b/src/main/java/frc/robot/command/chasePieces.java @@ -13,7 +13,7 @@ import frc.thunder.shuffleboard.LightningShuffleboard; import frc.thunder.vision.Limelight; -public class ChasePieces extends Command { +public class chasePieces extends Command { private Swerve drivetrain; private Limelight limelight; @@ -30,7 +30,7 @@ public class ChasePieces extends Command { * Creates a new ChasePieces. * @param drivetrain to request movement */ - public ChasePieces(Swerve drivetrain) { + public chasePieces(Swerve drivetrain) { this.drivetrain = drivetrain; limelight = drivetrain.getLimelights()[0]; From c989c556f1fa398d831588021dedf709fb6bed70 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:16:28 -0500 Subject: [PATCH 5/8] [#108] - fixed wrong import --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7207efa..627dec20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,7 +29,6 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; -import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; From 51b5f2d387a2dab02b3961ad1f204b4863f3922d Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:21:42 -0500 Subject: [PATCH 6/8] [#108] - fixing naming conventions --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/command/chasePieces.java | 93 ------------------- 2 files changed, 1 insertion(+), 95 deletions(-) delete mode 100644 src/main/java/frc/robot/command/chasePieces.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 627dec20..6f4660e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Collector; import frc.robot.command.TipDetection; -import frc.robot.command.chasePieces; import frc.robot.command.PointAtTag; import frc.robot.command.Collect; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -103,7 +102,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - new Trigger(driver::getXButton).whileTrue(new chasePieces(drivetrain)); + // new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/chasePieces.java b/src/main/java/frc/robot/command/chasePieces.java deleted file mode 100644 index 3107946d..00000000 --- a/src/main/java/frc/robot/command/chasePieces.java +++ /dev/null @@ -1,93 +0,0 @@ -package frc.robot.command; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; - -import frc.robot.Constants.VisionConstants; -import frc.robot.subsystems.Swerve; -import frc.thunder.shuffleboard.LightningShuffleboard; -import frc.thunder.vision.Limelight; - -public class chasePieces extends Command { - - private Swerve drivetrain; - private Limelight limelight; - - private RobotCentric noteChase; - - private int limelightId = 0; - private double pidOutput; - private double targetHeading; - private boolean onTarget; - private PIDController headingController = VisionConstants.CHASE_CONTROLLER; - - /** - * Creates a new ChasePieces. - * @param drivetrain to request movement - */ - public chasePieces(Swerve drivetrain) { - this.drivetrain = drivetrain; - - limelight = drivetrain.getLimelights()[0]; - limelightId = limelight.getPipeline(); - - limelight.setPipeline(VisionConstants.NOTE_PIPELINE); - - noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - targetHeading = limelight.getTargetX(); - - if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ - onTarget = true; - } - else{ - onTarget = false; - } - - LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); - LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); - LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); - LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); - - headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); - headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); - headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); - - - pidOutput = headingController.calculate(0, targetHeading); - - drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) - .withVelocityX(1.5) - ); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - limelight.setPipeline(limelightId); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file From 5d00f60e67aade7084d97b95afa74e2a988322b5 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:23:02 -0500 Subject: [PATCH 7/8] [#108] - (hopefully) fixed the naming --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/command/ChasePieces.java | 93 +++++++++++++++++++ 2 files changed, 95 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/command/ChasePieces.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f4660e4..32754737 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.ChasePieces; import frc.robot.command.Climb; import frc.robot.command.ManualClimb; import frc.robot.subsystems.Climber; @@ -102,7 +103,7 @@ protected void configureButtonBindings() { //TODO decide on comp buttons new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - // new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); + new Trigger(driver::getXButton).whileTrue(new ChasePieces(drivetrain)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java new file mode 100644 index 00000000..5adc2ac2 --- /dev/null +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -0,0 +1,93 @@ +package frc.robot.command; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.vision.Limelight; + +public class ChasePieces extends Command { + + private Swerve drivetrain; + private Limelight limelight; + + private RobotCentric noteChase; + + private int limelightId = 0; + private double pidOutput; + private double targetHeading; + private boolean onTarget; + private PIDController headingController = VisionConstants.CHASE_CONTROLLER; + + /** + * Creates a new ChasePieces. + * @param drivetrain to request movement + */ + public ChasePieces(Swerve drivetrain) { + this.drivetrain = drivetrain; + + limelight = drivetrain.getLimelights()[0]; + limelightId = limelight.getPipeline(); + + limelight.setPipeline(VisionConstants.NOTE_PIPELINE); + + noteChase = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + targetHeading = limelight.getTargetX(); + + if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ + onTarget = true; + } + else{ + onTarget = false; + } + + LightningShuffleboard.setBool("ChasePieces", "On Target", onTarget); + LightningShuffleboard.setDouble("ChasePieces", "Drivetrain Angle", drivetrain.getPigeon2().getAngle()); + LightningShuffleboard.setDouble("ChasePieces", "Target Heading", targetHeading); + LightningShuffleboard.setDouble("ChasePieces", "Pid Output", pidOutput); + + headingController.setP(LightningShuffleboard.getDouble("ChasePieces", "Pid P", 0.1)); + headingController.setI(LightningShuffleboard.getDouble("ChasePieces", "Pid I", 0)); + headingController.setD(LightningShuffleboard.getDouble("ChasePieces", "Pid D", 0)); + + + pidOutput = headingController.calculate(0, targetHeading); + + drivetrain.setControl(noteChase.withRotationalRate(-pidOutput) + .withVelocityX(1.5) + ); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + limelight.setPipeline(limelightId); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file From 52a7e32b49ba805c3202213b5f9c1872ee6e8bb9 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Sat, 27 Jan 2024 13:25:50 -0500 Subject: [PATCH 8/8] [#108] - fixed linter issues --- src/main/java/frc/robot/command/ChasePieces.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java index 5adc2ac2..39041f8d 100644 --- a/src/main/java/frc/robot/command/ChasePieces.java +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -56,8 +56,7 @@ public void execute() { if(Math.abs(targetHeading) < VisionConstants.ALIGNMENT_TOLERANCE){ onTarget = true; - } - else{ + } else{ onTarget = false; }