From fe5a07bd17e96db246df6cedaf395eaf2623314d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 16 Mar 2023 19:10:26 -0600 Subject: [PATCH] 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() {