Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

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

@@ -0,0 +1,57 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Station"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto

@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": []
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,245 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Station"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralInspire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasChercher2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "CoralInspire"
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralInspire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueBasPorter2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "CoralExpire"
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,57 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Station"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralInspire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautPorter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,245 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautChercher"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Station"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralInspire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautPorter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautChercher2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Station"
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralInspire"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueHautPorter2"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "CoralExpire"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueMilieuStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "CoralExpire"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L3"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "CoraletAlgue"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,57 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BlueMilieuStart"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AprilTag"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "L4"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "CoralExpire"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Sortir"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.226639344262295,
+        "y": 2.748309426229507
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.699180327868851,
+        "y": 2.1728995901639334
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.296618852459016,
+        "y": 1.6334528688524586
+      },
+      "prevControl": {
+        "x": 4.5553278688524586,
+        "y": 1.9091700819672122
+      },
+      "nextControl": {
+        "x": 2.853313965378558,
+        "y": 1.5363479888253109
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2227459016393443,
+        "y": 1.141956967213115
+      },
+      "prevControl": {
+        "x": 2.2217919676666584,
+        "y": 1.3902362024139756
+      },
+      "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": -125.00000000000001
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.7761270491803276,
+        "y": 2.7602971311475404
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.152766393442623,
+        "y": 1.2138831967213104
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1987704918032787,
+        "y": 1.153944672131147
+      },
+      "prevControl": {
+        "x": 2.049897540983607,
+        "y": 1.1899077868852443
+      },
+      "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": -125.00000000000001
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 59.99999999999999
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1987704918032787,
+        "y": 1.153944672131147
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1697745901639345,
+        "y": 1.0820184426229504
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.788114754098361,
+        "y": 2.7722848360655736
+      },
+      "prevControl": {
+        "x": 3.5483606557377048,
+        "y": 1.1899077868852452
+      },
+      "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": 59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -125.00000000000001
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2107581967213117,
+        "y": 1.141956967213115
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.392520491803279,
+        "y": 1.7293545081967205
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9969262295081966,
+        "y": 4.019006147540983
+      },
+      "prevControl": {
+        "x": 1.738217213114754,
+        "y": 3.5035348360655734
+      },
+      "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": -125.00000000000001
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.204610655737704,
+        "y": 1.058043032786885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.40062663880838,
+        "y": 1.8077964923366423
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.214651639344262,
+        "y": 2.7722848360655736
+      },
+      "prevControl": {
+        "x": 6.021565749152438,
+        "y": 2.0272293626478377
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.262602459016393,
+        "y": 5.33765368852459
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.567315573770491,
+        "y": 6.272694672131147
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3066598360655735,
+        "y": 6.920030737704917
+      },
+      "prevControl": {
+        "x": 2.074600566094311,
+        "y": 6.608197684403563
+      },
+      "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": 125.00000000000001
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.728176229508197,
+        "y": 5.277715163934426
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6884488572050125,
+        "y": 5.738000701502482
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2946721311475409,
+        "y": 6.88406762295082
+      },
+      "prevControl": {
+        "x": 1.5370661625758562,
+        "y": 6.822870126858832
+      },
+      "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": 125.00000000000001
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.99999999999999
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2946721311475409,
+        "y": 6.88406762295082
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8197466808473133,
+        "y": 6.152201615570423
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.7401639344262296,
+        "y": 5.313678278688524
+      },
+      "prevControl": {
+        "x": 2.9533875048545335,
+        "y": 6.158498893994983
+      },
+      "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": -59.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 125.00000000000001
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2467213114754097,
+        "y": 6.896055327868852
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.728176229508196,
+        "y": 6.28468237704918
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9969262295081966,
+        "y": 3.9950307377049183
+      },
+      "prevControl": {
+        "x": 2.6372950819672134,
+        "y": 4.906096311475409
+      },
+      "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": 125.00000000000001
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.22858606557377,
+        "y": 7.075870901639344
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.75022271481963,
+        "y": 5.75328924697144
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.226639344262295,
+        "y": 5.313678278688524
+      },
+      "prevControl": {
+        "x": 6.307613733266616,
+        "y": 6.263998815998496
+      },
+      "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": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.300512295081968,
+        "y": 3.971055327868852
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.294286688638351,
+        "y": 3.9754565484205786
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.9818647540983605,
+        "y": 3.971055327868852
+      },
+      "prevControl": {
+        "x": 6.956229117816136,
+        "y": 3.9576132249181755
+      },
+      "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/New Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.8820696721311476,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8706512438743716,
+        "y": 5.690872003047288
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8820696721311476,
+        "y": 3.1279999999999997
+      },
+      "prevControl": {
+        "x": 1.8863317961339294,
+        "y": 3.3779636659576444
+      },
+      "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.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.504303278688524,
+        "y": 6.464497950819672
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.119564732833961,
+        "y": 6.478684258786044
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.815652951699465,
+        "y": 6.464497950819672
+      },
+      "prevControl": {
+        "x": 6.633711863494241,
+        "y": 6.455611583097
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 50.0,
+    "maxAngularAcceleration": 100.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/settings.json

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

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

@@ -4,42 +4,155 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.GrimperHaut;
+import frc.robot.TunerConstants.TunerConstants;
-import frc.robot.commands.GrimpeurBas;
+import frc.robot.commands.AprilTag3;
-import frc.robot.commands.GrimpeurManuel;
+import frc.robot.commands.AprilTag3G;
-import frc.robot.commands.ResetGrimpeur;
+import frc.robot.commands.Forme3;
+import frc.robot.commands.RainBow;
+import frc.robot.commands.StationPince;
+import frc.robot.commands.reset;
+import frc.robot.commands.Elevateur.Depart;
+import frc.robot.commands.Elevateur.ElevateurManuel;
+import frc.robot.commands.Elevateur.L2;
+import frc.robot.commands.Elevateur.L3;
+import frc.robot.commands.Elevateur.L4;
+import frc.robot.commands.Pince.AlgueExpire;
+import frc.robot.commands.Pince.Algue_inspire;
+import frc.robot.commands.Pince.CorailAspir;
+import frc.robot.commands.Pince.CoralAlgueInspire;
+import frc.robot.commands.Pince.CoralExpire;
+import frc.robot.commands.Pince.PinceManuel;
 import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Elevateur;
 import frc.robot.subsystems.Grimpeur;
+import frc.robot.subsystems.Limelight3;
+import frc.robot.subsystems.Limelight3G;
+import frc.robot.subsystems.Pince;
 
 public class RobotContainer {
-  CommandXboxController manette1 = new CommandXboxController(0);
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
-  CommandXboxController manette2 = new CommandXboxController(1);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-  Grimpeur grimpeur = new Grimpeur();
+
+  /* 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 Telemetry logger = new Telemetry(MaxSpeed);
+  private final CommandXboxController manette1 = new CommandXboxController(0);
+  private final CommandXboxController manette2 = new CommandXboxController(1);
+  private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
+  public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private final SendableChooser<Command> autoChooser;
+  public double getAngle() {
+    return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
+  }
+  
+  Elevateur elevateur = new Elevateur();
+  Pince pince = new Pince();
+  ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
+  PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY);
   Bougie bougie = new Bougie();
-  GrimpeurManuel grimpeurManuel = new GrimpeurManuel(grimpeur, manette2::getLeftY);
+  Limelight3G limelight3g = new Limelight3G();
+  Limelight3 limelight3 = new Limelight3();
+  Pose2d pose = new Pose2d();
+  Grimpeur Grimpeur = new Grimpeur();
+  
   public RobotContainer() {
-    grimpeur.setDefaultCommand(new RunCommand(()->{
+    autoChooser = AutoBuilder.buildAutoChooser("New Auto");
-      if(grimpeur.stop()){
+    SmartDashboard.putData("Auto Mode", autoChooser);
-       grimpeur.grimpe(0); 
-      }
-      else{
-        grimpeur.grimpe(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
-    }}, grimpeur));
     configureBindings();
-  }
+    NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
-  
+    NamedCommands.registerCommand("Station",new StationPince(pince, elevateur));
+    NamedCommands.registerCommand("L4", new L4(elevateur, pince));
+    NamedCommands.registerCommand("L3", new L3(elevateur, pince));
+    NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
+    NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
+    }
   private void configureBindings() {
-    manette2.leftBumper().whileTrue(new GrimperHaut(grimpeur,bougie));
+    
-    manette2.rightBumper().whileTrue(new GrimpeurBas(grimpeur));
+ drivetrain.setDefaultCommand(
-    manette2.a().whileTrue(new ResetGrimpeur(grimpeur));
+          // Drivetrain will execute this command periodically
+          drivetrain.applyRequest(() ->
+            drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
+              .withVelocityY(MathUtil.applyDeadband(manette1.getLeftY()*manette1.getLeftY()*manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left)
+              .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*manette1.getRightX()*manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
+        )
+      );
+    // Elevateur manuel
+    drivetrain.registerTelemetry(logger::telemeterize);
+    elevateur.setDefaultCommand(new RunCommand(()->{
+      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
+    }, elevateur));
+
+    //Pince manuel
+    pince.setDefaultCommand(new RunCommand(()->{
+      pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05));
+    }, pince));
+     
+    
+    // manette1
+
+    // reset the field-centric heading on start press
+    manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
+
+    //elevateur
+    manette1.a().whileTrue(new Depart(elevateur, pince));
+    manette1.b().whileTrue(new L2(elevateur,pince));
+    manette1.x().whileTrue(new L3(elevateur, pince));
+    manette1.y().whileTrue(new L4(elevateur, pince));
+
+    //pince
+    manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
+    manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
+    manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie));
+
+    //manette2
+
+    //pince
+    manette2.a().whileTrue(new CorailAspir(pince));
+    manette2.start().whileTrue(new reset(elevateur,pince));
+    manette2.b().whileTrue(new Algue_inspire(pince));
+    manette2.start().whileTrue(new reset(elevateur,pince));
+    manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
+
+    //limelight
+    manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
+    manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
   }
   
-  public Command getAutonomousCommand() {
+    // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
-    return Commands.print("No autonomous command configured");
+    
-  }
+    public Command getAutonomousCommand() {
+      return new SequentialCommandGroup(Commands.runOnce(()->{
+        boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
+        if(flip){
+            drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()));
+        }
+        else{
+            drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose());
+        }
+      }),autoChooser.getSelected(), new RainBow(bougie));
+    }    
 }

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

@@ -0,0 +1,124 @@
+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();
+    }
+
+    /* 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];
+    private final double[] m_moduleStatesArray = new double[8];
+    private final double[] m_moduleTargetsArray = new double[8];
+
+    /** 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 */
+        m_poseArray[0] = state.Pose.getX();
+        m_poseArray[1] = state.Pose.getY();
+        m_poseArray[2] = state.Pose.getRotation().getDegrees();
+        for (int i = 0; i < 4; ++i) {
+            m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
+            m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
+            m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
+            m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
+        }
+
+        SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
+        SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
+        SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
+        SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
+
+        /* Telemeterize the pose to a Field2d */
+        fieldTypePub.set("Field2d");
+        fieldPub.set(m_poseArray);
+
+        /* Telemeterize the module states 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));
+
+            SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
+        }
+    }
+}

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

@@ -0,0 +1,290 @@
+package frc.robot.TunerConstants;
+
+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 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(68.294).withKI(0).withKD(4.7806)
+     //   .withKS(0.20754).withKV(2.4832).withKA(0.099824)
+      //  .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+        private static final Slot0Configs steerGains = new Slot0Configs()
+        .withKP(43.502).withKI(0).withKD(2.7353)
+        .withKS(0.027275).withKV(2.5818).withKA(0.1055)
+        .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(63.167).withKI(0).withKD(0.54521)
+        .withKS(0.18227).withKV(0.12483);
+
+    // 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.0);
+
+    // 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(5.21);
+
+    // 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(2);
+
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final int kPigeonId = 13;
+
+    // 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 = 4;
+    private static final int kFrontLeftSteerMotorId = 5;
+    private static final int kFrontLeftEncoderId = 12;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375);
+    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final boolean kFrontLeftEncoderInverted = false;
+
+    private static final Distance kFrontLeftXPos = Inches.of(13.5);
+    private static final Distance kFrontLeftYPos = Inches.of(10.5);
+
+    // Front Right
+    private static final int kFrontRightDriveMotorId = 2;
+    private static final int kFrontRightSteerMotorId = 6;
+    private static final int kFrontRightEncoderId = 9;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625);
+    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final boolean kFrontRightEncoderInverted = false;
+
+    private static final Distance kFrontRightXPos = Inches.of(13.5);
+    private static final Distance kFrontRightYPos = Inches.of(-10.5);
+
+    // Back Left
+    private static final int kBackLeftDriveMotorId = 18;
+    private static final int kBackLeftSteerMotorId = 8;
+    private static final int kBackLeftEncoderId = 11;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625);
+    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final boolean kBackLeftEncoderInverted = false;
+
+    private static final Distance kBackLeftXPos = Inches.of(-13.5);
+    private static final Distance kBackLeftYPos = Inches.of(10.5);
+
+    // Back Right
+    private static final int kBackRightDriveMotorId = 3;
+    private static final int kBackRightSteerMotorId = 7;
+    private static final int kBackRightEncoderId = 10;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125);
+    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final boolean kBackRightEncoderInverted = false;
+
+    private static final Distance kBackRightXPos = Inches.of(-13.5);
+    private static final Distance kBackRightYPos = Inches.of(-10.5);
+
+
+    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/commands/AprilTag3.java

@@ -0,0 +1,81 @@
+// 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 static edu.wpi.first.units.Units.*;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.TunerConstants.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 AprilTag3 extends Command {
+  private Limelight3 limelight3;
+  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private DoubleSupplier x;
+  private DoubleSupplier y;
+    /* 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
+  /** Creates a new AprilTag3G. */
+  public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
+    this.limelight3 = limelight3;
+    this.drivetrain = drivetrain;
+    this.x = x;
+    this.y = y;
+    addRequirements(limelight3,drivetrain);
+    
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight3.Apriltag();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double a = limelight3.getX();
+   
+    if(limelight3.getV() == true){
+      drivetrain.setControl(drive.
+      withRotationalRate(a/10).
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/10);
+    }
+    else{
+      drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.
+    withRotationalRate(0)
+    .withVelocityX(0)
+    .withVelocityY(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,78 @@
+// 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 static edu.wpi.first.units.Units.*;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.TunerConstants.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 AprilTag3G extends Command {
+  private Limelight3G limelight3g;
+  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private DoubleSupplier x;
+  private DoubleSupplier y;
+    /* 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
+  /** Creates a new AprilTag3G. */
+  /** Creates a new AprilTag3G. */
+  public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
+    this.limelight3g = limelight3g;
+    this.drivetrain = drivetrain;
+    this.x = x;
+    this.y = y;
+    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() {
+    double a = limelight3g.getX();
+    if(limelight3g.getV() == true){
+      drivetrain.setControl(drive.
+      withRotationalRate(-a/5).
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/5);
+    }
+    else{
+      drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return limelight3g.getX()<1 && limelight3g.getX()>-1;
+  }
+}

src/main/java/frc/robot/commands/Elevateur/Depart.java

@@ -0,0 +1,49 @@
+// 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.Elevateur;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevateur;
+import frc.robot.subsystems.Pince;
+
+/* 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 Depart extends Command {
+  private Elevateur elevateur;
+  /** Creates a new L2. */
+  public Depart(Elevateur elevateur, Pince pince) {
+    this.elevateur = elevateur;
+    addRequirements(elevateur);
+    // 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(elevateur.limit2()==true){
+      elevateur.vitesse(0);
+      elevateur.reset();
+    }
+    else{
+      elevateur.vitesse(.5);
+    }
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevateur.vitesse(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevateur.limit2() == true;
+  }
+}

src/main/java/frc/robot/commands/Elevateur/ElevateurManuel.java

@@ -0,0 +1,46 @@
+// 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.Elevateur;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevateur;
+
+/* 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 ElevateurManuel extends Command {
+  private DoubleSupplier doubleSupplier;
+  private Elevateur elevateur;
+  /** Creates a new ElevateurManuel. */
+  public ElevateurManuel(Elevateur elevateur,DoubleSupplier doubleSupplier) {
+    this.doubleSupplier = doubleSupplier;
+    this.elevateur = elevateur;
+    addRequirements(elevateur);
+    // 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 (elevateur.limit2())
+    elevateur.vitesse(doubleSupplier.getAsDouble()/3.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevateur.vitesse(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Elevateur/L2.java

@@ -0,0 +1,78 @@
+// 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.Elevateur;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.Elevateur;
+import frc.robot.subsystems.Pince;
+
+/* 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 L2 extends Command {
+  private Elevateur elevateur;
+  private Pince pince;
+
+  NetworkTableInstance networktable = NetworkTableInstance.getDefault();
+  NetworkTable tabelevateur = networktable.getTable("tabelevateur");
+  private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeurelevateurbasL2").subscribe(-1);
+  private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurelevateurhautL2").subscribe(-0.9);
+  private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL2").subscribe(-1);
+  private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpinceautL2").subscribe(-0.9);
+  /** Creates a new L2. */
+  public L2(Elevateur elevateur,Pince pince) {
+    this.elevateur = elevateur;
+    this.pince = pince;
+    addRequirements(elevateur,pince);
+    // 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 encodeurbase = encodeur1.get();
+    double encodeurhaute = encodeur2.get();
+    double encodeurbasp = encodeur3.get();
+    double encodeurhautp = encodeur4.get();
+    if(elevateur.position()<=encodeurbase && elevateur.position()>=-encodeurhaute){
+      elevateur.vitesse(0);
+    }
+    else if(elevateur.position()>=encodeurbase){
+      elevateur.vitesse(-0.2);
+    }
+    else{
+      elevateur.vitesse(.2);
+    // }
+    // if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){
+    //   pince.pivote(0);
+     
+    // }
+    // else if(pince.encodeurpivot()>=510){
+    //   pince.pivote(0.2);
+    // }
+    // else{
+    //   pince.pivote(-0.2);
+     }
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevateur.vitesse(0);
+    pince.pivote(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Elevateur/L3.java

@@ -0,0 +1,76 @@
+// 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.Elevateur;
+
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.Elevateur;
+import frc.robot.subsystems.Pince;
+
+/* 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 L3 extends Command {
+  private Elevateur elevateur;
+  private Pince pince;
+    NetworkTableInstance networktable = NetworkTableInstance.getDefault();
+  NetworkTable tabelevateur = networktable.getTable("tabelevateur");
+  private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeurhautL3").subscribe(-2.9);
+  private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurbasL3").subscribe(-3);
+  private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL3").subscribe(-1);
+  private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpincehautL3").subscribe(-0.9);
+  /** Creates a new L2. */
+  public L3(Elevateur elevateur,Pince pince) {
+    this.elevateur = elevateur;
+    this.pince = pince;  
+    addRequirements(elevateur,pince);
+    // 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 encodeurHaute = encodeur1.get();
+    double encodeurBase = encodeur2.get();
+    double encodeurbase = encodeur3.get();
+    double encodeurhaute = encodeur4.get();
+    if(elevateur.position()<=-encodeurHaute && elevateur.position()>=encodeurBase){
+      elevateur.vitesse(0);
+    }
+    else if(elevateur.position()>=-encodeurHaute){
+      elevateur.vitesse(-0.2);
+    }
+    else{
+      elevateur.vitesse(.2);
+    }
+    // if(pince.encodeurpivot()>=700 && pince.encodeurpivot()<=710){
+    //   pince.pivote(0);
+    // }
+    // else if(pince.encodeurpivot()>=710){
+    //   pince.pivote(0.2);
+    // }
+    // else{
+    //   pince.pivote(-0.2);
+    // }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevateur.vitesse(0);
+    pince.pivote(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Elevateur/L4.java

@@ -0,0 +1,79 @@
+// 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.Elevateur;
+
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.DoubleTopic;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.Elevateur;
+import frc.robot.subsystems.Pince;
+
+/* 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 L4 extends Command {
+  private Elevateur elevateur;
+  private Pince pince;
+  NetworkTableInstance networktable = NetworkTableInstance.getDefault();
+  NetworkTable tabelevateur = networktable.getTable("tabelevateur");
+  private DoubleTopic encodeur1topic = tabelevateur.getDoubleTopic("encodeurbasL4");
+  DoubleSubscriber encodeur1 = encodeur1topic.subscribe(-6.4);
+  private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurhautL4").subscribe(-6.5);
+  private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL4").subscribe(-1);
+  private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpincehautL4").subscribe(-0.9);
+  /** Creates a new L2. */
+  public L4(Elevateur elevateur,Pince pince) {
+    this.elevateur = elevateur;
+    this.pince = pince;
+    encodeur1topic.setPersistent(true);
+    addRequirements(elevateur,pince);
+    // 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 encodeurBase = encodeur1.get();
+    double encodeurhaute = encodeur2.get();
+    double encodeurbasp = encodeur3.get();
+    double encodeurhautp = encodeur4.get();
+    if(elevateur.position()<=encodeurhaute && elevateur.position()>=encodeurBase){
+      elevateur.vitesse(0);
+    }
+    else if(elevateur.position()>=encodeurhaute){
+      elevateur.vitesse(-0.2);
+    }
+    else{
+      elevateur.vitesse(.2);
+    }
+    // if(pince.encodeurpivot()>=800 && pince.encodeurpivot()<=809){
+    //   pince.pivote(0);
+    // }
+    // else if(pince.encodeurpivot()>=810){
+    //   pince.pivote(0.2);
+    // }
+    // else{
+    //   pince.pivote(-0.2);
+    // }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevateur.vitesse(0);
+    pince.pivote(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,79 @@
+// 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 static edu.wpi.first.units.Units.*;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.TunerConstants.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 Forme3 extends Command {
+  private Limelight3 limelight3;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private DoubleSupplier x;
+  private DoubleSupplier y;
+    /* 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
+  /** Creates a new Forme3. */
+  public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
+    this.limelight3 = limelight3;
+    this.drivetrain = drivetrain;
+    this.x = x;
+    this.y = y;
+    addRequirements(limelight3,drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight3.Forme();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double a = limelight3.getX();
+    if(limelight3.getV() == true){
+      drivetrain.setControl(drive.
+      withRotationalRate(a/10).
+      withVelocityX(x.getAsDouble()).
+      withVelocityY(y.getAsDouble()));  
+       System.out.println(a/10);
+    }
+    else{
+      drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.
+      withRotationalRate(0).
+      withVelocityX(0).
+      withVelocityY(0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/Algue1Test.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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 Algue1Test extends Command {
+  private Pince pince;
+  /** Creates a new AlgueTest. */
+  public Algue1Test(Pince pince) {
+    this.pince = pince;
+    addRequirements(pince);
+    // 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() {
+    pince.algue1Test(0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.algue1Test(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/Algue2Test.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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 Algue2Test extends Command {
+  private Pince pince;
+  /** Creates a new AlgueTest. */
+  public Algue2Test(Pince pince) {
+    this.pince = pince;
+    addRequirements(pince);
+    // 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() {
+    pince.algue2Test(0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.algue2Test(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/AlgueExpire.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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Pince;
+
+/* 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 AlgueExpire extends Command {
+  private Pince pince;
+  private Bougie bougie;
+  /** Creates a new CoralAlgue. */
+  public AlgueExpire(Pince pince,Bougie bougie) {
+    this.pince = pince;
+    this.bougie = bougie;
+    addRequirements(pince,bougie);
+    // 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(pince.emperagealgue()>60){
+      pince.aspirealgue(-0.5);
+    }
+    else{
+      pince.aspirealgue(-0.5);
+      bougie.Jaune();
+    }
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirealgue(0); 
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/Algue_inspire.java

@@ -0,0 +1,44 @@
+// 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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 Algue_inspire extends Command {
+  /** Creates a new Algue_inspire. */
+  private Pince pince;
+  public Algue_inspire(Pince pince) {
+    this.pince = pince;
+    addRequirements(pince);
+    // 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.
+
+  //ajouter l'amperage pour arreter les moteurs
+  @Override
+  public void execute() {
+    pince.aspirealgue(0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirealgue(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/CorailAspir.java

@@ -0,0 +1,42 @@
+// 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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 CorailAspir extends Command {
+  /** Creates a new CorailAspir. */
+  private Pince pince;
+  public CorailAspir(Pince pince) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.pince = pince;
+    addRequirements(pince);
+  }
+
+  // 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() {
+
+    pince.aspirecoral(0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirecoral(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/CorailTest.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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 CorailTest extends Command {
+  private Pince pince;
+  /** Creates a new AlgueTest. */
+  public CorailTest(Pince pince) {
+    this.pince = pince;
+    addRequirements(pince);
+    // 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() {
+    pince.aspirecoral(0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirecoral(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/CoralAlgueInspire.java

@@ -0,0 +1,53 @@
+// 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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Pince;
+
+/* 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 CoralAlgueInspire extends Command {
+  private Pince pince;
+  private Bougie bougie;
+  /** Creates a new CoralAlgue. */
+  public CoralAlgueInspire(Pince pince, Bougie bougie) {
+    this.pince = pince;
+    this.bougie = bougie;
+    addRequirements(pince,bougie);
+    // 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() {
+    pince.aspirecoral(-.5);
+    if(pince.emperagealgue()>60){
+      pince.aspirealgue(0);
+      bougie.Bleu();
+     } 
+     else{
+      pince.aspirealgue(0.5);
+      }
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirecoral(0);
+    pince.aspirealgue(0); 
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/CoralExpire.java

@@ -0,0 +1,49 @@
+// 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.Pince;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+import frc.robot.subsystems.Bougie;
+/* 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 CoralExpire extends Command {
+  private Pince pince;
+  Bougie bougie;
+  /** Creates a new CoralAlgue. */
+  public CoralExpire(Pince pince, Bougie bougie) {
+    this.pince = pince;
+    this.bougie = bougie;
+    addRequirements(pince,bougie);
+    // 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(pince.emperagecoral() > 60){
+    //  pince.aspirecoral(0);
+   //}
+   //else{
+    pince.aspirecoral(-.5);
+    //bougie.Jaune();
+   // }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.aspirecoral(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/Pince/DepartPince.java

@@ -2,18 +2,18 @@
 // 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;
+package frc.robot.commands.Pince;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Grimpeur;
+import frc.robot.subsystems.Pince;
 
 /* 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 GrimpeurBas extends Command {
+public class DepartPince extends Command {
-  private Grimpeur grimpeur;
+  private Pince pince;
-  /** Creates a new GrimpeurBas. */
+  /** Creates a new DepartPince. */
-  public GrimpeurBas(Grimpeur grimpeur) {
+  public DepartPince(Pince pince) {
-    this.grimpeur = grimpeur;
+    this.pince = pince;
-    addRequirements(grimpeur);
+    addRequirements(pince);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -24,20 +24,19 @@ public class GrimpeurBas extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.encodeur()>=-38.5 && grimpeur.encodeur()<=-39.19){
+    if(pince.position()==true){
-      grimpeur.grimpe(0);
+      pince.pivote(0);
+      pince.reset();
     }
-    else if(grimpeur.encodeur()>=-38.5){
+    else{
-      grimpeur.grimpe(-0.5);
+      pince.pivote(.2);
     }
-   else{grimpeur.grimpe(0.5);
-  } 
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    grimpeur.grimpe(0);
+    pince.pivote(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/Pince/PinceManuel.java

@@ -0,0 +1,50 @@
+// 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.Pince;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pince;
+
+/* 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 PinceManuel extends Command {
+  private Pince pince;
+  private DoubleSupplier x;
+  /** Creates a new PinceManuel. */
+  public PinceManuel(Pince pince, DoubleSupplier x) {
+    this.pince = pince;
+    this.x = x;
+    //this.doubleSupplier = doubleSupplier;
+    addRequirements(pince);
+    // 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(pince.position()){
+       pince.pivote(0);
+     }
+     else{
+      pince.pivote(x.getAsDouble()/3.5);
+     }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.pivote(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -19,19 +19,15 @@ public class RainBow extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {bougie.RainBow();}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
+  public void execute() {}
-    bougie.RainBow();
-  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
+  public void end(boolean interrupted) {bougie.RainBowStop();}
-    bougie.RainBowStop();
-  }
 
   // Returns true when the command should end.
   @Override

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

@@ -0,0 +1,77 @@
+// 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.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevateur;
+import frc.robot.subsystems.Pince;
+
+/* 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 StationPince extends Command {
+  
+  private Pince pince;
+  private Elevateur elevateur;
+  NetworkTableInstance networktable = NetworkTableInstance.getDefault();
+  NetworkTable tabelevateur = networktable.getTable("tabelevateur");
+  private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeur bas Station").subscribe(-0.5);
+  private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeur haut Station").subscribe(-0.4);
+  private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeur pince bas Station").subscribe(-1);
+  private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeur pince haut Station").subscribe(-0.9);
+  /** Creates a new L2Pince. */
+  public StationPince(Pince pince,Elevateur elevateur) {
+    this.elevateur = elevateur;
+    this.pince = pince;
+    addRequirements(pince,elevateur);
+    // 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 encodeurBase = encodeur1.get();
+    double encodeurhaute = encodeur2.get();
+    double encodeurbasp = encodeur3.get();
+    double encodeurhautp = encodeur4.get();
+    pince.aspirecoral(0.5);
+    if(pince.encodeurpivot()<=encodeurBase && pince.encodeurpivot()>=encodeurhaute){
+      pince.pivote(0);
+    }
+    else if(pince.encodeurpivot()>=encodeurBase){
+      pince.pivote(0.2);
+    }
+    else{
+      pince.pivote(-0.2);
+    }
+    if(elevateur.position()>=400 && elevateur.position()<=410){
+      elevateur.vitesse(0);
+    }
+    else if(elevateur.position()>=410){
+      elevateur.vitesse(0.3);
+    }
+    else{
+      elevateur.vitesse(-.3);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    pince.pivote(0);
+    pince.aspirecoral(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/grimpeur/GrimperHaut.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Bougie;

src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java

@@ -0,0 +1,57 @@
+// 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.grimpeur;
+
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Grimpeur;
+
+/* 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 GrimpeurBas extends Command {
+  private Grimpeur grimpeur;
+   NetworkTableInstance networktable = NetworkTableInstance.getDefault();
+  NetworkTable tabelevateur = networktable.getTable("tabelevateur");
+  private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeurgrimpeurbas").subscribe(-39.19);
+  private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurgrimpeurhaut ").subscribe(-38.5);
+  /** Creates a new GrimpeurBas. */
+  public GrimpeurBas(Grimpeur grimpeur) {
+    this.grimpeur = grimpeur;
+    addRequirements(grimpeur);
+    // 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 encodeurbas = encodeur1.get();
+    double encodeurhaut = encodeur2.get();
+    if(grimpeur.encodeur()>=encodeurhaut && grimpeur.encodeur()<=encodeurbas){
+      grimpeur.grimpe(0);
+    }
+    else if(grimpeur.encodeur()>=encodeurhaut){
+      grimpeur.grimpe(-0.5);
+    }
+   else{grimpeur.grimpe(0.5);
+  } 
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    grimpeur.grimpe(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/grimpeur/GrimpeurManuel.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import java.util.function.DoubleSupplier;
 

src/main/java/frc/robot/commands/grimpeur/ResetGrimpeur.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Grimpeur;

src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java

@@ -0,0 +1,63 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseAlgue extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public BalayeuseAlgue(Requin requin, Bougie bougie) {
+    this.requin = requin;
+    this.bougie =bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=500 &&  requin.encodeur()<=510)
+     {
+       requin.rotationer(0);
+       if(requin.amp()> 60){
+       requin.balaye(0);
+       bougie.Vert();
+     }
+      else
+      {
+           requin.balaye(0.5);
+      }
+     }
+     else if(requin.encodeur()>=510){
+     requin.rotationer(0.5);
+     }
+     else{
+      requin.rotationer(-0.5);
+     }
+    
+   }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseBas.java

@@ -0,0 +1,40 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseBas extends Command {
+  private Requin requin;
+  /** Creates a new Balayeuse. */
+  public BalayeuseBas(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
+    // 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() {
+  requin.rotationer(-0.5);
+  }
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseCoral.java

@@ -0,0 +1,67 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseCoral extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public BalayeuseCoral(Requin requin, Bougie bougie) {
+    this.requin = requin;
+    this.bougie = bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=100 &&  requin.encodeur()<=110){
+       requin.rotationer(0);
+      if(requin.amp()>60){
+      requin.balaye(0);
+       bougie.Vert();
+       if(requin.enHaut()){
+        requin.rotationer(0);
+       }
+        else{
+          requin.rotationer(0.5);
+        }
+    }
+     else{
+       requin.balaye(0.5);
+     }
+     }
+     else if(requin.encodeur()>=110){
+     requin.rotationer(0.5);
+     }
+     else{
+       requin.rotationer(-0.5);
+     }
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseHaut.java

@@ -0,0 +1,47 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseHaut extends Command {
+  private Requin requin;
+  /** Creates a new Balayeuse. */
+  public BalayeuseHaut(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
+    // 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(requin.enHaut()==true){
+      requin.rotationer(0);
+      requin.reset();
+    }
+    else{
+      requin.rotationer(-0.5);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/ExpireAlgue.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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 ExpireAlgue extends Command {
+  private Requin requin;
+  /** Creates a new ExpireAlgue. */
+  public ExpireAlgue(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
+    // 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() {
+    requin.balaye(0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/L1Requin.java

@@ -0,0 +1,65 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 L1Requin extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public L1Requin(Requin requin,Bougie bougie) {
+    this.requin = requin;
+    this.bougie = bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=800 &&  requin.encodeur()<=810){
+      requin.rotationer(0);
+      if(requin.amp()>8){
+      requin.balaye(-0.5);
+    }
+        else{
+          requin.balaye(0); 
+          bougie.Rouge();
+      }
+    }
+    else if(requin.encodeur()>=810){
+    requin.rotationer(0.5);
+    }
+    else{
+      requin.rotationer(-0.5);
+    }
+    
+    
+      
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/aspire.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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 aspire extends Command {
+  /** Creates a new aspire. */
+  private Requin requin;
+  public aspire(Requin requin) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
+  }
+
+  // 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() {
+    requin.balaye(0.3);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/exspire.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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 exspire extends Command {
+  /** Creates a new aspire. */
+  private Requin requin;
+  public exspire(Requin requin) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
+  }
+
+  // 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() {
+    requin.balaye(-0.3);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/requin_manuel.java

@@ -0,0 +1,49 @@
+// 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.requin;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 requin_manuel extends Command {
+  /** Creates a new requin_manuel. */
+  private Requin requin;
+  private DoubleSupplier x;
+  public requin_manuel(Requin requin) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
+  }
+
+  // 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(requin.enHaut()){
+    requin.rotationer(0);  
+    }
+    else{
+      requin.rotationer(x.getAsDouble());
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,45 @@
+// 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.Elevateur;
+import frc.robot.subsystems.Pince;
+
+
+
+/* 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 reset extends Command {
+  /** Creates a new reset. */
+  private Elevateur elevateur;
+  private Pince pince;
+  public reset(Elevateur elevateur, Pince pince) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.elevateur = elevateur;
+    this.pince = pince;
+    addRequirements(elevateur,pince);
+  }
+
+  // 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() {
+    elevateur.reset();
+    pince.reset();
+  }
+
+  // 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/subsystems/Bougie.java

@@ -11,7 +11,7 @@ import com.ctre.phoenix.led.RainbowAnimation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Bougie extends SubsystemBase {
-  CANdle candle = new CANdle(5);
+  CANdle candle = new CANdle(23);
   CANdleConfiguration config = new CANdleConfiguration();
   RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 64);
   /** Creates a new Bougie. */
@@ -20,19 +20,37 @@ public class Bougie extends SubsystemBase {
     candle.configAllSettings(config);
   }
   public void Rouge() {
-   candle.setLEDs(255, 0, 0);
+   candle.setLEDs(255, 0, 0,0,8,8);
+   candle.setLEDs(255, 0, 0,0,24,8);
+   candle.setLEDs(255, 0, 0,0,40,8);
+   candle.setLEDs(255, 0, 0,0,56,8);
   }
   public void Vert() {
-   candle.setLEDs(0, 255, 0);
+   candle.setLEDs(0, 255, 0,0,8,8);
+   candle.setLEDs(0, 255, 0,0,24,8);
+   candle.setLEDs(0, 255, 0,0,40,8);
+   candle.setLEDs(0, 255, 0,0,56,8);
   }
   public void Bleu() {
-   candle.setLEDs(0, 0, 255);
+    candle.setLEDs(0, 0, 255,0,16,8);
+    candle.setLEDs(0, 0, 255,0,32,8);
+    candle.setLEDs(0, 0, 255,0,48,8);
+    candle.setLEDs(0, 0, 255,0,64,8);
+  }
+   public void Jaune() {
+   candle.setLEDs(255, 215, 0,0,16,8);
+   candle.setLEDs(255, 215, 0,0,32,8);
+   candle.setLEDs(255, 215, 0,0,48,8);
+   candle.setLEDs(255, 215, 0,0,64,8);
+   }
+  public void RainBow(){
+    candle.animate(rainbowAnim);
+  }
+  public void RainBowStop(){
+    candle.animate(null);
   }
-  public void RainBow(){candle.animate(rainbowAnim);}
-  public void RainBowStop(){candle.animate(null);}
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
   }
 }
-

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

@@ -0,0 +1,295 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.*;
+
+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.Rotation2d;
+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.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
+
+/**
+ * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Subsystem so it can easily be used in command-based projeScts.
+ */
+public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
+    
+    private static final double kSimLoopPeriod = 0.005; // 5 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();
+
+    /* Swerve requests to apply during SysId characterization */
+    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
+    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
+    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
+
+    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+
+    private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,        // Use default ramp rate (1 V/s)
+            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
+            null,        // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            output -> setControl(m_translationCharacterization.withVolts(output)),
+            null,
+            this
+        )
+    );
+
+    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
+    private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,        // Use default ramp rate (1 V/s)
+            Volts.of(7), // Use dynamic voltage of 7 V
+            null,        // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            volts -> setControl(m_steerCharacterization.withVolts(volts)),
+            null,
+            this
+        )
+    );
+
+    /*
+     * SysId routine for characterizing rotation.
+     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
+     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
+     */
+    private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            /* This is in radians per second², but SysId only supports "volts per second" */
+            Volts.of(Math.PI / 6).per(Second),
+            /* This is in radians per second, but SysId only supports "volts" */
+            Volts.of(Math.PI),
+            null, // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            output -> {
+                /* output is actually radians per second, but SysId only supports "volts" */
+                setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
+                /* also log the requested output for SysId */
+                SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
+            },
+            null,
+            this
+        )
+    );
+    
+    private void configureAutoBuilder() {
+        try {
+            var config = RobotConfig.fromGUISettings();
+            AutoBuilder.configure(
+                () -> getState().Pose,   // Supplier of current robot pose
+                this::resetPose,         // Consumer for seeding pose against auto
+                () -> getState().Speeds, // Supplier of current robot speeds
+                // Consumer of ChassisSpeeds and feedforwards to drive the robot
+                (speeds, feedforwards) -> setControl(
+                    m_pathApplyRobotSpeeds.withSpeeds(speeds)
+                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
+                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
+                ),
+                new PPHolonomicDriveController(
+                    // PID constants for translation
+                    new PIDConstants(63.167, 0, 0.54521),
+                    // PID constants for rotation
+                    new PIDConstants(7.9735, 0, 0.038499)
+                ),
+                config,
+                // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                this // Subsystem for requirements
+            );
+        } catch (Exception ex) {
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+        }
+         PPHolonomicDriveController.overrideRotationFeedback(()->{
+            return 0;
+         });  
+        }
+ 
+    /* The SysId routine to test */
+    private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer;
+
+    /**
+     * 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> requestSupplier) {
+        return run(() -> this.setControl(requestSupplier.get()));
+    }
+
+    /**
+     * Runs the SysId Quasistatic test in the given direction for the routine
+     * specified by {@link #m_sysIdRoutineToApply}.
+     *
+     * @param direction Direction of the SysId Quasistatic test
+     * @return Command to run
+     */
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return m_sysIdRoutineToApply.quasistatic(direction);
+    }
+
+    /**
+     * Runs the SysId Dynamic test in the given direction for the routine
+     * specified by {@link #m_sysIdRoutineToApply}.
+     *
+     * @param direction Direction of the SysId Dynamic test
+     * @return Command to run
+     */
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return m_sysIdRoutineToApply.dynamic(direction);
+    }
+    
+    @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);
+    }
+}

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

@@ -0,0 +1,40 @@
+// 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.subsystems;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+public class Elevateur extends SubsystemBase {
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  /** Creates a new Elevateur. */
+  public Elevateur() {
+    teb.addDouble("encodeur elevateur",this::position);
+    teb.addBoolean("limit elevateur", this::limit2);
+  }
+  final SparkMax  monte = new SparkMax(22, MotorType.kBrushless);
+  final DigitalInput limit2 = new DigitalInput(0);
+  
+  public double position(){
+    return monte.getEncoder().getPosition();
+  }
+  public void vitesse(double vitesse){
+    monte.set(vitesse);
+  }
+  public boolean limit2(){
+    return limit2.get();
+  } 
+  public void reset(){
+    monte.getEncoder().setPosition(0);
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -0,0 +1,39 @@
+// 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.subsystems;
+
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.LimelightHelpers;
+
+public class Limelight3 extends SubsystemBase {
+  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTableEntry pipeline = table.getEntry("pipeline");
+  /** Creates a new Limelight3. */
+  public Limelight3() {
+    for(int port = 5800; port <=5807; port++){
+      PortForwarder.add(port, "limelight.local", port);
+    }
+  }
+  public double getX(){
+    return LimelightHelpers.getTX("limelight-balon");
+  }
+  public boolean getV(){
+    return LimelightHelpers.getTV("limelight-balon");
+  }
+  public void Apriltag(){
+    pipeline.setNumber(1);
+  }
+  public void Forme(){
+    pipeline.setNumber(0);
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -0,0 +1,28 @@
+// 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.subsystems;
+
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.LimelightHelpers;
+
+public class Limelight3G extends SubsystemBase {
+  /** Creates a new Limelight3. */
+  public Limelight3G() {
+    for(int port = 5800; port <=5807; port++){
+      PortForwarder.add(port, "limelight.local", port);
+    }
+  }
+  public double getX(){
+    return LimelightHelpers.getTX("limelight-tag");
+  }
+  public boolean getV(){
+    return LimelightHelpers.getTV("limelight-tag");
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -0,0 +1,65 @@
+// 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.subsystems;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Pince extends SubsystemBase {
+  /** Creates a new Pince. */
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  public Pince() {
+    teb.addBoolean("limit pince",this::position);
+    teb.addDouble("encodeur pince", this::encodeurpivot);
+  }
+  final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
+  final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
+  final SparkMax algue1 = new SparkMax(16, MotorType.kBrushless);
+  final SparkMax algue2 = new SparkMax(19, MotorType.kBrushless);
+  final DigitalInput limit6 = new DigitalInput(9);
+  
+ 
+  public void aspirecoral(double vitesse){
+    coral.set(vitesse);
+  }
+public void pivote(double vitesse){
+  pivoti.set(vitesse);
+}
+public void aspirealgue(double vitesse){
+  algue2.set(-vitesse);
+  algue1.set(-vitesse);
+}
+public double encodeurpivot(){
+  return pivoti.getEncoder().getPosition();
+}
+public boolean position(){
+ return limit6.get();
+}
+public void reset(){
+  pivoti.getEncoder().setPosition(0);
+}
+public double emperagecoral(){
+  return coral.getOutputCurrent();
+}
+public double emperagealgue(){
+  return algue1.getOutputCurrent();
+}
+public void algue1Test(double vitesse){
+  algue1.set(vitesse);
+}
+public void algue2Test(double vitesse){
+  algue2.set(vitesse);
+}
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc/robot/subsystems/Requin.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.subsystems;
+
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Requin extends SubsystemBase {
+  /** Creates a new Requin. */
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  public Requin() {
+    teb.addBoolean("limit requin", this::enHaut);
+  }
+   
+  final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless);
+  final SparkMax rotatione = new SparkMax(17, MotorType.kBrushless);
+  final DigitalInput limit3 = new DigitalInput(1);
+    
+  public void balaye(double vitesse){
+    balaye.set(vitesse);
+  }
+  public void rotationer(double vitesse){
+  rotatione.set(vitesse);
+  }
+  public boolean enHaut(){
+  return limit3.get();
+  } 
+  public double encodeur(){
+  return rotatione.getEncoder().getPosition();
+  }
+  public void reset(){
+    rotatione.getEncoder().setPosition(0);
+  }
+  public double amp(){
+    return balaye.getOutputCurrent();
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

vendordeps/PathplannerLib-2025.2.3.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.2.json",
+    "fileName": "PathplannerLib-2025.2.3.json",
     "name": "PathplannerLib",
-    "version": "2025.2.2",
+    "version": "2025.2.3",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.2"
+            "version": "2025.2.3"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.2",
+            "version": "2025.2.3",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/Phoenix6-25.2.2.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.1.json",
+    "fileName": "Phoenix6-25.2.2.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.1",
+    "version": "25.2.2",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.1"
+            "version": "25.2.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -354,7 +354,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -370,7 +370,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.1",
+            "version": "25.2.2",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,