Add reverse mode

Add reverse mode
Add example auto sequential commands

TODO:
Run with the robot
Do math

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;
     
     }
    

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
     );
 
 

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> 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.

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;