27 Commits

Author SHA1 Message Date
Antoine PerreaultE
1d5e627dbf mode auto 2026-01-21 18:04:59 -05:00
Antoine PerreaultE
3f546b61d6 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2026-01-19 18:29:04 -05:00
Antoine PerreaultE
5a8a499e05 apriltag 2026-01-19 18:28:18 -05:00
Samuel
9fa5624504 s'enligner sur sible fixe avec limelight 2026-01-15 14:20:13 -05:00
Samuel
2b5a298433 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-12 11:09:54 -05:00
Samuel
efd3f23fa2 changement 2025-12-12 11:08:36 -05:00
Antoine PerreaultE
7032b95c60 LE MODE AUTO MARCHE 2025-12-10 19:01:01 -05:00
Antoine PerreaultE
fa30757990 auto I MARCHE(pas) 2025-12-10 18:31:12 -05:00
Antoine PerreaultE
5e9f2f5244 meillieur subsytem swerve (pas tent) 2025-12-08 19:23:02 -05:00
Antoine PerreaultE
0f9a71c375 mode auto dans subsystem fini 2025-12-08 18:29:44 -05:00
Antoine PerreaultE
cf2580ca85 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-03 17:58:01 -05:00
Antoine PerreaultE
92c1c59599 2025-12-03 17:57:22 -05:00
Samuel
ca694c12d8 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-12-01 18:59:05 -05:00
Samuel
b0272a5640 path palnner peut-etre 2025-12-01 18:59:04 -05:00
Antoine PerreaultE
2f147d5edc modifier le path pour les tests 2025-11-26 17:58:22 -05:00
Antoine PerreaultE
9911ae781a Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-24 18:36:50 -05:00
Antoine PerreaultE
1bf8f02db2 s'enligner avec un apriltag fini 2025-11-24 18:36:30 -05:00
Samuel
9e16c18d31 botz = boty 2025-11-24 18:34:29 -05:00
Samuel
68834d11d0 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-24 18:33:55 -05:00
Samuel
6d3b467a86 path planner est de retour 2025-11-24 18:33:53 -05:00
Antoine PerreaultE
f0c7508873 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-24 17:36:10 -05:00
Antoine PerreaultE
6f32b4424d s'enligne vers le apriltag 2025-11-24 17:35:46 -05:00
Samuel
a998218bd9 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-20 14:33:42 -05:00
Samuel
688d393d35 teste pente d'accélération dériver 2025-11-20 14:33:00 -05:00
Antoine PerreaultE
2b6839b686 2025-11-17 18:17:26 -05:00
Antoine PerreaultE
55ff246629 Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025 2025-11-17 18:17:19 -05:00
Antoine PerreaultE
3ec58dca1a 2025-11-17 18:14:43 -05:00
38 changed files with 310 additions and 2057 deletions

View File

@@ -56,5 +56,6 @@
"edu.wpi.first.math.proto.*",
"edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*",
]
],
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
}

View File

@@ -1,4 +1,9 @@
{
"Joysticks": {
"window": {
"visible": false
}
},
"System Joysticks": {
"window": {
"enabled": false

View File

@@ -1,57 +0,0 @@
{
"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
}

View File

@@ -1,151 +0,0 @@
{
"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
}

View File

@@ -1,126 +0,0 @@
{
"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
}

View File

@@ -1,245 +0,0 @@
{
"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
}

View File

@@ -1,57 +0,0 @@
{
"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
}

View File

@@ -1,151 +0,0 @@
{
"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
}

View File

@@ -1,245 +0,0 @@
{
"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
}

View File

@@ -1,87 +0,0 @@
{
"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
}

View File

@@ -1,57 +0,0 @@
{
"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
}

View File

@@ -9,6 +9,25 @@
"data": {
"pathName": "New Path"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "named",
"data": {
"name": "AprilTag"
}
}
]
}
}
]
}

View File

@@ -1,25 +0,0 @@
{
"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
}

View File

@@ -1,63 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "New Path"
}
},
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "named",
"data": {
"name": "baleeuse"
}
}
]
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "named",
"data": {
"name": "baleeuse sort"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

File diff suppressed because one or more lines are too long

View File

@@ -1,70 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 7.564241803278687,
"y": 3.899129098360655
"x": 14.0,
"y": 3.0
},
"prevControl": null,
"nextControl": {
"x": 6.506996649367605,
"y": 3.898163120386143
"x": 13.971750000000002,
"y": 4.727
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.269569672131147,
"y": 3.899129098360655
"x": 14.0,
"y": 5.0
},
"prevControl": {
"x": 7.259410179761819,
"y": 3.9002985318003183
"x": 13.9815,
"y": 3.7562499999999996
},
"nextControl": null,
"isLocked": false,
@@ -30,18 +30,29 @@
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 11.9,
"y": 4.0
},
"rotationOffset": 180.0,
"minWaypointRelativePos": 0.0,
"maxWaypointRelativePos": 1.0,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 0.1,
"maxAngularAcceleration": 0.1,
"maxVelocity": 1.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0.0,
"velocity": 0,
"rotation": 0.0
},
"reversed": false,

View File

@@ -1,54 +0,0 @@
{
"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
}

View File

@@ -9,7 +9,7 @@
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 45.3592,
"robotMass": 45.359,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.048,

View File

@@ -10,21 +10,22 @@ import java.util.Map;
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 edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
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.ParallelCommandGroup;
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.TunerConstants.TunerConstants;
import frc.robot.commands.RainBow;
import frc.robot.commands.reset;
import frc.robot.commands.Elevateur.Depart;
import frc.robot.commands.Elevateur.ElevateurManuel;
@@ -59,6 +60,7 @@ import frc.robot.commands.Elevateur.balonL2;
import frc.robot.commands.Elevateur.balonL3;
public class RobotContainer {
private final SendableChooser<Command> autoChooser;
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
@@ -83,7 +85,6 @@ public class RobotContainer {
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);
@@ -91,16 +92,19 @@ public class RobotContainer {
Bougie bougie = new Bougie();
Limelight3G limelight3g = new Limelight3G();
Limelight3 limelight3 = new Limelight3();
Pose2d pose = new Pose2d();
Requin requin = new Requin();
CorailAspir corailAspir = new CorailAspir(pince, bougie);
public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
CameraServer.startAutomaticCapture();
SmartDashboard.putData("Auto Chooser", autoChooser);
NamedCommands.registerCommand("AprilTag", new AprilTag3(limelight3,drivetrain));
NamedCommands.getCommand("AprilTag");
NamedCommands.registerCommand("L2", new L2(elevateur, pince));
configureBindings();
}
private void configureBindings() {
drivetrain.registerTelemetry(logger::telemeterize);
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.applyRequest(() ->
@@ -140,7 +144,7 @@ public class RobotContainer {
//limelight
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain));
//Pince manuel
pince.setDefaultCommand(new RunCommand(()->{
@@ -154,37 +158,39 @@ public class RobotContainer {
//Reset encodeur
manette2.start().whileTrue(new reset(elevateur, pince, requin));
drivetrain.registerTelemetry(logger::telemeterize);
}
public Command getAutonomousCommand() {
return new SequentialCommandGroup(
drivetrain.applyRequest(()->
drive.withVelocityX(0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
drivetrain.applyRequest(()->
drive.withVelocityX(0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
drivetrain.applyRequest(()->
drive.withVelocityX(-0.1*MaxSpeed)
.withVelocityY(0)
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
drivetrain.applyRequest(()->
drive.withVelocityX(0)
.withVelocityY(0)
.withRotationalRate(0)).withTimeout(0.1),
new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
new RainBow(bougie));
}
return autoChooser.getSelected();
// return new SequentialCommandGroup(
// drivetrain.applyRequest(()->
// drive.withVelocityX(0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
// drivetrain.applyRequest(()->
// drive.withVelocityX(0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
// drivetrain.applyRequest(()->
// drive.withVelocityX(-0.1*MaxSpeed)
// .withVelocityY(0)
// .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
// drivetrain.applyRequest(()->
// drive.withVelocityX(0)
// .withVelocityY(0)
// .withRotationalRate(0)).withTimeout(0.1),
// new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
// new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
// new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
// new RainBow(bougie));
}
}

View File

@@ -8,12 +8,15 @@ import static edu.wpi.first.units.Units.*;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.SynchronousInterrupt.WaitResult;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -22,7 +25,7 @@ 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 CommandSwerveDrivetrain drivetrain;
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;
@@ -32,7 +35,7 @@ public class AprilTag3 extends Command {
.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) {
public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain) {
this.limelight3 = limelight3;
this.drivetrain = drivetrain;
this.x = x;
@@ -51,27 +54,47 @@ public class AprilTag3 extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[7];
double[] BotPose = new double[6];
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
double y2 = 4;
double x2 = 0;
double angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- 360;
}
if(alliance.get() == DriverStation.Alliance.Red){
BotPose = limelight3.getBotPoseRed();
x2 =11.9;
}
else{
BotPose = limelight3.getBotPoseBlue();
x2 = 4.1;
}
double botx = BotPose[0];
double botz = BotPose[2];
System.out.println(botx);
double boty = BotPose[1];
double tx = limelight3.getTx();
double tagId = BotPose[7];
double tagId = limelight3.getTId();
if(limelight3.getV() == true){
if(tagId ==8){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX(2-botx).
withVelocityY(2-botz));
}
drivetrain.setControl(drive.
withRotationalRate(limelight3.Calcule(botx,x2,boty,y2,angle)).
withVelocityX(0).
withVelocityY(0));
// if(tagId ==10){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX((botx-5.81)*2).
// withVelocityY((boty-4)*4));
// }
// else if(tagId ==7){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX(2-botx).
// withVelocityY(2-boty));
// }
}
else{
else{
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).

View File

@@ -18,7 +18,7 @@ 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 CommandSwerveDrivetrain drivetrain;
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;

View File

@@ -20,7 +20,7 @@ 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 CommandSwerveDrivetrain drivetrain;
private DoubleSupplier x;
private DoubleSupplier y;
/* Setting up bindings for necessary control of the swerve drive platform */

View File

@@ -54,7 +54,7 @@ public class Bougie extends SubsystemBase {
if(led==15){
x=true;
System.out.println("false");
}}
}}
// candle.setLEDs(255, 0, 0,0,24,8);
// candle.setLEDs(255, 0, 0,0,40,8);
// candle.setLEDs(255, 0, 0,0,56,8);

View File

@@ -1,5 +1,6 @@
package frc.robot.subsystems;
import java.util.function.Supplier;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -11,6 +12,8 @@ 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.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
@@ -30,26 +33,49 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
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();
// Position des modules par rapport au centre du robot
Translation2d frontLeftLocation = new Translation2d(+0.3, +0.3);
Translation2d frontRightLocation = new Translation2d(+0.3, -0.3);
Translation2d backLeftLocation = new Translation2d(-0.3, +0.3);
Translation2d backRightLocation = new Translation2d(-0.3, -0.3);
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
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();
// private void configureAutoBuilder() {
private void configureAutoBuilder() {
try {
RobotConfig 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(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(10, 0, 0),
// PID constants for rotation
new PIDConstants(7, 0, 0)
),
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());
}
// try {
// var config = RobotConfig.fromGUISettings();
// AutoBuilder.configure(
// AutoBuilder.configure(
// () -> getState().Pose, // Supplier of current robot pose
// this::resetPose, // Consumer for seeding pose against auto
// () -> getState().Speeds, // Supplier of current robot speeds
@@ -62,11 +88,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// ),
// new PPHolonomicDriveController(
// // PID constants for translation
// new PIDConstants(63.167, 0, 0.54521),
// new PIDConstants(5, 0, 0),
// // // PID constants for rotation
// // new PIDConstants(7.9735, 0, 0.038499)
// // PID constants for rotation
// new PIDConstants(43.502,0,2.7353)
// new PIDConstants(5,0,0)
// ),
// config,
// // Assume the path needs to be flipped for Red vs Blue, this is normally the case
@@ -77,11 +103,24 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// } catch (Exception ex) {
// DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
// }
// PPHolonomicDriveController.overrideRotationFeedback(()->{
// return 0;
// });
// }
// PPHolonomicDriveController.overrideRotationFeedback(()->{
// return 0;
// });
}
// public void driveRobotRelative(ChassisSpeeds speeds) {
// // Convertir robot-relative ChassisSpeeds → SwerveModuleStates
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
// // Limitations des roues
// SwerveDriveKinematics.desaturateWheelSpeeds(states, TunerConstants.kSpeedAt12Volts);
// // Envoi aux modules
// frontLeft.setDesiredState(states[0]);
// frontRight.setDesiredState(states[1]);
// backLeft.setDesiredState(states[2]);
// backRight.setDesiredState(states[3]);
// }
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
@@ -100,9 +139,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
//configureAutoBuilder();
configureAutoBuilder();
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
@@ -125,7 +163,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
// configureAutoBuilder();
configureAutoBuilder();
}
/**
@@ -158,7 +196,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
//configureAutoBuilder();
configureAutoBuilder();
}

View File

@@ -8,6 +8,7 @@ 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.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;
@@ -16,25 +17,26 @@ public class Limelight3 extends SubsystemBase {
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[] getTargetPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
return targetPose;
}
public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
@@ -48,6 +50,11 @@ public class Limelight3 extends SubsystemBase {
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public double getTId(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getRx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry rx = table.getEntry("rx");
@@ -62,6 +69,17 @@ public class Limelight3 extends SubsystemBase {
public void Forme(){
pipeline.setNumber(0);
}
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
if (x1 > 4)
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
}
else
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run

View File

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

View File

@@ -1,7 +1,7 @@
{
"fileName": "Phoenix6-25.2.2.json",
"fileName": "Phoenix6-frc2025-latest.json",
"name": "CTRE-Phoenix (v6)",
"version": "25.2.2",
"version": "25.4.0",
"frcYear": "2025",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [
@@ -19,14 +19,14 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
"version": "25.2.2"
"version": "25.4.0"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -40,7 +40,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -54,7 +54,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -68,7 +68,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -82,7 +82,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -96,7 +96,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -110,7 +110,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -124,7 +124,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -138,7 +138,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -152,7 +152,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -166,7 +166,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -180,7 +180,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -194,7 +194,35 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
"version": "25.2.2",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
"version": "25.4.0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -210,7 +238,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -226,7 +254,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -242,7 +270,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -258,7 +286,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -274,7 +302,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -290,7 +318,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -306,7 +334,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -322,7 +350,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimCANCoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -338,7 +366,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -354,7 +382,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -370,7 +398,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -386,7 +414,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -402,7 +430,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
"version": "25.2.2",
"version": "25.4.0",
"libName": "CTRE_SimProCANrange",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -414,6 +442,38 @@
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
"version": "25.4.0",
"libName": "CTRE_SimProCANdi",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
"version": "25.4.0",
"libName": "CTRE_SimProCANdle",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
}
]
}