From 20bac49f8d383123f480fb1e756881c5b4643439 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Thu, 15 Feb 2024 15:38:47 -0700 Subject: [PATCH 1/5] Update - Remove unneeded limelight stuff - Make base code for autoAlign - Need to test with robot --- src/main/java/frc4388/robot/Constants.java | 7 +- src/main/java/frc4388/robot/Robot.java | 5 + .../java/frc4388/robot/RobotContainer.java | 15 +- .../robot/commands/Autos/AutoAlign.java | 78 ++++++++ .../robot/commands/Autos/PlaybackChooser.java | 8 +- .../frc4388/robot/subsystems/Apriltags.java | 36 ---- .../frc4388/robot/subsystems/Limelight.java | 166 ++---------------- src/main/java/frc4388/utility/AprilTag.java | 13 -- 8 files changed, 127 insertions(+), 201 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Autos/AutoAlign.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java delete mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7b5ba85..198b32f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -139,7 +139,12 @@ public final class Constants { public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value } - + + public static final class AutoAlignConstants { + public static final double MoveSpeed = 0.0; + public static final double RotSpeed = 0.0; + } + public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 14; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 44970b5..3ca9bfa 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -51,6 +51,11 @@ public class Robot extends TimedRobot { */ @Override public void robotPeriodic() { + // This has to be run after the m_robotContainer is initiated + // It will instantly return after it is run + m_robotContainer.autoAlign.updateAlliance(); + + m_robotTime.updateTimes(); //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3fda500..f4e37f8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,12 +19,14 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Intake; @@ -60,12 +62,15 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + /* Autos */ + private Limelight limelight = new Limelight(); + public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt"); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - - private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.pidIn()), new InstantCommand(() -> m_robotShooter.spin()) @@ -82,6 +87,12 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) ); + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( + new RunCommand(() -> MoveToSpeaker.execute()), + new RunCommand(() -> autoAlign.execute()) + ); + + private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java new file mode 100644 index 0000000..fe02b6d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -0,0 +1,78 @@ +package frc4388.robot.commands.Autos; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.Constants.AutoAlignConstants; + +import edu.wpi.first.wpilibj2.command.Command; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Pose2d; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class AutoAlign extends Command { + private SwerveDrive swerve; + private Limelight limelight; + private Pose2d pose; + + private Optional alliance; + + public AutoAlign(SwerveDrive swerve, Limelight limelight) { + this.swerve = swerve; + this.limelight = limelight; + } + + public void updateAlliance() { + if(alliance == null){ + alliance = DriverStation.getAlliance(); + } + } + + private Translation2d calcMoveStick(){ + //TODO - DO THE MATH! + + final double curX = pose.getX(); + final double curY = pose.getY(); + final double curYaw = pose.getRotation().getDegrees(); + + final boolean isred = alliance.get() == DriverStation.Alliance.Red; + + // This is not math + double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + + return new Translation2d(stickX, stickY); + } + + private Translation2d calcRotStick(){ + //TODO - DO THE MATH! + + final double curX = pose.getX(); + final double curY = pose.getY(); + final double curYaw = pose.getRotation().getDegrees(); + + final boolean isred = alliance.get() == DriverStation.Alliance.Red; + + // This is not math + double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + + return new Translation2d(stickX, stickY); + } + + public void execute() { + // Update limelight pos + pose = limelight.getPose(); + + Translation2d moveStick = calcMoveStick(); + Translation2d rotStick = calcRotStick(); + + // This would greatly benifit from having feild Relative implemented. + swerve.driveWithInput(moveStick, rotStick, false); + } +} diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..a270d7b 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -66,7 +66,13 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { + String[] dirs = m_dir.list(); + + if(dirs == null){ // Fix funny error + return; + } + + for (String auto : dirs) { chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); } for (var cmd_name : m_commandPool.keySet()) { diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -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; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..ce7bfe1 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,163 +3,33 @@ // the WPILib BSD license file in the root directory of this project. 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.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; + +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; + private double[] cameraPose; - private boolean lightOn; + public Pose2d getPose() { + //TODO - Get actual values! - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } + double x = 0; + double y = 0; + double yaw = 0; - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + Rotation2d rot = Rotation2d.fromDegrees(yaw); - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } - - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } - - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } - - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } - - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - 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; - - 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() { - 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 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 distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; + return new Pose2d(x, y, rot); } @Override - public void periodic() {} -} + public void periodic() { + // This method will be called once per scheduler run + cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} From 4b7cb07c1bb8a81b75d9515120c2d7e1b761c1c1 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Fri, 16 Feb 2024 12:59:22 -0700 Subject: [PATCH 2/5] Add reverse mode - Add reverse mode of autoAlign TODO: - Shoot between normal and reverse mode - Use the robot - Make the math in autoAlign --- gradlew | 0 src/main/java/frc4388/robot/Constants.java | 34 +++++---- src/main/java/frc4388/robot/Robot.java | 7 +- .../java/frc4388/robot/RobotContainer.java | 16 +++-- .../robot/commands/Autos/AutoAlign.java | 72 ++++++++++++++++--- .../frc4388/robot/subsystems/Limelight.java | 59 +++++++++++++-- .../frc4388/robot/subsystems/Shooter.java | 30 +++++++- .../java/frc4388/robot/subsystems/Vision.java | 38 ---------- 8 files changed, 175 insertions(+), 81 deletions(-) mode change 100644 => 100755 gradlew delete mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 198b32f..658699a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -9,6 +9,8 @@ package frc4388.robot; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.geometry.Translation2d; + import frc4388.utility.LEDPatterns; import frc4388.utility.Gains; @@ -119,24 +121,29 @@ public final class Constants { } public static final class VisionConstants { - public static final String NAME = "photonCamera"; + // public static final String NAME = "photonCamera"; - public static final int LIME_HIXELS = 640; - public static final int LIME_VIXELS = 480; + // public static final int LIME_HIXELS = 640; + // public static final int LIME_VIXELS = 480; - public static final double H_FOV = 59.6; - public static final double V_FOV = 45.7; + // public static final double H_FOV = 59.6; + // public static final double V_FOV = 45.7; - public static final double LIME_HEIGHT = 6.0; - public static final double LIME_ANGLE = 55.0; + // public static final double LIME_HEIGHT = 6.0; + // public static final double LIME_ANGLE = 55.0; - // public static final double HIGH_TARGET_HEIGHT = 46.0; - public static final double HIGH_TAPE_HEIGHT = 44.0; + // // public static final double HIGH_TARGET_HEIGHT = 46.0; + // public static final double HIGH_TAPE_HEIGHT = 44.0; - // public static final double MID_TARGET_HEIGHT = 34.0; - public static final double MID_TAPE_HEIGHT = 24.0; + // // 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 + // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); + public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); + + public static final double SpeakerBubbleDistance = 1.5; } @@ -160,9 +167,10 @@ public final class Constants { public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final + public static final double SHOOTER_SPEED = 1.0; // final public static final double SHOOTER_IDLE = 0.4; // final - + public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3ca9bfa..4190119 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -50,12 +50,7 @@ public class Robot extends TimedRobot { * LiveWindow and SmartDashboard integrated updating. */ @Override - public void robotPeriodic() { - // This has to be run after the m_robotContainer is initiated - // It will instantly return after it is run - m_robotContainer.autoAlign.updateAlliance(); - - + public void robotPeriodic() { m_robotTime.updateTimes(); //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f4e37f8..06829b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,11 +51,14 @@ public class RobotContainer { m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, + m_robotMap.gyro); - - private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); + /* Limelight */ + private final Limelight limelight = new Limelight(); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); @@ -64,8 +67,8 @@ public class RobotContainer { private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); /* Autos */ - private Limelight limelight = new Limelight(); - public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + // This shoud be in a SequentialCommandGroup vvv + //public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight, false); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt"); @@ -87,9 +90,10 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) ); + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( new RunCommand(() -> MoveToSpeaker.execute()), - new RunCommand(() -> autoAlign.execute()) + new RunCommand(() -> new AutoAlign(m_robotSwerveDrive, limelight, false)) ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index fe02b6d..4b4ee83 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -22,15 +22,18 @@ public class AutoAlign extends Command { private Optional alliance; - public AutoAlign(SwerveDrive swerve, Limelight limelight) { + private boolean isFinished; + private boolean isReverseFinished; + + private boolean reverseAfterFinish; + private Translation2d[] moveStickReplayArr; + private Translation2d[] rotStickReplayArr; + private int replayIndex; + + public AutoAlign(SwerveDrive swerve, Limelight limelight, boolean reverseAfterFinish) { this.swerve = swerve; this.limelight = limelight; - } - - public void updateAlliance() { - if(alliance == null){ - alliance = DriverStation.getAlliance(); - } + this.reverseAfterFinish = reverseAfterFinish; } private Translation2d calcMoveStick(){ @@ -42,6 +45,8 @@ public class AutoAlign extends Command { final boolean isred = alliance.get() == DriverStation.Alliance.Red; + + // This is not math double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; @@ -65,14 +70,63 @@ public class AutoAlign extends Command { return new Translation2d(stickX, stickY); } + // Called when the command is initially scheduled. + @Override + public final void initialize() { + if(reverseAfterFinish){ + isReverseFinished = false; + replayIndex = 0; + moveStickReplayArr = new Translation2d[]{}; + rotStickReplayArr = new Translation2d[]{}; + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override public void execute() { // Update limelight pos pose = limelight.getPose(); - Translation2d moveStick = calcMoveStick(); - Translation2d rotStick = calcRotStick(); + // These must be 0, or it will error + Translation2d moveStick = new Translation2d(0, 0); + Translation2d rotStick = new Translation2d(0, 0); + + // Regular replay + if(!isFinished){ + moveStick = calcMoveStick(); + rotStick = calcRotStick(); + + // This is a way of appending... + if(reverseAfterFinish){ + moveStickReplayArr[moveStickReplayArr.length] = moveStick; + rotStickReplayArr[rotStickReplayArr.length] = rotStick; + } + + // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ + // replayIndex + // } + isFinished = limelight.isNearSpeaker(); + + // If reverseAfterFinish, then loop back over and replay + }else if(reverseAfterFinish && !isReverseFinished){ + // Get reverse direction + moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; + rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; + + // Invert sticks + moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); + rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); + + replayIndex++; + } // This would greatly benifit from having feild Relative implemented. swerve.driveWithInput(moveStick, rotStick, false); } + + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return isFinished && (isReverseFinished || !reverseAfterFinish); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index ce7bfe1..a0ea95f 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -6,30 +6,75 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; + // Look at vvv for networktables stuff // https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { + + // [X, Y, Z, Roll, Pitch, Yaw] private double[] cameraPose; + private boolean isTag; - public Pose2d getPose() { - //TODO - Get actual values! + private Pose2d pose; + private boolean isNearSpeaker; - double x = 0; - double y = 0; - double yaw = 0; + public boolean getIsTag() { + return isTag; + } + + private void update() { + if(!isTag){ + return; + } + + double x = cameraPose[0]; + double y = cameraPose[1]; + double yaw = cameraPose[5]; Rotation2d rot = Rotation2d.fromDegrees(yaw); - return new Pose2d(x, y, rot); + pose = new Pose2d(x, y, rot); + + boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; + + double distance; + + if(isRed){ + distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + }else{ + distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + } + + isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; + } + + public Pose2d getPose() { + return pose; + } + + public boolean isNearSpeaker() { + return isNearSpeaker; } @Override public void periodic() { // This method will be called once per scheduler run - cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); + double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + + SmartDashboard.putBoolean("Apriltag", isTag); + + if(newPose != cameraPose){ + cameraPose = newPose; + update(); + } } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8d340ed..4eba3cb 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -20,16 +20,28 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; + private Limelight limelight; + + // 0 = Stop + // 1 = Idle, no limelight + // 2 = limelight + // 3 = Shooting + private int shooterMode; + + /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) { leftShooter = leftTalonFX; rightShooter = rightTalonFX; + this.limelight = limelight; + leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); } public void spin() { + shooterMode = 3; spin(ShooterConstants.SHOOTER_SPEED); } @@ -39,16 +51,30 @@ public class Shooter extends SubsystemBase { } public void stop() { + shooterMode = 0; spin(0.d); } public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); + if(limelight.isNearSpeaker()){ + shooterMode = 2; + spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + }else{ + shooterMode = 1; + spin(ShooterConstants.SHOOTER_IDLE); + } } @Override public void periodic() { // This method will be called once per scheduler run + + if(limelight.isNearSpeaker() && shooterMode == 0){ + shooterMode = 1; + spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + } + + SmartDashboard.putNumber("Shooter Speed mode", shooterMode); SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -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.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -} From 84c5313a50d5c42e510158a6ee60639ada9368f5 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Fri, 16 Feb 2024 17:04:46 -0700 Subject: [PATCH 3/5] Add reverse mode Add reverse mode Add example auto sequential commands TODO: Run with the robot Do math --- .gitignore | 1 + src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 14 +++-- .../robot/commands/Autos/AutoAlign.java | 58 ++++++++++++++----- .../frc4388/robot/subsystems/Limelight.java | 6 +- 5 files changed, 60 insertions(+), 22 deletions(-) diff --git a/.gitignore b/.gitignore index 5528d4f..01960f5 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,4 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ +simgui.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 658699a..eed58d9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -143,7 +143,8 @@ public final class Constants { public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); - public static final double SpeakerBubbleDistance = 1.5; + public static final double SpeakerBubbleDistance = 3; + public static final double targetPosDistance = 1.5; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 06829b7..d432c84 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -68,7 +68,6 @@ public class RobotContainer { /* Autos */ // This shoud be in a SequentialCommandGroup vvv - //public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight, false); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt"); @@ -90,10 +89,17 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) ); - + + private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( - new RunCommand(() -> MoveToSpeaker.execute()), - new RunCommand(() -> new AutoAlign(m_robotSwerveDrive, limelight, false)) + MoveToSpeaker, + autoAlign, + new InstantCommand(() -> m_robotShooter.spin()), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotShooter.idle()), + new InstantCommand(() -> autoAlign.reverse()), + autoAlign ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index 4b4ee83..26ed6ab 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -2,7 +2,7 @@ package frc4388.robot.commands.Autos; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.Constants.AutoAlignConstants; - +import frc4388.robot.Constants.VisionConstants; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.math.geometry.Translation2d; @@ -18,24 +18,52 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; public class AutoAlign extends Command { private SwerveDrive swerve; private Limelight limelight; + private Pose2d pose; + private Translation2d targetPos; private Optional alliance; + private boolean isRed; private boolean isFinished; private boolean isReverseFinished; - + private boolean reverseAfterFinish; private Translation2d[] moveStickReplayArr; private Translation2d[] rotStickReplayArr; private int replayIndex; - public AutoAlign(SwerveDrive swerve, Limelight limelight, boolean reverseAfterFinish) { + public AutoAlign(SwerveDrive swerve, Limelight limelight) { this.swerve = swerve; this.limelight = limelight; this.reverseAfterFinish = reverseAfterFinish; } + // Calc the closest point on a circle, to the center of the speaker + private Translation2d calcTargetPos(){ + final double R = VisionConstants.targetPosDistance; + double cX; + double cY; + if(isRed){ + cX = VisionConstants.RedSpeakerCenter.getX(); + cY = VisionConstants.RedSpeakerCenter.getY(); + }else{ + cX = VisionConstants.BlueSpeakerCenter.getX(); + cY = VisionConstants.BlueSpeakerCenter.getY(); + } + final double pX = pose.getX(); + final double pY = pose.getY(); + + // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point + double vX = pX - cX; + double vY = pY - cY; + double magV = Math.sqrt(vX*vX + vY*vY); + double aX = cX + vX / magV * R; + double aY = cY + vY / magV * R; + + return new Translation2d(aX, aY); + } + private Translation2d calcMoveStick(){ //TODO - DO THE MATH! @@ -43,10 +71,6 @@ public class AutoAlign extends Command { final double curY = pose.getY(); final double curYaw = pose.getRotation().getDegrees(); - final boolean isred = alliance.get() == DriverStation.Alliance.Red; - - - // This is not math double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; @@ -61,8 +85,6 @@ public class AutoAlign extends Command { final double curY = pose.getY(); final double curYaw = pose.getRotation().getDegrees(); - final boolean isred = alliance.get() == DriverStation.Alliance.Red; - // This is not math double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; @@ -70,12 +92,18 @@ public class AutoAlign extends Command { return new Translation2d(stickX, stickY); } + public void reverse() { + this.reverseAfterFinish = true; + } + // Called when the command is initially scheduled. @Override public final void initialize() { + isRed = alliance.get() == DriverStation.Alliance.Red; if(reverseAfterFinish){ - isReverseFinished = false; + // isReverseFinished = false; replayIndex = 0; + }else{ moveStickReplayArr = new Translation2d[]{}; rotStickReplayArr = new Translation2d[]{}; } @@ -93,14 +121,14 @@ public class AutoAlign extends Command { // Regular replay if(!isFinished){ + targetPos = calcTargetPos(); + moveStick = calcMoveStick(); rotStick = calcRotStick(); // This is a way of appending... - if(reverseAfterFinish){ - moveStickReplayArr[moveStickReplayArr.length] = moveStick; - rotStickReplayArr[rotStickReplayArr.length] = rotStick; - } + moveStickReplayArr[moveStickReplayArr.length] = moveStick; + rotStickReplayArr[rotStickReplayArr.length] = rotStick; // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ // replayIndex @@ -121,7 +149,7 @@ public class AutoAlign extends Command { } // This would greatly benifit from having feild Relative implemented. - swerve.driveWithInput(moveStick, rotStick, false); + swerve.driveWithInput(moveStick, rotStick, true); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index a0ea95f..9ece406 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -53,6 +53,10 @@ public class Limelight extends SubsystemBase { } isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; + + SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + SmartDashboard.putBoolean("Apriltag", isTag); + SmartDashboard.putNumber("speakerDistance", distance); } public Pose2d getPose() { @@ -69,8 +73,6 @@ public class Limelight extends SubsystemBase { isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - - SmartDashboard.putBoolean("Apriltag", isTag); if(newPose != cameraPose){ cameraPose = newPose; From e77d2d5a6c88ff488da7bf2e44bd0673e8514770 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Mon, 19 Feb 2024 10:55:15 -0700 Subject: [PATCH 4/5] Update --- .../java/frc4388/robot/RobotContainer.java | 5 +- .../robot/commands/Autos/AutoAlign.java | 90 ++++++++++++++----- 2 files changed, 73 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d432c84..0b0f67f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ 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.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; @@ -96,10 +97,12 @@ public class RobotContainer { MoveToSpeaker, autoAlign, new InstantCommand(() -> m_robotShooter.spin()), + new WaitCommand(3.0), new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(3.0), new InstantCommand(() -> m_robotShooter.idle()), new InstantCommand(() -> autoAlign.reverse()), - autoAlign + autoAlign.asProxy() ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index 26ed6ab..ce8b37c 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import java.util.Optional; +import org.opencv.core.RotatedRect; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; @@ -18,9 +20,9 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; public class AutoAlign extends Command { private SwerveDrive swerve; private Limelight limelight; - private Pose2d pose; private Translation2d targetPos; + private Rotation2d targetRot; private Optional alliance; private boolean isRed; @@ -33,17 +35,20 @@ public class AutoAlign extends Command { private Translation2d[] rotStickReplayArr; private int replayIndex; + // PID Stuff + private double prevError; + private double cumError; + public AutoAlign(SwerveDrive swerve, Limelight limelight) { this.swerve = swerve; this.limelight = limelight; - this.reverseAfterFinish = reverseAfterFinish; } // Calc the closest point on a circle, to the center of the speaker private Translation2d calcTargetPos(){ final double R = VisionConstants.targetPosDistance; - double cX; - double cY; + final double cX; + final double cY; if(isRed){ cX = VisionConstants.RedSpeakerCenter.getX(); cY = VisionConstants.RedSpeakerCenter.getY(); @@ -64,32 +69,70 @@ public class AutoAlign extends Command { return new Translation2d(aX, aY); } - private Translation2d calcMoveStick(){ - //TODO - DO THE MATH! + // Calculate the angle facing the center, at the target rot + private Rotation2d calcTargetRot() { + final double R = VisionConstants.targetPosDistance; + final double cX; + final double cY; + if(isRed){ + cX = VisionConstants.RedSpeakerCenter.getX(); + cY = VisionConstants.RedSpeakerCenter.getY(); + }else{ + cX = VisionConstants.BlueSpeakerCenter.getX(); + cY = VisionConstants.BlueSpeakerCenter.getY(); + } + final double pX = pose.getX(); + final double pY = pose.getY(); + final double dX = pX - cX; + final double dY = pY - cY; + + final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); + + return Rotation2d.fromDegrees(yaw); + } + + // Clamp to a circle, like an xbox controller + private Translation2d clamp(double oldX, double oldY){ + // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley + final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; + final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); + final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); + + final double newX = Math.max(-maxX, Math.min(maxX, oldX)); + final double newY = Math.max(-maxY, Math.min(maxY, oldY)); + + return new Translation2d(newX, newY); + } + + private Translation2d calcMoveStick(){ final double curX = pose.getX(); final double curY = pose.getY(); - final double curYaw = pose.getRotation().getDegrees(); - // This is not math - double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + // I think this might work, assuming the constants are good + double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; + double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; - return new Translation2d(stickX, stickY); + return clamp(stickX, stickY); } private Translation2d calcRotStick(){ - //TODO - DO THE MATH! + double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); + cumError += error * .02; // 20 ms + double delta = error - prevError; - final double curX = pose.getX(); - final double curY = pose.getY(); - final double curYaw = pose.getRotation().getDegrees(); + final double kP = 4; + final double kI = 4; + final double kD = 4; + final double kF = 4; - // This is not math - double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; - double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; + double output = error * kP; + output += cumError * kI; + output += delta * kD; + output += kF; - return new Translation2d(stickX, stickY); + prevError = error; + return clamp(output, 0); } public void reverse() { @@ -122,6 +165,7 @@ public class AutoAlign extends Command { // Regular replay if(!isFinished){ targetPos = calcTargetPos(); + targetRot = calcTargetRot(); moveStick = calcMoveStick(); rotStick = calcRotStick(); @@ -135,7 +179,7 @@ public class AutoAlign extends Command { // } isFinished = limelight.isNearSpeaker(); - // If reverseAfterFinish, then loop back over and replay + // If reverseAfterFinish, then loop back over and replay in reverse }else if(reverseAfterFinish && !isReverseFinished){ // Get reverse direction moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; @@ -146,6 +190,10 @@ public class AutoAlign extends Command { rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); replayIndex++; + + if(replayIndex >= moveStickReplayArr.length){ + reverseAfterFinish = true; + } } // This would greatly benifit from having feild Relative implemented. @@ -157,4 +205,4 @@ public class AutoAlign extends Command { public final boolean isFinished() { return isFinished && (isReverseFinished || !reverseAfterFinish); } -} +} \ No newline at end of file From 904a2f26880a89d1535d077c33f5755110918388 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 19:58:04 -0700 Subject: [PATCH 5/5] fixes --- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- src/main/java/frc4388/robot/subsystems/Shooter.java | 13 +++---------- .../utility/Configurable/ConfigurableDouble.java | 2 +- .../utility/configurable/ConfigurableDouble.java | 2 +- 4 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bb321d4..4e11195 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,7 +57,7 @@ public class RobotContainer { /* Limelight */ private final Limelight limelight = new Limelight(); - private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); @@ -73,7 +73,6 @@ public class RobotContainer { private final VirtualController m_virtualDriver = new VirtualController(0); private final VirtualController m_virtualOperator = new VirtualController(1); - private Limelight limelight = new Limelight(); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index c14d779..d3100a2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -22,13 +22,13 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; + private double smartDashboardShooterSpeed; + /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) { + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { leftShooter = leftTalonFX; rightShooter = rightTalonFX; - this.limelight = limelight; - leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); @@ -63,18 +63,11 @@ public class Shooter extends SubsystemBase { } public void stop() { - shooterMode = 0; spin(0.d); } public void idle() { - if(limelight.isNearSpeaker()){ - shooterMode = 2; - spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - }else{ - shooterMode = 1; spin(ShooterConstants.SHOOTER_IDLE); - } } @Override diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java index c0384db..fe15818 100644 --- a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.configurable; +package frc4388.utility.Configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java index c0384db..fe15818 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.configurable; +package frc4388.utility.Configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;