Have Climber

build.gradle

@@ -33,7 +33,7 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                    deleteOldFiles = true // Change to true to delete files on roboRIO that no
                                            // longer exist in deploy directory of this project
                 }
             }

src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back To Shoot Blue"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Depot Auto Blue.auto

@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intaker_down"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "To Depot Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_timelimit"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_timelimit"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot to Shoot Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoAim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoShoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto

@@ -0,0 +1,67 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back To Shoot Blue"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Shoot to Climb Step1 Blue"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step2 Blue"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step3 Blue"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_down"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto

@@ -2,6 +2,10 @@
   "version": "2025.0",
   "command": {
     "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
           "data": {
             "commands": [
               {
@@ -10,48 +14,45 @@
                   "pathName": "Back To Shoot Red"
                 }
               },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climber_init"
+                }
+              }
+            ]
+          }
+        },
         {
           "type": "named",
           "data": {
             "name": "shoot"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "Shoot to Climb Step1 Red"
           }
         },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
         {
           "type": "named",
           "data": {
             "name": "climb_up"
           }
         },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
         {
           "type": "path",
           "data": {
             "pathName": "Climb Step2 Red"
           }
         },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
         {
           "type": "path",
           "data": {
@@ -61,7 +62,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 2.0
+            "waitTime": 0.25
           }
         },
         {

src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto

@@ -0,0 +1,158 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Back To Shoot Blue"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climber_init"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot to Depot Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "blue depot go 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "reset_backboard0"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto

@@ -0,0 +1,139 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Back To Shoot Red"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot to Depot Red"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_timelimit"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_timelimit"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake Red"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot to Shoot Red"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoAim"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto

@@ -19,6 +19,12 @@
                 "data": {
                   "name": "intaker_down"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climber_init"
+                }
               }
             ]
           }
@@ -29,6 +35,12 @@
             "name": "shoot"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -38,12 +50,6 @@
                 "data": {
                   "pathName": "Shoot to Depot Red"
                 }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "start_intake_timelimit"
-                }
               }
             ]
           }
@@ -52,12 +58,6 @@
           "type": "parallel",
           "data": {
             "commands": [
-              {
-                "type": "named",
-                "data": {
-                  "name": "start_intake_timelimit"
-                }
-              },
               {
                 "type": "path",
                 "data": {
@@ -74,13 +74,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "intaker_up"
+                  "name": "AutoAim"
-                }
-              },
-              {
-                "type": "path",
-                "data": {
-                  "pathName": "Depot to Shoot Red"
                 }
               }
             ]
@@ -89,7 +83,32 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot"
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
           }
         }
       ]

src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto

@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Back To Shoot Blue"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climber_init"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot to Depot Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake Blue"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot To Climb Blue"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step2 Blue"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step3 Blue"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto

@@ -0,0 +1,157 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Back To Shoot Red"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climber_init"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot to Depot Red"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake Red"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot To Climb Red"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step2 Red"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step3 Red"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_down"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test of individual command Blue.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": false,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test of individual command.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": false,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Climb Step2 Red.path

@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 15.9,
-        "y": 3.365
+        "y": 3.465
       },
       "prevControl": null,
       "nextControl": {
         "x": 15.522355156858092,
-        "y": 3.365
+        "y": 3.465
       },
       "isLocked": false,
       "linkedName": null
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 15.517,
-        "y": 3.365
+        "y": 3.465
       },
       "prevControl": {
         "x": 15.77785583456356,
-        "y": 3.365
+        "y": 3.465
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Climb Step3 Blue.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.3229961597714217,
+        "x": 1.323,
-        "y": 4.702614323435373
+        "y": 4.704
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 4.604
       },
       "prevControl": {
-        "x": 0.7939837109919805,
+        "x": 0.7925211774538647,
-        "y": 4.592976954720596
+        "y": 4.604
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": "Blue Middle",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Climb Step3 Red.path

@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 15.517,
-        "y": 3.365
+        "y": 3.465
       },
       "prevControl": null,
       "nextControl": {
         "x": 15.767,
-        "y": 3.365
+        "y": 3.465
       },
       "isLocked": false,
       "linkedName": null
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 15.517,
-        "y": 3.465
+        "y": 3.565
       },
       "prevControl": {
         "x": 15.239842716853756,
-        "y": 3.465
+        "y": 3.565
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path

@@ -0,0 +1,63 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 5.969
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.9200056277857709,
+        "y": 5.902393157337589
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.485927152317881,
+        "y": 5.276448675496688
+      },
+      "prevControl": {
+        "x": 5.220827814571248,
+        "y": 5.6757450331143255
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.4,
+      "rotationDegrees": 135.0
+    },
+    {
+      "waypointRelativePos": 0.68,
+      "rotationDegrees": 135.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Blue Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Depot Intake Blue.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.5191395924055082,
+        "x": 1.39,
-        "y": 5.975819656893259
+        "y": 5.969
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 0.54,
+        "x": 0.64,
         "y": 5.969
       },
       "prevControl": {
-        "x": 0.2904111750287428,
+        "x": 0.39,
-        "y": 5.954667573496888
+        "y": 5.969
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.5,
+    "maxVelocity": 0.25,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 0.5,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Depot Intake Red.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 16.0,
+        "x": 15.9,
         "y": 2.1
       },
       "prevControl": {
-        "x": 15.75,
+        "x": 15.65,
         "y": 2.1
       },
       "nextControl": null,

src/main/deploy/pathplanner/paths/Depot To Climb Blue.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 5.97
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.6399999999999998,
+        "y": 4.9700000000000015
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 4.705
+      },
+      "prevControl": {
+        "x": 0.6400000000000002,
+        "y": 5.705
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Depot To Climb Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.9,
+        "y": 2.1
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.9,
+        "y": 3.0999999999999983
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.9,
+        "y": 3.465
+      },
+      "prevControl": {
+        "x": 15.9,
+        "y": 2.465000000000005
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Depot to Shoot Red.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 16.024211489072027,
+        "x": 16.18921636103803,
-        "y": 1.797481861769637
+        "y": 1.647541846848796
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 4.0
       },
       "prevControl": {
-        "x": 13.313026105373341,
+        "x": 13.247149348869588,
-        "y": 4.165954098256533
+        "y": 4.238251681778358
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5,
+        "y": 2.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8524186062054477,
+        "y": 2.382931901404091
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 3.0
+      },
+      "prevControl": {
+        "x": 1.7628130762195064,
+        "y": 3.0790086272982027
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 13.951314803780512,
+        "x": 14.25,
-        "y": 3.7446544288446453
+        "y": 4.0
       },
       "isLocked": true,
       "linkedName": null
@@ -17,15 +17,15 @@
     {
       "anchor": {
         "x": 14.567,
-        "y": 3.3650694444444444
+        "y": 3.4
       },
       "prevControl": {
-        "x": 14.32971291116739,
+        "x": 14.088847622018482,
-        "y": 3.4437767342805516
+        "y": 3.5461858523613685
       },
       "nextControl": {
-        "x": 14.804287088832611,
+        "x": 14.806076188990758,
-        "y": 3.2863621546083372
+        "y": 3.326907073819316
       },
       "isLocked": true,
       "linkedName": null
@@ -36,8 +36,8 @@
         "y": 3.365
       },
       "prevControl": {
-        "x": 15.593954129696199,
+        "x": 15.572533463420765,
-        "y": 3.387182687738287
+        "y": 3.399470424413628
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 0.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Shoot to Depot Red.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 13.72461981440462,
+        "x": 13.699377937127288,
-        "y": 3.8902460070118927
+        "y": 3.7771427571164287
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 2.1
       },
       "prevControl": {
-        "x": 15.201612527520673,
+        "x": 15.073650526430217,
-        "y": 2.25153433233069
+        "y": 2.4474296685616617
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/To Depot Blue.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.54,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.539999999999999,
+        "y": 4.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 5.97
+      },
+      "prevControl": {
+        "x": 2.1399999999999992,
+        "y": 5.97
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/blue depot go 1.path

@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 5.969
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.678433584781481,
+        "y": 5.9229373186851895
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6943708609271524,
+        "y": 7.381829470198674
+      },
+      "prevControl": {
+        "x": 2.178364289572292,
+        "y": 7.226083943133918
+      },
+      "nextControl": {
+        "x": 3.0531960414062747,
+        "y": 7.490133162361142
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.0,
+        "y": 7.2
+      },
+      "prevControl": {
+        "x": 5.804119228777829,
+        "y": 7.355340669064503
+      },
+      "nextControl": {
+        "x": 6.195880771222171,
+        "y": 7.044659330935497
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.195529801324503,
+        "y": 5.969
+      },
+      "prevControl": {
+        "x": 7.354483593458411,
+        "y": 5.7760396621938845
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 1.7,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 135.0
+  },
+  "reversed": false,
+  "folder": "Blue Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -7,7 +7,8 @@
     "Red Middle"
   ],
   "autoFolders": [
-    "Red Middle"
+    "Red Middle",
+    "Blue Middle"
   ],
   "defaultMaxVel": 1.5,
   "defaultMaxAccel": 3.0,

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

@@ -17,7 +17,8 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed =25;
+        public static final double conveyorSpeed = 25;
+        public static final double conveyorSpeedForAuto = 10;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
             .withKV(0.099).withKA(0.0).withKS(0.0);
@@ -135,7 +136,7 @@ public final class Constants {
         .withKV(0.00).withKA(0.00).withKS(0.00);
     //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
 
-    public static final double IntakeGetterSpeedforAuto = 0.4;
+    public static final double IntakeGetterSpeedforAuto = 0.5;
     // 单位RPS
     public static final double positionToleranceRotations = 0.00;
     //rotater容差

src/main/java/frc/robot/Robot.java

@@ -50,7 +50,9 @@ public class Robot extends TimedRobot {
     }
 
     @Override
-    public void autonomousPeriodic() {}
+    public void autonomousPeriodic() {
+        m_robotContainer.driveControls.pushDistanceData();
+    }
 
     @Override
     public void autonomousExit() {}

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

@@ -58,7 +58,7 @@ public class RobotContainer {
   /*     控制封装/日志        */
   /* ====================== */
 
-  private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
+  public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
   private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
   private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
   private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
@@ -82,7 +82,6 @@ public class RobotContainer {
     configueClimber();
     bingdingLED();
     configueIntaker();
-    configureBindings();
     Autocommand.preNameCommands(
         climberSubsystem,
         IntakerSubsystem,
@@ -134,6 +133,12 @@ public class RobotContainer {
     // Start：强行使用metaTag2全场定位
     controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
 
+    controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+                                                      System.out.println("Try using shooter for metatag2");
+                                                    }));
+    controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+                                                      System.out.println("Try using intaker for metatag2");
+                                                      }));
     //LM 按下时autoaimming，松开时alltelop
     // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
     // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
@@ -169,8 +174,7 @@ public class RobotContainer {
     controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
     controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
     controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
-    controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem));
+    
-    controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem));
   }
   /* ====================== */
   /*        LED 绑定          */
@@ -202,19 +206,6 @@ public class RobotContainer {
     controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
   }
 
-  private void configureBindings() {
-        teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand());
-    }
-
-  private Command handleClimberAtTeleopStartCommand() {
-        return new InstantCommand(() -> {
-            if (climberSubsystem.wasAutoClimbUsedInAuto()) {
-                climberSubsystem.climbUp();
-                climberSubsystem.clearAutoClimbUsedInAutoFlag();
-            }
-        }, climberSubsystem);
-    }
-
   public ClimberSubsystem getClimberSubsystem() {
       return climberSubsystem;
   }

src/main/java/frc/robot/controls/Autocommand.java

@@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.RobotStatusManager;
 import frc.robot.subsystems.IntakerSubsystem;
-import frc.robot.subsystems.LimelightSupplier;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.Constants;
@@ -34,16 +33,19 @@ import frc.robot.Constants.RobotStatus;
         NamedCommands.registerCommand("climb_up", climb_up(climber));
         NamedCommands.registerCommand("climb_down", climb_down(climber));
         NamedCommands.registerCommand("start_intake", start_intake(intaker));
+        NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter));
+        NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter));
         NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
         NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
         NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
         NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
-        NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
+        //NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
         NamedCommands.registerCommand("shoot", shoot(shooter));
         NamedCommands.registerCommand("Shake", shake(driveControls));
         NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
         NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
         NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
+        NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
 
     }
 
@@ -58,12 +60,9 @@ import frc.robot.Constants.RobotStatus;
             }, climber));
         }
         public static Command climb_down(ClimberSubsystem climber) {
-            return new InstantCommand(() -> {
+            return new RunCommand(() -> {
-                climber.markAutoClimbUsedInAuto();   // 标记自动攀爬已使用
-            }, climber)
-            .andThen(new RunCommand(() -> {
                 climber.climbDown();  // 控制爬升器下降
-            }, climber).withTimeout(2.0))
+            }, climber).withTimeout(2.0)
             .andThen(new InstantCommand(() -> {
                 climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
                 climber.holdPosition();         // 停止并锁定当前爬升器位置
@@ -71,51 +70,70 @@ import frc.robot.Constants.RobotStatus;
         }
 
         public static Command start_intake(IntakerSubsystem intaker) {
-            return new RunCommand(() -> {
+            return new InstantCommand(() -> {
                 intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 启动吸球
             }, intaker);
         }
+        public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
+            return new InstantCommand(() -> {
+                shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto);
+                intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 启动吸球
+                
+            }, intaker, shooter);
+        }
+        public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
+            return new InstantCommand(() -> {
+                intaker.setIntakerGetterSpeed(0);  // 启动吸球
+                shooter.setConveyorSpeedByRPS(0);
+            }, intaker, shooter);
+        }
         public static Command start_intake_timelimit(IntakerSubsystem intaker) {
             return new RunCommand(() -> {
                 intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto);  // 吸球机构下降
-            }, intaker).withTimeout(3).andThen(new InstantCommand(() -> {
+            }, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
                 intaker.setIntakerGetterSpeed(0);  // 停止旋转
             }, intaker));
         }
         public static Command stop_intake(IntakerSubsystem intaker) {
-            return new InstantCommand(() -> {
+            return new RunCommand(() -> {
                 intaker.setIntakerGetterSpeed(0);  // 停止吸球
             }, intaker);
         }
         public static Command intaker_down(IntakerSubsystem intaker) {
             return new RunCommand(() -> {
                 intaker.IntakerDownNonStop();   // 吸球机构下降
-            }, intaker).withTimeout(1.0).andThen(new InstantCommand(() -> {
+            }, intaker).withTimeout(0.6)
-                intaker.stopIntakerotater();  // 停止旋转
+
+            .andThen(new RunCommand(() -> {
+                intaker.IntakerUpNonStop();     // 吸球机构上升
+            }, intaker).withTimeout(0.1))
+
+            .andThen(new InstantCommand(() -> {
+                intaker.stopIntakerotater();    // 停止
             }, intaker));
         }
         public static Command intaker_up(IntakerSubsystem intaker) {
-            return new InstantCommand(() -> {
+            return new RunCommand(() -> {
                 intaker.IntakerUpNonStop();  // 吸球机构上升
-            }, intaker).withTimeout(0.8).andThen(new InstantCommand(() -> {
+            }, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
-                intaker.stopIntakerotater();;  // 停止旋转
+                intaker.stopIntakerotater();  // 停止旋转
             }, intaker));
         }
-        public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
+        // public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
-        return Commands.sequence(
+        // return Commands.sequence(
-            new InstantCommand(() -> {
+        //     new InstantCommand(() -> {
-                LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+        //         LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
-            }),
+        //     }),
-            Commands.runEnd(
+        //     Commands.runEnd(
-                () -> {
+        //         () -> {
-                    drivetrain.driveToAprilTag();
+        //             drivetrain.driveToAprilTag();
-                },
+        //         },
-                () -> {
+        //         () -> {
-                    LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+        //             LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
-                },
+        //         },
-                drivetrain
+        //         drivetrain
-            ).until(() -> drivetrain.isAlignedToAprilTag()));
+        //     ).until(() -> drivetrain.isAlignedToAprilTag()));
-        }
+        // }
 
         public static Command shoot(ShooterSubsystem shooter) {
             return new RunCommand(() -> {
@@ -126,7 +144,7 @@ import frc.robot.Constants.RobotStatus;
                     }else{
                         shooter.outputBackboard();
                     }  
-            }, shooter).withTimeout(1.0).andThen(new RunCommand(() -> {
+            }, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
                 shooter.setFlywheelSpeedByRPS(64);
                 shooter.setBackboardPosition(175);
                 if(shooter.isBackboardAtTarget()){
@@ -135,7 +153,7 @@ import frc.robot.Constants.RobotStatus;
                         shooter.outputBackboard();
                     }  
                 shooter.setConveyorSpeedByRPS(25);
-            }, shooter).withTimeout(3.0).andThen(new InstantCommand(() -> {
+            }, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
                 shooter.setFlywheelSpeedByRPS(0);  // 停止射击
                 //shooter.setBackboardPosition(0);  // 恢复背板位置
                 shooter.setConveyorSpeedByRPS(0);  // 停止输送
@@ -148,6 +166,9 @@ import frc.robot.Constants.RobotStatus;
         public static Command autoShoot(ShooterControls shooterControl) {
             return shooterControl.autoShootToHubCommand();
         }
+        public static Command resetBackboard0Command(ShooterControls shooterControl) {
+            return shooterControl.resetBackboard0Command();
+        }
 
         public static Command shake(DriveControls driveControls) {
             return driveControls.shakeCommand();

src/main/java/frc/robot/controls/ClimberControls.java

@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.subsystems.ClimberSubsystem;
-import frc.robot.subsystems.LimelightSupplier;
+
 
 public class ClimberControls {
 

src/main/java/frc/robot/controls/DriveControls.java

@@ -20,7 +20,6 @@ import frc.robot.Constants.DriveMode;
 import frc.robot.Constants.RobotStatus;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.subsystems.LimelightSupplier;
 
 import java.util.Optional;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -138,12 +137,12 @@ public class DriveControls {
                 vomega = 0;
                 break;
             case PassingBall:
-                LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
                 vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                 break;
             case AutoAimming:
-                LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
                 vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                 vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
@@ -153,7 +152,7 @@ public class DriveControls {
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
             case Climbing:
-                alignToClimb();
+                // alignToClimb();
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -406,16 +405,16 @@ public class DriveControls {
     public Command getAllianceColorCommand() {
         return Commands.runOnce(this::setTeamColors);
     }
-    public void alignToClimb(){
+    // public void alignToClimb(){
-        LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+    //     LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
-        if(!drivetrain.isAlignedToAprilTag()){
+    //     if(!drivetrain.isAlignedToAprilTag()){
-            drivetrain.driveToAprilTag();
+    //         drivetrain.driveToAprilTag();
-        }
+    //     }
-        else{
+    //     else{
-            LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+    //         LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
-            drivetrain.stop();
+    //         drivetrain.stop();
-        }
+    //     }
-    }
+    // }
     public Command autoAimCommand() {
         return drivetrain.applyRequest(() -> {
         double[] distanceAndRotation = calculateDistanceAndRotationToHub();
@@ -438,22 +437,54 @@ public class DriveControls {
         new InstantCommand(timer::restart),
         drivetrain.applyRequest(() -> {
             double t = timer.get();
-                boolean positive = ((int) (t / switchPeriod)) % 2 == 0;
+            int phase = ((int) (t / switchPeriod)) % 4;
-                double vy = positive ? amplitude : -amplitude;
+
+            double vx = 0.0;
+            double vy = 0.0;
+            double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+            double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
+
+
+            switch (phase) {
+                case 0:
+                    vx = amplitude;   // 前
+                    vy = 0.0;
+                    break;
+                case 1:
+                    vx = 0.0;
+                    vy = amplitude;   // 右
+                    break;
+                case 2:
+                    vx = -amplitude;  // 后
+                    vy = 0.0;
+                    break;
+                case 3:
+                    vx = 0.0;
+                    vy = -amplitude;  // 左
+                    break;
+            }
+
             return robotCentric
-                    .withVelocityX(0.0)
+                .withVelocityX(vx)
                 .withVelocityY(vy)
-                    .withRotationalRate(0.0);
+                .withRotationalRate(vomega);
         }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
         new InstantCommand(() -> {
             timer.stop();
-                autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0);
             drivetrain.stop();
         }, drivetrain)
     );
-    }
+}
+
         public Command shakeCommand() {
-        return shakeCommand(0.25, 0.2, 3.0);
+            return shakeCommand(0.3, 0.15, 5.0);
+        }
+
+    public void pushDistanceData(){
+        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        // 将距离值写入 NetworkTable
+        autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
+        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
     }
     
 }

src/main/java/frc/robot/controls/IntakerControls.java

@@ -73,12 +73,10 @@ public class IntakerControls {
     public Command defaultIntakerCommand() {
         return Commands.run(() -> {
         double getterspeed = 0;
-        double conveyoyspeedforintake = 0;
         double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
         
         if (controller.getLeftTriggerAxis() > 0.1) {
-            getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3;
+            getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
-            ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
             }
 
         IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
@@ -87,9 +85,6 @@ public class IntakerControls {
                     IntakerSubsystem.IntakerUpNonStop();
             } else if(controller.getLeftY() > 0.5){
                     IntakerSubsystem.IntakerDownNonStop();
-                    // double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2;
-                    // IntakerSubsystem.setIntakerRotater(rotaterspeed);
-
             } else {
                     IntakerSubsystem.stopIntakerotater();
             }

src/main/java/frc/robot/controls/ShooterControls.java

@@ -47,14 +47,16 @@ public class ShooterControls {
                     }else{
                         shooterSubsystem.outputBackboard();
                     }  
+                    shooterSubsystem.setFlywheelSpeedByRPS(0);
+                    if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
+                    else{shooterSubsystem.setConveyorSpeedByRPS(0);}
                     break;
                 case PassingBall:
                     var xP = shooterSubsystem.getDistanceToPassball();
-                    xP = Math.max(1.2, Math.min(4.5, xP));
+                    xP = Math.max(2.5, Math.min(10.0, xP));
-                    flywheelSpeed = -1.4964026859575532*xP*xP*xP*xP*xP*xP*xP+29.82445640241506*xP*xP*xP*xP*xP*xP-247.71603153159896*xP*xP*xP*xP*xP+1108.8727302184625*xP*xP*xP*xP-2881.517500617481*xP*xP*xP+4332.05363145275*xP*xP-3467.2508337871454*xP+1192.1508062663759;
+                    flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104;
                     flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = +34.5286998307899*xP*xP*xP*xP*xP*xP*xP*xP-788.5788623683884*xP*xP*xP*xP*xP*xP*xP+7658.073303693148*xP*xP*xP*xP*xP*xP-41191.69338972519*xP*xP*xP*xP*xP+133870.54588688645*xP*xP*xP*xP-268559.6987062158*xP*xP*xP+324254.6530324262*xP*xP-215015.48893131423*xP+59977.46545382827;
+                    backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075;
-                    backboardPosition += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;
@@ -63,6 +65,7 @@ public class ShooterControls {
                     if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
+                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -90,6 +93,7 @@ public class ShooterControls {
                     if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
+                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -182,7 +186,7 @@ public class ShooterControls {
                 shooterSubsystem.isBackboardAtTarget()
                 && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
             )
-            .withTimeout(1.0)
+            .withTimeout(0.3)
             .andThen(new RunCommand(() -> {
                 shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
                 shooterSubsystem.setBackboardPosition(backboardTargetPosition);
@@ -191,12 +195,8 @@ public class ShooterControls {
                 } else {
                     shooterSubsystem.outputBackboard();
                 }
-                if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) {
+                shooterSubsystem.setConveyorSpeedByRPS(25);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed);
+            }, shooterSubsystem).withTimeout(5.0))
-                } else {
-                    shooterSubsystem.setConveyorSpeedByRPS(0.0);
-                }
-            }, shooterSubsystem).withTimeout(3.0))
             .andThen(new InstantCommand(() -> {
                 shooterSubsystem.setFlywheelSpeedByRPS(0.0);
                 shooterSubsystem.setConveyorSpeedByRPS(0.0);
@@ -204,4 +204,22 @@ public class ShooterControls {
             }, shooterSubsystem));
         }, Set.of(shooterSubsystem));
     }
+    
+    public Command resetBackboard0Command() {
+        return new RunCommand(() -> {
+            shooterSubsystem.setBackboardPosition(0);
+
+            if (shooterSubsystem.isBackboardAtTarget()) {
+                shooterSubsystem.backboardMotor.set(0);
+            } else {
+                shooterSubsystem.outputBackboard();
+            }
+
+        }, shooterSubsystem)
+        .until(() -> shooterSubsystem.isBackboardAtTarget())
+        .andThen(new InstantCommand(() -> {
+            shooterSubsystem.backboardMotor.set(0);
+        }));
+    }
 }
+

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -114,18 +114,6 @@ public class ClimberSubsystem extends SubsystemBase {
         stopMotor();
     }
 
-    public void markAutoClimbUsedInAuto() {
-        autoClimbUsedInAuto = true;
-    }
-
-    public boolean wasAutoClimbUsedInAuto() {
-        return autoClimbUsedInAuto;
-    }
-
-    public void clearAutoClimbUsedInAutoFlag() {
-        autoClimbUsedInAuto = false;
-    }
-
     public void stopAll() {
         climberMotor.set(0);
     }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -173,6 +173,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
     }
     
+    public void forceUsingLimelightmt2WithllName(String llString) {
+        LimelightHelpers.PoseEstimate mt2 =
+        LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
+
+        if (mt2 == null || mt2.tagCount == 0) {
+            return;
+        }
+
+        var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
+        addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
+    }
     private void fuseLimelightmt2() {
         Pose2d current = getState().Pose;
 
@@ -276,10 +287,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
         // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
         //Pose2d current = getState().Pose;
-        if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
+        // if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
-            //System.out.println("NoUsingMetaTag2ForTooChangingPose");
+        //     //System.out.println("NoUsingMetaTag2ForTooChangingPose");
-            return false;
+        //     return false;
-        }
+        // }
 
         return true;
     }
@@ -287,128 +298,128 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         return Math.max(min, Math.min(max, value));
     }
 
-    public String driveToAprilTag() {
+//     public String driveToAprilTag() {
-        System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
+//         System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
-        System.out.println("TV = " + LimelightSupplier.isTargetVisible());
+//         System.out.println("TV = " + LimelightSupplier.isTargetVisible());
 
-    if (!LimelightSupplier.isTargetVisible()) {
+//     if (!LimelightSupplier.isTargetVisible()) {
-        isAlignedToAprilTag = false;
+//         isAlignedToAprilTag = false;
-        //stop();
+//         //stop();
-        return "No Target";
+//         return "No Target";
-    }
+//     }
 
-    RawFiducial[] fiducials =
+//     RawFiducial[] fiducials =
-        LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+//         LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
 
-    boolean foundDesiredTag = false;
+//     boolean foundDesiredTag = false;
-    for (RawFiducial fiducial : fiducials) {
+//     for (RawFiducial fiducial : fiducials) {
-        if (fiducial.id == 16 || fiducial.id == 32) {
+//         if (fiducial.id == 16 || fiducial.id == 32) {
-            foundDesiredTag = true;
+//             foundDesiredTag = true;
-            break;
+//             break;
-        }
+//         }
-    }
+//     }
 
-    if (!foundDesiredTag) {
+//     if (!foundDesiredTag) {
-        isAlignedToAprilTag = false;
+//         isAlignedToAprilTag = false;
-        stop();
+//         stop();
-        return "No Desired Tag";
+//         return "No Desired Tag";
-    }
+//     }
 
-    // ===== 读取原始误差 =====
+//     // ===== 读取原始误差 =====
-    double tx = LimelightSupplier.getTX();                 // deg
+//     double tx = LimelightSupplier.getTX();                 // deg
-    double tz = -LimelightSupplier.getTargetTZ();          // m，方向按你当前经验保留负号
+//     double tz = -LimelightSupplier.getTargetTZ();          // m，方向按你当前经验保留负号
-    double ry = LimelightSupplier.getTargetRotationY();    // deg
+//     double ry = LimelightSupplier.getTargetRotationY();    // deg
 
-    // ===== 用原始误差判断是否已对齐 =====
+//     // ===== 用原始误差判断是否已对齐 =====
-    isAlignedToAprilTag =
+//     isAlignedToAprilTag =
-        Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
+//         Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
-        Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
+//         Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
-        Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
+//         Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
 
-    if (isAlignedToAprilTag) {
+//     if (isAlignedToAprilTag) {
-        stop();
+//         stop();
-        return "Aligned";
+//         return "Aligned";
-    }
+//     }
 
-    // ===== 计算输出 =====
+//     // ===== 计算输出 =====
-    double vx = 0.0;
+//     double vx = 0.0;
-    double vy = 0.0;
+//     double vy = 0.0;
-    double omega = 0.0;
+//     double omega = 0.0;
 
-    // 1) 前后控制：根据 tz 前进/后退
+//     // 1) 前后控制：根据 tz 前进/后退
-    if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
+//     if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
-        vx = tz * Constants.Limelight.AutoClimbKpTZ;
+//         vx = tz * Constants.Limelight.AutoClimbKpTZ;
 
-        // 最小输出，避免快到目标时不动
+//         // 最小输出，避免快到目标时不动
-        if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
+//         if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
-            vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
+//             vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
-        }
+//         }
 
-        // 最大输出限幅
+//         // 最大输出限幅
-        vx = clamp(vx,
+//         vx = clamp(vx,
-            -Constants.Limelight.AutoClimbMaxVX,
+//             -Constants.Limelight.AutoClimbMaxVX,
-             Constants.Limelight.AutoClimbMaxVX);
+//              Constants.Limelight.AutoClimbMaxVX);
-    }
+//     }
 
-    // 2) 旋转控制：tx 为主，ry 为辅
+//     // 2) 旋转控制：tx 为主，ry 为辅
-    //    tx 负责“镜头中心对准 tag”
+//     //    tx 负责“镜头中心对准 tag”
-    //    ry 负责“机器人姿态微调”
+//     //    ry 负责“机器人姿态微调”
-    // double omegaFromTx = 0.0;
+//     // double omegaFromTx = 0.0;
-    // double omegaFromRy = 0.0;
+//     // double omegaFromRy = 0.0;
 
-    // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
+//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
-    //     omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
+//     //     omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
-    // }
+//     // }
 
-    // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
+//     // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
-    //     omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
+//     //     omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
-    // }
+//     // }
 
-    // omega = omegaFromTx + omegaFromRy;
+//     // omega = omegaFromTx + omegaFromRy;
 
-    // // 大偏角时直接给较明显的转速，先把朝向拉回来
+//     // // 大偏角时直接给较明显的转速，先把朝向拉回来
-    // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
+//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
-    //     omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
+//     //     omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
-    // }
+//     // }
 
-    // // 最小输出，避免转不动
+//     // // 最小输出，避免转不动
-    // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
+//     // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
-    //     omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
+//     //     omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
-    // }
+//     // }
 
-    // // 最大输出限幅
+//     // // 最大输出限幅
-    // omega = clamp(omega,
+//     // omega = clamp(omega,
-    //     -Constants.Limelight.AutoClimbMaxOmega,
+//     //     -Constants.Limelight.AutoClimbMaxOmega,
-    //      Constants.Limelight.AutoClimbMaxOmega);
+//     //      Constants.Limelight.AutoClimbMaxOmega);
 
-    Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
+//     Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
-    double climbCurrentRad = ClimbcurrentAngle.getRadians();
+//     double climbCurrentRad = ClimbcurrentAngle.getRadians();
-    double TargetAngleforClimbing = Math.toRadians(0);
+//     double TargetAngleforClimbing = Math.toRadians(0);
-    double Climberror = TargetAngleforClimbing - climbCurrentRad;
+//     double Climberror = TargetAngleforClimbing - climbCurrentRad;
-    if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
+//     if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
-            omega = 0.0;
+//             omega = 0.0;
-        } else {
+//         } else {
-            omega = Climberror * Constants.AutoPositioning.TurningkP;
+//             omega = Climberror * Constants.AutoPositioning.TurningkP;
-        }
+//         }
 
 
-    // ===== 调试输出 =====
+//     // ===== 调试输出 =====
-    driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
+//     driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
-    driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
+//     driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
-    driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
+//     driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
-    driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
+//     driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
-    driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
+//     driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
-    driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
+//     driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
 
-    // ===== 下发到底盘 =====
+//     // ===== 下发到底盘 =====
-    driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
+//     driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
 
-    return "Driving to AprilTag";
+//     return "Driving to AprilTag";
-}
+// }
 
-    public boolean isAlignedToAprilTag(){
+//     public boolean isAlignedToAprilTag(){
-        return isAlignedToAprilTag;
+//         return isAlignedToAprilTag;
-    }
+//     }
     @Override
     public void periodic() {
 
@@ -420,10 +431,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
-        positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
+        //positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
-        positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
+        // positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
-        positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
+        // positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
-        positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
+        // positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
         positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
         // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
         LimelightHelpers.SetRobotOrientation(

src/main/java/frc/robot/subsystems/IntakerSubsystem.java

@@ -83,26 +83,13 @@ public class IntakerSubsystem extends SubsystemBase{
         stopIntakerotater();
     }
     public void IntakerDownNonStop(){
-        double currentPosition = getIntakerotaterPosition();
+            intakerotater.set(-0.1);
-        if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
-            // move down (negative power) while above the lower limit
-            intakerotater.set(-0.05);
-        } else {
-            // at or below lower limit: stop
-            stopIntakerotater();
-        }
         }
 
     public void IntakerUpNonStop(){
-        double currentPosition = getIntakerotaterPosition();
+            intakerotater.set(0.2);
-        if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
-            // move up while below the upper limit
-            intakerotater.set(0.3);
-        } else {
-            // at or above upper limit: stop
-            stopIntakerotater();
-            }
         }
+
     public void setIntakerRotater(double rotaterspeed){
         intakerotater.set(rotaterspeed);
     }

src/main/java/frc/robot/subsystems/LimelightSupplier.java

@@ -1,108 +0,0 @@
-package frc.robot.subsystems;
-
-import frc.robot.Constants;
-
-import java.lang.annotation.Target;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.subsystems.LimelightHelpers;
-import frc.robot.subsystems.LimelightHelpers.RawFiducial;
-
-public class LimelightSupplier extends SubsystemBase {
-
-    public LimelightSupplier() {
-        setPipeline(8);
-    }
-
-    public static double getTX() {
-        return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-
-    public static double getTY() {
-        return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-
-    public static double getTA() {
-        // Logic to get area from Limelight
-        return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-    
-    public static boolean isTargetVisible() {
-        if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){
-            return false;
-        }
-        // Logic to check if a target is visible
-        return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-    public static int getAprilTagID(){
-        RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-        int id = -1;
-        for (RawFiducial fiducial : fiducials) {
-            id = fiducial.id;                    // 标签ID
-        }   
-        return id;
-    }
-    public static void setPipeline(int pipeline) {
-        if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){
-            LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline);
-        }
-    }
-
-    public static double[] getTargetPose() {
-        // Logic to get the target pose from Limelight
-        return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-
-    public static Pose2d getBotPose2d_wpiBlue(){
-        return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-    }
-    public static double getTargetRotationY(){
-        if(isTargetVisible()){
-            return getTargetPose()[4];
-        }
-        else{
-            return 0.00;
-        }
-    }
-    public static double getTargetTZ(){
-        if(isTargetVisible()){
-            return getTargetPose()[2];
-        }
-        else{
-            return 0.00;
-        }
-    }
-    @Override
-    public void periodic() {
-        SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
-        SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker));
-        if (isTargetVisible()){
-            SmartDashboard.putString("LimelightTargetVisible", "YES");
-            SmartDashboard.putNumber("LimelightTA", getTA());
-            SmartDashboard.putNumber("TargetRY", getTargetRotationY());
-            SmartDashboard.putNumber("TargetTX", getTX());
-            SmartDashboard.putNumber("TargetTZ", getTargetTZ());
-            RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
-            int id = -1;
-            for (RawFiducial fiducial : fiducials) {
-                id = fiducial.id;                    // 标签ID
-            }
-            SmartDashboard.putNumber("LimelightTargetID", id);
-            
-        }
-        else {
-            SmartDashboard.putString("LimelightTargetVisible", "NO");
-            SmartDashboard.putNumber("TargetRY", 0);
-            SmartDashboard.putNumber("TargetTX", 0);
-            SmartDashboard.putNumber("TargetTZ", 0);
-            SmartDashboard.putNumber("LimelightTargetID", -1);
-        }
-        
-    }
-}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;