Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

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

@@ -17,7 +17,13 @@
               {
                 "type": "named",
                 "data": {
-                  "name": null
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
                 }
               }
             ]
@@ -32,13 +38,13 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Limelighter"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Lancer"
           }
         }
       ]

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

@@ -17,7 +17,13 @@
               {
                 "type": "named",
                 "data": {
-                  "name": null
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
                 }
               }
             ]
@@ -32,19 +38,19 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Limelighter"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Lancer"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "TournerA180"
           }
         },
         {
@@ -56,13 +62,25 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "GrimperMur"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "MonterGrimpeur"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MonterMur"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreGrimpeur"
           }
         }
       ]

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

@@ -17,7 +17,13 @@
               {
                 "type": "named",
                 "data": {
-                  "name": null
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
                 }
               }
             ]
@@ -32,19 +38,19 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Limelighter"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Lancer"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "TournerAZero"
           }
         },
         {
@@ -56,13 +62,25 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "GrimperReservoir"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "MonterGrimpeur"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MonterReservoir"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "DescendreGrimpeur"
           }
         }
       ]

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

@@ -0,0 +1,56 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ChercherMilieuDroite"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "RetourMilieuDroite"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Limelighter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lancer"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Milieu",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,62 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ChercherMilieuDroiteProche"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PartirMilieuDroiteProche"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "RetourMilieuDroite"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Limelighter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lancer"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Milieu",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,56 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ChercherMilieuGauche"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "RetourMilieuGauche"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Limelighter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lancer"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Milieu",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,62 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ChercherMilieuGaucheProche"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "DescendreBalayeuse"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Aspirer"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PartirMilieuGaucheProche"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "RetourMilieuGauche"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Limelighter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lancer"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Milieu",
+  "choreoAuto": false
+}

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

@@ -13,13 +13,13 @@
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Limelighter"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": null
+            "name": "Lancer"
           }
         }
       ]

src/main/deploy/pathplanner/paths/ChercherMilieuDroite.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.026,
+        "y": 0.685
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.399509272467903,
+        "y": 0.7238159771754633
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.295877318116975,
+        "y": 1.238
+      },
+      "prevControl": {
+        "x": 8.295877318116975,
+        "y": 0.48503351833476316
+      },
+      "nextControl": {
+        "x": 8.295877318116975,
+        "y": 2.0013808844507857
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.295877318116975,
+        "y": 3.322
+      },
+      "prevControl": {
+        "x": 8.295877318116975,
+        "y": 2.2023305396701525
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7420042643923158,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/ChercherMilieuDroiteProche.path

@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.000242510699001,
+        "y": 0.6515406562054205
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.772958630527817,
+        "y": 0.6310199714693305
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.0057346647646215,
+        "y": 1.2725962910128386
+      },
+      "prevControl": {
+        "x": 6.0057346647646215,
+        "y": 0.5196298093476017
+      },
+      "nextControl": {
+        "x": 6.0057346647646215,
+        "y": 2.0359771754636244
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.8,
+        "y": 3.322
+      },
+      "prevControl": {
+        "x": 5.8,
+        "y": 2.2023305396701525
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7420042643923158,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0.7292370020256648,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/ChercherMilieuGauche.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.026119828815976,
+        "y": 7.314950071326677
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.399629101283878,
+        "y": 7.35376604850214
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.295877318116975,
+        "y": 6.771526390870186
+      },
+      "prevControl": {
+        "x": 8.295877318116975,
+        "y": 7.524492872535423
+      },
+      "nextControl": {
+        "x": 8.295877318116975,
+        "y": 6.0081455064194005
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.295877318116975,
+        "y": 4.688402282453637
+      },
+      "prevControl": {
+        "x": 8.32422939366089,
+        "y": 5.807712722058167
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7420042643923158,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/ChercherMilieuGaucheProche.path

@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.064935805991441,
+        "y": 7.392582025677603
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.617574893009986,
+        "y": 7.314950071326676
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.915164051355206,
+        "y": 6.758587731811698
+      },
+      "prevControl": {
+        "x": 5.915164051355206,
+        "y": 7.511554213476935
+      },
+      "nextControl": {
+        "x": 5.915164051355206,
+        "y": 5.995206847360913
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.8,
+        "y": 4.688402282453637
+      },
+      "prevControl": {
+        "x": 5.828352075543914,
+        "y": 5.807712722058167
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7420042643923158,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.8426738690074239,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/MonterMur.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0761055634807417,
+        "y": 4.85660485021398
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8292699165532256,
+        "y": 4.81695433438033
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0761055634807417,
+        "y": 4.623708987161199
+      },
+      "prevControl": {
+        "x": 1.3604504533921022,
+        "y": 4.6356950204744996
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/MonterReservoir.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0502282453637657,
+        "y": 2.4758915834522113
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0408884389937225,
+        "y": 2.742159765250622
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0502282453637657,
+        "y": 2.7734807417974334
+      },
+      "prevControl": {
+        "x": 1.0547546338516156,
+        "y": 2.523521721541598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/PartirMilieuDroiteProche.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.8,
+        "y": 3.322
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.486128029253273,
+        "y": 1.6749552651197097
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.820870185449357,
+        "y": 0.8714978601997154
+      },
+      "prevControl": {
+        "x": 6.300963707518119,
+        "y": 2.122306196505451
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/PartirMilieuGaucheProche.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.8,
+        "y": 4.688402282453637
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.22710768166824,
+        "y": 6.07993837753099
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.963195435092723,
+        "y": 6.913851640513553
+      },
+      "prevControl": {
+        "x": 6.833040037863721,
+        "y": 6.563509208473397
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Milieu",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,36 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [
+    "Milieu"
+  ],
+  "autoFolders": [
+    "Milieu"
+  ],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 74.088,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

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

@@ -4,7 +4,16 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -12,28 +21,82 @@ import frc.robot.commands.Aspirer;
 import frc.robot.commands.DescendreBalyeuse;
 import frc.robot.commands.DescendreGrimpeur;
 import frc.robot.commands.Lancer;
+import frc.robot.commands.LancerBaseVitesse;
+import frc.robot.commands.Limelighter;
+import frc.robot.commands.ModeOposer;
 import frc.robot.commands.MonterBalyeuse;
 import frc.robot.commands.MonterGrimpeur;
+import frc.robot.commands.ModeAuto.AspirerAuto;
+import frc.robot.commands.ModeAuto.GrimperMur;
+import frc.robot.commands.ModeAuto.GrimperReservoir;
+import frc.robot.commands.ModeAuto.LancerAuto;
+import frc.robot.commands.ModeAuto.RetourMilieuDroite;
+import frc.robot.commands.ModeAuto.RetourMilieuGauche;
+import frc.robot.commands.ModeAuto.TournerA180;
+import frc.robot.commands.ModeAuto.TournerAZero;
+import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Balayeuse;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Lanceur;
 import frc.robot.subsystems.Led;
 import frc.robot.subsystems.LimeLight3;
+import frc.robot.subsystems.Limelight3G;
 
 public class RobotContainer {
+  private final SendableChooser<Command> autoChooser;
   Balayeuse balayeuse = new Balayeuse();
   Grimpeur grimpeur = new Grimpeur();
   Lanceur lanceur = new Lanceur();
   LimeLight3 limeLight3 = new LimeLight3();
+  Limelight3G limeLight3G = new Limelight3G();
   Led led = new Led();
   CommandXboxController manette = new CommandXboxController(0);
+  private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
+    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+
+    /* Setting up bindings for necessary control of the swerve drive platform */
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
+    private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+    private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+
+    private final Telemetry logger = new Telemetry(MaxSpeed);
+
+    public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+
+
   public RobotContainer() {
+    autoChooser = AutoBuilder.buildAutoChooser();
     CameraServer.startAutomaticCapture();
     configureBindings();
+    NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
+    NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
+    NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur));
+    NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite());
+    NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
+    NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
+    NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
+    NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
+    NamedCommands.registerCommand("TournerAZero", new TournerAZero(drivetrain));
+    NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain));
+    NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
+    NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
   }
 
   private void configureBindings() {
-    manette.a().whileTrue(new Lancer(lanceur,limeLight3,balayeuse,led));
+    drivetrain.setDefaultCommand(
+          drivetrain.applyRequest(() ->
+            drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) 
+              .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed, 0.05)) 
+             .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05))
+        )
+      );
+    manette.povUp().whileTrue(new LancerAuto(lanceur));
+    manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led));
+    manette.y().whileTrue(new ModeOposer(lanceur, balayeuse));
+    manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led));
     manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
     manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur));
     manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
@@ -42,6 +105,6 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/Telemetry.java

@@ -0,0 +1,121 @@
+package frc.robot;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Telemetry {
+    private final double MaxSpeed;
+
+    /**
+     * Construct a telemetry object, with the specified max speed of the robot
+     * 
+     * @param maxSpeed Maximum speed in meters per second
+     */
+    public Telemetry(double maxSpeed) {
+        MaxSpeed = maxSpeed;
+        SignalLogger.start();
+
+        /* Set up the module state Mechanism2d telemetry */
+        for (int i = 0; i < 4; ++i) {
+            SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
+        }
+    }
+
+    /* What to publish over networktables for telemetry */
+    private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+    /* Robot swerve drive state */
+    private final NetworkTable driveStateTable = inst.getTable("DriveState");
+    private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
+    private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
+    private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
+    private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
+    private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
+    private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
+    private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();
+
+    /* Robot pose for field positioning */
+    private final NetworkTable table = inst.getTable("Pose");
+    private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+    private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+
+    /* Mechanisms to represent the swerve module states */
+    private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+    };
+    /* A direction and length changing ligament for speed representation */
+    private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+    };
+    /* A direction changing and length constant ligament for module direction */
+    private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+    };
+
+    private final double[] m_poseArray = new double[3];
+
+    /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
+    public void telemeterize(SwerveDriveState state) {
+        /* Telemeterize the swerve drive state */
+        drivePose.set(state.Pose);
+        driveSpeeds.set(state.Speeds);
+        driveModuleStates.set(state.ModuleStates);
+        driveModuleTargets.set(state.ModuleTargets);
+        driveModulePositions.set(state.ModulePositions);
+        driveTimestamp.set(state.Timestamp);
+        driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
+
+        /* Also write to log file */
+        // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose);
+        // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds);
+        // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates);
+        // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets);
+        // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions);
+        // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
+
+        /* Telemeterize the pose to a Field2d */
+        fieldTypePub.set("Field2d");
+
+        m_poseArray[0] = state.Pose.getX();
+        m_poseArray[1] = state.Pose.getY();
+        m_poseArray[2] = state.Pose.getRotation().getDegrees();
+        fieldPub.set(m_poseArray);
+
+        /* Telemeterize each module state to a Mechanism2d */
+        for (int i = 0; i < 4; ++i) {
+            m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
+            m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
+            m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
+        }
+    }
+}

src/main/java/frc/robot/commands/Aspirer.java

@@ -14,12 +14,14 @@ public class Aspirer extends Command {
   private Balayeuse balayeuse;
   private Timer timer;
   private Led led;
+  private double temp;
   /** Creates a new Aspirer. */
   public Aspirer(Balayeuse balayeuse, Led led) {
     this.balayeuse = balayeuse;
     this.led = led;
     this.timer = new Timer();
     addRequirements(balayeuse, led); 
+    this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -27,6 +29,7 @@ public class Aspirer extends Command {
   @Override
   public void initialize() {
     timer.reset();
+    temp = balayeuse.Amp();  
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -40,13 +43,16 @@ public class Aspirer extends Command {
       moyenneAmp += balayeuse.Amp() / nbFois;
     }
     else{
-      timer.reset();
+      nbFois++;
+      moyenneAmp -= temp;
+      moyenneAmp += balayeuse.Amp() / nbFois;
+      temp = balayeuse.Amp();
     }
-    if(moyenneAmp < 40){
+    if(moyenneAmp < balayeuse.AmpMax()){
       timer.reset();
-      balayeuse.Balayer(0.5);
+      balayeuse.Balayer(-0.5);
     }
-    else if(moyenneAmp > 40){
+    else{
         balayeuse.Balayer(0);
         led.Jaune2();
     }

src/main/java/frc/robot/commands/DescendreBalyeuse.java

@@ -24,9 +24,12 @@ public class DescendreBalyeuse extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(balayeuse.Distance() < balayeuse.EncodeurBalayeuse()){
+    if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
       balayeuse.Pivoter(-0.2);
     }
+    else{
+      balayeuse.Pivoter(0);
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/DescendreGrimpeur.java

@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
   @Override
   public void execute() {
     if(!grimpeur.Limit()){
-      grimpeur.Grimper(-0.5);
+      grimpeur.Grimper(-0.3);
     }
     else{
       grimpeur.Reset();

src/main/java/frc/robot/commands/Lancer.java

@@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Lanceur;
 import frc.robot.subsystems.Led;
-import frc.robot.subsystems.LimeLight3;
 import frc.robot.subsystems.Limelight3G;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
@@ -21,55 +20,71 @@ public class Lancer extends Command {
   private Timer timer;
   private Balayeuse balayeuse;
   private Led led;
+  private double temp;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
+  public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
     this.balayeuse = balayeuse;
     this.led = led;
     this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
-    addRequirements(lanceur);
+    addRequirements(lanceur, balayeuse, led, limeLight3G);
+    this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pidController = new PIDController(0, 0,0, 0);
+    pidController = new PIDController(1, 0,0, 0);
     timer.reset();
+    timer.start();
+    temp = lanceur.Amp();    
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    timer.start();
+    double botx = 0;
-    if(lanceur.Amp() > 40){ 
+    double boty = 0;
+    double[] BotPose = new double[6];
+    if(limeLight3G.getV()){
+      BotPose = limeLight3G.getBotPoseBlue();
+      botx = BotPose[0];
+      boty = BotPose[1];
+    }
+    int nbFois = 0;
+
+    double moyenneAmp = 0;
+    if(timer.get() < 3){
+      nbFois++;
+      moyenneAmp += balayeuse.Amp() / nbFois;
+    }
+    else{
+      nbFois++;
+      moyenneAmp -= temp;
+      moyenneAmp += balayeuse.Amp() / nbFois;
+      temp = balayeuse.Amp();
+    }
+    // if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
       timer.reset();
-      double vitesse = (100-limeLight3G.getTA())/100;
+      balayeuse.Balayer(-0.5);
-      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      // led.Jaune2();
+    // }
+    // else{
+                                                
+         double vitesse = 0.5;
+         if(limeLight3G.getV()){
+                                          //pythagore |
+                                          //         \/
+            vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
+         }                                   
+      double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/
       lanceur.Lancer(output);
       if(lanceur.Vitesse() >= vitesse){
         lanceur.Demeler(0.5);
       }
-    }
+    // }
-    else if(lanceur.Amp() < 40){
-      lanceur.Lancer(0);
-      lanceur.Demeler(0);
-      if(!balayeuse.GetLimiSwtich()){
-        balayeuse.Pivoter(0.2);
-      }
-      else{
-        balayeuse.Reset();
-        balayeuse.Pivoter(0);
-        double vitesse = (100-limeLight3G.getTA())/lanceur.Vitesse();
-        double output = pidController.calculate(lanceur.Vitesse(),vitesse);
-        lanceur.Lancer(output);
-        if(lanceur.Vitesse() >= vitesse){
-          lanceur.Demeler(0.5);
-        }
-      }
-      led.Vert2();
-    }
 
   }
 

src/main/java/frc/robot/commands/LancerBaseVitesse.java

@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Balayeuse;
+import frc.robot.subsystems.Lanceur;
+import frc.robot.subsystems.Led;
+import frc.robot.subsystems.LimeLight3;
+import frc.robot.subsystems.Limelight3G;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LancerBaseVitesse extends Command {
+  private Lanceur lanceur;
+  private PIDController pidController;
+  private Limelight3G limeLight3G;
+  private Timer timer;
+  private Balayeuse balayeuse;
+  private Led led;
+  private double temp;
+  /** Creates a new Lancer. */
+  public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
+    this.lanceur = lanceur;
+    this.balayeuse = balayeuse;
+    this.led = led;
+    this.timer = new Timer();
+    this.limeLight3G = new Limelight3G();
+    addRequirements(lanceur, balayeuse, led, limeLight3G);
+    this.temp = 0;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    pidController = new PIDController(1, 0,0, 0);
+    timer.reset();
+    timer.start();
+    temp = lanceur.Amp();    
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // int nbFois = 0;
+
+    // double moyenneAmp = 0;
+    // if(timer.get() < 3){
+    //   nbFois++;
+    //   moyenneAmp += balayeuse.Amp() / nbFois;
+    // }
+    // else{
+    //   nbFois++;
+    //   moyenneAmp -= temp;
+    //   moyenneAmp += balayeuse.Amp() / nbFois;
+    //   temp = balayeuse.Amp();
+    // }
+    // if(moyenneAmp > 30 && nbFois > 10){
+    //   timer.reset();
+    //   balayeuse.Balayer(0.5);
+    //   led.Jaune2();
+    // }
+    // else{
+      double vitesse = 0.4;
+      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      lanceur.Lancer(output);
+      if(lanceur.Vitesse() >= vitesse){
+        lanceur.Demeler(0.5);
+      }
+    // }
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    lanceur.Demeler(0);
+    lanceur.Lancer(0);
+    balayeuse.Pivoter(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Limelighter.java

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
+import static edu.wpi.first.units.Units.*;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class Limelighter extends Command {
+  Timer timer;
+  Limelight3G limelight3g;
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+    double botx;
+    double boty;
+    double angle;
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new Limelighter. */
+  public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) {
+    this.limelight3g = limelight3g;
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain,limelight3g);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double[] BotPose = new double[6];
+    BotPose = limelight3g.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    angle = BotPose[5];
+    drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90));
+    if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){
+      timer.start();
+    }
+    else{
+      timer.reset();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withRotationalRate(0));
+    timer.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return timer.get() > 1;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java

@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Balayeuse;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class AspirerAuto extends Command {
+  Balayeuse balayeuse;
+  /** Creates a new AspirerAuto. */
+  public AspirerAuto(Balayeuse balayeuse) {
+    this.balayeuse = balayeuse;
+    addRequirements(balayeuse);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    balayeuse.Balayer(0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    balayeuse.Balayer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.LimeLight3;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class GrimperMur extends Command {
+  LimeLight3 limeLight3;
+  CommandSwerveDrivetrain drivetrain;
+  double[] BotPose = new double[6];
+  double botx;
+  double boty;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new GrimperMur. */
+  public GrimperMur(LimeLight3 limeLight3, CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    this.limeLight3 = limeLight3;
+    addRequirements(limeLight3,drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    BotPose = limeLight3.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){
+      drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx));
+    }
+    else{
+       drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05);
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java

@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class GrimperReservoir extends Command {
+  Limelight3G limeLight3G;
+  CommandSwerveDrivetrain drivetrain;
+  double[] BotPose = new double[6];
+  double botx;
+  double boty;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new GrimperMur. */
+  public GrimperReservoir(Limelight3G limeLight3G, CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    this.limeLight3G = limeLight3G;
+    addRequirements(limeLight3G,drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    BotPose = limeLight3G.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){
+      drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx));
+    }
+    else{
+       drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Lanceur;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LancerAuto extends Command {
+  Lanceur lanceur;
+  Timer timer;
+  /** Creates a new LancerAuto. */
+  public LancerAuto(Lanceur lanceur) {
+    this.lanceur = lanceur;
+    addRequirements(lanceur);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    timer.start();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    lanceur.Lancer(0.5);
+    if(timer.get() > 1){
+      lanceur.Demeler(0.5);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    lanceur.Lancer(0);
+    lanceur.Demeler(0);
+    timer.reset();
+    timer.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return timer.get() > 3;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class RetourMilieuDroite extends Command {
+  /** Creates a new RetourMilieu. */
+  public RetourMilieuDroite() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class RetourMilieuGauche extends Command {
+  /** Creates a new RetourMilieuGauche. */
+  public RetourMilieuGauche() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/TournerA180.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TournerA180 extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerA180(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(-0.5));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(0.5));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return pigeon2.getYaw().getValueAsDouble() > 165  && pigeon2.getYaw().getValueAsDouble() < 195;
+  }
+}

src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ModeAuto;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class TournerAZero extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerAZero(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(0.5));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(-0.5));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return pigeon2.getYaw().getValueAsDouble() > 345 || pigeon2.getYaw().getValueAsDouble() < 15;
+  }
+}

src/main/java/frc/robot/commands/ModeOposer.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Balayeuse;
+import frc.robot.subsystems.Lanceur;
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ModeOposer extends Command {
+  private Lanceur lanceur;
+  private Balayeuse balayeuse;
+  /** Creates a new Lancer. */
+  public ModeOposer(Lanceur lanceur, Balayeuse balayeuse) {
+    this.lanceur = lanceur;
+    this.balayeuse = balayeuse;
+    addRequirements(lanceur ,balayeuse);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {  
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    lanceur.Lancer(-0.2);
+    lanceur.Demeler(-0.2);
+    balayeuse.Balayer(-0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    lanceur.Demeler(0);
+    lanceur.Lancer(0);
+    balayeuse.Balayer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/MonterGrimpeur.java

@@ -23,8 +23,13 @@ public class MonterGrimpeur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.Position() < grimpeur.PositionFinal()){
+    
-      grimpeur.Grimper(0.5);
+    if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
+      grimpeur.Grimper(0.3);
+      System.out.println("monte");
+    }
+    else {
+      grimpeur.Grimper(0);
     }
   }
 

src/main/java/frc/robot/generated/TunerConstants.java

@@ -0,0 +1,286 @@
+package frc.robot.generated;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class TunerConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    private static final Slot0Configs steerGains = new Slot0Configs()
+        .withKP(100).withKI(0).withKD(0.5)
+        .withKS(0.1).withKV(2.66).withKA(0)
+        .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    private static final Slot0Configs driveGains = new Slot0Configs()
+        .withKP(0.1).withKI(0).withKD(0)
+        .withKS(0).withKV(0.124);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The type of motor used for the drive motor
+    private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
+    // The type of motor used for the drive motor
+    private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    private static final Current kSlipCurrent = Amps.of(120);
+
+    // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+    // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+    private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                // Swerve azimuth does not require much torque output, so we can set a relatively low
+                // stator current limit to help avoid brownouts without impacting performance.
+                .withStatorCurrentLimit(Amps.of(60))
+                .withStatorCurrentLimitEnable(true)
+        );
+    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;
+
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
+
+    // Theoretical free speed (m/s) at 12 V applied output;
+    // This needs to be tuned to your individual robot
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(9.82);
+
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    private static final double kCoupleRatio = 3.5714285714285716;
+
+    private static final double kDriveGearRatio = 6.122448979591837;
+    private static final double kSteerGearRatio = 21.428571428571427;
+    private static final Distance kWheelRadius = Inches.of(3.895);
+
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final int kPigeonId = 16;
+
+    // These are only used for simulation
+    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+    // Simulated voltage necessary to overcome friction
+    private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+    private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+            .withCANBusName(kCANBus.getName())
+            .withPigeon2Id(kPigeonId)
+            .withPigeon2Configs(pigeonConfigs);
+
+    private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+            .withDriveMotorGearRatio(kDriveGearRatio)
+            .withSteerMotorGearRatio(kSteerGearRatio)
+            .withCouplingGearRatio(kCoupleRatio)
+            .withWheelRadius(kWheelRadius)
+            .withSteerMotorGains(steerGains)
+            .withDriveMotorGains(driveGains)
+            .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+            .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+            .withSlipCurrent(kSlipCurrent)
+            .withSpeedAt12Volts(kSpeedAt12Volts)
+            .withDriveMotorType(kDriveMotorType)
+            .withSteerMotorType(kSteerMotorType)
+            .withFeedbackSource(kSteerFeedbackType)
+            .withDriveMotorInitialConfigs(driveInitialConfigs)
+            .withSteerMotorInitialConfigs(steerInitialConfigs)
+            .withEncoderInitialConfigs(encoderInitialConfigs)
+            .withSteerInertia(kSteerInertia)
+            .withDriveInertia(kDriveInertia)
+            .withSteerFrictionVoltage(kSteerFrictionVoltage)
+            .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+
+    // Front Left
+    private static final int kFrontLeftDriveMotorId = 6;
+    private static final int kFrontLeftSteerMotorId = 9;
+    private static final int kFrontLeftEncoderId = 24;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.36767578125);
+    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final boolean kFrontLeftEncoderInverted = false;
+
+    private static final Distance kFrontLeftXPos = Inches.of(11.375);
+    private static final Distance kFrontLeftYPos = Inches.of(10.625);
+
+    // Front Right
+    private static final int kFrontRightDriveMotorId = 7;
+    private static final int kFrontRightSteerMotorId = 5;
+    private static final int kFrontRightEncoderId = 22;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23486328125);
+    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final boolean kFrontRightEncoderInverted = false;
+
+    private static final Distance kFrontRightXPos = Inches.of(11.375);
+    private static final Distance kFrontRightYPos = Inches.of(-10.625);
+
+    // Back Left
+    private static final int kBackLeftDriveMotorId = 10;
+    private static final int kBackLeftSteerMotorId = 11;
+    private static final int kBackLeftEncoderId = 21;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(0.202392578125);
+    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final boolean kBackLeftEncoderInverted = false;
+
+    private static final Distance kBackLeftXPos = Inches.of(-11.375);
+    private static final Distance kBackLeftYPos = Inches.of(10.625);
+
+    // Back Right
+    private static final int kBackRightDriveMotorId = 14;
+    private static final int kBackRightSteerMotorId = 13;
+    private static final int kBackRightEncoderId = 23;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.33203125);
+    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final boolean kBackRightEncoderInverted = false;
+
+    private static final Distance kBackRightXPos = Inches.of(-11.375);
+    private static final Distance kBackRightYPos = Inches.of(-10.625);
+
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
+        ConstantCreator.createModuleConstants(
+            kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
+            kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
+        ConstantCreator.createModuleConstants(
+            kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
+            kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
+        ConstantCreator.createModuleConstants(
+            kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
+            kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
+        ConstantCreator.createModuleConstants(
+            kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
+            kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
+        );
+
+    /**
+     * Creates a CommandSwerveDrivetrain instance.
+     * This should only be called once in your robot program,.
+     */
+    public static CommandSwerveDrivetrain createDrivetrain() {
+        return new CommandSwerveDrivetrain(
+            DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
+        );
+    }
+
+
+    /**
+     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+     */
+    public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+         * @param modules               Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+         *                                unspecified or set to 0 Hz, this is 250 Hz on
+         *                                CAN FD, and 100 Hz on CAN 2.0.
+         * @param modules                 Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+         *                                  unspecified or set to 0 Hz, this is 250 Hz on
+         *                                  CAN FD, and 100 Hz on CAN 2.0.
+         * @param odometryStandardDeviation The standard deviation for odometry calculation
+         *                                  in the form [x, y, theta]ᵀ, with units in meters
+         *                                  and radians
+         * @param visionStandardDeviation   The standard deviation for vision calculation
+         *                                  in the form [x, y, theta]ᵀ, with units in meters
+         *                                  and radians
+         * @param modules                   Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation,
+            Matrix<N3, N1> visionStandardDeviation,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency,
+                odometryStandardDeviation, visionStandardDeviation, modules
+            );
+        }
+    }
+}

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

@@ -9,7 +9,6 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -18,12 +17,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Balayeuse extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
-  SparkFlex Balaye1 = new SparkFlex(5, MotorType.kBrushless);
+  SparkFlex Balaye1 = new SparkFlex(2, MotorType.kBrushless);
-  SparkFlex Balaye2 = new SparkFlex(6, MotorType.kBrushless);
+  SparkFlex Balaye2 = new SparkFlex(20, MotorType.kBrushless);
-  SparkMax Pivot = new SparkMax(7, MotorType.kBrushless);
+  SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
-  DigitalInput limit = new DigitalInput(0);
+  DigitalInput limit = new DigitalInput(9);
   private GenericEntry EncodeurBalayeuse =
   teb.add("Position bas balayeuse", 10).getEntry();
+  private GenericEntry AmpBaleyeuse =
+  teb.add("Ampérage Baleyeuse", 40).getEntry();
   public void Balayer(double vitesse){
     Balaye1.set(vitesse);
     Balaye2.set(vitesse);
@@ -38,11 +39,14 @@ public class Balayeuse extends SubsystemBase {
     Pivot.getEncoder().setPosition(0);
   }
   public boolean GetLimiSwtich(){
-    return limit.get();
+    return !limit.get();
   }
   public double Amp(){
     return Balaye2.getOutputCurrent();
   }
+  public double AmpMax(){
+    return AmpBaleyeuse.getDouble(40);
+  }
   public void Temps(){
     Timer timer = new Timer();
     timer.start();

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

@@ -0,0 +1,246 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.*;
+
+import java.util.Optional;
+import java.util.function.Supplier;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+
+/**
+ * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Subsystem so it can easily be used in command-based projects.
+ *
+ * Generated by the 2026 Tuner X Swerve Project Generator
+ * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+ */
+public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
+    private static final double kSimLoopPeriod = 0.004; // 4 ms
+    private Notifier m_simNotifier = null;
+    private double m_lastSimTime;
+
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    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 final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
+     private void configureAutoBuilder() {
+        try {
+            RobotConfig config = RobotConfig.fromGUISettings();
+            AutoBuilder.configure(
+                () -> getState().Pose,  
+                this::resetPose,         
+                () -> getState().Speeds, 
+                (speeds, feedforwards) -> setControl(
+                    m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
+                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
+                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
+                ),
+                new PPHolonomicDriveController(
+                    new PIDConstants(10, 0, 0),
+                    new PIDConstants(7, 0, 0)
+                ),
+                config,
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                this 
+            );
+        } catch (Exception ex) {
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+        }
+    }
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+     * @param modules               Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+        configureAutoBuilder();
+    }
+
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+     * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+     *                                unspecified or set to 0 Hz, this is 250 Hz on
+     *                                CAN FD, and 100 Hz on CAN 2.0.
+     * @param modules                 Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        double odometryUpdateFrequency,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, odometryUpdateFrequency, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+        configureAutoBuilder();
+    }
+
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+     * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+     *                                  unspecified or set to 0 Hz, this is 250 Hz on
+     *                                  CAN FD, and 100 Hz on CAN 2.0.
+     * @param odometryStandardDeviation The standard deviation for odometry calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  and radians
+     * @param visionStandardDeviation   The standard deviation for vision calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  and radians
+     * @param modules                   Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        double odometryUpdateFrequency,
+        Matrix<N3, N1> odometryStandardDeviation,
+        Matrix<N3, N1> visionStandardDeviation,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+        configureAutoBuilder();
+    }
+
+    /**
+     * Returns a command that applies the specified control request to this swerve drivetrain.
+     *
+     * @param request Function returning the request to apply
+     * @return Command to run
+     */
+    public Command applyRequest(Supplier<SwerveRequest> request) {
+        return run(() -> this.setControl(request.get()));
+    }
+
+    @Override
+    public void periodic() {
+        /*
+         * Periodically try to apply the operator perspective.
+         * If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
+         * This allows us to correct the perspective in case the robot code restarts mid-match.
+         * Otherwise, only check and apply the operator perspective if the DS is disabled.
+         * This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
+         */
+        if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+            DriverStation.getAlliance().ifPresent(allianceColor -> {
+                setOperatorPerspectiveForward(
+                    allianceColor == Alliance.Red
+                        ? kRedAlliancePerspectiveRotation
+                        : kBlueAlliancePerspectiveRotation
+                );
+                m_hasAppliedOperatorPerspective = true;
+            });
+        }
+    }
+
+    private void startSimThread() {
+        m_lastSimTime = Utils.getCurrentTimeSeconds();
+
+        /* Run simulation at a faster rate so PID gains behave more reasonably */
+        m_simNotifier = new Notifier(() -> {
+            final double currentTime = Utils.getCurrentTimeSeconds();
+            double deltaTime = currentTime - m_lastSimTime;
+            m_lastSimTime = currentTime;
+
+            /* use the measured time delta, get battery voltage from WPILib */
+            updateSimState(deltaTime, RobotController.getBatteryVoltage());
+        });
+        m_simNotifier.startPeriodic(kSimLoopPeriod);
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     */
+    @Override
+    public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     * <p>
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    @Override
+    public void addVisionMeasurement(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+    }
+
+    /**
+     * Return the pose at a given timestamp, if the buffer is not empty.
+     *
+     * @param timestampSeconds The timestamp of the pose in seconds.
+     * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+     */
+    @Override
+    public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
+        return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+}

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

@@ -16,12 +16,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Grimpeur extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
-  SparkMax grimpeur1 = new SparkMax(8,MotorType.kBrushless);
+  SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless);
-  SparkMax grimpeur2 = new SparkMax(9,MotorType.kBrushless);
+  SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless);
   SparkMaxConfig slaveConfig = new SparkMaxConfig();
-  DigitalInput limit = new DigitalInput(1);
+  DigitalInput limit = new DigitalInput(0);
     private GenericEntry EncodeurGrimpeur =
-  teb.add("Position haut grimpeur", 10).getEntry();
+  teb.add("Position haut grimpeur", 105).getEntry();
   public void Grimper(double vitesse){
     grimpeur1.set(vitesse);
     grimpeur2.set(vitesse);
@@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase {
     return limit.get();
   }
   public double PositionFinal(){
-    return EncodeurGrimpeur.getDouble(10);
+    return EncodeurGrimpeur.getDouble(105);
   }
   /** Creates a new Grimpeur. */
   public Grimpeur() {

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

@@ -14,14 +14,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Lanceur extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
-  SparkFlex moteur1 = new SparkFlex(2, MotorType.kBrushless);
+  SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless);
-  SparkFlex moteur2 = new SparkFlex(3, MotorType.kBrushless);
+  SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
-  SparkFlex Demeleur = new SparkFlex(4, MotorType.kBrushless);
+  SparkFlex Demeleur = new SparkFlex(19, MotorType.kBrushless);
   GenericEntry vitesse = 
-  teb.add("vitesse lanceur",100).getEntry();
+  teb.add("vitesse lanceur",0.5).getEntry();
+  GenericEntry AmpLanceur = 
+  teb.add("ampérage lanceur",30).getEntry();
   public void Lancer(double vitesse){
-    moteur1.set(vitesse);
+    // moteur1.set(-vitesse);
-    moteur2.set(-vitesse);
+    moteur2.set(vitesse);
   }
   public void Demeler(double vitesse){
     Demeleur.set(vitesse);
@@ -32,8 +34,11 @@ public class Lanceur extends SubsystemBase {
   public double Amp(){
     return moteur1.getOutputCurrent();
   }
+  public double AmpBas(){
+    return AmpLanceur.getDouble(30);
+  }
   public double vitesseDemander(){
-    return vitesse.getDouble(100);
+    return vitesse.getDouble(0.5);
   }
   /** Creates a new Lanceur. */
   public Lanceur() {

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

@@ -4,8 +4,6 @@
 
 package frc.robot.subsystems;
 
-import java.util.Optional;
-
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.RainbowAnimation;
 
@@ -20,7 +18,7 @@ public class Led extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   private GenericEntry equipe = 
   teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
-  CANdle CANDle = new CANdle(7);
+  CANdle CANDle = new CANdle(17);
   RainbowAnimation rainbowAnim = new RainbowAnimation();
     public void bleu(){
     CANDle.setLEDs(0, 0, 255,0,0,8);