add auto-alignment and creep

Author: IsaacThoman

src/main/java/frc/robot/Constants.java

@@ -72,7 +72,7 @@ public static class EFCConstants{
 
   public static class ArmConstants{
 
-    public static final double armkP = 0.3;
+    public static final double armkP = 0.8;
     public static final double armkI = 0.1;
     public static final double armkD = 0.0;
     public static final double armIZone = 0.15;

src/main/java/frc/robot/RobotContainer.java

@@ -39,7 +39,7 @@ public class RobotContainer {
   PneumaticControlSub pneumaticControlSub = new PneumaticControlSub();
 
 
-  VisionSubsystem visionSub = new VisionSubsystem();
+  VisionSubsystem visionSub = new VisionSubsystem(stateController);
 
   SendableChooser<Command> autoChooser = new SendableChooser<>();
 

src/main/java/frc/robot/StateControllerSub.java

@@ -215,6 +215,7 @@ public StateControllerSub(){
         SmartDashboard.putBoolean("alignWhenClose",alignWhenCloseEnabled);
 
 
+
     }
 
     double climbAngle = 0.0;
@@ -297,14 +298,25 @@ public void setDuckMode(boolean down){
    public boolean alignWhenClose() {
         if(objective == Objective.SPEAKER && distanceToMySpeaker() < 6)
             return alignWhenCloseEnabled;
+        if(armState == ArmState.INTAKE)
+            return alignWhenCloseEnabled;
         return false;
     }
 
 
 
-    public double alignWhenCloseAngDiff() {
+    public double
+    alignWhenCloseAngDiff() {
+        if(armState == ArmState.INTAKE){
+            return noteAlignAngleDiff;
+        }
         return MBUtils.angleDiffRad(angleToObjective(objective),robotPose.getRotation().getRadians());
     }
+    double noteAlignAngleDiff = 0;
+
+    public void feedNoteAlignAngleDiff(double noteAlignAngleDiff){
+        this.noteAlignAngleDiff = noteAlignAngleDiff;
+    }
 
 
 

src/main/java/frc/robot/subsystems/drive/SwerveSubsystem.java

@@ -131,6 +131,8 @@ public SwerveModulePosition[] getPositions(){
 
   public void joystickDrive(double joystickX, double joystickY, double rad){
 
+    double rawJoyHypot = Math.hypot(joystickX,joystickY);
+
     if(Math.abs(rad)<controllerDeadband)
       rad = 0;
 
@@ -182,9 +184,18 @@ public void joystickDrive(double joystickX, double joystickY, double rad){
 
 
 
-    if(stateController.alignWhenClose() && stateController.getArmState() != StateControllerSub.ArmState.INTAKE && stateController.getArmState() != StateControllerSub.ArmState.HOLD)
+    if(stateController.alignWhenClose() && stateController.getArmState() != StateControllerSub.ArmState.HOLD)
       rad+=MBUtils.clamp(stateController.alignWhenCloseAngDiff() * alignmentkP,1);
 
+    double creep = 0;
+
+    if(stateController.getArmState() == StateControllerSub.ArmState.INTAKE && rawJoyHypot<0.1)
+     creep = -0.3;
+
+    SmartDashboard.putNumber("rawJoyHypot",rawJoyHypot);
+
+    SmartDashboard.putNumber("alignWhenCloseAngDiff",stateController.alignWhenCloseAngDiff());
+
 
 
     /* Angular adjustment stuff */
@@ -194,7 +205,7 @@ public void joystickDrive(double joystickX, double joystickY, double rad){
 
 
 
-  drive(-joystickY ,-joystickX ,-rad);
+  drive(-joystickY + creep ,-joystickX ,-rad);
 }
 
 

src/main/java/frc/robot/subsystems/drive/VisionSubsystem.java

@@ -5,13 +5,15 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.StateControllerSub;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
+import java.util.List;
 import java.util.Optional;
 
 public class VisionSubsystem extends SubsystemBase {
@@ -20,28 +22,32 @@ public class VisionSubsystem extends SubsystemBase {
 
 
     AprilTagFieldLayout aprilTagFieldLayout;
-    PhotonCamera cam;
+    PhotonCamera shooterSideArducam;
+
+    PhotonCamera intakeAssistCamera = new PhotonCamera("intake_assist_scary");
     PhotonPoseEstimator photonPoseEstimator;
-    public VisionSubsystem() {
 
-    //    SmartDashboard.putData("poseStrategy",selectedPoseStrategy);
+    StateControllerSub stateControllerSub;
+    public VisionSubsystem(StateControllerSub stateControllerSub) {
 
+    //    SmartDashboard.putData("poseStrategy",selectedPoseStrategy);
         try{
             aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
         }catch (Exception e){
             e.printStackTrace();
         }
 
-        cam = new PhotonCamera("Arducam_OV2311_USB_Camera");
+        shooterSideArducam = new PhotonCamera("Arducam_OV2311_shooter");
         Transform3d robotToCam = new Transform3d(new Translation3d(0.357,0.076,0.224), new Rotation3d(0,Units.degreesToRadians(-30),0));
-        photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cam, robotToCam);
+        photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, shooterSideArducam, robotToCam);
        // photonPoseEstimator.setPrimaryStrategy();
         photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY);
 
         //1st + is cam forward
         //2nd + is cam to left
         //3rd + is cam up
 
+        this.stateControllerSub = stateControllerSub;
 
     }
 
@@ -54,8 +60,20 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedR
 
     }
 
+
+
+
     public void periodic(){
 
+      List<PhotonTrackedTarget> targets = intakeAssistCamera.getLatestResult().getTargets();
+      double assistAngle = 0;
+      if(!targets.isEmpty())
+          assistAngle = Units.degreesToRadians(targets.get(0).getYaw());
+      stateControllerSub.feedNoteAlignAngleDiff(assistAngle);
+
+        SmartDashboard.putNumber("noteAssistAngle",assistAngle);
+
+
     }