Source AutoAim

BadRobots1014 · Apr 5, 2024 · 2b4ca2a · 2b4ca2a
1 parent e793290
commit 2b4ca2a
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -28,6 +28,9 @@ public static final class DriveConstants {
     public static final Integer kAutoAimInactive = 0;
     public static final Integer kFlexibleAutoAim = 1;
     public static final Integer kRigidAutoAim = 2;
+    public static final Double kAutoAimZOffset = 2.0;//Offset how far forwards the robot will try to move
+    public static final Double kAutoAimXOffset = -0.2;//unsure how much to the left limelight is 
+    public static final Double kAutoAimSourceXOffset = 0.0;//Offset for source
 
     // TODO turn field oriented on or off
     public static final boolean kFieldOriented = true;

diff --git a/src/main/java/frc/robot/commands/SwerveDriveCommand.java b/src/main/java/frc/robot/commands/SwerveDriveCommand.java
@@ -96,8 +96,15 @@ public void execute() {
       fasterMode = fasterModeFunction.get();
     }else if(autoAimMode.get() == DriveConstants.kFlexibleAutoAim){
         //Flexible autoaim will allow the driver to move around the field while button is held while locking the rotation and shooter alignments to point towards the speaker
+            if(AprilTagCurrentID == 3){
+              m_LimelightSubsystem.setPriorityID(3);//ID 3 and 7
+            }
+            if(AprilTagCurrentID ==7){
+              m_LimelightSubsystem.setPriorityID(7);//ID 3 and 7
+            }
+
 
-            m_LimelightSubsystem.setPriorityID(3);
+            m_LimelightSubsystem.setPriorityID(3);//ID 3 and 7
             currentTx = m_LimelightSubsystem.getTx();
                 //currentTargetYaw = m_LimelightSubsystem.getCameraPoseYaw();//set turningspeed while it sees tag
                 //need to potentially add something to clear previous yaw if hasent seen tag in a while
@@ -113,6 +120,25 @@ public void execute() {
 
             m_WinchRunToPosition.schedule();
 
+            m_LimelightSubsystem.resetPriorityID();
+    }else if(autoAimMode.get() == DriveConstants.kRigidAutoAim){//This will now be for source autoaim
+            if(AprilTagCurrentID == 1){
+              m_LimelightSubsystem.setPriorityID(1);//ID 1 and 9
+            }
+            if(AprilTagCurrentID ==9){
+              m_LimelightSubsystem.setPriorityID(9);//ID 1 and 9
+            }
+            currentTx = m_LimelightSubsystem.getTx();
+            currentTargetX = m_LimelightSubsystem.getCameraPoseX();
+            currentTargetZ = m_LimelightSubsystem.getCameraPoseZ();
+
+
+            xSpeed = MathUtil.clamp((currentTargetX + DriveConstants.kAutoAimXOffset)/8, -0.3, 0.3); //clamped to 0.5 speed max
+            ySpeed = MathUtil.clamp((currentTargetZ + DriveConstants.kAutoAimZOffset)/8,-0.3,0.3);
+            turningSpeed = MathUtil.clamp((0 - currentTx)/80, -1.0, 1.0); //rotate robot to face tag
+
+            //Winch will be manual for now
+
             m_LimelightSubsystem.resetPriorityID();
     }
     else {