Add reverse mode

- Add reverse mode of autoAlign

TODO:
-  Shoot between normal and reverse mode
- Use the robot
- Make the math in autoAlign

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 {

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

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

src/main/java/frc4388/robot/commands/Autos/AutoAlign.java

@@ -22,15 +22,18 @@ public class AutoAlign extends Command {
 
     private Optional<Alliance> 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);
+	}
 }

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();
+    }
   }
 }

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

src/main/java/frc4388/robot/subsystems/Vision.java

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