Merge pull request #1 from Team4388/AutoAlign

Auto align

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,25 +121,36 @@ 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_HIXELS = 640;
-        public static final int LIME_VIXELS = 480;
+        // public static final int LIME_VIXELS = 480;
     
-        public static final double H_FOV = 59.6;
+        // public static final double H_FOV = 59.6;
-        public static final double V_FOV = 45.7;
+        // public static final double V_FOV = 45.7;
     
-        public static final double LIME_HEIGHT = 6.0;
+        // public static final double LIME_HEIGHT = 6.0;
-        public static final double LIME_ANGLE = 55.0;
+        // public static final double LIME_ANGLE = 55.0;
     
-        // public static final double HIGH_TARGET_HEIGHT = 46.0;
+        // // public static final double HIGH_TARGET_HEIGHT = 46.0;
-        public static final double HIGH_TAPE_HEIGHT = 44.0;
+        // public static final double HIGH_TAPE_HEIGHT = 44.0;
         
-        // public static final double MID_TARGET_HEIGHT = 34.0;
+        // // public static final double MID_TARGET_HEIGHT = 34.0;
-        public static final double MID_TAPE_HEIGHT = 24.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 = 3;
+        public static final double targetPosDistance = 1.5;
+    
+    }
+   
+    public static final class AutoAlignConstants {
+        public static final double MoveSpeed = 0.0;
+        public static final double RotSpeed = 0.0;
     }
 
     public static final class DriveConstants {
@@ -157,7 +170,7 @@ public final class Constants {
         public static final int RIGHT_SHOOTER_ID = 16; // final
         public static final double SHOOTER_SPEED = 0.35; // 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/RobotContainer.java

@@ -15,8 +15,10 @@ 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;
+import frc4388.robot.commands.Autos.AutoAlign;
 import frc4388.robot.commands.Autos.PlaybackChooser;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
@@ -50,7 +52,10 @@ public class RobotContainer {
                                                                   m_robotMap.rightFront,
                                                                   m_robotMap.leftBack,
                                                                   m_robotMap.rightBack,
+                                              
                                                                   m_robotMap.gyro);
+    /* Limelight */
+    private final Limelight limelight = new Limelight();
 
     private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter);
 
@@ -68,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(
@@ -76,6 +80,33 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotShooter.spin())
     );
 
+    private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotShooter.spin()),
+        new InstantCommand(() -> m_robotIntake.handoff())
+    );
+ 
+    private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake),
+        new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()),
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)
+    );
+
+
+    private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
+
+    private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
+        // 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.asProxy()
+    );
+
+
     private SequentialCommandGroup i = new SequentialCommandGroup(
         intakeToShootStuff, intakeToShoot
     );

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

@@ -0,0 +1,208 @@
+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;
+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;
+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> alliance;
+    private boolean isRed;
+
+    private boolean isFinished;
+    private boolean isReverseFinished;
+
+    private boolean reverseAfterFinish;
+    private Translation2d[] moveStickReplayArr;
+    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;
+    }
+
+    // Calc the closest point on a circle, to the center of the speaker 
+    private Translation2d calcTargetPos(){
+        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();
+
+        // 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);
+    }
+
+    // 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();
+
+        // 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 clamp(stickX, stickY);
+    }
+
+    private Translation2d calcRotStick(){
+        double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+        final double kP = 4;
+        final double kI = 4;
+        final double kD = 4;
+        final double kF = 4;
+
+		double output = error * kP;
+		output += cumError * kI;
+		output += delta * kD;
+		output += kF;
+
+        prevError = error;
+        return clamp(output, 0);
+    }
+
+    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;
+            replayIndex = 0;
+        }else{
+            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();
+
+        // These must be 0, or it will error
+        Translation2d moveStick = new Translation2d(0, 0);
+        Translation2d rotStick = new Translation2d(0, 0);
+
+        // Regular replay
+        if(!isFinished){
+            targetPos = calcTargetPos();
+            targetRot = calcTargetRot();
+
+            moveStick = calcMoveStick();
+            rotStick = calcRotStick();
+
+            // This is a way of appending...
+            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 in reverse
+        }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++;
+
+            if(replayIndex >= moveStickReplayArr.length){
+                reverseAfterFinish = true;
+            }
+        }
+
+        // This would greatly benifit from having feild Relative implemented.
+        swerve.driveWithInput(moveStick, rotStick, true);
+    }
+
+    // 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,65 @@ 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;
+  private boolean isTag;
+
+  private Pose2d pose;
+  private boolean isNearSpeaker;
 
   public boolean getIsTag() {
     return isTag;
   }
 
-  public Pose2d getPose() {
+  private void update() {
-    //TODO - Get actual values!
+    if(!isTag){
+      return;
+    }
 
-    double x = 0;
+    double x = cameraPose[0];
-    double y = 0;
+    double y = cameraPose[1];
-    double yaw = 0;
+    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;
+
+    SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
+    SmartDashboard.putBoolean("Apriltag", isTag);
+    SmartDashboard.putNumber("speakerDistance", distance);
+  }
+
+  public Pose2d getPose() {
+    return pose;
+  }
+
+  public boolean isNearSpeaker() {
+    return isNearSpeaker;
   }
 
   @Override
@@ -37,6 +72,11 @@ public class Limelight extends SubsystemBase {
     // This method will be called once per scheduler run
 
     isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false);
-    cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]);
+    double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
+
+    if(newPose != cameraPose){
+      cameraPose = newPose;
+      update();
+    }
   }
 }

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

@@ -23,6 +23,7 @@ public class Shooter extends SubsystemBase {
   private TalonFX rightShooter;
 
   private double smartDashboardShooterSpeed;
+
   /** Creates a new Shooter. */
   public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) {
     leftShooter  = leftTalonFX;

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

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;
 

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;