改个射速

0

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

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

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

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

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

@@ -7,13 +7,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Line"
+            "pathName": "Back To Shoot Red"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
           }
         }
       ]
     }
   },
-  "resetOdom": false,
-  "folder": null,
+  "resetOdom": true,
+  "folder": "Red Middle",
   "choreoAuto": false
 }

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

@@ -0,0 +1,152 @@
+{
+  "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": "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 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": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red 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/Depot Auto Red.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 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": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot to Shoot Red"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

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

@@ -3,10 +3,29 @@
   "command": {
     "type": "sequential",
     "data": {
-      "commands": []
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoAim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoShoot"
+          }
+        }
+      ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "Blue Middle",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/RotateAndLine.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "RotateAndLine"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "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

@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "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": "named",
+          "data": {
+            "name": "climb_up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step2 Red"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step3 Red"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb_down"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

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": "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": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blue depot go 1"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue Middle",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,151 @@
+{
+  "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": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoShoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,120 @@
+{
+  "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"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red Middle",
+  "choreoAuto": false
+}

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

@@ -5,33 +5,27 @@
     "data": {
       "commands": [
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Line"
+            "name": "start_intake"
           }
         },
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.0
+            "waitTime": 2.0
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "LineBack"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
+            "name": "stop_intake"
           }
         }
       ]
     }
   },
   "resetOdom": false,
-  "folder": null,
+  "folder": "Blue Middle",
   "choreoAuto": false
 }

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

@@ -5,15 +5,21 @@
     "data": {
       "commands": [
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Rotate"
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
           }
         }
       ]
     }
   },
   "resetOdom": false,
-  "folder": null,
+  "folder": "Red Middle",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/paths/Back To Shoot Blue.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
-        "y": 6.668017118402283
+        "x": 3.53,
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.299415121255347,
-        "y": 7.128787446504993
+        "x": 3.9033070768298836,
+        "y": 3.9994852751840226
       },
       "isLocked": false,
-      "linkedName": "Point1"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
-        "y": 4.15
+        "x": 3.04,
+        "y": 4.0
       },
       "prevControl": {
-        "x": 14.299415121255347,
-        "y": 4.597831669044222
+        "x": 2.783639997726092,
+        "y": 4.003292975326709
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "StartingPointRed2"
+      "linkedName": null
     }
   ],
   "rotationTargets": [],
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Blue Middle",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Back To Shoot Red.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
-        "y": 4.15
+        "x": 13.01022182786158,
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.299415121255347,
-        "y": 4.61077032810271
+        "x": 13.865906250551847,
+        "y": 4.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
-        "y": 6.668017118402283
+        "x": 13.5,
+        "y": 4.0
       },
       "prevControl": {
-        "x": 14.247660485021397,
-        "y": 6.305734664764623
+        "x": 13.231071353115471,
+        "y": 4.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 4.704
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.9248292506025121,
+        "y": 4.704
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.073,
+        "y": 4.704
+      },
+      "prevControl": {
+        "x": 0.823,
+        "y": 4.704
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 1.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/Climb Step2 Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.9,
+        "y": 3.465
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.522355156858092,
+        "y": 3.465
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.517,
+        "y": 3.465
+      },
+      "prevControl": {
+        "x": 15.77785583456356,
+        "y": 3.465
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 1.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/Climb Step3 Blue.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.073,
+        "y": 4.704
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.323,
+        "y": 4.704
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.073,
+        "y": 4.604
+      },
+      "prevControl": {
+        "x": 0.7925211774538647,
+        "y": 4.604
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.25,
+    "maxAcceleration": 0.5,
+    "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/Climb Step3 Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.517,
+        "y": 3.465
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.767,
+        "y": 3.465
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.517,
+        "y": 3.565
+      },
+      "prevControl": {
+        "x": 15.239842716853756,
+        "y": 3.565
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.25,
+    "maxAcceleration": 0.5,
+    "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/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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 5.969
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.39,
+        "y": 5.969
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 5.969
+      },
+      "prevControl": {
+        "x": 0.39,
+        "y": 5.969
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.25,
+    "maxAcceleration": 0.5,
+    "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 Intake Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.4,
+        "y": 2.1
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.65,
+        "y": 2.1
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.9,
+        "y": 2.1
+      },
+      "prevControl": {
+        "x": 15.65,
+        "y": 2.1
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 3.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 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 Blue.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.84,
+        "y": 5.969
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.2669518600756762,
+        "y": 5.612104897874479
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.04,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.8248463530252574,
+        "y": 4.180015650217846
+      },
+      "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/Depot to Shoot Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.7,
+        "y": 2.1
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 16.18921636103803,
+        "y": 1.647541846848796
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.5,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 13.247149348869588,
+        "y": 4.238251681778358
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.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/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 Blue.path

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.04,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3282933300270625,
+        "y": 3.975244391494451
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.97314,
+        "y": 4.704
+      },
+      "prevControl": {
+        "x": 1.4585920284163918,
+        "y": 4.684160090340553
+      },
+      "nextControl": {
+        "x": 2.2475848605490505,
+        "y": 4.714582028383155
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.64,
+        "y": 4.704
+      },
+      "prevControl": {
+        "x": 0.3610340465991982,
+        "y": 4.704101661030112
+      },
+      "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

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 14.25,
+        "y": 4.0
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 14.567,
+        "y": 3.4
+      },
+      "prevControl": {
+        "x": 14.088847622018482,
+        "y": 3.5461858523613685
+      },
+      "nextControl": {
+        "x": 14.806076188990758,
+        "y": 3.326907073819316
+      },
+      "isLocked": true,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.9,
+        "y": 3.365
+      },
+      "prevControl": {
+        "x": 15.572533463420765,
+        "y": 3.399470424413628
+      },
+      "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": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.04,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.4702773700848932,
+        "y": 3.562911556367572
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 5.969
+      },
+      "prevControl": {
+        "x": 0.8995187790061673,
+        "y": 6.223926270411972
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.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/Shoot to Depot Red.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.699377937127288,
+        "y": 3.7771427571164287
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.400452528841061,
+        "y": 2.1
+      },
+      "prevControl": {
+        "x": 15.073650526430217,
+        "y": 2.4474296685616617
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.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/To Depot Blue.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
-        "y": 4.15
+        "x": 3.54,
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.064,
-        "y": 4.15
+        "x": 2.539999999999999,
+        "y": 4.0
       },
       "isLocked": false,
-      "linkedName": "StartingPointRed1"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
-        "y": 4.15
+        "x": 1.14,
+        "y": 5.97
       },
       "prevControl": {
-        "x": 14.650181888846735,
-        "y": 4.15
+        "x": 2.1399999999999992,
+        "y": 5.97
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,10 +45,10 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Blue Middle",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
-        "y": 4.15
+        "x": 13.0,
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.312353780313838,
-        "y": 4.74015691868759
+        "x": 14.000000000000002,
+        "y": 4.0
       },
       "isLocked": false,
-      "linkedName": "StartingPointRed1"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
-        "y": 6.357489300998574
+        "x": 15.400452528841061,
+        "y": 2.1
       },
       "prevControl": {
-        "x": 14.312353780313838,
-        "y": 6.098716119828817
+        "x": 14.400452528841061,
+        "y": 2.1
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,10 +42,10 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

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

@@ -2,8 +2,14 @@
   "robotWidth": 0.85,
   "robotLength": 0.85,
   "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
+  "pathFolders": [
+    "Blue Middle",
+    "Red Middle"
+  ],
+  "autoFolders": [
+    "Red Middle",
+    "Blue Middle"
+  ],
   "defaultMaxVel": 1.5,
   "defaultMaxAccel": 3.0,
   "defaultMaxAngVel": 360.0,

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

@@ -1,9 +1,23 @@
 package frc.robot;
 
+import javax.net.ssl.TrustManager;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
 
 public final class Constants {
+    public static final class DemoMode {
+        public static final boolean ENABLED = true;
+
+        public static final double normalDriveScale = 0.12;
+        public static final double normalTurnScale = 0.20;
+        public static final double boostDriveScale = 0.25;
+        public static final double boostTurnScale = 0.30;
+
+        public static final double maxLinearSpeedMetersPerSecond = 2.4;
+        public static final double maxAngularRateRadiansPerSecond = 1.6;
+    }
+
     public static final boolean LEDUsing = false;
     public enum RobotStatus{
         Stopped,
@@ -15,43 +29,40 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed = 20;
+        public static final double conveyorSpeed = 80;
+        public static final double conveyorSpeedForAuto = 10;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.0088).withKA(0.0).withKS(0.0);
+            .withKV(0.099).withKA(0.0).withKS(0.0);
 
         public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
-            .withKV(0.02).withKA(0.15).withKS(0);
+            .withKV(0.16).withKA(0.15).withKS(0);
 
         public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
             .withKP(0.01).withKI(0).withKD(0)
-            .withKV(0.008).withKA(0.0).withKS(0.010);
+            .withKV(0.03).withKA(0.0).withKS(0.010);
 
         public class backboardPositionPID {
-            public static final double kP = 0.1;
+            public static final double kP = 0.3;
             public static final double kI = 0.0;
             public static final double kD = 0.0;
             
         }
-        public static final double backboardSpeedMax = 30; //也就是output
+        public static final double backboardSpeedMax = 60; //也就是output
         // 背板的上下限位角度
         public static final double backboardUpLimit = 3000.0;  // 背板上限角度（根据实际需求设置）
         public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
     }
-    public class ShooterCalculation{
-        public static final double positionNear = 0;  // 近距离背板角度
-        public static final double positionMid  = 1600;  // 中距离背板角度
-        public static final double postionFar  = 3200;  // 远距离背板角度
-    }
     public class AutoPositioning {
-        public static final double autoRotationkP = 0.1;
+        public static final double autoRotationkP = 0.08;
         public static final double autoRotationForBumpTargetDegrees = 45;
         public static final double autoRotationForTrenchTargetDegrees = 0;
         public static final boolean teleopOffsetEnabled = true;
         public static final double teleopOffsetkP = 15;
 
         public static final double autoPositioningkP = 1.0;
+        public static final double autoPositioningAngleError = 1.0; // 角度误差容忍度，单位为度
 
         public static final double[] bumpY = {2.51,5.556};
         public static final double[] trenchY = {0.639,7.43};
@@ -81,22 +92,51 @@ public final class Constants {
     public class Limelight {
         public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
         public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
-        public static final Boolean UsingMetaTag2 = true;
+        public static final Boolean UsingMetaTag2 = false; // Use Limelight's own pose/yaw estimate instead of gyro-assisted MegaTag2.
+        public static final int climbPipelineIndex = 8;
+        public static final int locatePipelineIndex = 0;
+
+        // 误差死区 / 对齐判定
+        public static final double AutoClimbToleranceTX = 1.5;   // deg
+        public static final double AutoClimbToleranceTZ = 0.10;  // m
+        public static final double AutoClimbToleranceRY = 2.0;   // deg
+
+        // P 控制系数
+        public static final double AutoClimbKpTX = 0.020;   // deg -> rad/s
+        public static final double AutoClimbKpTZ = 0.90;    // m -> m/s
+        public static final double AutoClimbKpRY = 0.015;   // deg -> rad/s
+
+        // 最小输出（防止快到目标时推不动）
+        public static final double AutoClimbMinVX = 0.18;      // m/s
+        public static final double AutoClimbMinOmega = 0.10;   // rad/s
+
+        // 最大输出（防止冲太猛）
+        public static final double AutoClimbMaxVX = 1.20;      // m/s
+        public static final double AutoClimbMaxOmega = 0.60;   // rad/s
+
+        // 远距离 / 大角度时额外限速，可后续微调
+        public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg
+        public static final double AutoClimbFastTurnOmega = 0.45;       // rad/s
     }
 
     public class Field {
         public static final double RedHubPositionX = 11.914;
         public static final double RedHubPositionY = 4.034;
-        public static final double PassingBallPosX = 0;
-        public static final double PassingBallPosY1 = 2.51;
-        public static final double PassingBallPosY2 = 5.556;
+        public static final double BlueHubPositionX = 4.625;
+        public static final double BlueHubPositionY = 4.034;
+        public static final double PassingBallPosY1 = 1.405;
+        public static final double PassingBallPosY2 = 6.664;
+        public static final double RedPassingBallPosX = 13.355;
+        public static final double BluePassingBallPosX = 3.184;
     }
     
 
     public class Intaker {
 
     public static final double intakeRotaterUpLimit = 0.0;  // rotator上限角度
-    public static final double intakeRotaterDownLimit = 0.0;  // rotator下限角度
+    public static final double intakeRotaterDownLimit = -7.6;  // rotator下限角度
+    public static final double intakeRotaterVelocity = 5; // rotater的速度（如果你用速度控制的话）
+    public static final double IntakeRotaterGravityFF = 0.0; // 重力前馈增益（需要实测调整）
 
     public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
         .withKP(0.00).withKI(0.00).withKD(0.00)
@@ -108,10 +148,11 @@ public final class Constants {
         .withKV(0.00).withKA(0.00).withKS(0.00);
     //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
 
-    public static final double IntakeGetterSpeed = 0.0;
+    public static final double IntakeGetterSpeedforAuto = 0.5;
     // 单位RPS
     public static final double positionToleranceRotations = 0.00;
     //rotater容差
+
     }   
     public static class Climber {
 
@@ -119,8 +160,10 @@ public final class Constants {
 
     public static final boolean inverted = false;
 
-    public static final double climbUpTicks = 94; // TODO: 实测后调整
-    public static final double climbDownTicks = 0; // TODO: 实测后调整
+    public static double climbUpTicks = 94; // TODO: 实测后调整
+    public static double climbDownTicks = 0; // TODO: 实测后调整
+    public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底
+    public static final double climbVelocity = -200;
     public static final double holdToleranceRot = 0.5; 
     public static final double deltaDownTicks = 1;
 
@@ -138,4 +181,4 @@ public final class Constants {
         slot0Configs.kA = 0.0;
     }
 }
-}
+}

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

@@ -3,9 +3,12 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.ShooterSubsystem;
 
 public class Robot extends TimedRobot {
     private Command m_autonomousCommand;
@@ -24,7 +27,12 @@ public class Robot extends TimedRobot {
     }
 
     @Override
-    public void disabledInit() {}
+    public void disabledInit() {
+        System.out.println("Robot disabledInit called");
+        m_robotContainer.shooterSubsystem.stopAll();
+        // m_robotContainer.climberSubsystem.stopAll();
+        m_robotContainer.IntakerSubsystem.stopAll();
+    }
 
     @Override
     public void disabledPeriodic() {}
@@ -42,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

@@ -9,14 +9,18 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.RobotStatus;
 import frc.robot.controls.DriveControls;
+import frc.robot.controls.IntakerControls;
 import frc.robot.controls.ShooterControls;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.IntakerSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.tuning.ConfigTalonFXMotorTuner;
@@ -24,6 +28,7 @@ import frc.robot.tuning.ConfigTalonFXSMotorTuner;
 import frc.robot.tuning.DriveGainsTuner;
 import frc.robot.utils.LedBindings;
 import frc.robot.controls.ClimberControls;
+import frc.robot.controls.Autocommand;
 import frc.robot.subsystems.ClimberSubsystem;
 public class RobotContainer {
 
@@ -31,8 +36,9 @@ public class RobotContainer {
   /*         手柄            */
   /* ====================== */
 
-  // 只使用一个 Xbox 手柄（端口 0）
-  private final CommandXboxController controller = new CommandXboxController(0);
+  // Demo mode uses one Xbox controller on port 0.
+  private final CommandXboxController controllerlower = new CommandXboxController(0);
+  private final CommandXboxController controllerupper = controllerlower;
 
   /* ====================== */
   /*         子系统           */
@@ -40,8 +46,9 @@ public class RobotContainer {
 
   // Phoenix TunerX 生成的底盘
   public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-  public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
+  //public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
   public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+  public final IntakerSubsystem IntakerSubsystem = new IntakerSubsystem();
 
   // LED 子系统
   private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72         
@@ -51,9 +58,10 @@ public class RobotContainer {
   /*     控制封装/日志        */
   /* ====================== */
 
-  private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager );
-  private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager);
-  private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller);
+  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);
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
   private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
   private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
@@ -63,15 +71,36 @@ public class RobotContainer {
   
   private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
 
+
   
   public RobotContainer() {
+
+    Pose2d startingPose = Constants.StartingPoints.Red.Point2;
+    drivetrain.resetPose(startingPose);
+    configueSwerve();
+    configueShooter();
+    // configueClimber();
+    bingdingLED();
+    configueIntaker();
+    // Autocommand.preNameCommands(
+    //     climberSubsystem,
+    //     IntakerSubsystem,
+    //     drivetrain,
+    //     shooterSubsystem,
+    //     driveControls,
+    //     shooterControls
+    // );
+
+    Autocommand.preNameCommands(
+        IntakerSubsystem,
+        drivetrain,
+        shooterSubsystem,
+        driveControls,
+        shooterControls
+    );
     SignalLogger.enableAutoLogging(false);
     autoChooser = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", autoChooser);
-    configueSwerve();
-    configueShooter();
-    configueClimber();
-    bingdingLED();
   }
 
   /* ====================== */
@@ -79,6 +108,7 @@ public class RobotContainer {
   /* ====================== */
 
   private void configueSwerve() {
+    enableTrigger.onTrue(driveControls.getAllianceColorCommand());
     Pose2d startingPose = Constants.StartingPoints.Red.Point2;
     drivetrain.resetPose(startingPose);
 
@@ -94,53 +124,84 @@ public class RobotContainer {
     // Disabled 时进入 idle（防止模块乱动）
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
+    if (!Constants.DemoMode.ENABLED) {
+
     
-    // //左保险：按住刹车
-    // controller.leftBumper()
-    //   .onTrue(Commands.runOnce(driveControls::emergencyStop))
-    //   .whileTrue(driveControls.brakeWhileHeld());
+    //左保险：按住刹车
+    controllerlower.leftBumper()
+      .onTrue(Commands.runOnce(driveControls::emergencyStop))
+      .whileTrue(driveControls.brakeWhileHeld());
     
-    // // 右保险：按住加速（Boost）
-    // controller.rightBumper()
-    //     .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
-    //     .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
+    // 右保险：按住加速（Boost）
+    controllerlower.rightBumper()
+        .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
+        .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
 
     // Back：重置场地坐标系角度
-    controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
+    controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
     
     // Start：强行使用metaTag2全场定位
-    controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
+    controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
 
-    //LM 按下时使用机器人坐标系
-    controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
-    controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    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));
 
-    controller.rightStick().and(controller.leftBumper())
-    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
-    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-    
-    controller.rightStick().and(controller.rightBumper())
+    //双操均可触发autoaimming模式，松开时alltelop
+    Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick());
+    autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    //双操均可触发passingball模式，松开时alltelop
+    Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick());
+    autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
+    autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    controllerlower.leftTrigger()
     .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
     .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    
+    controllerlower.rightTrigger()
+    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
+    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+
+    controllerlower.a()
+    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
+    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    }
   }
 
   private void configueShooter(){
     shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
-    controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
-    controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
-    controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
-    controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
+    controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
+    controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
+    controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
+    controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
+    controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
+    if (Constants.DemoMode.ENABLED) {
+      controllerupper.rightTrigger()
+          .onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming))
+          .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    } else {
+      controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
+    }
+    
   }
-  /* ====================== */
-  /*        LED 绑定          */
-  /* ====================== */
 
   private void bingdingLED() {
     LedBindings.bindModeIndicators(leds);
   }
   
   public Command getAutonomousCommand() {
-
+    if (Constants.DemoMode.ENABLED) {
+      return Commands.none();
+    }
     return autoChooser.getSelected();
   }
 
@@ -152,11 +213,17 @@ public class RobotContainer {
     shooterSubsystem.setRightFollowLeft();
   }
 
-  private void configueClimber() {
-    
-    enableTrigger.onTrue(climberControls.enableInitCommand());
+  // private void configueClimber() {
+  //   climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
+  // }
 
-    climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
+  private void configueIntaker() {
+    IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
+    //controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
   }
 
+  // public ClimberSubsystem getClimberSubsystem() {
+  //     return climberSubsystem;
+  // }
+
 }

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

@@ -0,0 +1,189 @@
+package frc.robot.controls;
+
+import com.pathplanner.lib.auto.NamedCommands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+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.ShooterSubsystem;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.Constants;
+import frc.robot.Constants.RobotStatus;
+
+    public class Autocommand extends SequentialCommandGroup{
+
+        public static void preNameCommands(
+            // ClimberSubsystem climber,
+            IntakerSubsystem intaker,
+            CommandSwerveDrivetrain drivetrain,
+            ShooterSubsystem shooter,
+            DriveControls driveControls,
+            ShooterControls shooterControls
+    ) {
+        // 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("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));
+
+    }
+
+        public static Command climb_up(ClimberSubsystem climber) {
+            return new RunCommand(() -> {
+                climber.climbUp();  // 控制爬升器上升
+            }, climber).withTimeout(2).andThen(new RunCommand(() -> {
+                climber.climbUpNonStopforAuto();
+            }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
+                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
+                climber.holdPosition();  // 停止并锁定当前爬升器位置
+            }, climber));
+        }
+        public static Command climb_down(ClimberSubsystem climber) {
+            return new RunCommand(() -> {
+                climber.climbDown();  // 控制爬升器下降
+            }, climber).withTimeout(2.0)
+            .andThen(new InstantCommand(() -> {
+                climber.captureHoldPosition();  // 捕获当前爬升器位置以保持
+                climber.holdPosition();         // 停止并锁定当前爬升器位置
+            }, climber));
+        }
+
+        public static Command start_intake(IntakerSubsystem intaker) {
+            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(1.5).andThen(new InstantCommand(() -> {
+                intaker.setIntakerGetterSpeed(0);  // 停止旋转
+            }, intaker));
+        }
+        public static Command stop_intake(IntakerSubsystem intaker) {
+            return new RunCommand(() -> {
+                intaker.setIntakerGetterSpeed(0);  // 停止吸球
+            }, intaker);
+        }
+        public static Command intaker_down(IntakerSubsystem intaker) {
+            return new RunCommand(() -> {
+                intaker.IntakerDownNonStop();   // 吸球机构下降
+            }, intaker).withTimeout(0.6)
+
+            .andThen(new RunCommand(() -> {
+                intaker.IntakerUpNonStop();     // 吸球机构上升
+            }, intaker).withTimeout(0.1))
+
+            .andThen(new InstantCommand(() -> {
+                intaker.stopIntakerotater();    // 停止
+            }, intaker));
+        }
+        public static Command intaker_up(IntakerSubsystem intaker) {
+            return new RunCommand(() -> {
+                intaker.IntakerUpNonStop();  // 吸球机构上升
+            }, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
+                intaker.stopIntakerotater();  // 停止旋转
+            }, intaker));
+        }
+        // public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
+        // return Commands.sequence(
+        //     new InstantCommand(() -> {
+        //         LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+        //     }),
+        //     Commands.runEnd(
+        //         () -> {
+        //             drivetrain.driveToAprilTag();
+        //         },
+        //         () -> {
+        //             LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+        //         },
+        //         drivetrain
+        //     ).until(() -> drivetrain.isAlignedToAprilTag()));
+        // }
+
+        public static Command shoot(ShooterSubsystem shooter) {
+            return new RunCommand(() -> {
+                shooter.setFlywheelSpeedByRPS(64);
+                shooter.setBackboardPosition(175);  // 设置自动射击转速
+                if(shooter.isBackboardAtTarget()){
+                        shooter.backboardMotor.set(0);
+                    }else{
+                        shooter.outputBackboard();
+                    }  
+            }, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
+                shooter.setFlywheelSpeedByRPS(64);
+                shooter.setBackboardPosition(175);
+                if(shooter.isBackboardAtTarget()){
+                        shooter.backboardMotor.set(0);
+                    }else{
+                        shooter.outputBackboard();
+                    }  
+                shooter.setConveyorSpeedByRPS(25);
+            }, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
+                shooter.setFlywheelSpeedByRPS(0);  // 停止射击
+                //shooter.setBackboardPosition(0);  // 恢复背板位置
+                shooter.setConveyorSpeedByRPS(0);  // 停止输送
+            }, shooter)));
+        }
+        public static Command autoAim(DriveControls driveControls) {
+            return driveControls.autoAimCommand();
+        }
+
+        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();
+        }
+
+        public static Command climber_Zeroencoder(ClimberSubsystem climber) {
+            return climber.initCommand();
+        }
+
+
+
+    }
+
+
+
+
+
+

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

@@ -1,12 +1,17 @@
 package frc.robot.controls;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants;
 import frc.robot.subsystems.ClimberSubsystem;
 
+
 public class ClimberControls {
 
     private final ClimberSubsystem climber;
@@ -23,14 +28,14 @@ public class ClimberControls {
     public Command defaultClimberCommand() {
         return Commands.run(()->{
             //climber.climbUp();
-                // 优先级：如果同时按，优先上升（你也可以反过来）
-                if(controller.y().getAsBoolean()&&controller.x().getAsBoolean()){
-                    climber.zeroEncoder();
-                }
-                else if (controller.y().getAsBoolean()) {
+                if(controller.getRightY() < -0.5){
+                    climber.climbUpNonStop();
+                } else if(controller.getRightY() > 0.5){
+                    climber.climbDownNonStop();
+                } else if (controller.y().getAsBoolean()) {
                     climber.climbUp();
                 } else if (controller.x().getAsBoolean()) {
-                    climber.climbDownNonStop();
+                    climber.climbDown();
                 } else {
                     // 松开：记录当前位置 + 锁死
                     climber.captureHoldPosition();
@@ -45,9 +50,8 @@ public class ClimberControls {
 
     }
 
-    public Command enableInitCommand() {
-        return Commands.run(climber::initialize, climber)
-                .withTimeout(0.5)
-                .andThen(Commands.runOnce(climber::zeroEncoder, climber));
+    public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
+        return Commands.runOnce(climber::zeroEncoder, climber);
     }
+    
 }

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

@@ -11,6 +11,8 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.RobotStatusManager;
@@ -19,7 +21,15 @@ import frc.robot.Constants.RobotStatus;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
+import java.util.Optional;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer; 
+
 public class DriveControls {
+
+    public boolean isRedAlliance;
+
     private final CommandSwerveDrivetrain drivetrain;
     private final CommandXboxController driver;
 
@@ -33,6 +43,8 @@ public class DriveControls {
     // 加速模式比例（按住 RB）
     private double boostDriveScale = 1.0;
     private double boostTurnScale  = 1.0;
+    //autoaim模式
+    private double slowDriveScale =0.1;
 
     private boolean boostEnabled = false;
 
@@ -95,9 +107,18 @@ public class DriveControls {
         SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
         double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
         double turnScale  = boostEnabled ? boostTurnScale  : normalTurnScale;
+        if (Constants.DemoMode.ENABLED) {
+            driveScale = boostEnabled ? Constants.DemoMode.boostDriveScale : Constants.DemoMode.normalDriveScale;
+            turnScale = boostEnabled ? Constants.DemoMode.boostTurnScale : Constants.DemoMode.normalTurnScale;
+        }
         double vx = -driver.getLeftY() * maxSpeed * driveScale;
         double vy = -driver.getLeftX() * maxSpeed * driveScale;
         double vomega = -driver.getRightX() * maxAngularRate * turnScale;
+        if (Constants.DemoMode.ENABLED) {
+            vx = clamp(vx, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
+            vy = clamp(vy, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
+            vomega = clamp(vomega, -Constants.DemoMode.maxAngularRateRadiansPerSecond, Constants.DemoMode.maxAngularRateRadiansPerSecond);
+        }
         // ====== execute 内替换 limiter：只限加速、不限减速 ======
         double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
         double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
@@ -115,26 +136,40 @@ public class DriveControls {
         // 将距离值写入 NetworkTable
         autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
         autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
+        autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
+        
+        RobotStatus currentStatus = robotStatusManager.getStatus();
+        if (Constants.DemoMode.ENABLED
+            && currentStatus != RobotStatus.Stopped
+            && currentStatus != RobotStatus.AllTelop
+            && currentStatus != RobotStatus.Climbing) {
+            currentStatus = RobotStatus.AllTelop;
+        }
 
-        switch (robotStatusManager.getStatus()) {
+        switch (currentStatus) {
             case Stopped:
                 vx = 0;
                 vy = 0;
                 vomega = 0;
                 break;
             case PassingBall:
+                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
                 vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                 break;
-
             case AutoAimming:
+                //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
                 vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
+                vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
+                vy = -driver.getLeftX() * maxSpeed * slowDriveScale;
                 break;
             case AllTelop:
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
             case Climbing:
+                // alignToClimb();
                 break;
             case CrossingBump:
                 double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -146,6 +181,7 @@ public class DriveControls {
                         Math.max(Math.abs(raw), 0.4),
                         raw
                     );
+                    vy = isRedAlliance ? -vy : vy;
                 }
                 autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
 
@@ -161,15 +197,8 @@ public class DriveControls {
                 errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
                 // 选误差绝对值更小的那个
                 double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
-
-                if (Math.abs(chosenError) < Math.toRadians(2.0)) {
-                    vomega = 0.0;
-                } else {
-                    vomega = chosenError * Constants.AutoPositioning.TurningkP;
-                }
-                // 限制最大角速度
-                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
-
+                vomega = calculatevomegaFromRotationAngle(chosenError);
+                autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
                 //vomega = calculateRotationSpeedForBump();
                 break;
             case CrossingTrench:
@@ -182,20 +211,15 @@ public class DriveControls {
                         Math.max(Math.abs(raw), 0.4),
                         raw
                     );
+                    vy = isRedAlliance ? -vy : vy;
                 }
                 autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
                 Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
                 double currentRad = currentAngle.getRadians();
                 double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
                 double TrenchAngleerror = targetRad - currentRad;
-                if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
-                    vomega = 0.0;
-                } else {
-                    vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
-                }
-                // 限幅（防止转太快）
-                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
-
+                vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
+                autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
                 //vomega = calculateRotationSpeedForTrench();
                 break;
         }        
@@ -205,13 +229,15 @@ public class DriveControls {
         autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
         
         boolean isAutoLike =
+            !Constants.DemoMode.ENABLED && (
             robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench);
 
         if (fieldCentricEnabled || isAutoLike) {
 
     // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
-        var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
+        //var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
+        var chosen = fieldCentric;
         return chosen
                 .withVelocityX(vx)
                 .withVelocityY(vy)
@@ -223,8 +249,9 @@ public class DriveControls {
                 .withRotationalRate(vomega);
         }
         }, () -> fieldCentricEnabled || 
+            (!Constants.DemoMode.ENABLED && (
             robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench
+            robotStatusManager.getStatus() == RobotStatus.CrossingTrench))
              ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
     }
 
@@ -238,37 +265,10 @@ public class DriveControls {
           double x2 = target2 - current;
           return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
     }
-    public double calculateRotationSpeedForTrench(){
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double currentAngle = currentPose.getRotation().getDegrees();
-        double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
-        double angleDifference = targetAngle - currentAngle;
-        // Normalize angle difference to the range [-180, 180]
-        if (angleDifference > 180) {
-            angleDifference -= 360;
-        } else if (angleDifference < -180) {
-            angleDifference += 360;
-        }
-        return calculateRotationSpeedFromRotationAngle(angleDifference);
-    }
-    public double calculateRotationSpeedForBump(){
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double currentAngle = currentPose.getRotation().getDegrees();
-        double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
-        double angleDifference = targetAngle - currentAngle;
-        // Normalize angle difference to the range [-180, 180]
-        if (angleDifference > 180) {
-            angleDifference -= 360;
-        } else if (angleDifference < -180) {
-            angleDifference += 360;
-        }
-        return calculateRotationSpeedFromRotationAngle(angleDifference);
-    }
-    // Calculate distance and rotation (angle difference)
     public double[] calculateDistanceAndRotationToHub() {
         Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double targetX = Constants.Field.RedHubPositionX;  // 目标 X 坐标
-        double targetY = Constants.Field.RedHubPositionY;  // 目标 Y 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
+        double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
 
         // 计算目标角度（相对于场地坐标系）
         double deltaX = targetX - currentPose.getX();
@@ -295,11 +295,10 @@ public class DriveControls {
     }
     public double[] calculateDistanceAndRotationToPassBall() {
         Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double targetX = Constants.Field.PassingBallPosX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX;  // 目标 X 坐标
         double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
         double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
        
-
         // 计算目标角度（相对于场地坐标系）
         double deltaX = targetX - currentPose.getX();
         double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
@@ -324,7 +323,6 @@ public class DriveControls {
         return distanceAndRotation;
     }
 
-    // Calculate rotation speed from rotation angle difference
     public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
         double calDifference = angleDifference;
 
@@ -341,8 +339,18 @@ public class DriveControls {
             return 0;
         }
     }
-
-
+    
+    private double calculatevomegaFromRotationAngle(double error) {
+        double vomega;
+        if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
+                vomega = 0.0;
+            } else {
+                vomega = error * Constants.AutoPositioning.TurningkP;
+            }
+            // 限制最大角速度
+            vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
+        return vomega;
+    }
 
         /**
      * 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
@@ -358,6 +366,10 @@ public class DriveControls {
         return limiter.calculate(target);
     }
 
+    private double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
         /** 紧急停止：清零限速器，让速度输出立刻归零 */
     public void emergencyStop() {
         vxLimiter.reset(0.0);
@@ -395,5 +407,113 @@ public class DriveControls {
     public double getMaxAngularRate() {
         return maxAngularRate;
     }
+    public void setTeamColors() {
+        Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
+        if (ally.isPresent()) {
+            // 根据队伍颜色设置布尔值
+            if (ally.get() == Alliance.Red) {
+                isRedAlliance = true; // 红队
+            } else if (ally.get() == Alliance.Blue) {
+                isRedAlliance = false; // 蓝队
+            }
+        } else {
+            isRedAlliance = true;
+        }
+        if (isRedAlliance) {
+            System.out.println("Robot is on the Red Alliance.");
+        } else {
+            System.out.println("Robot is on the Blue Alliance.");
+        }
+    }
+    public Command getAllianceColorCommand() {
+        return Commands.runOnce(this::setTeamColors);
+    }
+    // public void alignToClimb(){
+    //     LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
+    //     if(!drivetrain.isAlignedToAprilTag()){
+    //         drivetrain.driveToAprilTag();
+    //     }
+    //     else{
+    //         LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
+    //         drivetrain.stop();
+    //     }
+    // }
+    public Command autoAimCommand() {
+        if (Constants.DemoMode.ENABLED) {
+            return Commands.runOnce(drivetrain::stop, drivetrain);
+        }
+        return drivetrain.applyRequest(() -> {
+        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
+        return fieldCentric
+            .withVelocityX(0.0)
+            .withVelocityY(0.0)
+            .withRotationalRate(vomega);
+        }, () -> DriveMode.FIELD_CENTRIC)
+        .until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5)
+        .withTimeout(1.0)
+        .andThen(new InstantCommand(() -> {
+            drivetrain.stop();
+        }, drivetrain));
+    }
+    public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
+    if (Constants.DemoMode.ENABLED) {
+        return Commands.runOnce(drivetrain::stop, drivetrain);
+    }
+    Timer timer = new Timer();
 
+    return Commands.sequence(
+        new InstantCommand(timer::restart),
+        drivetrain.applyRequest(() -> {
+            double t = timer.get();
+            int phase = ((int) (t / switchPeriod)) % 4;
+
+            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(vx)
+                .withVelocityY(vy)
+                .withRotationalRate(vomega);
+        }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
+        new InstantCommand(() -> {
+            timer.stop();
+            drivetrain.stop();
+        }, drivetrain)
+    );
+}
+
+        public Command shakeCommand() {
+            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

@@ -1,71 +1,102 @@
 package frc.robot.controls;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.IntakerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.Constants;
+
 
 public class IntakerControls {
-    private final IntakerSubsystem intakerSubsystem;
+    private final IntakerSubsystem IntakerSubsystem;
     private final CommandXboxController controller;
+    private final ShooterSubsystem ShooterSubsystem;
 
-    private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI）
+    //private static final double ROTATER_STEP_PER_CYCLE = 0.00;
 
-    public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) {
-        this.intakerSubsystem = intakerSubsystem;
+    public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
+        this.ShooterSubsystem = Shooter;
+        this.IntakerSubsystem = Intaker;
         this.controller = controller;
     }
 
-    public void configureBindings() {
+    
 
         //B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
-        controller.a()
-            .whileTrue(
-                Commands.run(() -> {
-                    // rotater下降：范围会被 subsystem 限制
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
+        // controller.a()
+        //     .whileTrue(
+        //         Commands.run(() -> {
+        //             // rotater下降：范围会被 subsystem 限制
+        //             double cur = intakerSubsystem.getIntakerotaterPosition();
+        //             intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
 
-                    // getter 开
-                    intakerSubsystem.setIntakeGetterOn(true);
-                }, intakerSubsystem)
-            )
-            .onFalse(
-                Commands.runOnce(() -> {
-                    // 松：把当前位置作为target保持
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur);
+        //             // getter 开
+        //             intakerSubsystem.setIntakeGetterOn(true);
+        //         }, intakerSubsystem)
+        //     )
+        //     .onFalse(
+        //         Commands.runOnce(() -> {
+        //             // 松：把当前位置作为target保持
+        //             double cur = intakerSubsystem.getIntakerotaterPosition();
+        //             intakerSubsystem.setIntakerRotaterPosition(cur);
 
-                    // gtter继续转
-                    intakerSubsystem.setIntakeGetterOn(true);
-                }, intakerSubsystem)
-            );
+        //             // gtter继续转
+        //             intakerSubsystem.setIntakeGetterOn(true);
+        //         }, intakerSubsystem)
+        //     );
+        // // === B.B：按住上升+getter停；松开后rotater保持当前位置
+        // controller.b()
+        //     .whileTrue(
+        //         Commands.run(() -> {
+        //             // rotater升
+        //             double cur = intakerSubsystem.getIntakerotaterPosition();
+        //             intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
 
-        // === B.B：按住上升+getter停；松开后rotater保持当前位置
-        controller.b()
-            .whileTrue(
-                Commands.run(() -> {
-                    // rotater升
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
-
-                    // getter关
-                    intakerSubsystem.setIntakeGetterOn(false);
-                }, intakerSubsystem)
-            )
-            .onFalse(
-                Commands.runOnce(() -> {
-                    // 松：保持当前位置
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur);
-
-                    // getter仍然停止
-                    intakerSubsystem.setIntakeGetterOn(false);
-                }, intakerSubsystem)
-            );
-    }
+        //             // getter关
+        //             intakerSubsystem.setIntakeGetterOn(false);
+        //         }, intakerSubsystem)
+        //     )
+        //     .onFalse(
+        //         Commands.runOnce(() -> {
+        //             // 松：保持当前位置
+        //             double cur = intakerSubsystem.getIntakerotaterPosition();
+        //             intakerSubsystem.setIntakerRotaterPosition(cur);
 
+        //             // getter仍然停止
+        //             intakerSubsystem.setIntakeGetterOn(false);
+        //         }, intakerSubsystem)
+        //     );
+    
     public Command defaultIntakerCommand() {
-        return Commands.none();
+        return Commands.run(() -> {
+        double getterspeed = 0;
+        double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
+        
+        if (controller.getLeftTriggerAxis() > 0.1) {
+            getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
+            }
+
+        IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
+
+        boolean demoModeEnabled = Constants.DemoMode.ENABLED;
+        if (demoModeEnabled) {
+            IntakerSubsystem.stopIntakerotater();
+            return;
+        }
+
+        if(controller.getLeftY() < -0.5){
+                    IntakerSubsystem.IntakerUpNonStop();
+            } else if(controller.getLeftY() > 0.5){
+                    IntakerSubsystem.IntakerDownNonStop();
+            } else {
+                    IntakerSubsystem.stopIntakerotater();
+            }
+        }, IntakerSubsystem);
+    }
+    public Command resetIntakerRotaterEncoderCommand(IntakerSubsystem intakerSubsystem) {
+        return Commands.runOnce(intakerSubsystem::zeroEncoder, intakerSubsystem);
     }
 }

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

@@ -1,13 +1,18 @@
 package frc.robot.controls;
 
+import java.util.Set;
+
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.RobotStatusManager;
 import frc.robot.Constants.Intaker;
+import frc.robot.Constants.RobotStatus;
 import frc.robot.subsystems.IntakerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
@@ -36,25 +41,31 @@ public class ShooterControls {
                     shooterSubsystem.stopMotors();
                     break;
                 case AllTelop:
+                    shooterSubsystem.setBackboardPosition(0); 
+                    if(shooterSubsystem.isBackboardAtTarget()){
+                        shooterSubsystem.backboardMotor.set(0);
+                    }else{
+                        shooterSubsystem.outputBackboard();
+                    }  
+                    shooterSubsystem.setFlywheelSpeedByRPS(0);
+                    if(!Constants.DemoMode.ENABLED && controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
+                    else{shooterSubsystem.setConveyorSpeedByRPS(0);}
+                    break;
                 case PassingBall:
-                    var xP = shooterSubsystem.getDistance();
-                    //xP = Math.max(1.2, Math.min(4.5, xP));
-                    flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
-                    
+                    var xP = shooterSubsystem.getDistanceToPassball();
+                    xP = Math.max(2.5, Math.min(10.0, xP));
+                    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 = xP*xP;//todo:实测传球的时候的背板位置 打表
-                    backboardPosition += backboardPositionOffset;
+                    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;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
+                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -68,24 +79,23 @@ public class ShooterControls {
                     }  
                     break;
                 case AutoAimming:
-                
-                    var x = shooterSubsystem.getDistance();
+                    var x = shooterSubsystem.getDistanceToHub();
                     x = Math.max(1.2, Math.min(4.5, x));
-                    flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
+                    //flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+                    flywheelSpeed = 60;
                     flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
+                    //backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+                    backboardPosition = 500;
                     backboardPosition += backboardPositionOffset;
                     conveyorSpeed = Constants.Shooter.conveyorSpeed;
                     if (controller.getRightTriggerAxis() < 0.1) {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
+                    //if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -110,8 +120,6 @@ public class ShooterControls {
                     break;
                     
             }
-                
-
         }, shooterSubsystem);
     }
 
@@ -145,73 +153,6 @@ public class ShooterControls {
                 shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
     }
 
-    //打表参数
-    // private static final double[][] FLYWHEEL_TABLE = {
-    // {1.5, 0.42},
-    // {2.0, 0.46},
-    // {2.5, 0.51},
-    // {3.0, 0.57},
-    // {3.5, 0.64},
-    // {4.0, 0.72}
-    // };
-
-    // //线性插值函数
-    // private static double lerp(double x0, double y0, double x1, double y1, double x)  {
-    // if (Math.abs(x1 - x0) < 1e-9) {
-    //     return y0;
-    // }
-    // double t = (x - x0) / (x1 - x0);
-    // return y0 + t * (y1 - y0);
-    // }
-    // private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
-    //     return defaultSpeed + flywheelSpeedOffset;
-    // }
-    // private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
-    //     return defaultPosition + backboardPositionOffset;
-    // }
-
-
-    // private double calculateFlywheelSpeed(double distance) {
-    //     // 小于最小距离：用第一个点
-    //     if (distance <= FLYWHEEL_TABLE[0][0]) {
-    //     return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
-    //     }
-    //     // 大于最大距离：用最后一个点
-    //     int last = FLYWHEEL_TABLE.length - 1;
-    //     if (distance >= FLYWHEEL_TABLE[last][0]) {
-    //     return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
-    //     }
-    //     // 查找区间并插值
-    //     for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
-    //         double d0 = FLYWHEEL_TABLE[i][0];
-    //         double s0 = FLYWHEEL_TABLE[i][1];
-    //         double d1 = FLYWHEEL_TABLE[i + 1][0];
-    //         double s1 = FLYWHEEL_TABLE[i + 1][1];
-    //         if (distance >= d0 && distance <= d1) {
-    //             return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset;
-    //         }
-    //     }
-    //     return 0;
-    // }
-    
-    // private double calculateBackboardPosition(double distance) {
-    //     double BackboardPosition;
-    //     // 距离阈值（单位自己按distance进行修改）
-    //     final double DISTANCE_NEAR = 2.0;   // 近距离阈值
-    //     final double DISTANCE_MID  = 4.0;   // 中距离阈值
-    //     if (distance < DISTANCE_NEAR) {
-    //     BackboardPosition = Constants.ShooterCalculation.positionNear;
-    //     } 
-    //     else if (distance < DISTANCE_MID) {
-    //     BackboardPosition = Constants.ShooterCalculation.positionMid;
-    //     } 
-    //     else {
-    //     BackboardPosition = Constants.ShooterCalculation.postionFar;
-    //     }
-    //     BackboardPosition += backboardPositionOffset;
-    //     return BackboardPosition;
-    // }
-
     public void adjustFlywheelSpeedOffset(double offset) {
         flywheelSpeedOffset += offset;
     }
@@ -219,4 +160,68 @@ public class ShooterControls {
     public void adjustBackboardRateOffset(double offset) {
         backboardPositionOffset += offset;
     }
-}
+    public void resetOffsets(){
+        flywheelSpeedOffset = 0.0;
+        backboardPositionOffset = 0.0;
+    }
+
+    public Command autoShootToHubCommand() {
+        return Commands.defer(() -> {
+            double x = shooterSubsystem.getDistanceToHub();
+            x = Math.max(1.2, Math.min(4.5, x));
+
+            double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+            double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+            double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
+
+            return new RunCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
+                shooterSubsystem.setConveyorSpeedByRPS(0.0);
+                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
+                if (shooterSubsystem.isBackboardAtTarget()) {
+                    shooterSubsystem.backboardMotor.set(0);
+                } else {
+                    shooterSubsystem.outputBackboard();
+                }
+            }, shooterSubsystem)
+            .until(() ->
+                shooterSubsystem.isBackboardAtTarget()
+                && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
+            )
+            .withTimeout(0.3)
+            .andThen(new RunCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
+                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
+                if (shooterSubsystem.isBackboardAtTarget()) {
+                    shooterSubsystem.backboardMotor.set(0);
+                } else {
+                    shooterSubsystem.outputBackboard();
+                }
+                shooterSubsystem.setConveyorSpeedByRPS(25);
+            }, shooterSubsystem).withTimeout(5.0))
+            .andThen(new InstantCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(0.0);
+                shooterSubsystem.setConveyorSpeedByRPS(0.0);
+                shooterSubsystem.backboardMotor.set(0.0);
+            }, 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/generated/TunerConstants.java

@@ -66,7 +66,17 @@ public class TunerConstants {
         );
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs = null;
+    private static final Pigeon2Configuration pigeonConfigs =
+    new Pigeon2Configuration()
+        .withGyroTrim(
+            new GyroTrimConfigs()
+                .withGyroScalarZ(-5.0)
+        )
+        .withPigeon2Features(
+            new Pigeon2FeaturesConfigs()
+                .withEnableCompass(false)
+        );
+
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus

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

@@ -10,12 +10,16 @@ import com.ctre.phoenix6.signals.InvertedValue;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 
 public class ClimberSubsystem extends SubsystemBase {
 
+    private boolean autoClimbUsedInAuto = false;
+
     private final TalonFX climberMotor;
     private double targetPositionTicks;
 
@@ -25,7 +29,6 @@ public class ClimberSubsystem extends SubsystemBase {
     private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
     private double holdPositionTicks = 0.0;
-    private double deltaPositionTicks = 0.0;
 
     private final NetworkTable climberTable;
     
@@ -46,14 +49,7 @@ public class ClimberSubsystem extends SubsystemBase {
         holdPositionTicks = getPositionTicks();
     }
     public void initialize() {
-        
         climberMotor.set(-0.07);
-
-    }
-    public void zeroEncoder(){
-
-        climberMotor.setPosition(0);
-        stopMotor();
     }
 
     public double getPositionTicks() {
@@ -72,12 +68,21 @@ public class ClimberSubsystem extends SubsystemBase {
     public void climbDown() {
         targetPositionTicks = Constants.Climber.climbDownTicks;  
         climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-        
     }
-    public void climbDownNonStop(){
+    public void climbDownAuto() {
+        targetPositionTicks = Constants.Climber.climbDownAutoTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
 
-        deltaPositionTicks = Constants.Climber.deltaDownTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks-deltaPositionTicks));
+    public void climbDownNonStop(){
+        climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
+    }
+    
+    public void climbUpNonStop(){
+        climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
+    }
+    public void climbUpNonStopforAuto(){
+        climberMotor.set(0.07);
     }
 
     public void captureHoldPosition() {
@@ -93,6 +98,25 @@ public class ClimberSubsystem extends SubsystemBase {
     public void stopMotor() {
         climberMotor.set(0);
     }
+    public void setMotor(double speed) {
+        climberMotor.set(speed);
+    }
+
+    public Command initCommand() {
+        return Commands.sequence(
+            Commands.run(() -> climberMotor.set(-0.07), this).withTimeout(0.5),
+            Commands.runOnce(this::zeroEncoder, this)
+        );
+    }
+
+    public void zeroEncoder() {
+        climberMotor.setPosition(0);
+        stopMotor();
+    }
+
+    public void stopAll() {
+        climberMotor.set(0);
+    }
 
     @Override
     public void periodic() {

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

@@ -15,6 +15,7 @@ import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants.DriveMode;
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+import frc.robot.subsystems.LimelightHelpers.RawFiducial;
 import frc.robot.Constants;
 
 import edu.wpi.first.networktables.*;
@@ -46,6 +48,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
     /* Keep track if we've ever applied the operator perspective before or not */
     private boolean m_hasAppliedOperatorPerspective = false;
+    private boolean isAlignedToAprilTag = false;
 
     private DriveMode m_driveMode = DriveMode.OTHER;
     
@@ -63,6 +66,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private Field2d shooter_ll_field;
     private Field2d intaker_ll_field;
 
+
     public CommandSwerveDrivetrain(
         SwerveDrivetrainConstants drivetrainConstants,
         SwerveModuleConstants<?, ?, ?>... modules
@@ -74,8 +78,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         SmartDashboard.putData("Field", m_field);
         SmartDashboard.putData("PositioningShooterll", shooter_ll_field);
         SmartDashboard.putData("PositioningIntakerll", intaker_ll_field);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 1);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 1);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 0);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0);
         positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
         driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
         ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter");
@@ -127,12 +131,15 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
         setControl(
             new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled)
-        );
+        ); 
+    }
+    
+    public void stop() {
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
+        ); 
     }
 
-
-
-        
     private static final double kMaxVisionJumpMeters = 1.0;
     private static final double kMaxOmegaDegPerSec = 360.0;
 
@@ -154,7 +161,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         });
     }
     
-    public void forceUsingLimelightMT2() {
+    public void forceUsingLimelightmt2() {
+        if (Constants.DemoMode.ENABLED) {
+            return;
+        }
         LimelightHelpers.PoseEstimate mt2 =
         LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
 
@@ -166,7 +176,21 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
     }
     
-    private void fuseLimelightMT2() {
+    public void forceUsingLimelightmt2WithllName(String llString) {
+        if (Constants.DemoMode.ENABLED) {
+            return;
+        }
+        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;
 
         LimelightHelpers.PoseEstimate mt2_shooter =
@@ -263,31 +287,169 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
         double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
         if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
-            System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
+            //System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
             return false;
         }      
 
         // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
         //Pose2d current = getState().Pose;
-        if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
-            System.out.println("NoUsingMetaTag2ForTooChangingPose");
-            return false;
-        }
+        // if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
+        //     //System.out.println("NoUsingMetaTag2ForTooChangingPose");
+        //     return false;
+        // }
 
         return true;
     }
+    private double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
+//     public String driveToAprilTag() {
+//         System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
+//         System.out.println("TV = " + LimelightSupplier.isTargetVisible());
+
+//     if (!LimelightSupplier.isTargetVisible()) {
+//         isAlignedToAprilTag = false;
+//         //stop();
+//         return "No Target";
+//     }
+
+//     RawFiducial[] fiducials =
+//         LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+
+//     boolean foundDesiredTag = false;
+//     for (RawFiducial fiducial : fiducials) {
+//         if (fiducial.id == 16 || fiducial.id == 32) {
+//             foundDesiredTag = true;
+//             break;
+//         }
+//     }
+
+//     if (!foundDesiredTag) {
+//         isAlignedToAprilTag = false;
+//         stop();
+//         return "No Desired Tag";
+//     }
+
+//     // ===== 读取原始误差 =====
+//     double tx = LimelightSupplier.getTX();                 // deg
+//     double tz = -LimelightSupplier.getTargetTZ();          // m，方向按你当前经验保留负号
+//     double ry = LimelightSupplier.getTargetRotationY();    // deg
+
+//     // ===== 用原始误差判断是否已对齐 =====
+//     isAlignedToAprilTag =
+//         Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
+//         Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
+//         Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
+
+//     if (isAlignedToAprilTag) {
+//         stop();
+//         return "Aligned";
+//     }
+
+//     // ===== 计算输出 =====
+//     double vx = 0.0;
+//     double vy = 0.0;
+//     double omega = 0.0;
+
+//     // 1) 前后控制：根据 tz 前进/后退
+//     if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
+//         vx = tz * Constants.Limelight.AutoClimbKpTZ;
+
+//         // 最小输出，避免快到目标时不动
+//         if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
+//             vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
+//         }
+
+//         // 最大输出限幅
+//         vx = clamp(vx,
+//             -Constants.Limelight.AutoClimbMaxVX,
+//              Constants.Limelight.AutoClimbMaxVX);
+//     }
+
+//     // 2) 旋转控制：tx 为主，ry 为辅
+//     //    tx 负责“镜头中心对准 tag”
+//     //    ry 负责“机器人姿态微调”
+//     // double omegaFromTx = 0.0;
+//     // double omegaFromRy = 0.0;
+
+//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
+//     //     omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
+//     // }
+
+//     // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
+//     //     omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
+//     // }
+
+//     // omega = omegaFromTx + omegaFromRy;
+
+//     // // 大偏角时直接给较明显的转速，先把朝向拉回来
+//     // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
+//     //     omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
+//     // }
+
+//     // // 最小输出，避免转不动
+//     // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
+//     //     omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
+//     // }
+
+//     // // 最大输出限幅
+//     // omega = clamp(omega,
+//     //     -Constants.Limelight.AutoClimbMaxOmega,
+//     //      Constants.Limelight.AutoClimbMaxOmega);
+
+//     Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
+//     double climbCurrentRad = ClimbcurrentAngle.getRadians();
+//     double TargetAngleforClimbing = Math.toRadians(0);
+//     double Climberror = TargetAngleforClimbing - climbCurrentRad;
+//     if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
+//             omega = 0.0;
+//         } else {
+//             omega = Climberror * Constants.AutoPositioning.TurningkP;
+//         }
+
+
+//     // ===== 调试输出 =====
+//     driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
+//     driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
+//     driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
+//     driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
+//     driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
+//     driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
+
+//     // ===== 下发到底盘 =====
+//     driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
+
+//     return "Driving to AprilTag";
+// }
+
+//     public boolean isAlignedToAprilTag(){
+//         return isAlignedToAprilTag;
+//     }
     @Override
     public void periodic() {
 
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);
-        shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
-        intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
+        LimelightHelpers.PoseEstimate shooterEstimate =
+            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+        LimelightHelpers.PoseEstimate intakerEstimate =
+            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+        if (shooterEstimate != null && shooterEstimate.tagCount > 0) {
+            shooter_ll_field.setRobotPose(shooterEstimate.pose);
+        }
+        if (intakerEstimate != null && intakerEstimate.tagCount > 0) {
+            intaker_ll_field.setRobotPose(intakerEstimate.pose);
+        }
         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("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
+        // positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
+        // positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
+        positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
         // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Shooter,
@@ -300,7 +462,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             0, 0, 0, 0, 0
         );
 
-        fuseLimelightMT2();
+        if (!Constants.DemoMode.ENABLED) {
+            fuseLimelightmt2();
+        }
 
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {

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

@@ -4,6 +4,7 @@ import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
@@ -18,12 +19,23 @@ public class IntakerSubsystem extends SubsystemBase{
  
     private double intakeRotaterTargetPosition = 0.0;
 
+    public double getGravityFFVolts(){
+        double intakerotaterPosition = getIntakerotaterPosition();
+        double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
+        return gravityFFVolts;
+    }
+
+    public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
+        double gravityFeedforward = getGravityFFVolts();
+        intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
+    } 
+
     public IntakerSubsystem() {
         //初始化rotater&getter
         intakegetter = new TalonFX(47);
         intakerotater = new TalonFX(48);
         intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
-        intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);        
+        intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);      
     }
 
     public void setIntakerRotaterPosition(double targetPosition) {
@@ -35,7 +47,6 @@ public class IntakerSubsystem extends SubsystemBase{
         }
 
         intakeRotaterTargetPosition = targetPosition;
-
         intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
     }
 
@@ -50,18 +61,40 @@ public class IntakerSubsystem extends SubsystemBase{
         return error <= Constants.Intaker.positionToleranceRotations;
     }
 
-     public void stopintakerotater() {
-        intakerotater.setControl(velocityRequest.withVelocity(0));
+     public void stopIntakerotater() {
+        intakerotater.set(0);
         // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
     }
 
     //intakegetter的on/off
-    public void setIntakeGetterOn(boolean on) {
-        if (on) {
-            intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
+    // public void setIntakeGetterOn(boolean on) {
+    //     if (on) {
+    //         intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
+    //     }
+    //     else {
+    //         intakegetter.setControl(velocityRequest.withVelocity(0));
+    //     }
+    // }
+    public void setIntakerGetterSpeed(double speed) {
+        intakegetter.set(speed);
+    }
+    public void zeroEncoder(){
+        intakerotater.setPosition(0);
+        stopIntakerotater();
+    }
+    public void IntakerDownNonStop(){
+            intakerotater.set(-0.1);
         }
-        else {
-            intakegetter.setControl(velocityRequest.withVelocity(0));
+
+    public void IntakerUpNonStop(){
+            intakerotater.set(0.2);
         }
+
+    public void setIntakerRotater(double rotaterspeed){
+        intakerotater.set(rotaterspeed);
+    }
+    public void stopAll() {
+        stopIntakerotater();
+        setIntakerGetterSpeed(0);
     }
 }

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

@@ -4,12 +4,14 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.TalonFXS;
 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;
@@ -31,7 +33,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
     private NetworkTable shooterNetworkTable;
 
-    private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
 
 
     // 初始化各个部件
@@ -94,9 +96,17 @@ public class ShooterSubsystem extends SubsystemBase {
     public void setConveyorSpeedByRPS(double speed) {
         conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
     }
+    public void setConveyorSpeedOpen(double speed){
+        conveyorMotor.set(speed);
+    }
 
     //调试用
     public void setBackboardSpeedByRPS(double speed){
+         if (speed > 0 && speed < 10) {
+        speed = 10;
+        } else if (speed < 0 && speed > -10) {
+        speed = -10;
+        }
         backboardMotor.setControl(velocityRequest.withVelocity(speed));
     }
 
@@ -146,6 +156,13 @@ public class ShooterSubsystem extends SubsystemBase {
     public void conveyorRun(){
         setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
     }
+    public void resetbackboardencoder(){
+        backboardEncoder.reset();
+    }
+    public double getConveyorStatorCurrent(){
+        double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
+        return statorCurrent;
+    }
 
     @Override
     public void periodic() {
@@ -157,11 +174,21 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
-        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
+        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
+        shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
+        shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
     }
 
-    public double getDistance(){
+    public double getDistanceToHub(){
         //获得车子距离Hub的距离
         return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
     }
+    public double getDistanceToPassball(){
+        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
+    }
+    public void stopAll() {
+        setFlywheelSpeedByRPS(0);
+        setConveyorSpeedByRPS(0);
+        backboardMotor.set(0);
+    }
 }