diff --git a/.gitignore b/.gitignore index 2c96eb1..cb86d5a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ target/ Cargo.lock +.DS_Store +.idea/ \ No newline at end of file diff --git a/Cargo.toml b/Cargo.toml index e48c87d..58e6e0a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,14 +4,14 @@ version = "0.1.0" edition = "2021" [dependencies] -frcrs = { git = "https://github.com/Team-2502/frcrs.git" } -#frcrs = { path = "../../frcrs" } +#frcrs = { git = "https://github.com/Team-2502/frcrs.git" } +frcrs = { path = "../frcrs" } tokio = { version = "1.36.0", features = ["rt", "full"] } serde = { version = "1.0.217", features = ["derive"] } serde_json = "1.0.134" uom = { version = "0.35.0", features = ["f64"] } nalgebra = "0.32.3" -wpi-trajectory = { git = "https://github.com/Speedy6451/trajectory-rs" } +wpi-trajectory = { git = "https://github.com/Sha-dos/trajectory-rs" } axum = "0.7.4" # limelightlib-rust = { git = "https://github.com/LimelightVision/limelightlib-rust.git" } diff --git a/Makefile b/Makefile index 8bd98a1..7e5378f 100644 --- a/Makefile +++ b/Makefile @@ -2,6 +2,7 @@ LIB=RobotCode2025 OUT=target/arm-unknown-linux-gnueabi/release/$(LIB) DEPLOY=javastub/src/main/deploy/$(LIB) TEAM=25.02 +PATHS=auto/* .PHONY: check check: @@ -27,6 +28,10 @@ deploy-scp: $(OUT) scp $(OUT) lvuser@10.$(TEAM).2: ssh lvuser@10.$(TEAM).2 /usr/local/frc/bin/frcRunRobot.sh +.PHONY: deploy-paths +deploy-paths: + scp -r $(PATHS) admin@10.$(TEAM).2:/home/lvuser/deploy/choreo/ + # Deploys the "deploy" directory and robotcode .PHONY: deploy deploy: $(OUT) diff --git a/auto/BlueTriangle.traj b/auto/BlueTriangle.traj new file mode 100644 index 0000000..1658e9f --- /dev/null +++ b/auto/BlueTriangle.traj @@ -0,0 +1,117 @@ +{ + "name":"BlueTriangle", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":8.075126647949219, "y":2.0993127822875977, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.206175327301025, "y":2.837056875228882, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.265194892883301, "y":0.9189223051071168, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.075126647949219, "y":2.0993127822875977, "heading":0.0, "intervals":40, "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":17.548, "h":8.052}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"8.075126647949219 m", "val":8.075126647949219}, "y":{"exp":"2.0993127822875977 m", "val":2.0993127822875977}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.206175327301025 m", "val":6.206175327301025}, "y":{"exp":"2.837056875228882 m", "val":2.837056875228882}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.265194892883301 m", "val":6.265194892883301}, "y":{"exp":"0.9189223051071167 m", "val":0.9189223051071168}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.075126647949219 m", "val":8.075126647949219}, "y":{"exp":"2.0993127822875977 m", "val":2.0993127822875977}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.14967,2.19995,3.43458], + "samples":[ + {"t":0.0, "x":8.07513, "y":2.09931, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.98044, "ay":4.19861, "alpha":0.0, "fx":[-135.74501,-135.74501,-135.74501,-135.74501], "fy":[71.41722,71.41722,71.41722,71.41722]}, + {"t":0.04599, "x":8.06669, "y":2.10375, "heading":0.0, "vx":-0.367, "vy":0.19308, "omega":0.0, "ax":-7.97772, "ay":4.19718, "alpha":0.0, "fx":[-135.6988,-135.6988,-135.6988,-135.6988], "fy":[71.39291,71.39291,71.39291,71.39291]}, + {"t":0.09197, "x":8.04138, "y":2.11707, "heading":0.0, "vx":-0.73387, "vy":0.3861, "omega":0.0, "ax":-7.97232, "ay":4.19434, "alpha":0.0, "fx":[-135.60691,-135.60691,-135.60691,-135.60691], "fy":[71.34456,71.34456,71.34456,71.34456]}, + {"t":0.13796, "x":7.9992, "y":2.13926, "heading":0.0, "vx":-1.10049, "vy":0.57898, "omega":0.0, "ax":-7.95636, "ay":4.18594, "alpha":0.0, "fx":[-135.33536,-135.33536,-135.33536,-135.33536], "fy":[71.2017,71.2017,71.2017,71.2017]}, + {"t":0.18395, "x":7.94018, "y":2.17031, "heading":0.0, "vx":-1.46637, "vy":0.77148, "omega":0.0, "ax":-6.59366, "ay":3.46901, "alpha":0.0, "fx":[-112.15626,-112.15626,-112.15626,-112.15626], "fy":[59.00687,59.00687,59.00687,59.00687]}, + {"t":0.22993, "x":7.86577, "y":2.20946, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":-0.00018, "ay":0.00009, "alpha":0.0, "fx":[-0.00307,-0.00307,-0.00307,-0.00307], "fy":[0.00162,0.00162,0.00162,0.00162]}, + {"t":0.27592, "x":7.78439, "y":2.25227, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.32191, "x":7.70301, "y":2.29509, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.36789, "x":7.62164, "y":2.3379, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.41388, "x":7.54026, "y":2.38071, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.45987, "x":7.45888, "y":2.42353, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.50586, "x":7.3775, "y":2.46634, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.55184, "x":7.29612, "y":2.50916, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.59783, "x":7.21474, "y":2.55197, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.64382, "x":7.13336, "y":2.59479, "heading":0.0, "vx":-1.7696, "vy":0.93101, "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]}, + {"t":0.6898, "x":7.05199, "y":2.6376, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,-0.00002,-0.00002,-0.00002], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":0.73579, "x":6.97061, "y":2.68041, "heading":0.0, "vx":-1.7696, "vy":0.93101, "omega":0.0, "ax":-0.00003, "ay":-0.00005, "alpha":0.0, "fx":[-0.00044,-0.00044,-0.00044,-0.00044], "fy":[-0.00083,-0.00083,-0.00083,-0.00083]}, + {"t":0.78178, "x":6.88923, "y":2.72323, "heading":0.0, "vx":-1.76961, "vy":0.93101, "omega":0.0, "ax":-0.00069, "ay":-0.00132, "alpha":0.0, "fx":[-0.01179,-0.01179,-0.01179,-0.01179], "fy":[-0.02241,-0.02241,-0.02241,-0.02241]}, + {"t":0.82776, "x":6.80785, "y":2.76604, "heading":0.0, "vx":-1.76964, "vy":0.93095, "omega":0.0, "ax":-0.01875, "ay":-0.03569, "alpha":0.0, "fx":[-0.319,-0.319,-0.319,-0.319], "fy":[-0.60707,-0.60707,-0.60707,-0.60707]}, + {"t":0.87375, "x":6.72645, "y":2.80882, "heading":0.0, "vx":-1.7705, "vy":0.92931, "omega":0.0, "ax":-0.48853, "ay":-0.95947, "alpha":0.0, "fx":[-8.3097,-8.3097,-8.3097,-8.3097], "fy":[-16.32029,-16.32029,-16.32029,-16.32029]}, + {"t":0.91974, "x":6.64451, "y":2.85054, "heading":0.0, "vx":-1.79297, "vy":0.88519, "omega":0.0, "ax":-2.79557, "ay":-7.2209, "alpha":0.0, "fx":[-47.55185,-47.55185,-47.55185,-47.55185], "fy":[-122.82542,-122.82542,-122.82542,-122.82542]}, + {"t":0.96572, "x":6.5591, "y":2.88361, "heading":0.0, "vx":-1.92153, "vy":0.55312, "omega":0.0, "ax":-1.56969, "ay":-8.72645, "alpha":0.0, "fx":[-26.70005,-26.70005,-26.70005,-26.70005], "fy":[-148.43435,-148.43435,-148.43435,-148.43435]}, + {"t":1.01171, "x":6.46908, "y":2.89982, "heading":0.0, "vx":-1.99371, "vy":0.15182, "omega":0.0, "ax":0.24856, "ay":-8.96434, "alpha":0.0, "fx":[4.22793,4.22793,4.22793,4.22793], "fy":[-152.48081,-152.48081,-152.48081,-152.48081]}, + {"t":1.0577, "x":6.37766, "y":2.89732, "heading":0.0, "vx":-1.98228, "vy":-0.26042, "omega":0.0, "ax":2.10111, "ay":-8.74723, "alpha":0.0, "fx":[35.73935,35.73935,35.73935,35.73935], "fy":[-148.78781,-148.78781,-148.78781,-148.78781]}, + {"t":1.10368, "x":6.28872, "y":2.87609, "heading":0.0, "vx":-1.88566, "vy":-0.66268, "omega":0.0, "ax":3.94358, "ay":-8.09856, "alpha":0.0, "fx":[67.07922,67.07922,67.07922,67.07922], "fy":[-137.75416,-137.75416,-137.75416,-137.75416]}, + {"t":1.14967, "x":6.20618, "y":2.83706, "heading":0.0, "vx":-1.7043, "vy":-1.03511, "omega":0.0, "ax":5.33072, "ay":-7.26008, "alpha":0.0, "fx":[90.67403,90.67403,90.67403,90.67403], "fy":[-123.49192,-123.49192,-123.49192,-123.49192]}, + {"t":1.19343, "x":6.1367, "y":2.78481, "heading":0.0, "vx":-1.47102, "vy":-1.35282, "omega":0.0, "ax":6.70106, "ay":-6.00193, "alpha":0.0, "fx":[113.98318,113.98318,113.98318,113.98318], "fy":[-102.09111,-102.09111,-102.09111,-102.09111]}, + {"t":1.23719, "x":6.07874, "y":2.71986, "heading":0.0, "vx":-1.17777, "vy":-1.61548, "omega":0.0, "ax":7.73085, "ay":-4.55171, "alpha":0.0, "fx":[131.49957,131.49957,131.49957,131.49957], "fy":[-77.42326,-77.42326,-77.42326,-77.42326]}, + {"t":1.28096, "x":6.0346, "y":2.6448, "heading":0.0, "vx":-0.83945, "vy":-1.81467, "omega":0.0, "ax":8.39872, "ay":-2.933, "alpha":0.0, "fx":[142.85983,142.85983,142.85983,142.85983], "fy":[-49.88952,-49.88952,-49.88952,-49.88952]}, + {"t":1.32472, "x":6.00591, "y":2.56258, "heading":0.0, "vx":-0.47191, "vy":-1.94302, "omega":0.0, "ax":8.34838, "ay":-1.22651, "alpha":0.0, "fx":[142.00357,142.00357,142.00357,142.00357], "fy":[-20.86255,-20.86255,-20.86255,-20.86255]}, + {"t":1.36848, "x":5.99325, "y":2.47638, "heading":0.0, "vx":-0.10657, "vy":-1.9967, "omega":0.0, "ax":2.83016, "ay":-0.06333, "alpha":0.0, "fx":[48.14017,48.14017,48.14017,48.14017], "fy":[-1.07715,-1.07715,-1.07715,-1.07715]}, + {"t":1.41224, "x":5.99129, "y":2.38894, "heading":0.0, "vx":0.01728, "vy":-1.99947, "omega":0.0, "ax":0.13457, "ay":0.00136, "alpha":0.0, "fx":[2.28906,2.28906,2.28906,2.28906], "fy":[0.02314,0.02314,0.02314,0.02314]}, + {"t":1.456, "x":5.99218, "y":2.30144, "heading":0.0, "vx":0.02317, "vy":-1.99941, "omega":0.0, "ax":0.00577, "ay":0.00007, "alpha":0.0, "fx":[0.09819,0.09819,0.09819,0.09819], "fy":[0.00114,0.00114,0.00114,0.00114]}, + {"t":1.49977, "x":5.9932, "y":2.21394, "heading":0.0, "vx":0.02342, "vy":-1.99941, "omega":0.0, "ax":0.00025, "ay":0.0, "alpha":0.0, "fx":[0.00421,0.00421,0.00421,0.00421], "fy":[0.00005,0.00005,0.00005,0.00005]}, + {"t":1.54353, "x":5.99422, "y":2.12644, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.00001, "ay":0.0, "alpha":0.0, "fx":[0.00018,0.00018,0.00018,0.00018], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.58729, "x":5.99525, "y":2.03894, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.63105, "x":5.99628, "y":1.95145, "heading":0.0, "vx":0.02343, "vy":-1.99941, "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]}, + {"t":1.67481, "x":5.9973, "y":1.86395, "heading":0.0, "vx":0.02343, "vy":-1.99941, "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]}, + {"t":1.71857, "x":5.99833, "y":1.77645, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.76234, "x":5.99935, "y":1.68895, "heading":0.0, "vx":0.02343, "vy":-1.99941, "omega":0.0, "ax":0.00004, "ay":0.0, "alpha":0.0, "fx":[0.00071,0.00071,0.00071,0.00071], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":1.8061, "x":6.00038, "y":1.60146, "heading":0.0, "vx":0.02344, "vy":-1.99941, "omega":0.0, "ax":0.00098, "ay":0.00001, "alpha":0.0, "fx":[0.01661,0.01661,0.01661,0.01661], "fy":[0.00019,0.00019,0.00019,0.00019]}, + {"t":1.84986, "x":6.0014, "y":1.51396, "heading":0.0, "vx":0.02348, "vy":-1.99941, "omega":0.0, "ax":0.02277, "ay":0.00027, "alpha":0.0, "fx":[0.38739,0.38739,0.38739,0.38739], "fy":[0.00465,0.00465,0.00465,0.00465]}, + {"t":1.89362, "x":6.00245, "y":1.42646, "heading":0.0, "vx":0.02448, "vy":-1.99939, "omega":0.0, "ax":0.52918, "ay":0.00955, "alpha":0.0, "fx":[9.00127,9.00127,9.00127,9.00127], "fy":[0.16251,0.16251,0.16251,0.16251]}, + {"t":1.93738, "x":6.00403, "y":1.33897, "heading":0.0, "vx":0.04763, "vy":-1.99898, "omega":0.0, "ax":6.28872, "ay":0.58681, "alpha":0.0, "fx":[106.96941,106.96941,106.96941,106.96941], "fy":[9.98147,9.98147,9.98147,9.98147]}, + {"t":1.98115, "x":6.01214, "y":1.25206, "heading":0.0, "vx":0.32284, "vy":-1.9733, "omega":0.0, "ax":8.48329, "ay":2.24286, "alpha":0.0, "fx":[144.29828,144.29828,144.29828,144.29828], "fy":[38.15046,38.15046,38.15046,38.15046]}, + {"t":2.02491, "x":6.03439, "y":1.16785, "heading":0.0, "vx":0.69408, "vy":-1.87514, "omega":0.0, "ax":8.04232, "ay":3.91331, "alpha":0.0, "fx":[136.79755,136.79755,136.79755,136.79755], "fy":[66.56434,66.56434,66.56434,66.56434]}, + {"t":2.06867, "x":6.07246, "y":1.08954, "heading":0.0, "vx":1.04603, "vy":-1.70389, "omega":0.0, "ax":7.15375, "ay":5.43776, "alpha":0.0, "fx":[121.68321,121.68321,121.68321,121.68321], "fy":[92.49478,92.49478,92.49478,92.49478]}, + {"t":2.11243, "x":6.12509, "y":1.02018, "heading":0.0, "vx":1.35909, "vy":-1.46592, "omega":0.0, "ax":5.92113, "ay":6.78081, "alpha":0.0, "fx":[100.71675,100.71675,100.71675,100.71675], "fy":[115.33956,115.33956,115.33956,115.33956]}, + {"t":2.15619, "x":6.19024, "y":0.96252, "heading":0.0, "vx":1.61821, "vy":-1.16918, "omega":0.0, "ax":4.32624, "ay":7.9036, "alpha":0.0, "fx":[73.58817,73.58817,73.58817,73.58817], "fy":[134.43797,134.43797,134.43797,134.43797]}, + {"t":2.19995, "x":6.26519, "y":0.91892, "heading":0.0, "vx":1.80754, "vy":-0.82331, "omega":0.0, "ax":3.09235, "ay":8.46349, "alpha":0.0, "fx":[52.60006,52.60006,52.60006,52.60006], "fy":[143.96162,143.96162,143.96162,143.96162]}, + {"t":2.24568, "x":6.35108, "y":0.89012, "heading":0.0, "vx":1.94894, "vy":-0.4363, "omega":0.0, "ax":1.09366, "ay":8.93568, "alpha":0.0, "fx":[18.60291,18.60291,18.60291,18.60291], "fy":[151.99338,151.99338,151.99338,151.99338]}, + {"t":2.29141, "x":6.44134, "y":0.87951, "heading":0.0, "vx":1.99895, "vy":-0.0277, "omega":0.0, "ax":-0.79311, "ay":8.94925, "alpha":0.0, "fx":[-13.49063,-13.49063,-13.49063,-13.49063], "fy":[152.22421,152.22421,152.22421,152.22421]}, + {"t":2.33713, "x":6.53192, "y":0.8876, "heading":0.0, "vx":1.96268, "vy":0.38152, "omega":0.0, "ax":-2.58929, "ay":8.54977, "alpha":0.0, "fx":[-44.04309,-44.04309,-44.04309,-44.04309], "fy":[145.42922,145.42922,145.42922,145.42922]}, + {"t":2.38286, "x":6.61896, "y":0.91399, "heading":0.0, "vx":1.84428, "vy":0.77247, "omega":0.0, "ax":-4.12822, "ay":7.6306, "alpha":0.0, "fx":[-70.21989,-70.21989,-70.21989,-70.21989], "fy":[129.79426,129.79426,129.79426,129.79426]}, + {"t":2.42859, "x":6.69898, "y":0.95729, "heading":0.0, "vx":1.65551, "vy":1.12139, "omega":0.0, "ax":-2.50049, "ay":3.33719, "alpha":0.0, "fx":[-42.53258,-42.53258,-42.53258,-42.53258], "fy":[56.76466,56.76466,56.76466,56.76466]}, + {"t":2.47431, "x":6.77206, "y":1.01206, "heading":0.0, "vx":1.54117, "vy":1.27399, "omega":0.0, "ax":-0.12696, "ay":0.15288, "alpha":0.0, "fx":[-2.15951,-2.15951,-2.15951,-2.15951], "fy":[2.6004,2.6004,2.6004,2.6004]}, + {"t":2.52004, "x":6.8424, "y":1.07047, "heading":0.0, "vx":1.53537, "vy":1.28098, "omega":0.0, "ax":-0.00478, "ay":0.00573, "alpha":0.0, "fx":[-0.08131,-0.08131,-0.08131,-0.08131], "fy":[0.09744,0.09744,0.09744,0.09744]}, + {"t":2.56577, "x":6.9126, "y":1.12905, "heading":0.0, "vx":1.53515, "vy":1.28125, "omega":0.0, "ax":-0.00018, "ay":0.00021, "alpha":0.0, "fx":[-0.00305,-0.00305,-0.00305,-0.00305], "fy":[0.00366,0.00366,0.00366,0.00366]}, + {"t":2.61149, "x":6.9828, "y":1.18764, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00011,-0.00011,-0.00011,-0.00011], "fy":[0.00014,0.00014,0.00014,0.00014]}, + {"t":2.65722, "x":7.053, "y":1.24623, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":2.70295, "x":7.1232, "y":1.30481, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.74867, "x":7.19339, "y":1.3634, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.7944, "x":7.26359, "y":1.42199, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.84013, "x":7.33379, "y":1.48058, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.88586, "x":7.40398, "y":1.53916, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.93158, "x":7.47418, "y":1.59775, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":2.97731, "x":7.54438, "y":1.65634, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":3.02304, "x":7.61457, "y":1.71493, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":3.06876, "x":7.68477, "y":1.77352, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":3.11449, "x":7.75497, "y":1.8321, "heading":0.0, "vx":1.53514, "vy":1.28126, "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]}, + {"t":3.16022, "x":7.82517, "y":1.89069, "heading":0.0, "vx":1.53514, "vy":1.28126, "omega":0.0, "ax":-0.00019, "ay":-0.00016, "alpha":0.0, "fx":[-0.00331,-0.00331,-0.00331,-0.00331], "fy":[-0.00276,-0.00276,-0.00276,-0.00276]}, + {"t":3.20594, "x":7.89536, "y":1.94928, "heading":0.0, "vx":1.53513, "vy":1.28125, "omega":0.0, "ax":-5.91038, "ay":-4.93291, "alpha":0.0, "fx":[-100.53393,-100.53393,-100.53393,-100.53393], "fy":[-83.90742,-83.90742,-83.90742,-83.90742]}, + {"t":3.25167, "x":7.95938, "y":2.00271, "heading":0.0, "vx":1.26487, "vy":1.05568, "omega":0.0, "ax":-6.90198, "ay":-5.76052, "alpha":0.0, "fx":[-117.40073,-117.40073,-117.40073,-117.40073], "fy":[-97.98475,-97.98475,-97.98475,-97.98475]}, + {"t":3.2974, "x":8.01, "y":2.04496, "heading":0.0, "vx":0.94926, "vy":0.79227, "omega":0.0, "ax":-6.9159, "ay":-5.77213, "alpha":0.0, "fx":[-117.63747,-117.63747,-117.63747,-117.63747], "fy":[-98.18234,-98.18234,-98.18234,-98.18234]}, + {"t":3.34312, "x":8.04618, "y":2.07515, "heading":0.0, "vx":0.63302, "vy":0.52833, "omega":0.0, "ax":-6.92062, "ay":-5.77608, "alpha":0.0, "fx":[-117.71784,-117.71784,-117.71784,-117.71784], "fy":[-98.24941,-98.24941,-98.24941,-98.24941]}, + {"t":3.38885, "x":8.06789, "y":2.09327, "heading":0.0, "vx":0.31657, "vy":0.26421, "omega":0.0, "ax":-6.923, "ay":-5.77806, "alpha":0.0, "fx":[-117.75829,-117.75829,-117.75829,-117.75829], "fy":[-98.28318,-98.28318,-98.28318,-98.28318]}, + {"t":3.43458, "x":8.07513, "y":2.09931, "heading":0.0, "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":[] +} diff --git a/auto/paths.chor b/auto/paths.chor new file mode 100644 index 0000000..cdfc74d --- /dev/null +++ b/auto/paths.chor @@ -0,0 +1,78 @@ +{ + "name":"paths", + "version":1, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[] +} diff --git a/src/auto/mod.rs b/src/auto/mod.rs index 5d113a9..2989128 100644 --- a/src/auto/mod.rs +++ b/src/auto/mod.rs @@ -1,18 +1,25 @@ mod path; +use crate::auto::path::drive; +use nalgebra::Vector2; use serde::{Deserialize, Serialize}; +use std::ops::Deref; +use std::time::Duration; +use tokio::time::sleep; use crate::Ferris; #[derive(Serialize, Deserialize)] pub enum Auto { Nothing, + BlueTriangle, } impl Auto { pub fn from_dashboard(s: &str) -> Self { match s { "Nothing" => Auto::Nothing, + "BlueTriangle" => Auto::BlueTriangle, _ => Auto::Nothing, } } @@ -20,12 +27,13 @@ impl Auto { pub fn name(&self) -> &'static str { match self { Auto::Nothing => "Nothing", + Auto::BlueTriangle => "BlueTriangle", _ => "none", } } pub fn iterator() -> Vec { - vec![Auto::Nothing] + vec![Auto::Nothing, Auto::BlueTriangle] } pub fn names() -> Vec { @@ -35,9 +43,37 @@ impl Auto { .collect() } - pub async fn run_auto<'a>(_ferris: Ferris, chosen: Auto) { + pub async fn run_auto<'a>(ferris: Ferris, chosen: Auto) { match chosen { Auto::Nothing => {} + Auto::BlueTriangle => { + blue_triangle(ferris).await.expect("Failed running auto"); + } } } } + +pub async fn blue_triangle(robot: Ferris) -> Result<(), Box> { + println!("RUNNING"); + + let mut drivetrain = robot.drivetrain.deref().borrow_mut(); + + drivetrain + .odometry + .set_abs(Vector2::new(8.075126647949219, 2.0993127822875977)); + + drive("BlueTriangle", &mut drivetrain, 1).await?; + println!("BlueTriangle.1 done"); + + sleep(Duration::from_secs_f64(1.)).await; + + drive("BlueTriangle", &mut drivetrain, 2).await?; + println!("BlueTriangle.2 done"); + + sleep(Duration::from_secs_f64(1.)).await; + + drive("BlueTriangle", &mut drivetrain, 3).await?; + println!("BlueTriangle.3 done"); + + Ok(()) +} diff --git a/src/auto/path.rs b/src/auto/path.rs index 8b13789..3557829 100644 --- a/src/auto/path.rs +++ b/src/auto/path.rs @@ -1 +1,132 @@ +use std::time::Duration; +use tokio::fs::File; +use nalgebra::Vector2; +use tokio::io::AsyncReadExt; +use tokio::time::{sleep, Instant}; +use uom::si::{ + angle::radian, + f64::{Length, Time}, + length::{foot, meter}, + time::{millisecond, second}, + velocity::meter_per_second, +}; +use wpi_trajectory::Path; + +use crate::subsystems::SwerveControlStyle; +use crate::{ + constants::drivetrain::{ + SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KF, SWERVE_DRIVE_KFA, SWERVE_DRIVE_KI, + SWERVE_DRIVE_KP, SWERVE_DRIVE_MAX_ERR, SWERVE_TURN_KP, + }, + subsystems::Drivetrain, +}; + +// TODO: Test +pub async fn drive( + name: &str, + drivetrain: &mut Drivetrain, + waypoint_index: usize, +) -> Result<(), Box> { + let mut path_content = String::new(); + File::open(format!("/home/lvuser/deploy/choreo/{}.traj", name)) + .await? + .read_to_string(&mut path_content) + .await?; + + let path = Path::from_trajectory(&path_content)?; + let waypoints = path.waypoints(); + + if waypoint_index >= waypoints.len() { + return Err("Waypoint index out of bounds".into()); + } + + // Get start and end times for the waypoint + let start_time = if waypoint_index == 0 { + 0.0 + } else { + waypoints[waypoint_index - 1] + }; + let end_time = waypoints[waypoint_index]; + + // Follow path for this segment + follow_path_segment(drivetrain, path, 0., 3.4).await; + drivetrain.set_speeds(0., 0., 0., SwerveControlStyle::FieldOriented); + Ok(()) +} + +pub async fn follow_path_segment( + drivetrain: &mut Drivetrain, + path: Path, + start_time: f64, + end_time: f64, +) { + let start = Instant::now(); + let mut last_error = Vector2::zeros(); + let mut last_loop = Instant::now(); + let mut i = Vector2::zeros(); + + loop { + let now = Instant::now(); + let dt = now - last_loop; + last_loop = now; + + //println!("x: {}, y: {}", drivetrain.odometry.position.x, drivetrain.odometry.position.y); + + let elapsed = start.elapsed().as_secs_f64() + start_time; + + // Exit if we've reached the end time for this segment + if elapsed > end_time { + break; + } + + let setpoint = path.get(Time::new::(elapsed)); + + let angle = -setpoint.heading; + let position = Vector2::new(setpoint.x.get::(), setpoint.y.get::()); + + let mut error_position = position - drivetrain.odometry.position; + let mut error_angle = (angle - drivetrain.get_angle()).get::(); + + if error_position.abs().max() < SWERVE_DRIVE_IE { + i += error_position; + } + + if elapsed > path.length().get::() + && error_position.abs().max() < SWERVE_DRIVE_MAX_ERR + && error_angle.abs() < 0.075 + { + break; + } + + error_angle *= SWERVE_TURN_KP; + error_position *= -SWERVE_DRIVE_KP; + + let mut speed = error_position; + + let velocity = Vector2::new(setpoint.velocity_x, setpoint.velocity_y); + let velocity = velocity.map(|x| x.get::()); + + let velocity_next = Vector2::new(setpoint.velocity_x, setpoint.velocity_y) + .map(|x| x.get::()); + + let acceleration = (velocity_next - velocity) * 1000. / 20.; + + speed += velocity * -SWERVE_DRIVE_KF; + speed += acceleration * -SWERVE_DRIVE_KFA; + speed += i * -SWERVE_DRIVE_KI * dt.as_secs_f64() * 9.; + + let speed_s = speed; + speed += (speed - last_error) * -SWERVE_DRIVE_KD * dt.as_secs_f64() * 9.; + last_error = speed_s; + + drivetrain.set_speeds( + speed.x, + speed.y, + error_angle, + SwerveControlStyle::FieldOriented, + ); + + sleep(Duration::from_millis(20)).await; + } +} diff --git a/src/constants.rs b/src/constants.rs index 139ba34..ceb8d9d 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -24,12 +24,12 @@ pub mod robotmap { pub mod indexer { pub const MOTOR: i32 = 12; - pub const DISTANCE: i32 = 0; + pub const LASER_CAN: i32 = 0; // Cant save can id } pub mod climber { - pub const RAISE: i32 = 0; - pub const GRAB: i32 = 1; + pub const RAISE: i32 = 1; + pub const GRAB: i32 = 0; } } @@ -63,15 +63,29 @@ pub mod drivetrain { pub const SWERVE_DRIVE_SUGGESTION_ERR: f64 = 0.35; pub const SWERVE_DRIVE_IE: f64 = 0.0; //0.175; // integral enable - pub const PODIUM_SHOT_ANGLE: f64 = 34.34; // degrees + pub const LINEUP_2D_TX_STR_KP: f64 = 0.005; + pub const LINEUP_2D_TX_FWD_KP: f64 = 0.005; + pub const LINEUP_2D_TY_STR_KP: f64 = 0.005; + pub const LINEUP_2D_TY_FWD_KP: f64 = 0.005; + pub const TARGET_TY_LEFT: f64 = -7.4; + pub const TARGET_TY_RIGHT: f64 = 3.45; + pub const TARGET_TX_LEFT: f64 = -7.4; + pub const TARGET_TX_RIGHT: f64 = 3.45; + pub const TX_ACCEPTABLE_ERROR: f64 = 1.8; + pub const TY_ACCEPTABLE_ERROR: f64 = 1.8; + pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02; + } pub mod elevator { pub const BOTTOM: f64 = 0.0; // unit is rotations pub const L2: f64 = 1.; // unit is rotations pub const L3: f64 = 15.75; // unit is rotations - pub const L4: f64 = 39.7; // unit is rotations - pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async - pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async + pub const L4: f64 = 38.8; // unit is rotations + pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // sleep.await this long in between updating the elevator trapezoidal when running its async function + pub const POSITION_TOLERANCE: f64 = 0.25; // unit is rotations. finish elevator async move when within this distance of target +} +pub mod indexer { + pub const LASER_TRIP_DISTANCE_MM: i32 = 2; } pub mod joystick_map { // Joystick IDs (set in driver station) @@ -82,13 +96,23 @@ pub mod joystick_map { //Right drive pub const LINEUP_LEFT: usize = 3; pub const LINEUP_RIGHT: usize = 4; + pub const INTAKE: usize = 1; + pub const RESET_HEADING: usize = 5; + pub const CLIMB: usize = 2; + pub const CLIMB_FALL: usize = 6; + pub const CLIMBER_GRAB: usize = 8; + pub const CLIMBER_RAISE: usize = 9; //Left drive - pub const INDEXER_IN: usize = 1; - pub const INDEXER_OUT: usize = 2; + pub const SLOW_MODE: usize = 1; + + pub const SCORE_L2: usize = 2; + pub const SCORE_L3: usize = 3; + pub const SCORE_L4: usize = 4; //Operator pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET: usize = 1; + pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET_ASYNC: usize = 2; pub const ELEVATOR_UP_MANUAL: usize = 3; pub const ELEVATOR_DOWN_MANUAL: usize = 4; pub const CLIMB_FULL: usize = 8; diff --git a/src/container.rs b/src/container.rs index 7e86b09..59c7eea 100644 --- a/src/container.rs +++ b/src/container.rs @@ -1,5 +1,6 @@ use crate::constants::drivetrain::SWERVE_TURN_KP; -use crate::subsystems::{Drivetrain, DrivetrainControlState}; +use crate::constants::joystick_map::*; +use crate::subsystems::{Drivetrain, DrivetrainControlState, SwerveControlStyle}; use crate::Controllers; use frcrs::deadzone; use nalgebra::ComplexField; @@ -34,8 +35,16 @@ pub async fn control_drivetrain( let saved_angle = &mut state.saved_angle; let joystick_range = 0.04..1.; - let power_translate = if left_drive.get(1) { 0.0..0.3 } else { 0.0..1. }; - let power_rotate = if left_drive.get(1) { 0.0..0.2 } else { 0.0..1. }; + let power_translate = if left_drive.get(SLOW_MODE) { + 0.0..0.3 + } else { + 0.0..1. + }; + let power_rotate = if left_drive.get(SLOW_MODE) { + 0.0..0.2 + } else { + 0.0..1. + }; let deadly = deadzone(left_drive.get_y(), &joystick_range, &power_translate); let deadlx = deadzone(left_drive.get_x(), &joystick_range, &power_translate); let deadrz = deadzone(right_drive.get_z(), &joystick_range, &power_rotate); @@ -53,18 +62,20 @@ pub async fn control_drivetrain( } else { 0. } - } else if left_drive.get(2) { + } + /*else if right_drive.get(HOLD_90) { let angle = (drivetrain.get_angle() - drivetrain.offset).get::(); let goal = (angle / 90.).round() * 90.; let error = angle - goal; -error.to_radians() * SWERVE_TURN_KP - } else { + } */ + else { deadrz }; - drivetrain.set_speeds(deadly, deadlx, rot); + drivetrain.set_speeds(deadly, deadlx, rot, SwerveControlStyle::FieldOriented); - if left_drive.get(4) { + if right_drive.get(RESET_HEADING) { drivetrain.reset_heading(); } } diff --git a/src/lib.rs b/src/lib.rs index c15e605..4f5bc6d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -5,20 +5,27 @@ pub mod subsystems; pub mod swerve; use crate::auto::Auto; +use crate::constants::elevator; use crate::container::control_drivetrain; use crate::subsystems::{ Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide, + Vision, }; +use constants::joystick_map::*; +use frcrs::ctre::ControlMode; use frcrs::input::Joystick; use frcrs::networktables::NetworkTable; use frcrs::telemetry::Telemetry; use frcrs::{Robot, TaskManager}; use std::cell::RefCell; use std::cmp::PartialEq; +use std::net::{IpAddr, Ipv4Addr, SocketAddr}; use std::ops::Deref; use std::rc::Rc; use std::sync::Arc; -use tokio::task::spawn_local; +use std::time::{Duration, Instant}; +use tokio::task::{spawn_local, AbortHandle}; +use tokio::time::sleep; #[derive(Clone)] pub struct Controllers { @@ -40,11 +47,14 @@ pub struct Ferris { pub drivetrain: Rc>, pub elevator: Rc>, pub indexer: Rc>, - pub climber: Arc>>, + pub climber: Rc>, teleop_state: Rc>, auto_handle: Option, + elevator_trapezoid_handle: Option, + indexer_intake_handle: Option, + climb_handle: Option, } impl Default for Ferris { @@ -65,11 +75,14 @@ impl Ferris { drivetrain: Rc::new(RefCell::new(Drivetrain::new())), elevator: Rc::new(RefCell::new(Elevator::new())), indexer: Rc::new(RefCell::new(Indexer::new())), - climber: Arc::new(Rc::new(RefCell::new(Climber::new()))), + climber: Rc::new(RefCell::new(Climber::new())), teleop_state: Default::default(), auto_handle: None, + elevator_trapezoid_handle: None, + indexer_intake_handle: None, + climb_handle: None, } } @@ -99,7 +112,7 @@ impl Robot for Ferris { serde_json::to_string(&Auto::names()).unwrap(), ) .await; - Telemetry::put_string("selected auto", Auto::Nothing.name().to_string()).await; + Telemetry::put_string("selected auto", Auto::BlueTriangle.name().to_string()).await; } fn disabled_init(&mut self) { @@ -142,7 +155,11 @@ impl Robot for Ferris { let handle = spawn_local(auto_task).abort_handle(); self.auto_handle = Some(handle); } else { - eprintln!("Failed to get selected auto from telemetry."); + eprintln!("Failed to get selected auto from telemetry, running default"); + + let auto_task = Auto::run_auto(f, Auto::Nothing); + let handle = spawn_local(auto_task).abort_handle(); + self.auto_handle = Some(handle); } } } @@ -153,137 +170,165 @@ impl Robot for Ferris { } = *self.teleop_state.deref().borrow_mut(); if let Ok(mut drivetrain) = self.drivetrain.try_borrow_mut() { - //drivetrain.update_limelight().await; - drivetrain.post_odo().await; + if let Ok(mut elevator) = self.elevator.try_borrow_mut() { + if let Ok(mut indexer) = self.indexer.try_borrow_mut() { + drivetrain.update_limelight().await; + drivetrain.post_odo().await; + + let side = if self.controllers.right_drive.get(LINEUP_LEFT) { + LineupSide::Left + } else if self.controllers.right_drive.get(LINEUP_RIGHT) { + LineupSide::Right + } else { + LineupSide::Left + }; - if self - .controllers - .right_drive - .get(constants::joystick_map::LINEUP_LEFT) - { - drivetrain.lineup(LineupSide::Left).await; - } else if self - .controllers - .right_drive - .get(constants::joystick_map::LINEUP_RIGHT) - { - drivetrain.lineup(LineupSide::Right).await; - } else { - control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await; + if self.controllers.left_drive.get(SCORE_L2) { + score( + &mut drivetrain, + &mut elevator, + &mut indexer, + ElevatorPosition::L2, + side, + ) + } else if self.controllers.left_drive.get(SCORE_L3) { + score( + &mut drivetrain, + &mut elevator, + &mut indexer, + ElevatorPosition::L3, + side, + ) + } else if self.controllers.left_drive.get(SCORE_L4) { + score( + &mut drivetrain, + &mut elevator, + &mut indexer, + ElevatorPosition::L4, + side, + ) + } else if self.controllers.right_drive.get(INTAKE) { + elevator.set_target(ElevatorPosition::L2); + elevator.run_to_target_trapezoid(); + + if indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM + || indexer.get_laser_dist() == -1 + { + println!("Dist: {}", indexer.get_laser_dist()); + indexer.set_speed(-0.25); + } else { + indexer.stop(); + } + } else { + control_drivetrain( + &mut drivetrain, + &mut self.controllers, + drivetrain_state, + ) + .await; + + elevator.stop(); + indexer.stop(); + } + } } } - if let Ok(mut elevator) = self.elevator.try_borrow_mut() { - // Setting the target position - if self - .controllers - .operator - .get(constants::joystick_map::SET_TARGET_L2) - { - elevator.set_target(ElevatorPosition::L2); - } else if self - .controllers - .operator - .get(constants::joystick_map::SET_TARGET_L3) - { - elevator.set_target(ElevatorPosition::L3); - } else if self - .controllers - .operator - .get(constants::joystick_map::SET_TARGET_L4) - { - elevator.set_target(ElevatorPosition::L4); + if self.controllers.right_drive.get(CLIMB) { + if self.climb_handle.is_none() { + let f = self.clone(); + let climb_task = Climber::climb(f); + let handle = spawn_local(climb_task).abort_handle(); + self.climb_handle = Some(handle); } - - // Setting the actual elevator operation - if self - .controllers - .operator - .get(constants::joystick_map::ELEVATOR_UP_MANUAL) - { - // Manual up - elevator.set_speed(0.1); - } else if self - .controllers - .operator - .get(constants::joystick_map::ELEVATOR_DOWN_MANUAL) - { - // Manual down - elevator.set_speed(-0.1); - } else if self - .controllers - .operator - .get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET) - { - // Trapezoidal to stored target - elevator.run_to_target_trapezoid(); - } else { - elevator.set_speed(0.0); + } else if self.controllers.right_drive.get(CLIMB_FALL) { + if let Some(handle) = self.climb_handle.take() { + handle.abort(); } - } - if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self - .controllers - .left_drive - .get(constants::joystick_map::INDEXER_OUT) - { - // Out the front - indexer.set_speed(0.3); - } else if self - .controllers - .left_drive - .get(constants::joystick_map::INDEXER_IN) - { - // In, score out the left - indexer.set_speed(-0.5); - } else { - indexer.set_speed(0.0); + if let Ok(mut climber) = self.climber.try_borrow_mut() { + climber.fall() + } + } else { + if let Some(handle) = self.climb_handle.take() { + handle.abort(); } - } - // TODO: make more ergonomic, maybe move away from frcrs task manager in favor for abort handle in ferris struct - // Untested - let climber = Arc::clone(&self.climber); - let climb = { - let climber = Arc::clone(&climber); - move || { - let climber = Arc::clone(&climber); - async move { - if let Ok(climber) = climber.try_borrow_mut() { - climber.climb().await; - }; + if let Ok(mut climber) = self.climber.try_borrow_mut() { + if self.controllers.right_drive.get(CLIMBER_RAISE) { + climber.set_raise(true); + } else { + climber.set_raise(false); } - } - }; - let climber = Arc::clone(&self.climber); - let cancel_climb = { - let climber = Arc::clone(&climber); - move || { - let climber = Arc::clone(&climber); - async move { - if let Ok(climber) = climber.try_borrow_mut() { - climber.set_raise(false); - climber.set_grab(false); - }; + if self.controllers.right_drive.get(CLIMBER_GRAB) { + climber.set_grab(true); + } else { + climber.set_grab(false); } } + } + } + + async fn test_periodic(&mut self) { + // println!("Test periodic"); + } +} +pub async fn elevator_move_to_target_async(robot: Ferris) { + println!("Called elevator_move_to_target_async"); + if let Ok(mut elevator) = robot.elevator.try_borrow_mut() { + //println!("Borrowed elevator"); + let target_position = match elevator.get_target() { + ElevatorPosition::Bottom => elevator::BOTTOM, + ElevatorPosition::L2 => elevator::L2, + ElevatorPosition::L3 => elevator::L3, + ElevatorPosition::L4 => elevator::L4, }; + //println!("Error: {}", (elevator.get_position() - target_position).abs()); + //println!("{}", (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE); + while (elevator.get_position() - target_position).abs() > elevator::POSITION_TOLERANCE { + elevator.run_to_target_trapezoid(); + } + println!("End of elevator_move_to_target_async"); + } +} - if self - .controllers - .operator - .get(constants::joystick_map::CLIMB_FULL) - { - self.task_manager.run_task(climb); +pub fn score( + drivetrain: &mut Drivetrain, + elevator: &mut Elevator, + indexer: &mut Indexer, + elevator_position: ElevatorPosition, + lineup_side: LineupSide, +) { + let drivetrain_at_position = drivetrain.lineup_2d(lineup_side); + elevator.set_target(elevator_position); + let elevator_at_target = elevator.run_to_target_trapezoid(); + + if elevator_at_target && drivetrain_at_position { + if indexer.get_laser_dist() < constants::indexer::LASER_TRIP_DISTANCE_MM { + let indexer_speed = match elevator_position { + ElevatorPosition::Bottom => -0.25, + ElevatorPosition::L2 => -0.25, + ElevatorPosition::L3 => -0.25, + ElevatorPosition::L4 => -0.1 + }; + indexer.set_speed(indexer_speed); } else { - self.task_manager.run_task(cancel_climb); - self.task_manager.abort_task(climb); + indexer.stop(); + + //elevator.set_target(ElevatorPosition::Bottom); + //elevator.run_to_target_trapezoid(); } } +} - async fn test_periodic(&mut self) { - // println!("Test periodic"); +pub fn after_score(robot: &Ferris) { + if let Ok(mut elevator) = robot.elevator.try_borrow_mut() { + if let Ok(mut indexer) = robot.indexer.try_borrow_mut() { + elevator.set_target(ElevatorPosition::Bottom); + elevator.run_to_target_trapezoid(); + + indexer.stop(); + } } } diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs index 4317ed3..32f945d 100644 --- a/src/subsystems/climber.rs +++ b/src/subsystems/climber.rs @@ -1,4 +1,5 @@ use crate::constants::robotmap::climber::*; +use crate::Ferris; use frcrs::solenoid::{ModuleType, Solenoid}; use std::time::Duration; use tokio::time::sleep; @@ -35,14 +36,21 @@ impl Climber { } pub fn set_grab(&self, engaged: bool) { - self.grab.set(engaged); + self.grab.set(!engaged); } - pub async fn climb(&self) { - self.set_raise(true); - sleep(Duration::from_secs(1)).await; - self.set_grab(true); - sleep(Duration::from_secs_f64(0.25)).await; + pub async fn climb(ferris: Ferris) { + if let Ok(climber) = ferris.climber.try_borrow_mut() { + climber.set_raise(true); + sleep(Duration::from_secs(1)).await; + climber.set_grab(true); + sleep(Duration::from_secs_f64(0.25)).await; + climber.set_raise(false); + } + } + + pub fn fall(&self) { + self.set_grab(false); self.set_raise(false); } diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index a5e17a0..f478e1f 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -5,9 +5,7 @@ use std::ops::Add; use frcrs::ctre::{talon_encoder_tick, ControlMode, Pigeon, Talon}; -use crate::constants::drivetrain::{ - SWERVE_DRIVE_IE, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, -}; +use crate::constants::drivetrain::{LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, SWERVE_DRIVE_IE, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, TARGET_TX_LEFT, TARGET_TX_RIGHT, TARGET_TY_LEFT, TARGET_TY_RIGHT, TX_ACCEPTABLE_ERROR, TY_ACCEPTABLE_ERROR, YAW_ACCEPTABLE_ERROR}; use crate::constants::robotmap::swerve::*; use crate::swerve::kinematics::{ModuleState, Swerve}; use crate::swerve::odometry::{ModuleReturn, Odometry}; @@ -17,6 +15,7 @@ use nalgebra::{Quaternion, Rotation2, Vector2}; use serde::Deserialize; use serde::Serialize; +use crate::constants; use crate::constants::vision::ROBOT_CENTER_TO_LIMELIGHT_INCHES; use crate::subsystems::Vision; use uom::si::angle::{degree, radian, revolution}; @@ -33,6 +32,11 @@ pub enum LineupSide { Left, Right, } +#[derive(Clone, Copy)] +pub enum SwerveControlStyle { + RobotOriented, + FieldOriented, +} #[derive(Debug)] pub struct LineupTarget { @@ -60,7 +64,8 @@ pub struct Drivetrain { pub offset: Angle, - pub vision: Vision, + pub limelight_lower: Vision, + pub limelight_upper: Vision, } #[derive(Serialize, Deserialize)] @@ -89,10 +94,14 @@ impl Drivetrain { let bl_turn = Talon::new(BL_TURN, Some("can0".to_owned())); let br_turn = Talon::new(BR_TURN, Some("can0".to_owned())); - let vision = Vision::new(SocketAddr::new( + let limelight_lower = Vision::new(SocketAddr::new( IpAddr::V4(Ipv4Addr::new(10, 25, 2, 11)), 5807, )); + let limelight_upper = Vision::new(SocketAddr::new( + IpAddr::V4(Ipv4Addr::new(10, 25, 2, 12)), + 5807, + )); let offset = if alliance_station().red() { Angle::new::(180.) @@ -120,16 +129,20 @@ impl Drivetrain { offset, - vision, + limelight_lower: limelight_lower, + limelight_upper: limelight_upper, } } pub async fn update_limelight(&mut self) { - self.vision + self.limelight_lower + .update(self.get_offset().get::() + 180.) + .await; + self.limelight_upper .update(self.get_offset().get::() + 180.) .await; - let pose = self.vision.get_botpose(); + let pose = self.limelight_lower.get_botpose(); // Calculate offset from robot center to limelight let robot_center_to_limelight_unrotated: Vector2 = Vector2::new( @@ -157,6 +170,8 @@ impl Drivetrain { if pose.x.get::() != 0.0 { self.update_odo(pose + robot_to_limelight); } + //println!("lower ll tx: {}", self.limelight_lower.get_tx().get::()); + //println!("upper ll tx: {}", self.limelight_upper.get_tx().get::()); } pub async fn post_odo(&self) { @@ -223,13 +238,18 @@ impl Drivetrain { ((angle % 360.0) + 360.0) % 360.0 } - pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) { - println!( + pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) { + /*println!( "ODO XY: {}, {}", self.odometry.position.x, self.odometry.position.y - ); + );*/ let mut transform = Vector2::new(-str, fwd); - transform = Rotation2::new(self.get_offset().get::()) * transform; + match style { + SwerveControlStyle::FieldOriented => { + transform = Rotation2::new(self.get_offset().get::()) * transform; + } + SwerveControlStyle::RobotOriented => {} + } let wheel_speeds = self.kinematics.calculate(transform, -rot); @@ -352,7 +372,12 @@ impl Drivetrain { speed.x *= -1. } - self.set_speeds(speed.x, -speed.y, error_angle); + self.set_speeds( + speed.x, + -speed.y, + error_angle, + SwerveControlStyle::FieldOriented, + ); Telemetry::put_number("error_position_x", error_position.x).await; Telemetry::put_number("error_position_y", error_position.y).await; @@ -372,12 +397,12 @@ impl Drivetrain { // The target position will be to the side of the apriltag, half a robot length away from the edge // Will account for the robot's orientation with the hexagon lineup pub fn calculate_target_lineup_position(&mut self, side: LineupSide) -> Option { - let tag_id = self.vision.get_saved_id(); + let tag_id = self.limelight_lower.get_saved_id(); if tag_id == -1 { return None; } - let tag_position = self.vision.get_tag_position(tag_id)?; + let tag_position = self.limelight_lower.get_tag_position(tag_id)?; let tag_coords = tag_position.coordinate?; let tag_rotation = tag_position.quaternion?; @@ -419,6 +444,91 @@ impl Drivetrain { angle: robot_angle, }) } + + /// Set drivetrain speeds using tx and ty from the lower limelight. + /// Cameras are positioned on the robot such that the tag on the base of the reef is in the exact center of the limelight's fov when the robot is fully lined up. + /// Uses a basic PID: tx from the limelight (horizontal position of the tag on the screen) feeds into drivetrain strafe, while ty feeds into drivetrain forward. + pub fn lineup_2d(&mut self, side: LineupSide) -> bool { + // The lower limelight points at the tag when lined up on the right, the upper when lined up on the left + let mut limelight = self.limelight_lower.clone(); + match side { + LineupSide::Left => { + limelight = self.limelight_upper.clone(); + } + LineupSide::Right => { + limelight = self.limelight_lower.clone(); + } + } + + // Only try if a tag is detected + if limelight.get_id() != -1 { + // Figure out target angle from the tagmap + let tag_position = limelight.get_tag_position(limelight.get_id()).unwrap(); + let tag_rotation = tag_position.quaternion.unwrap(); + // None of us actually know how the quaternions provided by said map work, this is copied code + // Flip the tag normal to be out the back of the tag and wrap to the [0, 360] range + let tag_yaw = -(quaternion_to_yaw(tag_rotation) + std::f64::consts::PI) + % (std::f64::consts::PI * 2.); + // We score out the left, so forward-to-the-tag isn't very helpful + let mut perpendicular_yaw = tag_yaw + std::f64::consts::PI / 2.0; + // Convert to angle on [0,180] + if perpendicular_yaw > std::f64::consts::PI { + perpendicular_yaw = perpendicular_yaw - (std::f64::consts::PI * 2.); + } + let target_ty = match side { + LineupSide::Left => TARGET_TY_LEFT, + LineupSide::Right => TARGET_TY_RIGHT + }; + let target_tx = match side { + LineupSide::Left => TARGET_TX_LEFT, + LineupSide::Right => TARGET_TX_RIGHT, + }; + + // Calculate errors (difference between where you are (tx, ty, or drivetrain angle) and where you want to be (0 deg, 0 deg, or perpendicular_yaw)) + // Center of the limelight screen is (0,0) so we don't have to subtract anything for ty and tx + let error_ty = target_ty - limelight.get_ty().get::(); + let error_tx = target_tx - limelight.get_tx().get::(); + let error_yaw = perpendicular_yaw - self.get_offset().get::(); + + // Neither limelight faces perfectly straight out. + let off_straight_multiplier_str = match side { + LineupSide::Left => -1.0, + LineupSide::Right => 1.0, + }; + let off_straight_multiplier_fwd = match side { + LineupSide::Left => 1.0, + LineupSide::Right => -1.0, + }; + + // Calculate PID stuff + // KP - proportional: multiply the error by a tuned constant (KP) + let str = constants::drivetrain::LINEUP_2D_TY_STR_KP * error_ty + + off_straight_multiplier_str * LINEUP_2D_TX_STR_KP * error_tx; + + let fwd = LINEUP_2D_TX_FWD_KP * error_tx + + off_straight_multiplier_fwd * LINEUP_2D_TY_FWD_KP * error_yaw; + + let mut transform = Vector2::new(fwd, str); + + self.set_speeds( + transform.x, + transform.y, + error_yaw * SWERVE_TURN_KP, + SwerveControlStyle::RobotOriented, + ); + + return if error_ty.abs() < TX_ACCEPTABLE_ERROR && error_tx.abs() < TY_ACCEPTABLE_ERROR && error_yaw.abs() < YAW_ACCEPTABLE_ERROR { + println!("Drivetrain at target"); + true + } else { + println!("Drivetrain not at target ty: {} tx: {} yaw: {}", error_ty.abs(), error_tx.abs(), error_yaw.abs()); + false + }; + } else { + println!("can't lineup - no tag seen"); + false + } + } } fn quaternion_to_yaw(quaternion: Quaternion) -> f64 { diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs index 6e7ec0b..3ee0c9c 100644 --- a/src/subsystems/elevator.rs +++ b/src/subsystems/elevator.rs @@ -1,3 +1,4 @@ +use crate::constants; use crate::constants::{elevator, robotmap}; use frcrs::ctre::{ControlMode, Talon}; use std::fmt::Display; @@ -28,6 +29,7 @@ impl Elevator { pub fn new() -> Self { let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string())); let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config + println!("left pos: {} right pos: {} diff: {}",right.get_position(),left.get_position(),right.get_position() - left.get_position()); //right.follow(&left, true); @@ -50,11 +52,15 @@ impl Elevator { pub fn get_target(&self) -> ElevatorPosition { self.target } + /// in rotations, from left motor + pub fn get_position(&self) -> f64 { + self.left.get_position() + } /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position. /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner). /// Those configuration files have been saved to Documents on the driver station laptop. - pub fn run_to_target_trapezoid(&mut self) { + pub fn run_to_target_trapezoid(&mut self) -> bool { //load position to run to in rotations from constants.rs let target_position = match self.get_target() { ElevatorPosition::Bottom => elevator::BOTTOM, @@ -65,7 +71,16 @@ impl Elevator { // current implementation is to just set the control mode and call this function every frame self.right.set(ControlMode::MotionMagic, target_position); - self.left.set(ControlMode::MotionMagic, target_position); + self.left.follow(&self.right, true); + + println!("left pos: {} right pos: {} diff: {}",self.right.get_position(),self.left.get_position(),self.right.get_position() - self.left.get_position()); + if (target_position - self.right.get_position()).abs() < 0.5 { + println!("Elevator at target"); + true + } else { + // println!("Elevator not at target {}", (target_position - self.right.get_position()).abs()); + false + } // below is for when we eventually make this function async /* diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs index 474e817..00af9c3 100644 --- a/src/subsystems/indexer.rs +++ b/src/subsystems/indexer.rs @@ -1,6 +1,9 @@ use crate::constants::robotmap; +use crate::{constants, Ferris}; use frcrs::ctre::{ControlMode, Talon}; use frcrs::laser_can::LaserCan; +use std::cell::RefCell; +use std::rc::Rc; use std::time::Duration; use std::time::Instant; use tokio::time::sleep; @@ -19,6 +22,7 @@ impl Default for Indexer { impl Indexer { pub fn new() -> Self { let motor = Talon::new(robotmap::indexer::MOTOR, None); + let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN); Self { motor, laser_can } } @@ -27,19 +31,21 @@ impl Indexer { self.motor.set(ControlMode::Percent, speed); } - pub async fn intake_coral(&self) { - let mut last_loop = Instant::now(); + pub async fn intake_coral(indexer: Rc>) { + if let Ok(indexer) = indexer.try_borrow_mut() { + while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM + || indexer.laser_can.get_measurement() == -1 + { + println!("Dist: {}", indexer.get_laser_dist()); + indexer.set_speed(-0.3); + } - while self.laser_can.get_measurement() > robotmap::indexer::DISTANCE { - self.motor.set(ControlMode::Percent, 1.0); //may be -1 need to test - - // Cap at 250 hz - let elapsed = last_loop.elapsed().as_secs_f64(); - let left = (1. / 250. - elapsed).max(0.); - sleep(Duration::from_secs_f64(left)).await; - last_loop = Instant::now(); + indexer.motor.stop(); } - self.motor.stop(); + } + + pub fn get_laser_dist(&self) -> i32 { + self.laser_can.get_measurement() } pub fn stop(&self) { diff --git a/src/subsystems/vision.rs b/src/subsystems/vision.rs index 6545ed6..9deedbb 100644 --- a/src/subsystems/vision.rs +++ b/src/subsystems/vision.rs @@ -16,10 +16,12 @@ use uom::si::{ use std::net::SocketAddr; +#[derive(Clone)] pub struct Vision { tag_map_values: Value, limelight: Limelight, results: LimelightResults, + last_results: LimelightResults, saved_id: i32, } @@ -41,11 +43,13 @@ impl Vision { tag_map_values: tag_values, limelight, results: LimelightResults::default(), + last_results: LimelightResults::default(), saved_id: 0, } } /// Updates the results from the limelight, also posts telemetry data pub async fn update(&mut self, dt_angle: f64) { + self.last_results = self.results.clone(); self.results = self.limelight.results().await.unwrap(); self.limelight .update_robot_orientation(dt_angle) @@ -78,9 +82,16 @@ impl Vision { /// Gets the id of the targeted tag as of the last update /// RETURNS -1 IF NO TAG FOUND pub fn get_id(&self) -> i32 { - self.results.Fiducial[0].fID + if self.results.Fiducial.len() != 0 { + self.results.Fiducial[0].fID + } else { + -1 + } } + pub fn get_last_results(&self) -> LimelightResults { + self.last_results.clone() + } pub fn get_saved_id(&self) -> i32 { self.saved_id }