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;