Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Choreo boilerplate and setup. #8

Merged
merged 23 commits into from
Jan 18, 2025
+918 −112
Merged

Choreo boilerplate and setup. #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
b54b5fb
started working on Autos file
PGgit08 Jan 2, 2025
c259e87
created follow trajectory method for swerve samples and built choreo …
PGgit08 Jan 4, 2025
fb27db9
ignoring vision measurements for sim for now
PGgit08 Jan 5, 2025
759f4ef
trying to get simple trajectory following
PGgit08 Jan 5, 2025
9b4552e
got simple choreo routine working
PGgit08 Jan 5, 2025
ff05cbf
added logging to choreo paths and also copied choreo config as it is …
PGgit08 Jan 6, 2025
09a85a0
tested branching auto (works)
elvizer Jan 6, 2025
dc00cec
Merge branch 'choreo-boilerplate' of https://github.com/Team334/Swerv…
elvizer Jan 6, 2025
6c6c73f
spotless apply
PGgit08 Jan 7, 2025
f112281
started holonomic controller
PGgit08 Jan 9, 2025
fe244ad
spotless
PGgit08 Jan 9, 2025
98f4be7
worked more on holonomic controller class
PGgit08 Jan 9, 2025
c70cb3d
holonomic controller for following a trajectory
elvizer Jan 9, 2025
3578063
worked out a calculate method in the holonomic controller for just ba…
PGgit08 Jan 10, 2025
2d112c2
re-worked holonomic controller
PGgit08 Jan 11, 2025
101be8f
re-working holonomic controller again
PGgit08 Jan 11, 2025
84562b2
almost done with holonomic controller
PGgit08 Jan 11, 2025
e53673c
drive to pose almost works
PGgit08 Jan 15, 2025
a12c522
drive to pose works
PGgit08 Jan 15, 2025
ccb86af
testing commit as peter
PGgit08 Jan 15, 2025
e8cc1c5
deleted test print
PGgit08 Jan 15, 2025
da697ca
testing profile again
PGgit08 Jan 15, 2025
a8d438c
removed test print
PGgit08 Jan 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 143 additions & 38 deletions advantagescope-layout-telemetry.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,19 +13,20 @@
"/Robot/Swerve/Desired Module States/0",
"/Robot/Swerve/Module States/0",
"/Robot/Swerve/Pose/translation",
"/Robot",
"/NT/Faults",
"/NT/Faults/Active Faults/errors",
"/NT/Faults/Active Faults",
"/Robot/Faults",
"/Robot/Faults/Counts",
"/Robot/Swerve",
"/Robot/Swerve/left-arducam",
"/Robot/Swerve/left-arducam/Estimate"
"/Robot/Swerve/left-arducam/Estimate",
"/Robot/Swerve/blue-arducam/Estimate",
"/Robot",
"/Robot/Swerve"
]
},
"tabs": {
"selected": 1,
"selected": 3,
"tabs": [
{
"type": 0,
Expand All @@ -35,6 +36,66 @@
"renderer": "#/",
"controlsHeight": 0
},
{
"type": 9,
"title": "Swerve",
"controller": {
"sources": [
{
"type": "chassisSpeeds",
"logKey": "NT:/Robot/Swerve/Speeds",
"logType": "ChassisSpeeds",
"visible": true,
"options": {
"color": "#ff0000"
}
},
{
"type": "chassisSpeeds",
"logKey": "NT:/Robot/Swerve/Desired Speeds",
"logType": "ChassisSpeeds",
"visible": true,
"options": {
"color": "#0000ff"
}
},
{
"type": "states",
"logKey": "NT:/Robot/Swerve/Module States",
"logType": "SwerveModuleState[]",
"visible": true,
"options": {
"color": "#ff0000",
"arrangement": "0,1,2,3"
}
},
{
"type": "states",
"logKey": "NT:/Robot/Swerve/Desired Module States",
"logType": "SwerveModuleState[]",
"visible": true,
"options": {
"color": "#0000ff",
"arrangement": "0,1,2,3"
}
},
{
"type": "rotation",
"logKey": "NT:/Robot/Swerve/Pose/rotation",
"logType": "Rotation2d",
"visible": true,
"options": {}
}
],
"maxSpeed": 4.7,
"sizeX": 0.65,
"sizeY": 0.65,
"orientation": 1
},
"controllerUUID": "tj8b7ik3e8qxcapobn65d8jzeb8a406h",
"renderer": null,
"controlsHeight": 200
},
{
"type": 2,
"title": "Odometry",
Expand All @@ -61,7 +122,7 @@
"type": "ghost",
"logKey": "NT:/Robot/Swerve/Accepted Estimates",
"logType": "Pose3d[]",
"visible": true,
"visible": false,
"options": {
"color": "#00ff00"
}
Expand All @@ -70,13 +131,32 @@
"type": "ghost",
"logKey": "NT:/Robot/Swerve/Rejected Estimates",
"logType": "Pose3d[]",
"visible": true,
"visible": false,
"options": {
"color": "#ffff00"
}
},
{
"type": "trajectory",
"logKey": "NT:/Robot/Auto/Current Trajectory",
"logType": "Pose2d[]",
"visible": true,
"options": {
"color": "#ff00ff",
"size": "normal"
}
},
{
"type": "ghost",
"logKey": "NT:/Robot/Auto/Current Trajectory Desired Pose",
"logType": "Pose2d",
"visible": true,
"options": {
"color": "#ff00ff"
}
}
],
"game": "2024 Field",
"game": "2025 Field",
"bumpers": "auto",
"origin": "blue",
"orientation": 0,
Expand All @@ -87,63 +167,88 @@
"controlsHeight": 200
},
{
"type": 9,
"title": "Swerve",
"type": 3,
"title": "3D Field",
"controller": {
"sources": [
{
"type": "chassisSpeeds",
"logKey": "NT:/Robot/Swerve/Speeds",
"logType": "ChassisSpeeds",
"type": "trajectory",
"logKey": "NT:/Robot/Auto/Current Trajectory",
"logType": "Pose2d[]",
"visible": true,
"options": {
"color": "#ff0000"
"color": "#ff00ff",
"size": "normal"
}
},
{
"type": "chassisSpeeds",
"logKey": "NT:/Robot/Swerve/Desired Speeds",
"logType": "ChassisSpeeds",
"type": "ghost",
"logKey": "NT:/Robot/Auto/Current Trajectory Desired Pose",
"logType": "Pose2d",
"visible": true,
"options": {
"color": "#0000ff"
"model": "Crab Bot",
"color": "#ff00ff"
}
},
{
"type": "states",
"logKey": "NT:/Robot/Swerve/Module States",
"logType": "SwerveModuleState[]",
"type": "robot",
"logKey": "NT:/Robot/Swerve/Pose",
"logType": "Pose2d",
"visible": true,
"options": {
"color": "#ff0000",
"arrangement": "0,1,2,3"
"model": "Crab Bot"
}
},
{
"type": "states",
"logKey": "NT:/Robot/Swerve/Desired Module States",
"logType": "SwerveModuleState[]",
"type": "vision",
"logKey": "NT:/Robot/Swerve/Detected Tags",
"logType": "Pose3d[]",
"visible": true,
"options": {
"color": "#0000ff",
"arrangement": "0,1,2,3"
"color": "#00ff00",
"size": "normal"
}
},
{
"type": "rotation",
"logKey": "NT:/Robot/Swerve/Pose/rotation",
"logType": "Rotation2d",
"type": "ghost",
"logKey": "NT:/Robot/Swerve/Accepted Estimates",
"logType": "Pose3d[]",
"visible": true,
"options": {}
"options": {
"model": "Crab Bot",
"color": "#00ff00"
}
},
{
"type": "ghost",
"logKey": "NT:/Robot/Swerve/Rejected Estimates",
"logType": "Pose3d[]",
"visible": true,
"options": {
"model": "Crab Bot",
"color": "#ff0000"
}
}
],
"maxSpeed": 4.7,
"sizeX": 0.65,
"sizeY": 0.65,
"orientation": 1
"game": "2025 Field",
"origin": "blue"
},
"controllerUUID": "s72u8uqxhizena798en69qzlt8fabtzk",
"renderer": {
"cameraIndex": -1,
"orbitFov": 50,
"cameraPosition": [
-3.8929782247951725,
3.65017916178649,
-5.250724325879612
],
"cameraTarget": [
0,
0.5,
0
]
},
"controllerUUID": "tj8b7ik3e8qxcapobn65d8jzeb8a406h",
"renderer": null,
"controlsHeight": 200
}
]
Expand All @@ -152,5 +257,5 @@
}
],
"satellites": [],
"version": "4.0.0-beta-1"
"version": "4.1.0"
}
Binary file removed real-logs/SHOP_20241218_185500.wpilog
View file
Open in desktop
Binary file not shown.
5 changes: 0 additions & 5 deletions simgui-ds.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,6 @@
"visible": false
}
},
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"System Joysticks": {
"window": {
"visible": false
Expand Down
63 changes: 63 additions & 0 deletions src/main/deploy/choreo/M1toM2.traj
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
{
"name":"M1toM2",
"version":1,
"snapshot":{
"waypoints":[
{"x":7.6879377365112305, "y":7.49647855758667, "heading":3.141592653589793, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
{"x":7.359314441680908, "y":6.682065486907959, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
{"x":7.6879377365112305, "y":5.810500621795654, "heading":3.141592653589793, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
"constraints":[
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}],
"targetDt":0.05
},
"params":{
"waypoints":[
{"x":{"exp":"7.6879377365112305 m", "val":7.6879377365112305}, "y":{"exp":"7.49647855758667 m", "val":7.49647855758667}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
{"x":{"exp":"7.359314441680908 m", "val":7.359314441680908}, "y":{"exp":"6.682065486907959 m", "val":6.682065486907959}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
{"x":{"exp":"7.6879377365112305 m", "val":7.6879377365112305}, "y":{"exp":"5.810500621795654 m", "val":5.810500621795654}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
"constraints":[
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}],
"targetDt":{
"exp":"0.05 s",
"val":0.05
}
},
"trajectory":{
"sampleType":"Swerve",
"waypoints":[0.0,0.46313,0.94221],
"samples":[
{"t":0.0, "x":7.68794, "y":7.49648, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.67366, "ay":-7.00724, "alpha":0.0, "fx":[-96.50733,-96.50733,-96.50733,-96.50733], "fy":[-119.19112,-119.19112,-119.19112,-119.19112]},
{"t":0.03563, "x":7.68434, "y":7.49203, "heading":3.14159, "vx":-0.20213, "vy":-0.24964, "omega":0.0, "ax":-5.45006, "ay":-7.18105, "alpha":0.0, "fx":[-92.7039,-92.7039,-92.7039,-92.7039], "fy":[-122.14756,-122.14756,-122.14756,-122.14756]},
{"t":0.07125, "x":7.67368, "y":7.47858, "heading":3.14159, "vx":-0.39629, "vy":-0.50546, "omega":0.0, "ax":-5.16327, "ay":-7.38816, "alpha":0.0, "fx":[-87.82576,-87.82576,-87.82576,-87.82576], "fy":[-125.67055,-125.67055,-125.67055,-125.67055]},
{"t":0.10688, "x":7.65628, "y":7.45589, "heading":3.14159, "vx":-0.58023, "vy":-0.76867, "omega":0.0, "ax":-4.78488, "ay":-7.63651, "alpha":0.0, "fx":[-81.38944,-81.38944,-81.38944,-81.38944], "fy":[-129.89484,-129.89484,-129.89484,-129.89484]},
{"t":0.1425, "x":7.63258, "y":7.42366, "heading":3.14159, "vx":-0.75069, "vy":-1.04072, "omega":0.0, "ax":-4.26865, "ay":-7.93395, "alpha":0.0, "fx":[-72.60858,-72.60858,-72.60858,-72.60858], "fy":[-134.9543,-134.9543,-134.9543,-134.9543]},
{"t":0.17813, "x":7.60312, "y":7.38154, "heading":3.14159, "vx":-0.90277, "vy":-1.32337, "omega":0.0, "ax":-3.53697, "ay":-8.28273, "alpha":0.0, "fx":[-60.16277,-60.16277,-60.16277,-60.16277], "fy":[-140.88688,-140.88688,-140.88688,-140.88688]},
{"t":0.21375, "x":7.56872, "y":7.32914, "heading":3.14159, "vx":-1.02877, "vy":-1.61845, "omega":0.0, "ax":-2.4584, "ay":-8.6601, "alpha":0.0, "fx":[-41.81666,-41.81666,-41.81666,-41.81666], "fy":[-147.3058,-147.3058,-147.3058,-147.3058]},
{"t":0.24938, "x":7.53051, "y":7.26599, "heading":3.14159, "vx":-1.11635, "vy":-1.92697, "omega":0.0, "ax":-0.82679, "ay":-8.95907, "alpha":0.0, "fx":[-14.06352,-14.06352,-14.06352,-14.06352], "fy":[-152.39113,-152.39113,-152.39113,-152.39113]},
{"t":0.285, "x":7.49021, "y":7.19166, "heading":3.14159, "vx":-1.14581, "vy":-2.24614, "omega":0.0, "ax":1.56924, "ay":-8.85352, "alpha":0.0, "fx":[26.69237,26.69237,26.69237,26.69237], "fy":[-150.59579,-150.59579,-150.59579,-150.59579]},
{"t":0.32063, "x":7.45039, "y":7.10602, "heading":3.14159, "vx":-1.0899, "vy":-2.56155, "omega":0.0, "ax":4.53421, "ay":-7.76045, "alpha":0.0, "fx":[77.12566,77.12566,77.12566,77.12566], "fy":[-132.003,-132.003,-132.003,-132.003]},
{"t":0.35625, "x":7.41444, "y":7.00984, "heading":3.14159, "vx":-0.92837, "vy":-2.83801, "omega":0.0, "ax":7.06228, "ay":-5.56188, "alpha":0.0, "fx":[120.12731,120.12731,120.12731,120.12731], "fy":[-94.60606,-94.60606,-94.60606,-94.60606]},
{"t":0.39188, "x":7.38585, "y":6.9052, "heading":3.14159, "vx":-0.67677, "vy":-3.03616, "omega":0.0, "ax":8.42223, "ay":-3.15735, "alpha":0.0, "fx":[143.25967,143.25967,143.25967,143.25967], "fy":[-53.70557,-53.70557,-53.70557,-53.70557]},
{"t":0.4275, "x":7.36708, "y":6.79504, "heading":3.14159, "vx":-0.37673, "vy":-3.14864, "omega":0.0, "ax":8.91181, "ay":-1.25769, "alpha":0.0, "fx":[151.58732,151.58732,151.58732,151.58732], "fy":[-21.39295,-21.39295,-21.39295,-21.39295]},
{"t":0.46313, "x":7.35931, "y":6.68207, "heading":3.14159, "vx":-0.05924, "vy":-3.19345, "omega":0.0, "ax":8.99933, "ay":0.16305, "alpha":0.0, "fx":[153.07606,153.07606,153.07606,153.07606], "fy":[2.77346,2.77346,2.77346,2.77346]},
{"t":0.49998, "x":7.36324, "y":6.56449, "heading":3.14159, "vx":0.27241, "vy":-3.18744, "omega":0.0, "ax":8.75938, "ay":2.04389, "alpha":0.0, "fx":[148.99458,148.99458,148.99458,148.99458], "fy":[34.76602,34.76602,34.76602,34.76602]},
{"t":0.53683, "x":7.37923, "y":6.44841, "heading":3.14159, "vx":0.59521, "vy":-3.11211, "omega":0.0, "ax":7.68331, "ay":4.66424, "alpha":0.0, "fx":[130.69089,130.69089,130.69089,130.69089], "fy":[79.33743,79.33743,79.33743,79.33743]},
{"t":0.57369, "x":7.40638, "y":6.33689, "heading":3.14159, "vx":0.87837, "vy":-2.94022, "omega":0.0, "ax":5.18246, "ay":7.34054, "alpha":0.0, "fx":[88.15211,88.15211,88.15211,88.15211], "fy":[124.86054,124.86054,124.86054,124.86054]},
{"t":0.61054, "x":7.44227, "y":6.23352, "heading":3.14159, "vx":1.06935, "vy":-2.66971, "omega":0.0, "ax":1.95564, "ay":8.77427, "alpha":0.0, "fx":[33.2649,33.2649,33.2649,33.2649], "fy":[149.24784,149.24784,149.24784,149.24784]},
{"t":0.64739, "x":7.48301, "y":6.14109, "heading":3.14159, "vx":1.14142, "vy":-2.34635, "omega":0.0, "ax":-0.63786, "ay":8.97355, "alpha":0.0, "fx":[-10.84985,-10.84985,-10.84985,-10.84985], "fy":[152.63751,152.63751,152.63751,152.63751]},
{"t":0.68425, "x":7.52464, "y":6.06072, "heading":3.14159, "vx":1.11792, "vy":-2.01565, "omega":0.0, "ax":-2.33662, "ay":8.69347, "alpha":0.0, "fx":[-39.74519,-39.74519,-39.74519,-39.74519], "fy":[147.87345,147.87345,147.87345,147.87345]},
{"t":0.7211, "x":7.56425, "y":5.99234, "heading":3.14159, "vx":1.03181, "vy":-1.69527, "omega":0.0, "ax":-3.42255, "ay":8.33075, "alpha":0.0, "fx":[-58.21652,-58.21652,-58.21652,-58.21652], "fy":[141.7036,141.7036,141.7036,141.7036]},
{"t":0.75795, "x":7.59995, "y":5.93552, "heading":3.14159, "vx":0.90568, "vy":-1.38826, "omega":0.0, "ax":-4.14298, "ay":8.00057, "alpha":0.0, "fx":[-70.47085,-70.47085,-70.47085,-70.47085], "fy":[136.08736,136.08736,136.08736,136.08736]},
{"t":0.7948, "x":7.63051, "y":5.88979, "heading":3.14159, "vx":0.753, "vy":-1.09342, "omega":0.0, "ax":-4.64423, "ay":7.72322, "alpha":0.0, "fx":[-78.99698,-78.99698,-78.99698,-78.99698], "fy":[131.36971,131.36971,131.36971,131.36971]},
{"t":0.83166, "x":7.65511, "y":5.85474, "heading":3.14159, "vx":0.58184, "vy":-0.8088, "omega":0.0, "ax":-5.00851, "ay":7.49435, "alpha":0.0, "fx":[-85.1933,-85.1933,-85.1933,-85.1933], "fy":[127.47669,127.47669,127.47669,127.47669]},
{"t":0.86851, "x":7.67315, "y":5.83002, "heading":3.14159, "vx":0.39727, "vy":-0.53261, "omega":0.0, "ax":-5.28317, "ay":7.30512, "alpha":0.0, "fx":[-89.86526,-89.86526,-89.86526,-89.86526], "fy":[124.25801,124.25801,124.25801,124.25801]},
{"t":0.90536, "x":7.68421, "y":5.81535, "heading":3.14159, "vx":0.20257, "vy":-0.2634, "omega":0.0, "ax":-5.49668, "ay":7.14733, "alpha":0.0, "fx":[-93.49696,-93.49696,-93.49696,-93.49696], "fy":[121.57408,121.57408,121.57408,121.57408]},
{"t":0.94221, "x":7.68794, "y":5.8105, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
"splits":[0]
},
"events":[]
}
Loading
Loading