From 0686f6604c0e820ccaed865bf9c7e1e7df6f6c6d Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 11 Jan 2025 11:53:40 -0500 Subject: [PATCH 01/79] added basic choreo paths for testing auton functionality --- src/main/deploy/Choreo paths.chor | 78 ++++++++++++++++++++ src/main/deploy/Diagonal.traj | 61 +++++++++++++++ src/main/deploy/move in the x direction.traj | 58 +++++++++++++++ src/main/deploy/move in the y direction.traj | 47 ++++++++++++ src/main/deploy/rotate while driving.traj | 78 ++++++++++++++++++++ 5 files changed, 322 insertions(+) create mode 100644 src/main/deploy/Choreo paths.chor create mode 100644 src/main/deploy/Diagonal.traj create mode 100644 src/main/deploy/move in the x direction.traj create mode 100644 src/main/deploy/move in the y direction.traj create mode 100644 src/main/deploy/rotate while driving.traj diff --git a/src/main/deploy/Choreo paths.chor b/src/main/deploy/Choreo paths.chor new file mode 100644 index 0000000..7d54267 --- /dev/null +++ b/src/main/deploy/Choreo paths.chor @@ -0,0 +1,78 @@ +{ + "name":"Choreo 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/main/deploy/Diagonal.traj b/src/main/deploy/Diagonal.traj new file mode 100644 index 0000000..7577be3 --- /dev/null +++ b/src/main/deploy/Diagonal.traj @@ -0,0 +1,61 @@ +{ + "name":"Diagonal", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.5299232006073, "y":6.882591247558594, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.022549152374268, "y":5.633831024169922, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.5299232006073 m", "val":2.5299232006073}, "y":{"exp":"6.882591247558594 m", "val":6.882591247558594}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.022549152374268 m", "val":6.022549152374268}, "y":{"exp":"5.633831024169922 m", "val":5.633831024169922}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.30082], + "samples":[ + {"t":0.0, "x":2.52992, "y":6.88259, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.49045, "ay":-3.04275, "alpha":0.0, "fx":[144.42017,144.42017,144.42017,144.42017], "fy":[-51.75637,-51.75637,-51.75637,-51.75637]}, + {"t":0.05003, "x":2.54055, "y":6.87878, "heading":0.0, "vx":0.42479, "vy":-0.15223, "omega":0.0, "ax":8.48988, "ay":-3.04188, "alpha":-0.00001, "fx":[144.41034,144.41036,144.41037,144.41033], "fy":[-51.74149,-51.74144,-51.74141,-51.74152]}, + {"t":0.10006, "x":2.57243, "y":6.86736, "heading":0.0, "vx":0.84955, "vy":-0.30442, "omega":0.0, "ax":8.4891, "ay":-3.04094, "alpha":-0.00002, "fx":[144.39709,144.39713,144.39716,144.39707], "fy":[-51.72566,-51.72554,-51.72548,-51.72572]}, + {"t":0.15009, "x":2.62556, "y":6.84832, "heading":0.0, "vx":1.27427, "vy":-0.45657, "omega":0.0, "ax":8.48803, "ay":-3.03993, "alpha":-0.00003, "fx":[144.37899,144.37906,144.3791,144.37895], "fy":[-51.70842,-51.70823,-51.70812,-51.70853]}, + {"t":0.20013, "x":2.69993, "y":6.82168, "heading":0.0, "vx":1.69894, "vy":-0.60866, "omega":0.0, "ax":8.48655, "ay":-3.03878, "alpha":-0.00005, "fx":[144.35368,144.35378,144.35384,144.35362], "fy":[-51.68898,-51.6887,-51.68853,-51.68915]}, + {"t":0.25016, "x":2.79556, "y":6.78742, "heading":0.0, "vx":2.12353, "vy":-0.76069, "omega":-0.00001, "ax":8.48439, "ay":-3.03742, "alpha":-0.00007, "fx":[144.31692,144.31707,144.31715,144.31684], "fy":[-51.66589,-51.66548,-51.66525,-51.66612]}, + {"t":0.30019, "x":2.91242, "y":6.74556, "heading":0.0, "vx":2.54802, "vy":-0.91266, "omega":-0.00001, "ax":8.48107, "ay":-3.03567, "alpha":-0.00009, "fx":[144.26039,144.2606,144.26071,144.26028], "fy":[-51.63618,-51.63562,-51.63531,-51.6365]}, + {"t":0.35022, "x":3.05051, "y":6.6961, "heading":0.0, "vx":2.97234, "vy":-1.06454, "omega":-0.00001, "ax":8.47545, "ay":-3.03312, "alpha":-0.00012, "fx":[144.16492,144.16519,144.16534,144.16477], "fy":[-51.59296,-51.5922,-51.59178,-51.59338]}, + {"t":0.40025, "x":3.20983, "y":6.63904, "heading":0.0, "vx":3.39638, "vy":-1.21629, "omega":-0.00002, "ax":8.46426, "ay":-3.02857, "alpha":-0.00016, "fx":[143.97448,143.97481,143.97502,143.97427], "fy":[-51.51559,-51.51462,-51.51408,-51.51613]}, + {"t":0.45028, "x":3.39035, "y":6.5744, "heading":0.0, "vx":3.81986, "vy":-1.36781, "omega":-0.00003, "ax":8.43195, "ay":-3.01618, "alpha":-0.00018, "fx":[143.4249,143.42524,143.42549,143.42465], "fy":[-51.30499,-51.3039,-51.30331,-51.30558]}, + {"t":0.50031, "x":3.59202, "y":6.50219, "heading":0.0, "vx":4.24172, "vy":-1.51872, "omega":-0.00004, "ax":7.55274, "ay":-2.68454, "alpha":0.00074, "fx":[128.47508,128.46474,128.46867,128.47114], "fy":[-45.6607,-45.66569,-45.67254,-45.65386]}, + {"t":0.55035, "x":3.81369, "y":6.42285, "heading":-0.00001, "vx":4.61959, "vy":-1.65303, "omega":0.0, "ax":0.0115, "ay":0.01483, "alpha":-0.00001, "fx":[0.19561,0.19555,0.1956,0.19555], "fy":[0.25219,0.25228,0.25221,0.25226]}, + {"t":0.60038, "x":4.04483, "y":6.34016, "heading":-0.00001, "vx":4.62017, "vy":-1.65229, "omega":0.0, "ax":0.00297, "ay":0.00832, "alpha":0.0, "fx":[0.05052,0.05052,0.05052,0.05052], "fy":[0.14159,0.14159,0.14159,0.14159]}, + {"t":0.65041, "x":4.27599, "y":6.25751, "heading":-0.00001, "vx":4.62032, "vy":-1.65187, "omega":0.0, "ax":0.00259, "ay":0.00726, "alpha":0.0, "fx":[0.04405,0.04405,0.04405,0.04405], "fy":[0.12345,0.12345,0.12345,0.12345]}, + {"t":0.70044, "x":4.50715, "y":6.17487, "heading":-0.00001, "vx":4.62045, "vy":-1.65151, "omega":0.0, "ax":0.00002, "ay":0.01743, "alpha":0.0, "fx":[0.00035,0.00038,0.00036,0.00036], "fy":[0.29655,0.29656,0.29656,0.29655]}, + {"t":0.75047, "x":4.73832, "y":6.09227, "heading":-0.00001, "vx":4.62045, "vy":-1.65063, "omega":0.0, "ax":-7.54205, "ay":2.71474, "alpha":0.00009, "fx":[-128.2882,-128.288,-128.28833,-128.28787], "fy":[46.17722,46.17655,46.17652,46.17724]}, + {"t":0.8005, "x":4.96005, "y":6.01308, "heading":-0.00001, "vx":4.24311, "vy":-1.51481, "omega":0.0, "ax":-8.43326, "ay":3.01245, "alpha":0.0001, "fx":[-143.44716,-143.44737,-143.44749,-143.44703], "fy":[51.24125,51.24065,51.24032,51.24159]}, + {"t":0.85053, "x":5.16178, "y":5.94106, "heading":-0.00001, "vx":3.82118, "vy":-1.36409, "omega":0.00001, "ax":-8.46607, "ay":3.02347, "alpha":0.00006, "fx":[-144.00528,-144.00542,-144.00549,-144.00521], "fy":[51.42851,51.42813,51.42792,51.42871]}, + {"t":0.90057, "x":5.34236, "y":5.8766, "heading":-0.00001, "vx":3.39761, "vy":-1.21283, "omega":0.00001, "ax":-8.47755, "ay":3.02721, "alpha":0.00003, "fx":[-144.20073,-144.2008,-144.20084,-144.20069], "fy":[51.49208,51.49189,51.49178,51.49219]}, + {"t":0.9506, "x":5.50174, "y":5.81971, "heading":0.0, "vx":2.97347, "vy":-1.06137, "omega":0.00001, "ax":-8.48343, "ay":3.02903, "alpha":0.00001, "fx":[-144.30068,-144.3007,-144.30071,-144.30067], "fy":[51.52288,51.52282,51.52279,51.52291]}, + {"t":1.00063, "x":5.63989, "y":5.7704, "heading":0.0, "vx":2.54903, "vy":-0.90982, "omega":0.00001, "ax":-8.48701, "ay":3.03005, "alpha":-0.00001, "fx":[-144.36166,-144.36164,-144.36163,-144.36167], "fy":[51.54022,51.54026,51.54029,51.54019]}, + {"t":1.05066, "x":5.7568, "y":5.72867, "heading":0.0, "vx":2.12441, "vy":-0.75823, "omega":0.00001, "ax":-8.48944, "ay":3.03067, "alpha":-0.00002, "fx":[-144.40293,-144.40288,-144.40286,-144.40295], "fy":[51.55073,51.55084,51.55091,51.55066]}, + {"t":1.10069, "x":5.85246, "y":5.69453, "heading":0.0, "vx":1.69967, "vy":-0.6066, "omega":0.00001, "ax":-8.4912, "ay":3.03106, "alpha":-0.00003, "fx":[-144.43286,-144.4328,-144.43276,-144.43289], "fy":[51.55729,51.55747,51.55756,51.5572]}, + {"t":1.15072, "x":5.92687, "y":5.66797, "heading":0.0, "vx":1.27485, "vy":-0.45495, "omega":0.00001, "ax":-8.49254, "ay":3.0313, "alpha":-0.00004, "fx":[-144.45567,-144.45559,-144.45555,-144.45571], "fy":[51.56138,51.5616,51.56172,51.56126]}, + {"t":1.20075, "x":5.98002, "y":5.649, "heading":0.0, "vx":0.84995, "vy":-0.30329, "omega":0.00001, "ax":-8.4936, "ay":3.03144, "alpha":-0.00004, "fx":[-144.47372,-144.47363,-144.47358,-144.47377], "fy":[51.56382,51.56407,51.56421,51.56368]}, + {"t":1.25079, "x":6.01192, "y":5.63762, "heading":0.0, "vx":0.42501, "vy":-0.15162, "omega":0.00001, "ax":-8.49481, "ay":3.03055, "alpha":-0.00016, "fx":[-144.49439,-144.49322,-144.49442,-144.49479], "fy":[51.54834,51.55162,51.54826,51.54721]}, + {"t":1.30082, "x":6.02255, "y":5.63383, "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/src/main/deploy/move in the x direction.traj b/src/main/deploy/move in the x direction.traj new file mode 100644 index 0000000..34e687a --- /dev/null +++ b/src/main/deploy/move in the x direction.traj @@ -0,0 +1,58 @@ +{ + "name":"move in the x direction", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.354316234588623, "y":6.1216278076171875, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.607905387878418, "y":6.102116107940674, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.354316234588623 m", "val":2.354316234588623}, "y":{"exp":"6.1216278076171875 m", "val":6.1216278076171875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.607905387878418 m", "val":6.607905387878418}, "y":{"exp":"6.102116107940674 m", "val":6.102116107940674}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.86742], + "samples":[ + {"t":0.0, "x":2.35432, "y":6.12163, "heading":0.0, "vx":4.89743, "vy":-0.02247, "omega":0.0, "ax":0.21753, "ay":-0.001, "alpha":0.0, "fx":[3.70006,3.70006,3.70006,3.70006], "fy":[-0.01697,-0.01697,-0.01697,-0.01697]}, + {"t":0.02991, "x":2.5009, "y":6.12096, "heading":0.0, "vx":4.90394, "vy":-0.02249, "omega":0.0, "ax":0.00026, "ay":0.0, "alpha":0.0, "fx":[0.00436,0.00436,0.00436,0.00436], "fy":[-0.00002,-0.00002,-0.00002,-0.00002]}, + {"t":0.05982, "x":2.64758, "y":6.12028, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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":0.08973, "x":2.79427, "y":6.11961, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.11964, "x":2.94095, "y":6.11894, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.14956, "x":3.08763, "y":6.11826, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.17947, "x":3.23431, "y":6.11759, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.20938, "x":3.38099, "y":6.11692, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.23929, "x":3.52768, "y":6.11625, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.2692, "x":3.67436, "y":6.11557, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.29911, "x":3.82104, "y":6.1149, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.32902, "x":3.96772, "y":6.11423, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.35893, "x":4.11441, "y":6.11355, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.38884, "x":4.26109, "y":6.11288, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.41875, "x":4.40777, "y":6.11221, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.44867, "x":4.55445, "y":6.11154, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.47858, "x":4.70113, "y":6.11086, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.50849, "x":4.84782, "y":6.11019, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.5384, "x":4.9945, "y":6.10952, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.56831, "x":5.14118, "y":6.10884, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.59822, "x":5.28786, "y":6.10817, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.62813, "x":5.43455, "y":6.1075, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.65804, "x":5.58123, "y":6.10683, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.68795, "x":5.72791, "y":6.10615, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.71787, "x":5.87459, "y":6.10548, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.74778, "x":6.02127, "y":6.10481, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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.77769, "x":6.16796, "y":6.10413, "heading":0.0, "vx":4.90395, "vy":-0.02249, "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":0.8076, "x":6.31464, "y":6.10346, "heading":0.0, "vx":4.90395, "vy":-0.02249, "omega":0.0, "ax":-0.00026, "ay":0.0, "alpha":0.0, "fx":[-0.00436,-0.00436,-0.00436,-0.00436], "fy":[0.00002,0.00002,0.00002,0.00002]}, + {"t":0.83751, "x":6.46132, "y":6.10279, "heading":0.0, "vx":4.90394, "vy":-0.02249, "omega":0.0, "ax":-0.21753, "ay":0.001, "alpha":0.0, "fx":[-3.70006,-3.70006,-3.70006,-3.70006], "fy":[0.01697,0.01697,0.01697,0.01697]}, + {"t":0.86742, "x":6.60791, "y":6.10212, "heading":0.0, "vx":4.89743, "vy":-0.02247, "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/src/main/deploy/move in the y direction.traj b/src/main/deploy/move in the y direction.traj new file mode 100644 index 0000000..78a01e5 --- /dev/null +++ b/src/main/deploy/move in the y direction.traj @@ -0,0 +1,47 @@ +{ + "name":"move in the y direction", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.7250418663024902, "y":7.409411907196045, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.7250418663024902, "y":5.633831024169922, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.7250418663024902 m", "val":2.7250418663024902}, "y":{"exp":"7.409411907196045 m", "val":7.409411907196045}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7250418663024902 m", "val":2.7250418663024902}, "y":{"exp":"5.633831024169922 m", "val":5.633831024169922}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.36234], + "samples":[ + {"t":0.0, "x":2.72504, "y":7.40941, "heading":0.0, "vx":0.0, "vy":-4.89133, "omega":0.0, "ax":0.0, "ay":-0.46895, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-7.97677,-7.97677,-7.97677,-7.97677]}, + {"t":0.02013, "x":2.72504, "y":7.31085, "heading":0.0, "vx":0.0, "vy":-4.90077, "omega":0.0, "ax":0.0, "ay":-0.00268, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.04558,-0.04558,-0.04558,-0.04558]}, + {"t":0.04026, "x":2.72504, "y":7.2122, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":-0.00002, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00026,-0.00026,-0.00026,-0.00026]}, + {"t":0.06039, "x":2.72504, "y":7.11355, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.08052, "x":2.72504, "y":7.01489, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.10065, "x":2.72504, "y":6.91624, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.12078, "x":2.72504, "y":6.81758, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.14091, "x":2.72504, "y":6.71893, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.16104, "x":2.72504, "y":6.62028, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.18117, "x":2.72504, "y":6.52162, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.2013, "x":2.72504, "y":6.42297, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.22143, "x":2.72504, "y":6.32431, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.24156, "x":2.72504, "y":6.22566, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.26169, "x":2.72504, "y":6.127, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.28182, "x":2.72504, "y":6.02835, "heading":0.0, "vx":0.0, "vy":-4.90082, "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.30195, "x":2.72504, "y":5.9297, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.00002, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00026,0.00026,0.00026,0.00026]}, + {"t":0.32208, "x":2.72504, "y":5.83104, "heading":0.0, "vx":0.0, "vy":-4.90082, "omega":0.0, "ax":0.0, "ay":0.00268, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.04558,0.04558,0.04558,0.04558]}, + {"t":0.34221, "x":2.72504, "y":5.73239, "heading":0.0, "vx":0.0, "vy":-4.90077, "omega":0.0, "ax":0.0, "ay":0.46895, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[7.97677,7.97677,7.97677,7.97677]}, + {"t":0.36234, "x":2.72504, "y":5.63383, "heading":0.0, "vx":0.0, "vy":-4.89133, "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/src/main/deploy/rotate while driving.traj b/src/main/deploy/rotate while driving.traj new file mode 100644 index 0000000..b2739c9 --- /dev/null +++ b/src/main/deploy/rotate while driving.traj @@ -0,0 +1,78 @@ +{ + "name":"rotate while driving", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.4909000396728516, "y":6.5899128913879395, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.6469292640686035, "y":6.570401191711426, "heading":3.141592653589793, "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":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.4909000396728516 m", "val":2.4909000396728516}, "y":{"exp":"6.5899128913879395 m", "val":6.5899128913879395}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.6469292640686035 m", "val":6.6469292640686035}, "y":{"exp":"6.570401191711426 m", "val":6.570401191711426}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.45421], + "samples":[ + {"t":0.0, "x":2.4909, "y":6.58991, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.63261, "ay":-0.09559, "alpha":16.96601, "fx":[115.31391,145.56663,111.96895,146.46487], "fy":[100.9204,-48.41461,-104.63215,45.62237]}, + {"t":0.03382, "x":2.49526, "y":6.58986, "heading":0.0, "vx":0.25813, "vy":-0.00323, "omega":0.57377, "ax":7.70998, "ay":-0.10361, "alpha":16.43555, "fx":[117.79766,145.76818,114.30255,146.70974], "fy":[97.97316,-47.76537,-102.0438,44.78671]}, + {"t":0.06764, "x":2.5084, "y":6.58969, "heading":0.0194, "vx":0.51887, "vy":-0.00674, "omega":1.1296, "ax":7.80044, "ay":-0.08886, "alpha":15.79543, "fx":[120.84828,146.34363,116.85868,146.68228], "fy":[94.13017,-45.92557,-99.07488,44.82414]}, + {"t":0.10146, "x":2.53041, "y":6.58941, "heading":0.05761, "vx":0.78267, "vy":-0.00974, "omega":1.66378, "ax":7.90905, "ay":-0.06134, "alpha":14.98828, "fx":[124.74728,147.22966,119.70358,146.44253], "fy":[88.82049,-42.94184,-95.58795,45.53594]}, + {"t":0.13528, "x":2.5614, "y":6.58905, "heading":0.11387, "vx":1.05014, "vy":-0.01182, "omega":2.17067, "ax":8.04188, "ay":-0.0352, "alpha":13.93373, "fx":[129.78535,148.33552,122.96662,146.07278], "fy":[81.17102,-38.87156,-91.3203,46.6259]}, + {"t":0.16909, "x":2.60152, "y":6.58863, "heading":0.18728, "vx":1.32211, "vy":-0.01301, "omega":2.64189, "ax":8.20187, "ay":-0.02935, "alpha":12.55915, "fx":[136.01578,149.54526,126.77717,145.70779], "fy":[70.07483,-33.80572,-85.91359,47.6473]}, + {"t":0.20291, "x":2.65092, "y":6.58817, "heading":0.27663, "vx":1.59949, "vy":-0.014, "omega":3.06663, "ax":8.38251, "ay":-0.06267, "alpha":10.85761, "fx":[142.82415,150.7239,131.21237,145.57568], "fy":[54.65415,-27.90188,-78.92075,47.90444]}, + {"t":0.23673, "x":2.70981, "y":6.58766, "heading":0.38034, "vx":1.88297, "vy":-0.01612, "omega":3.43382, "ax":8.56451, "ay":-0.13937, "alpha":8.93536, "fx":[148.68033,151.73081,136.26735,146.04109], "fy":[35.44067,-21.43208,-69.74635,46.25523]}, + {"t":0.27055, "x":2.77838, "y":6.58704, "heading":0.49647, "vx":2.17262, "vy":-0.02083, "omega":3.736, "ax":8.72812, "ay":-0.2356, "alpha":6.90316, "fx":[151.98794,152.44085,141.82989,147.59295], "fy":[15.6007,-14.8738,-57.44438,40.68734]}, + {"t":0.30437, "x":2.85685, "y":6.5862, "heading":0.62281, "vx":2.46779, "vy":-0.0288, "omega":3.96946, "ax":8.87064, "ay":-0.32072, "alpha":4.52769, "fx":[152.75142,152.76845,147.55101,150.47702], "fy":[0.09314,-9.17268,-40.19807,27.45606]}, + {"t":0.33819, "x":2.94538, "y":6.58504, "heading":0.75705, "vx":2.76778, "vy":-0.03965, "omega":4.12258, "ax":8.96725, "ay":-0.40599, "alpha":0.93307, "fx":[152.56893,152.65122,152.13271,152.7685], "fy":[-7.03266,-6.79716,-13.93702,0.14357]}, + {"t":0.37201, "x":3.04411, "y":6.58347, "heading":0.89647, "vx":3.07105, "vy":-0.05338, "omega":4.15413, "ax":8.79613, "ay":-0.53779, "alpha":-5.22484, "fx":[152.66387,151.0145,149.2498,145.5505], "fy":[-3.72513,-18.35606,31.22596,-45.73568]}, + {"t":0.40583, "x":3.153, "y":6.58135, "heading":1.03696, "vx":3.36852, "vy":-0.07156, "omega":3.97743, "ax":7.59699, "ay":-1.17566, "alpha":-15.31603, "fx":[152.3927,114.96155,121.99737,127.53892], "fy":[8.82149,-96.04456,90.77765,-83.54501]}, + {"t":0.43964, "x":3.27126, "y":6.57826, "heading":1.17147, "vx":3.62544, "vy":-0.11132, "omega":3.45947, "ax":7.55584, "ay":-0.91387, "alpha":-16.03331, "fx":[151.30326,110.96406,118.87482,132.9486], "fy":[18.55267,-100.56755,94.08228,-74.24578]}, + {"t":0.47346, "x":3.39819, "y":6.57397, "heading":1.28847, "vx":3.88097, "vy":-0.14223, "omega":2.91724, "ax":7.4343, "ay":-0.61232, "alpha":-17.03207, "fx":[149.42423,106.87265,112.78843,136.73623], "fy":[28.48056,-104.06757,100.22875,-66.30333]}, + {"t":0.50728, "x":3.53369, "y":6.56881, "heading":1.38713, "vx":4.13239, "vy":-0.16294, "omega":2.34123, "ax":7.18013, "ay":-0.2645, "alpha":-18.52445, "fx":[146.41834,101.05308,101.69957,139.35679], "fy":[39.1871,-107.82495,109.84594,-59.20432]}, + {"t":0.5411, "x":3.67755, "y":6.56315, "heading":1.4663, "vx":4.37521, "vy":-0.17188, "omega":1.71476, "ax":6.79538, "ay":0.15914, "alpha":-20.17157, "fx":[141.65844,94.74468,84.79444,141.15216], "fy":[50.78002,-109.08006,120.62607,-51.49843]}, + {"t":0.57492, "x":3.8294, "y":6.55743, "heading":1.5243, "vx":4.60502, "vy":-0.1665, "omega":1.03258, "ax":6.23584, "ay":0.56269, "alpha":-21.31259, "fx":[134.36458,84.9136,64.74214,140.25937], "fy":[60.5362,-104.44587,125.71657,-43.52243]}, + {"t":0.60874, "x":3.9887, "y":6.55212, "heading":1.55922, "vx":4.81591, "vy":-0.14747, "omega":0.31182, "ax":2.51954, "ay":2.54701, "alpha":-8.65482, "fx":[61.8905,21.30785,17.76223,70.46624], "fy":[61.76482,22.6527,71.16249,17.71574]}, + {"t":0.64256, "x":4.15301, "y":6.54859, "heading":1.56976, "vx":4.90112, "vy":-0.06133, "omega":0.01912, "ax":0.021, "ay":1.03692, "alpha":-0.0408, "fx":[0.46833,0.24621,0.24643,0.46818], "fy":[17.74558,17.52983,17.74599,17.52942]}, + {"t":0.67638, "x":4.31878, "y":6.54711, "heading":1.57041, "vx":4.90183, "vy":-0.02627, "omega":0.01774, "ax":0.00146, "ay":0.34479, "alpha":-0.00008, "fx":[0.02509,0.02468,0.02465,0.02512], "fy":[5.86505,5.86458,5.86502,5.86461]}, + {"t":0.71019, "x":4.48455, "y":6.54642, "heading":1.57101, "vx":4.90188, "vy":-0.01461, "omega":0.01774, "ax":0.00039, "ay":0.17198, "alpha":0.00006, "fx":[0.00653,0.0068,0.00686,0.00647], "fy":[2.92509,2.92548,2.92515,2.92542]}, + {"t":0.74401, "x":4.65033, "y":6.54602, "heading":1.57161, "vx":4.90189, "vy":-0.00879, "omega":0.01774, "ax":0.00017, "ay":0.24794, "alpha":0.0002, "fx":[0.00242,0.00351,0.00348,0.00246], "fy":[4.2169,4.21792,4.21687,4.21795]}, + {"t":0.77783, "x":4.8161, "y":6.54587, "heading":1.57221, "vx":4.9019, "vy":-0.00041, "omega":0.01775, "ax":-0.01255, "ay":0.6938, "alpha":0.03803, "fx":[-0.31597,-0.11095,-0.1106,-0.31628], "fy":[11.69969,11.90304,11.7001,11.90264]}, + {"t":0.81165, "x":4.98187, "y":6.54625, "heading":1.57281, "vx":4.90148, "vy":0.02306, "omega":0.01903, "ax":-2.48678, "ay":1.77879, "alpha":8.68702, "fx":[-67.78584,-18.42584,-21.31904,-61.66686], "fy":[5.39721,58.36128,6.69503,50.57347]}, + {"t":0.84547, "x":5.14621, "y":6.54805, "heading":1.57345, "vx":4.81738, "vy":0.08321, "omega":0.31282, "ax":-6.24566, "ay":0.45912, "alpha":21.41143, "fx":[-140.22171,-66.73355,-83.49553,-134.4968], "fy":[-43.93737,123.40669,-108.13132,59.90008]}, + {"t":0.87929, "x":5.30556, "y":6.55112, "heading":1.58403, "vx":4.60616, "vy":0.09874, "omega":1.03693, "ax":-6.60966, "ay":0.48992, "alpha":21.19107, "fx":[-143.76466,-73.9045,-94.17226,-137.87232], "fy":[-43.9427,127.28118,-110.33466,60.32951]}, + {"t":0.91311, "x":5.45755, "y":6.55474, "heading":1.6191, "vx":4.38262, "vy":0.11531, "omega":1.75358, "ax":-7.06503, "ay":0.08522, "alpha":19.35787, "fx":[-142.92748,-94.52694,-100.18001,-143.06211], "fy":[-50.06181,115.45253,-109.47616,49.88386]}, + {"t":0.94693, "x":5.60173, "y":6.55869, "heading":1.6784, "vx":4.14369, "vy":0.11819, "omega":2.40824, "ax":-7.38823, "ay":-0.31487, "alpha":17.65357, "fx":[-141.25761,-110.01691,-104.58004,-146.8323], "fy":[-56.07382,102.54294,-107.30868,39.41608]}, + {"t":0.98074, "x":5.73764, "y":6.56251, "heading":1.75985, "vx":3.89383, "vy":0.10754, "omega":3.00526, "ax":-7.59703, "ay":-0.664, "alpha":16.29508, "fx":[-139.04738,-120.01517,-108.37212,-149.45837], "fy":[-62.11549,91.92777,-104.53299,29.54302]}, + {"t":1.01456, "x":5.86498, "y":6.56577, "heading":1.86148, "vx":3.63691, "vy":0.08509, "omega":3.55634, "ax":-7.7889, "ay":-0.93433, "alpha":14.80487, "fx":[-136.70682,-127.38581,-114.59155,-151.26377], "fy":[-67.55051,82.42311,-98.31865,19.87521]}, + {"t":1.04838, "x":5.98352, "y":6.56811, "heading":1.98175, "vx":3.3735, "vy":0.05349, "omega":4.05703, "ax":-8.89156, "ay":-0.5838, "alpha":3.17163, "fx":[-149.63334,-151.90376,-150.79951,-152.63472], "fy":[-29.85335,12.88164,-21.64003,-1.10948]}, + {"t":1.0822, "x":6.09252, "y":6.56958, "heading":2.11896, "vx":3.0728, "vy":0.03375, "omega":4.16429, "ax":-8.93961, "ay":-0.43202, "alpha":-2.39795, "fx":[-152.3918,-150.9017,-152.83099,-152.11642], "fy":[10.55503,-24.05562,-2.78377,-13.11001]}, + {"t":1.11602, "x":6.19133, "y":6.57048, "heading":2.25979, "vx":2.77047, "vy":0.01914, "omega":4.08319, "ax":-8.81373, "ay":-0.34481, "alpha":-5.51804, "fx":[-148.41219,-146.12098,-153.06282,-152.08011], "fy":[36.9273,-45.28775,-1.31893,-13.78127]}, + {"t":1.14984, "x":6.27998, "y":6.57093, "heading":2.39788, "vx":2.4724, "vy":0.00748, "omega":3.89658, "ax":-8.68626, "ay":-0.27098, "alpha":-7.32834, "fx":[-144.43843,-140.80008,-153.09056,-152.67422], "fy":[50.69313,-60.055,-5.25775,-3.81772]}, + {"t":1.18366, "x":6.35863, "y":6.57103, "heading":2.52965, "vx":2.17864, "vy":-0.00169, "omega":3.64874, "ax":-8.5645, "ay":-0.16035, "alpha":-8.72357, "fx":[-142.30968,-135.54143,-152.8386,-152.0292], "fy":[56.65843,-71.25554,-11.21067,14.89797]}, + {"t":1.21748, "x":6.42741, "y":6.57088, "heading":2.65305, "vx":1.889, "vy":-0.00711, "omega":3.35372, "ax":-8.41732, "ay":-0.0275, "alpha":-10.25241, "fx":[-141.75868,-130.60625,-152.25491,-148.08499], "fy":[58.20424,-80.01305,-17.82377,37.76158]}, + {"t":1.2513, "x":6.48648, "y":6.57062, "heading":2.76647, "vx":1.60434, "vy":-0.00804, "omega":3.007, "ax":-8.24298, "ay":0.07207, "alpha":-11.93451, "fx":[-142.17505,-126.17708,-151.36974,-141.12138], "fy":[57.31111,-86.87278,-24.419,58.88407]}, + {"t":1.28511, "x":6.53602, "y":6.57039, "heading":2.86816, "vx":1.32557, "vy":-0.0056, "omega":2.60339, "ax":-8.07089, "ay":0.10693, "alpha":-13.49601, "fx":[-143.05535,-122.3737,-150.26832,-133.43666], "fy":[55.17723,-92.18101,-30.59122,74.87047]}, + {"t":1.31893, "x":6.57624, "y":6.57026, "heading":2.9562, "vx":1.05262, "vy":-0.00199, "omega":2.14697, "ax":-7.92555, "ay":0.08723, "alpha":-14.74292, "fx":[-144.05704,-119.23541,-149.06664,-126.88639], "fy":[52.58565,-96.22565,-36.07191,85.64702]}, + {"t":1.35275, "x":6.6073, "y":6.57024, "heading":3.02881, "vx":0.78459, "vy":0.00096, "omega":1.64838, "ax":-7.81275, "ay":0.03944, "alpha":-15.65509, "fx":[-144.97169,-116.71086,-147.88992,-121.99793], "fy":[50.07296,-99.28872,-40.68387,92.58304]}, + {"t":1.38657, "x":6.62937, "y":6.5703, "heading":3.08456, "vx":0.52037, "vy":0.0023, "omega":1.11894, "ax":-7.72701, "ay":-0.01311, "alpha":-16.30479, "fx":[-145.68514,-114.67245,-146.85513,-118.52432], "fy":[48.01018,-101.652,-44.31692,97.06673]}, + {"t":1.42039, "x":6.64255, "y":6.57037, "heading":3.1224, "vx":0.25905, "vy":0.00185, "omega":0.56754, "ax":-7.66005, "ay":-0.05476, "alpha":-16.78169, "fx":[-146.13969,-112.95769,-146.06082,-116.02315], "fy":[46.65159,-103.57186,-46.90369,100.09799]}, + {"t":1.45421, "x":6.64693, "y":6.5704, "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":[] +} From 66662eb064f3fcfc4d47859f2996c192a766c784 Mon Sep 17 00:00:00 2001 From: Red Date: Tue, 14 Jan 2025 06:23:37 +0000 Subject: [PATCH 02/79] Pushed a full test path to run --- .../pathplanner/autos/Top - 4 Coral Auto.auto | 61 +++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../paths/J,I to Station and back.path | 96 +++++++++++++++++ .../paths/J,I to Station to L, K.path | 100 ++++++++++++++++++ .../paths/L, K to Station and back.path | 96 +++++++++++++++++ .../deploy/pathplanner/paths/Top to J,I.path | 54 ++++++++++ src/main/deploy/pathplanner/settings.json | 32 ++++++ 7 files changed, 440 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/J,I to Station and back.path create mode 100644 src/main/deploy/pathplanner/paths/J,I to Station to L, K.path create mode 100644 src/main/deploy/pathplanner/paths/L, K to Station and back.path create mode 100644 src/main/deploy/pathplanner/paths/Top to J,I.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto b/src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto new file mode 100644 index 0000000..f08796f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to J,I" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "J,I to Station and back" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "J,I to Station to L, K" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "L, K to Station and back" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J,I to Station and back.path b/src/main/deploy/pathplanner/paths/J,I to Station and back.path new file mode 100644 index 0000000..6861d61 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/J,I to Station and back.path @@ -0,0 +1,96 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.2857169486380915, + "y": 5.207857863967483 + }, + "prevControl": null, + "nextControl": { + "x": 1.231399772034817, + "y": 7.532716062218043 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8349959089949324, + "y": 7.200054766680743 + }, + "prevControl": { + "x": 5.73826015519869, + "y": 4.891424218864765 + }, + "nextControl": { + "x": 1.6198163567610897, + "y": 7.327325188619397 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2857169486380915, + "y": 5.207857863967483 + }, + "prevControl": { + "x": 1.7386036147673978, + "y": 7.246865032045945 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 130.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.070874451754386, + "maxWaypointRelativePos": 1.4, + "constraints": { + "maxVelocity": 0.3, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Raise L4", + "waypointRelativePos": 1.8484100877192986, + "endWaypointRelativePos": null, + "command": null + } + ], + "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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path b/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path new file mode 100644 index 0000000..fc3e888 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path @@ -0,0 +1,100 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.2857169486380915, + "y": 5.207857863967483 + }, + "prevControl": null, + "nextControl": { + "x": 0.8694549570056491, + "y": 7.655828173694941 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8973330233959442, + "y": 7.240962381565823 + }, + "prevControl": { + "x": 5.693393352468902, + "y": 5.037692013550632 + }, + "nextControl": { + "x": 1.6811136855615914, + "y": 7.366458190047517 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.779771959459459, + "y": 5.207857863967483 + }, + "prevControl": { + "x": 1.7201828394381518, + "y": 7.591910063856189 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 130.0 + }, + { + "waypointRelativePos": 1.8369041582150103, + "rotationDegrees": -59.99999999999999 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0019248188405792, + "maxWaypointRelativePos": 1.5275324577294696, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Raise L4", + "waypointRelativePos": 1.9, + "endWaypointRelativePos": null, + "command": null + } + ], + "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": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L, K to Station and back.path b/src/main/deploy/pathplanner/paths/L, K to Station and back.path new file mode 100644 index 0000000..9d157a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L, K to Station and back.path @@ -0,0 +1,96 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.779771959459459, + "y": 5.207857863967483 + }, + "prevControl": null, + "nextControl": { + "x": 1.8725507069059188, + "y": 7.013568682904109 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.607935609206641, + "y": 7.156955540435973 + }, + "prevControl": { + "x": 1.4089293212996126, + "y": 7.319139314026312 + }, + "nextControl": { + "x": 3.8299091553650393, + "y": 5.346118012014594 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.779771959459459, + "y": 5.207857863967483 + }, + "prevControl": { + "x": 3.627209394964983, + "y": 5.4059100386964385 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 130.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5515762061403513, + "maxWaypointRelativePos": 0.7087719298245614, + "constraints": { + "maxVelocity": 0.3, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Raise L4", + "waypointRelativePos": 1.2336759868421046, + "endWaypointRelativePos": null, + "command": null + } + ], + "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": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top to J,I.path b/src/main/deploy/pathplanner/paths/Top to J,I.path new file mode 100644 index 0000000..decb39d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top to J,I.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.894500732421875, + "y": 5.207857863967483 + }, + "prevControl": null, + "nextControl": { + "x": 5.698075452302135, + "y": 5.2101679886381485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2857169486380915, + "y": 5.207857863967483 + }, + "prevControl": { + "x": 6.056471022591409, + "y": 5.212898489585596 + }, + "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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..c03e6b1 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.762, + "robotLength": 0.762, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 45.35924, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file From a4b765d93f2bf2825a34cf59eceb6a530e1c5602 Mon Sep 17 00:00:00 2001 From: Red Date: Tue, 14 Jan 2025 06:32:12 +0000 Subject: [PATCH 03/79] Auto Selector Setup --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a8a15e..426ee03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,20 +4,29 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.utility.IO; +import frc.robot.utility.Util; public class RobotContainer { public IO io = new IO(); - public RobotContainer() { - // SmartDashboard.putData("Autonomous", ); // TBD + private final SendableChooser auto_selector; + private Command current_auto; + public RobotContainer() { + auto_selector = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Autos",auto_selector); + SmartDashboard.putData("Run Test Auto", Util.Do(() -> {current_auto.schedule();})); + auto_selector.onChange((command) -> {current_auto = command;}); } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return auto_selector.getSelected(); } } From a544a13448e1a124052ba9f93293a2d50e3568aa Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 18 Jan 2025 13:42:30 -0500 Subject: [PATCH 04/79] asdf --- .../java/frc/robot/subsystems/Limelight.java | 47 + .../java/frc/robot/subsystems/Swerve.java | 4 + .../frc/robot/utility/LimelightHelpers.java | 1589 +++++++++++++++++ 3 files changed, 1640 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/Limelight.java create mode 100644 src/main/java/frc/robot/utility/LimelightHelpers.java diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 0000000..37afc1e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utility.IO; + +public class Limelight extends SubsystemBase { + double area; + double x; + double y; + double robotyaw; + + public IO io; + + /** Creates a new Limelight. */ + public Limelight(IO io) { + this.io = io; + + final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); + NetworkTableEntry tx = table.getEntry("tx"); + NetworkTableEntry ty = table.getEntry("ty"); + NetworkTableEntry ta = table.getEntry("ta"); + + x = tx.getDouble(0.0); + y= ty.getDouble(0.0); + area= ta.getDouble(0.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("LimelightX", x); + SmartDashboard.putNumber("LimelightY", y); //MIGHT NEED TO CHANGE THIS + SmartDashboard.putNumber("LimelightArea", area); + + // field localization + robotyaw = io.chassis.getYaw(); + frc.robot.utility.LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0285a55..26bba53 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -147,6 +147,10 @@ public SwerveModulePosition[] modulePositions() { return pos; } + public double getYaw(){ + return pigeon2.getYaw().getValueAsDouble(); + } + public SwerveModuleState[] moduleStates(KrakenSwerveModule[] modules) { SwerveModuleState[] state = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) diff --git a/src/main/java/frc/robot/utility/LimelightHelpers.java b/src/main/java/frc/robot/utility/LimelightHelpers.java new file mode 100644 index 0000000..0821968 --- /dev/null +++ b/src/main/java/frc/robot/utility/LimelightHelpers.java @@ -0,0 +1,1589 @@ +//LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER) + +package frc.robot; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.LimelightHelpers.LimelightResults; +import frc.robot.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file From 3f641ac5b1570b1ea7798117484e0e430c461300 Mon Sep 17 00:00:00 2001 From: 312Evan Date: Sat, 18 Jan 2025 14:56:20 -0500 Subject: [PATCH 05/79] poseEstimation --- src/main/java/frc/robot/subsystems/Swerve.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 26bba53..6a8a6de 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -5,6 +5,8 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -46,6 +48,8 @@ public class Swerve extends SubsystemBase { public final Pigeon2 pigeon2 = new Pigeon2(DriveConstants.PIGEON_ID); + public SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), pose()); + StructArrayPublisher current_states = NetworkTableInstance.getDefault().getTable("Debug") .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish(); StructArrayPublisher target_states = NetworkTableInstance.getDefault().getTable("Debug") @@ -248,6 +252,8 @@ public void periodic() { SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + + poseEstimator.update(rotation(), modulePositions()); } public static final class DriveConstants { From 7696fc2a93cf6e341310b32c377aea318d671f10 Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 18 Jan 2025 14:59:08 -0500 Subject: [PATCH 06/79] finished pose estimation for limelight --- .../java/frc/robot/subsystems/Limelight.java | 18 ++++++++++++++++-- .../frc/robot/utility/LimelightHelpers.java | 4 +--- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 37afc1e..3d49954 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -4,11 +4,15 @@ package frc.robot.subsystems; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utility.LimelightHelpers; import frc.robot.utility.IO; public class Limelight extends SubsystemBase { @@ -16,6 +20,7 @@ public class Limelight extends SubsystemBase { double x; double y; double robotyaw; + double latency; public IO io; @@ -27,10 +32,12 @@ public Limelight(IO io) { NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); NetworkTableEntry ta = table.getEntry("ta"); + NetworkTableEntry tl = table.getEntry("tl"); x = tx.getDouble(0.0); y= ty.getDouble(0.0); area= ta.getDouble(0.0); + latency = tl.getDouble(0.0); } @Override @@ -38,10 +45,17 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("LimelightX", x); SmartDashboard.putNumber("LimelightY", y); //MIGHT NEED TO CHANGE THIS - SmartDashboard.putNumber("LimelightArea", area); + SmartDashboard.putNumber("LimelightArea", area); + SmartDashboard.putNumber("Limelight Latency", latency); // field localization robotyaw = io.chassis.getYaw(); - frc.robot.utility.LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); + LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); + + //pose estimation + LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + + io.chassis.poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); + io.chassis.poseEstimator.addVisionMeasurement(limelightMeasurement.pose, limelightMeasurement.timestampSeconds); } } diff --git a/src/main/java/frc/robot/utility/LimelightHelpers.java b/src/main/java/frc/robot/utility/LimelightHelpers.java index 0821968..4732992 100644 --- a/src/main/java/frc/robot/utility/LimelightHelpers.java +++ b/src/main/java/frc/robot/utility/LimelightHelpers.java @@ -1,14 +1,12 @@ //LimelightHelpers v1.10 (REQUIRES LLOS 2024.9.1 OR LATER) -package frc.robot; +package frc.robot.utility; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; -import frc.robot.LimelightHelpers.LimelightResults; -import frc.robot.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; From ae0ee37a05f2059a3962e6a66bbc8bec5bec6227 Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 18 Jan 2025 15:05:31 -0500 Subject: [PATCH 07/79] asdf --- src/main/java/frc/robot/subsystems/Limelight.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 3d49954..1096b0d 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -57,5 +57,16 @@ public void periodic() { io.chassis.poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); io.chassis.poseEstimator.addVisionMeasurement(limelightMeasurement.pose, limelightMeasurement.timestampSeconds); + + /* LIMELIGHT OFFSET NEED TODO + LimelightHelpers.setCameraPose_RobotSpace("", + 0.5, // Forward offset (meters) + 0.0, // Side offset (meters) + 0.5, // Height offset (meters) + 0.0, // Roll (degrees) + 30.0, // Pitch (degrees) + 0.0 // Yaw (degrees) + ); + */ } } From 8122b3807963f83b6159a385f7b527f0d344e3f1 Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 25 Jan 2025 10:24:17 -0500 Subject: [PATCH 08/79] Removed recursive dependency on IO and left offset code commented out for when we know the offset --- .../java/frc/robot/subsystems/Limelight.java | 21 ----------------- src/main/java/frc/robot/utility/IO.java | 23 +++++++++++++++++++ 2 files changed, 23 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 1096b0d..2bf82c5 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -47,26 +47,5 @@ public void periodic() { SmartDashboard.putNumber("LimelightY", y); //MIGHT NEED TO CHANGE THIS SmartDashboard.putNumber("LimelightArea", area); SmartDashboard.putNumber("Limelight Latency", latency); - - // field localization - robotyaw = io.chassis.getYaw(); - LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); - - //pose estimation - LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); - - io.chassis.poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); - io.chassis.poseEstimator.addVisionMeasurement(limelightMeasurement.pose, limelightMeasurement.timestampSeconds); - - /* LIMELIGHT OFFSET NEED TODO - LimelightHelpers.setCameraPose_RobotSpace("", - 0.5, // Forward offset (meters) - 0.0, // Side offset (meters) - 0.5, // Height offset (meters) - 0.0, // Roll (degrees) - 30.0, // Pitch (degrees) - 0.0 // Yaw (degrees) - ); - */ } } diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index f8185cd..c0bdb7d 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -1,5 +1,6 @@ package frc.robot.utility; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; @@ -20,6 +21,8 @@ public class IO extends SubsystemBase { public CommandScheduler scheduler = CommandScheduler.getInstance(); + double robotyaw; + public IO(SendableChooser bindings) { bindings.setDefaultOption("Single Player", this::config1Player); bindings.addOption("Two Player", this::config2Player); @@ -63,5 +66,25 @@ public void configTesting() { @Override public void periodic() { + // field localization + robotyaw = chassis.getYaw(); + LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); + + //pose estimation + LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + + chassis.poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); + chassis.poseEstimator.addVisionMeasurement(limelightMeasurement.pose, limelightMeasurement.timestampSeconds); + + /* LIMELIGHT OFFSET NEED TODO + LimelightHelpers.setCameraPose_RobotSpace("", + 0.5, // Forward offset (meters) + 0.0, // Side offset (meters) + 0.5, // Height offset (meters) + 0.0, // Roll (degrees) + 30.0, // Pitch (degrees) + 0.0 // Yaw (degrees) + ); + */ } } \ No newline at end of file From 948699c8247d03d19c0f5cacf4a17ccf2754dbdc Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 25 Jan 2025 16:25:34 -0500 Subject: [PATCH 09/79] asdf --- src/main/java/frc/robot/subsystems/Limelight.java | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 2bf82c5..bcd0545 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -4,16 +4,11 @@ package frc.robot.subsystems; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.PoseEstimator; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.utility.LimelightHelpers; -import frc.robot.utility.IO; public class Limelight extends SubsystemBase { double area; @@ -21,13 +16,9 @@ public class Limelight extends SubsystemBase { double y; double robotyaw; double latency; - - public IO io; /** Creates a new Limelight. */ - public Limelight(IO io) { - this.io = io; - + public Limelight() { final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); From c103200b7797af63fb266c58e6d06f79230f1f60 Mon Sep 17 00:00:00 2001 From: Red Date: Sun, 2 Feb 2025 06:01:41 +0000 Subject: [PATCH 10/79] Merged --- .vscode/launch.json | 12 +- src/main/java/frc/robot/RobotContainer.java | 20 ++- .../java/frc/robot/commands/DefaultDrive.java | 35 +++--- .../frc/robot/modules/KrakenSwerveModule.java | 38 ++---- src/main/java/frc/robot/subsystems/Hang.java | 34 ++++++ .../java/frc/robot/subsystems/Swerve.java | 114 ++++++------------ .../robot/utility/AutomatedController.java | 99 ++++++++------- src/main/java/frc/robot/utility/IO.java | 12 +- src/main/java/frc/robot/utility/Rumble.java | 11 +- .../frc/robot/utility/RumblePatterns.java | 13 +- .../frc/robot/utility/SwerveConstants.java | 20 +++ 11 files changed, 219 insertions(+), 189 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Hang.java create mode 100644 src/main/java/frc/robot/utility/SwerveConstants.java diff --git a/.vscode/launch.json b/.vscode/launch.json index 5b804e8..67b538f 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "FRC-Reefscape-Robot" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 426ee03..c09ac36 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,21 +9,35 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.DefaultDrive; +import frc.robot.utility.AutomatedController; import frc.robot.utility.IO; import frc.robot.utility.Util; public class RobotContainer { - + public IO io = new IO(); + public final AutomatedController main; + public final AutomatedController backup; + private final SendableChooser auto_selector; private Command current_auto; public RobotContainer() { + main = new AutomatedController(0, io); + backup = new AutomatedController(1, io); + auto_selector = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Autos",auto_selector); - SmartDashboard.putData("Run Test Auto", Util.Do(() -> {current_auto.schedule();})); auto_selector.onChange((command) -> {current_auto = command;}); + + SmartDashboard.putData("Autos",auto_selector); + SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); + + SmartDashboard.putData("Main-Controller Mode", main.selector); + SmartDashboard.putData("Backup-Controller Mode", main.selector); + io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + // SmartDashboard.putData("Autonomous", ); // TBD } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index 4c4755d..beb2fb0 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -3,15 +3,15 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.utility.Util; -import frc.robot.subsystems.Swerve; import frc.robot.utility.IO; +import frc.robot.utility.SwerveConstants; import java.util.function.DoubleSupplier; public class DefaultDrive extends Command { private final IO io; + private CommandXboxController controller; private final DoubleSupplier x_supplier; private final DoubleSupplier y_supplier; private final DoubleSupplier rotation_supplier; @@ -21,9 +21,10 @@ public DefaultDrive(IO io, ChassisSpeeds chassisSpeeds) { } public DefaultDrive(IO io, CommandXboxController controller) { - this(io, () -> modifyAxis(controller.getLeftY()) * Swerve.Drive.MAX_VELOCITY, - () -> modifyAxis(controller.getLeftX()) * Swerve.Drive.MAX_VELOCITY, - () -> modifyAxis(controller.getRightX()) * Swerve.Drive.MAX_VELOCITY); + this(io, () -> modifyAxis(controller.getLeftY()) * SwerveConstants.MAX_VELOCITY, + () -> modifyAxis(controller.getLeftX()) * SwerveConstants.MAX_VELOCITY, + () -> modifyAxis(controller.getRightX()) * SwerveConstants.MAX_VELOCITY); + this.controller = controller; } public DefaultDrive(IO io, @@ -41,29 +42,21 @@ public DefaultDrive(IO io, @Override public void execute() { - double down_scale = 1 - modifyAxis(io.main.controller.getLeftTriggerAxis()); - double up_scale = modifyAxis(io.main.controller.getRightTriggerAxis()); + double down_scale = 1.25 - modifyAxis(controller.getLeftTriggerAxis()); + double up_scale = (SwerveConstants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis()); - // double scale = (double) DebugTable.get("Translation Scale", 1.0) * down_scale + up_scale; - // double rot_scale = (double) DebugTable.get("Rotation Scale", 0.65) * down_scale + up_scale; //0.65 for Shaan. 0.75 for Tristan. + double scale = 0.8 * down_scale + up_scale; + double rot_scale = .48 * down_scale + up_scale; //0.48 for Shaan. 0.6 for Tristan. - double scale = 1.0 * down_scale + up_scale; - double rot_scale = (double) Util.get("Rotation Scale", 0.65) * down_scale + up_scale; //0.65 for Shaan. 0.75 for Tristan. - - // double xSpeed = x_supplier.getAsDouble() * scale; - // double ySpeed = y_supplier.getAsDouble() * scale; - // double rotationSpeed = rotation_supplier.getAsDouble() * rot_scale; - - double xSpeed = Math.pow(x_supplier.getAsDouble() * scale, 3.0); - double ySpeed = Math.pow(y_supplier.getAsDouble() * scale, 3.0); - double rotationSpeed = Math.pow(rotation_supplier.getAsDouble() * rot_scale, 3.0); + double xSpeed = x_supplier.getAsDouble() * scale; + double ySpeed = y_supplier.getAsDouble() * scale; + double rotationSpeed = rotation_supplier.getAsDouble() * down_scale * rot_scale; ChassisSpeeds output = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - if (io.chassis.field_oritented){ + if (io.chassis.field_oritented) output = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed, io.chassis.rotation()); - } io.chassis.drive(output); } diff --git a/src/main/java/frc/robot/modules/KrakenSwerveModule.java b/src/main/java/frc/robot/modules/KrakenSwerveModule.java index aad7ab1..12c5f90 100644 --- a/src/main/java/frc/robot/modules/KrakenSwerveModule.java +++ b/src/main/java/frc/robot/modules/KrakenSwerveModule.java @@ -5,7 +5,6 @@ import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.SparkMax; @@ -32,8 +31,6 @@ public class KrakenSwerveModule { public static final double DRIVE_REDUCTION = (15.0 / 32.0) * (10.0 / 60.0); public static final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0); public static final double DRIVE_CONVERSION_FACTOR = Math.PI * WHEEL_DIAMETER * DRIVE_REDUCTION; - public double positional_error = 0; - public double in_volts = 0; public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerCANID) { driveMotor = new TalonFX(driveID, "rio"); @@ -44,33 +41,36 @@ public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerConfig .smartCurrentLimit(20) - .idleMode(IdleMode.kBrake) - .inverted(true); + .idleMode(IdleMode.kCoast) + .inverted(false); steerConfig.encoder - .positionConversionFactor(Math.PI * STEER_REDUCTION) + .positionConversionFactor(2 * Math.PI * STEER_REDUCTION) .velocityConversionFactor(Math.PI * STEER_REDUCTION / 60); steerConfig.closedLoop .positionWrappingEnabled(true) .positionWrappingMaxInput(PI2) - .feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(0.2, 0.0, 0.0); + // .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(0.2, 0.0, 0.0); + steerConfig.signals.primaryEncoderPositionAlwaysOn(false); steerConfig.signals.primaryEncoderPositionPeriodMs(20); CanandmagSettings settings = new CanandmagSettings(); settings.setInvertDirection(true); steerEncoder.clearStickyFaults(); - steerEncoder.resetFactoryDefaults(false); steerEncoder.setSettings(settings); TalonFXConfiguration config = new TalonFXConfiguration(); config.CurrentLimits.StatorCurrentLimit = 80; config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 20; config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -81,10 +81,9 @@ public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int driveMotor.getConfigurator().apply(config); tab.addDouble("Absolute Angle", () -> Math.toDegrees(angle())); - tab.addDouble("Current Angle", () -> Math.toDegrees(steerMotor.getEncoder().getPosition())); + // tab.addDouble("Current Angle", () -> Math.toDegrees(steerMotor.getEncoder().getPosition())); + tab.addDouble("Angle Difference", () -> Math.toDegrees(angle() - steerMotor.getEncoder().getPosition())); tab.addDouble("Target Angle", () -> Math.toDegrees(desiredAngle)); - tab.addDouble("Drive Volts", () -> in_volts); - tab.addDouble("Positional Error", () -> positional_error); tab.addBoolean("Active", steerEncoder::isConnected); } @@ -92,7 +91,7 @@ public void resetDrivePosition() { driveMotor.setPosition(0.0); } - public void resetSteerPosition() { + public void syncSteerEncoders() { steerMotor.getEncoder().setPosition(angle()); } @@ -118,21 +117,8 @@ public void stop() { } public void set(double driveVolts, double targetAngle) { - resetSteerPosition(); - - targetAngle %= PI2; - targetAngle += (targetAngle < 0.0) ? PI2 : 0.0; - - desiredAngle = targetAngle; - - double diff = targetAngle - angle(); - - if (diff > (Math.PI / 2.0) || diff < -(Math.PI / 2.0)) { - targetAngle = (targetAngle + Math.PI) % PI2; - driveVolts *= -1.0; - } + syncSteerEncoders(); - in_volts = driveVolts; driveMotor.set(driveVolts); steerMotor.getClosedLoopController().setReference(targetAngle, ControlType.kPosition); } diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java new file mode 100644 index 0000000..ea06033 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Hang extends SubsystemBase { + public TalonFX hang = new TalonFX(10, "rio"); + + public static final double HANG_MAX_ANGLE = 0; + + public Hang() { + hang.setNeutralMode(NeutralModeValue.Brake); + } + + public void hangSpeed(double speed) { + hang.set(speed); + } + + public void hangVoltage(double volts) { + hang.setVoltage(volts); + } + + public void stopHang() { + hang.stopMotor(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Hang Pos", hang.getPosition().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6b17fb7..6502ec1 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,16 +1,11 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.Pigeon2; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; 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.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; @@ -19,31 +14,30 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.modules.KrakenSwerveModule; +import frc.robot.utility.SwerveConstants; import frc.robot.utility.Util; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; public class Swerve extends SubsystemBase { public boolean field_oritented = true; public boolean slow_mode = false; - - public RobotConfig config; - private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( - new Translation2d(Drive.TRACKWIDTH / 2.0, - Drive.WHEELBASE / 2.0), - new Translation2d(Drive.TRACKWIDTH / 2.0, - -Drive.WHEELBASE / 2.0), - new Translation2d(-Drive.TRACKWIDTH / 2.0, - Drive.WHEELBASE / 2.0), - new Translation2d(-Drive.TRACKWIDTH / 2.0, - -Drive.WHEELBASE / 2.0)); - - public final Pigeon2 pigeon2 = new Pigeon2(Drive.PIGEON_ID); + new Translation2d(SwerveConstants.TRACKWIDTH / 2.0, + SwerveConstants.WHEELBASE / 2.0), + new Translation2d(SwerveConstants.TRACKWIDTH / 2.0, + -SwerveConstants.WHEELBASE / 2.0), + new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0, + SwerveConstants.WHEELBASE / 2.0), + new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0, + -SwerveConstants.WHEELBASE / 2.0)); + + public final Pigeon2 pigeon2 = new Pigeon2(SwerveConstants.PIGEON_ID); + public final Timer syncTimer = new Timer(); StructArrayPublisher current_states = Util.table .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish(); @@ -51,7 +45,7 @@ public class Swerve extends SubsystemBase { .getStructArrayTopic("Target Module States", SwerveModuleState.struct).publish(); StructPublisher posePublisher = NetworkTableInstance.getDefault().getTable("Debug") .getStructTopic("Current pose", Pose2d.struct).publish(); - + final SwerveDriveOdometry odometry; final KrakenSwerveModule[] modules = new KrakenSwerveModule[4]; ChassisSpeeds speeds = new ChassisSpeeds(); @@ -59,45 +53,21 @@ public class Swerve extends SubsystemBase { public boolean active = true; public Swerve() { + new SwerveConstants(); ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { modules[i] = new KrakenSwerveModule( - tab.getLayout(Drive.LAYOUT_TITLE[i], BuiltInLayouts.kList) + tab.getLayout(SwerveConstants.LAYOUT_TITLE[i], BuiltInLayouts.kList) .withSize(2, 4) .withPosition(i * 2, 0), - Drive.CHASSIS_ID[i], - Drive.CHASSIS_ID[i], - Drive.CHASSIS_ID[i]); + SwerveConstants.CHASSIS_ID[i], + SwerveConstants.CHASSIS_ID[i], + SwerveConstants.ENCODER_ID[i]); } - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - AutoBuilder.configure( - this::pose, - this::resetOdometry, - this::getSpeeds, - (speeds, feedforwards) -> drive(speeds), - new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, 0.0, 0.0), // Translation PID constants - new PIDConstants(AutoConstants.kPThetaController, 0, 0, 0.01) // Rotation PID constants - ), - config, - () -> { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; - }, - this); - odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), new Pose2d(0, 0, new Rotation2d())); - + syncTimer.start(); } public void zeroGyro() { @@ -182,6 +152,10 @@ public void setOdometry(Pose2d pose) { odometry.resetPosition(rotation(), modulePositions(), pose); } + public void zeroHeading(){ + pigeon2.setYaw(0); + } + public void resetPosition() { for (KrakenSwerveModule mod : modules) mod.resetDrivePosition(); @@ -189,7 +163,7 @@ public void resetPosition() { public void syncEncoders() { for (KrakenSwerveModule mod : modules) - mod.resetSteerPosition(); + mod.syncSteerEncoders(); } public void resetAbsolute() { @@ -202,10 +176,12 @@ public void resetSteerPositions() { mod.set(0, 0); } - public void setModuleStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, Drive.MAX_VELOCITY); + public void setModuleStates(SwerveModuleState[] states) { + SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_VELOCITY); for (int i = 0; i < modules.length; i++) { - modules[i].set((states[i].speedMetersPerSecond / Drive.MAX_VELOCITY), states[i].angle.getRadians()); + states[i].optimize(new Rotation2d(modules[i].angle())); + // states[i].cosineScale(new Rotation2d(modules[i].angle())); // Cosine Compensation + modules[i].set((states[i].speedMetersPerSecond / SwerveConstants.MAX_VELOCITY) * .8 , states[i].angle.getRadians()); } } @@ -213,6 +189,10 @@ public ChassisSpeeds getSpeeds() { return speeds; } + public double getRoll() { + return pigeon2.getRoll().getValueAsDouble(); + } + public void enable() { active = true; } @@ -228,8 +208,11 @@ public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) setModuleStates(states); + current_states.set(moduleStates(modules)); target_states.set(states); + + Pose2d pose = odometry.update(rotation(), modulePositions()); posePublisher.set(pose); @@ -243,27 +226,4 @@ public void periodic() { SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } - - public static final class Drive { - public static final double TRACKWIDTH = Units.inchesToMeters(30); - public static final double WHEELBASE = Units.inchesToMeters(30); - - public static final double MAX_VOLTAGE = 16; - public static final double MAX_VELOCITY = 20; - - public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; - - public static final int[] CHASSIS_ID = { 3, 2, 4, 5 }; // FL, FR, BL, BR - - public static final int PIGEON_ID = 6; - - public static final int TURBO = 1; - public static final int SLOW = 2; - } - - public static final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 0.25; - public static final double kPXController = 24.0; - public static final double kPThetaController = 20.0; - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 2570bd5..5ba5677 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,12 +5,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.*; +import frc.robot.commands.GrabAlgae; +import frc.robot.commands.ReleaseAlgae; public class AutomatedController { public final CommandXboxController controller; public final SendableChooser selector = new SendableChooser(); - public boolean manual = false; + public boolean manual = true; IO io; @@ -23,11 +24,6 @@ public AutomatedController(int port, IO io){ selector.onChange((x) -> {x.run();}); // TODO: See if this allows us to have the selector always be up-to-date and add the same for auton testing controller = new CommandXboxController(port); - controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); - controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry)); - controller.back().onTrue(Util.Do(io.elevator::rest)); - controller.start().onTrue(Util.Do(io.elevator::zero)); - controller.start().onTrue(Util.Do( () -> manual = !manual)).debounce(3); configure(); } public BooleanSupplier automated(){ @@ -38,60 +34,77 @@ public BooleanSupplier manual(){ return () -> { return manual; }; } + public BooleanSupplier toggleMode(){ + return () -> { + manual = !manual; + return manual; + }; + } + public void configure(){ - + controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); + controller.leftStick().onTrue(new InstantCommand(io.chassis::zeroHeading)); + + // controller.back().onTrue(Util.Do(io.elevator::rest)); + // controller.start().onTrue(Util.Do(io.elevator::zero)); + + controller.start().and(controller.getHID()::getBackButtonPressed).debounce(3).onTrue(Util.Do(this::toggleMode)); + controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + // AUTOMATED // Based on the nearest element and our field orientation // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - controller.y().and( automated() ).onTrue(Util.Do(io.elevator::L4)); - controller.b().and( automated() ).onTrue(Util.Do(io.elevator::L3)); - controller.a().and( automated() ).onTrue(Util.Do(io.elevator::L2)); - controller.x().and( automated() ) - .onTrue(Util.Do( - new ReleaseAlgae(io, false), - new GrabAlgae(io), - io.intake::grabbed)); + // controller.y().and( automated() ).onTrue(Util.Do(io.elevator::L4)); + // controller.b().and( automated() ).onTrue(Util.Do(io.elevator::L3)); + // controller.a().and( automated() ).onTrue(Util.Do(io.elevator::L2)); + // controller.x().and( automated() ) + // .onTrue(Util.Do( + // new ReleaseAlgae(io, false), + // new GrabAlgae(io), + // io.intake::grabbed)); - controller.povUp().and( automated() ) - .onTrue(Util.Do(() -> io.elevator.speed(0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); + // controller.povUp().and( automated() ) + // .onTrue(Util.Do(() -> io.elevator.speed(0.25))) + // .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - controller.povDown().and( automated() ) - .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); + // controller.povDown().and( automated() ) + // .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) + // .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - controller.povLeft().and( automated() ) - .onTrue(Util.Do(() -> io.intake.speed(-0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); + // controller.povLeft().and( automated() ) + // .onTrue(Util.Do(() -> io.intake.speed(-0.5))) + // .onFalse(Util.Do(() -> io.intake.speed(0.0))); - controller.povRight().and( automated() ) - .onTrue(Util.Do(() -> io.intake.speed(0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); + // controller.povRight().and( automated() ) + // .onTrue(Util.Do(() -> io.intake.speed(0.5))) + // .onFalse(Util.Do(() -> io.intake.speed(0.0))); - // MANUAL + // // MANUAL - controller.rightTrigger().and( manual() ) - .onTrue(Util.Do(() -> io.elevator.speed(0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); + // controller.rightTrigger().and( manual() ) + // .onTrue(Util.Do(() -> io.elevator.speed(0.25))) + // .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - controller.leftTrigger().and( manual() ) - .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); + // controller.leftTrigger().and( manual() ) + // .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) + // .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - controller.leftBumper().and( manual() ) - .onTrue(Util.Do(() -> io.intake.speed(-0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); + // controller.leftBumper().and( manual() ) + // .onTrue(Util.Do(() -> io.intake.speed(-0.5))) + // .onFalse(Util.Do(() -> io.intake.speed(0.0))); - controller.rightBumper().and( manual() ) - .onTrue(Util.Do(() -> io.intake.speed(0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); + // controller.rightBumper().and( manual() ) + // .onTrue(Util.Do(() -> io.intake.speed(0.5))) + // .onFalse(Util.Do(() -> io.intake.speed(0.0))); controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::resetAbsolute)); // Add the Rumble effect - } + controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::resetAbsolute)); // Add the Rumble effect + + // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::resetAbsolute)); // Add the Rumble effect + } } diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index cdd2ddb..29f1c26 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -1,31 +1,21 @@ package frc.robot.utility; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; -import frc.robot.commands.*; import frc.robot.subsystems.*; public class IO extends SubsystemBase { - public final AutomatedController main = new AutomatedController(0, this); - public final AutomatedController backup = new AutomatedController(1, this); - public final Swerve chassis = new Swerve(); public final AlgaeIntake intake = new AlgaeIntake(); public final Elevator elevator = new Elevator(); + public final Hang hang = new Hang(); public CommandScheduler scheduler = CommandScheduler.getInstance(); public IO() { - SmartDashboard.putData("Main-Controller Mode", main.selector); - SmartDashboard.putData("Backup-Controller Mode", main.selector); - DriverStation.silenceJoystickConnectionWarning(true); - chassis.setDefaultCommand(new DefaultDrive(this, main.controller)); } - // StructPublisher estimated_pose = Util.table.getStructTopic("Estimated Pose", Pose2d.struct).publish(); - @Override public void periodic() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/utility/Rumble.java b/src/main/java/frc/robot/utility/Rumble.java index 1fa97ac..cc9037e 100644 --- a/src/main/java/frc/robot/utility/Rumble.java +++ b/src/main/java/frc/robot/utility/Rumble.java @@ -17,23 +17,26 @@ public class Rumble extends Command { boolean condition = false; Runnable action; GenericHID controller; -int pattern; +RumblePatterns pattern = new RumblePatterns(); +int type; - public Rumble(int pattern, double duration, GenericHID controller, Runnable action) { + public Rumble(int type, double duration, GenericHID controller, Runnable action) { time.start(); - this.pattern = pattern; + this.type = type; this.controller = controller; } // Called when the command is initially scheduled. @Override public void initialize() { + // pattern.timer.start(); + action.run(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RumblePatterns.Run(pattern, controller); + pattern.Run(type, controller); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/utility/RumblePatterns.java b/src/main/java/frc/robot/utility/RumblePatterns.java index 99f08b6..4994638 100644 --- a/src/main/java/frc/robot/utility/RumblePatterns.java +++ b/src/main/java/frc/robot/utility/RumblePatterns.java @@ -1,14 +1,25 @@ package frc.robot.utility; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.GenericHID.RumbleType; public class RumblePatterns { - public static void Run(int pattern, GenericHID controller){ + + public Timer timer = new Timer(); + + public void Run(int pattern, GenericHID controller){ switch(pattern){ default: controller.setRumble(RumbleType.kBothRumble, .5); break; + + case 1: + if (timer.get()/5.0 % 2.0 == 0.0) + controller.setRumble(RumbleType.kBothRumble, .5); + else + controller.setRumble(RumbleType.kBothRumble, 0.0); + break; } } } diff --git a/src/main/java/frc/robot/utility/SwerveConstants.java b/src/main/java/frc/robot/utility/SwerveConstants.java new file mode 100644 index 0000000..fafa34e --- /dev/null +++ b/src/main/java/frc/robot/utility/SwerveConstants.java @@ -0,0 +1,20 @@ +package frc.robot.utility; + +public class SwerveConstants { + boolean compChassis = false; + + public static double TRACKWIDTH = 19.5; // 30.0 for MKi + public static double WHEELBASE = 21.5; // 30.0 for MKi + + public static double GEAR_RATIO; + + public static final double MAX_VELOCITY = 5.4; + + public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; + + public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR + public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR + + public static final int PIGEON_ID = 6; + +} \ No newline at end of file From 1728c6aee8b210d8c1af94eb018fee6281ef98f8 Mon Sep 17 00:00:00 2001 From: 312Evan Date: Sun, 2 Feb 2025 13:41:46 -0500 Subject: [PATCH 11/79] dog log setup --- Test/.gitattributes | 12 + Test/.gitignore | 5 + Test/app/build.gradle | 43 +++ Test/app/src/main/java/test/App.java | 14 + Test/app/src/test/java/test/AppTest.java | 14 + Test/gradle.properties | 5 + Test/gradle/libs.versions.toml | 10 + Test/gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43583 bytes Test/gradle/wrapper/gradle-wrapper.properties | 7 + Test/gradlew | 252 ++++++++++++++++++ Test/gradlew.bat | 94 +++++++ Test/settings.gradle | 14 + src/main/java/frc/robot/RobotContainer.java | 3 + .../frc/robot/subsystems/AlgaeIntake.java | 2 + .../java/frc/robot/subsystems/Elevator.java | 9 + .../java/frc/robot/subsystems/Swerve.java | 13 + vendordeps/DogLog.json | 20 ++ 17 files changed, 517 insertions(+) create mode 100644 Test/.gitattributes create mode 100644 Test/.gitignore create mode 100644 Test/app/build.gradle create mode 100644 Test/app/src/main/java/test/App.java create mode 100644 Test/app/src/test/java/test/AppTest.java create mode 100644 Test/gradle.properties create mode 100644 Test/gradle/libs.versions.toml create mode 100644 Test/gradle/wrapper/gradle-wrapper.jar create mode 100644 Test/gradle/wrapper/gradle-wrapper.properties create mode 100755 Test/gradlew create mode 100644 Test/gradlew.bat create mode 100644 Test/settings.gradle create mode 100644 vendordeps/DogLog.json diff --git a/Test/.gitattributes b/Test/.gitattributes new file mode 100644 index 0000000..f91f646 --- /dev/null +++ b/Test/.gitattributes @@ -0,0 +1,12 @@ +# +# https://help.github.com/articles/dealing-with-line-endings/ +# +# Linux start script should use lf +/gradlew text eol=lf + +# These are Windows script files and should use crlf +*.bat text eol=crlf + +# Binary files should be left untouched +*.jar binary + diff --git a/Test/.gitignore b/Test/.gitignore new file mode 100644 index 0000000..1b6985c --- /dev/null +++ b/Test/.gitignore @@ -0,0 +1,5 @@ +# Ignore Gradle project-specific cache directory +.gradle + +# Ignore Gradle build output directory +build diff --git a/Test/app/build.gradle b/Test/app/build.gradle new file mode 100644 index 0000000..d4f8fe4 --- /dev/null +++ b/Test/app/build.gradle @@ -0,0 +1,43 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * This generated file contains a sample Java application project to get you started. + * For more details on building Java & JVM projects, please refer to https://docs.gradle.org/8.11/userguide/building_java_projects.html in the Gradle documentation. + */ + +plugins { + // Apply the application plugin to add support for building a CLI application in Java. + id 'application' +} + +repositories { + // Use Maven Central for resolving dependencies. + mavenCentral() +} + +dependencies { + // Use JUnit Jupiter for testing. + testImplementation libs.junit.jupiter + + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + // This dependency is used by the application. + implementation libs.guava +} + +// Apply a specific Java toolchain to ease working on different environments. +java { + toolchain { + languageVersion = JavaLanguageVersion.of(21) + } +} + +application { + // Define the main class for the application. + mainClass = 'test.App' +} + +tasks.named('test') { + // Use JUnit Platform for unit tests. + useJUnitPlatform() +} diff --git a/Test/app/src/main/java/test/App.java b/Test/app/src/main/java/test/App.java new file mode 100644 index 0000000..507fe91 --- /dev/null +++ b/Test/app/src/main/java/test/App.java @@ -0,0 +1,14 @@ +/* + * This source file was generated by the Gradle 'init' task + */ +package test; + +public class App { + public String getGreeting() { + return "Hello World!"; + } + + public static void main(String[] args) { + System.out.println(new App().getGreeting()); + } +} diff --git a/Test/app/src/test/java/test/AppTest.java b/Test/app/src/test/java/test/AppTest.java new file mode 100644 index 0000000..7c490f3 --- /dev/null +++ b/Test/app/src/test/java/test/AppTest.java @@ -0,0 +1,14 @@ +/* + * This source file was generated by the Gradle 'init' task + */ +package test; + +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class AppTest { + @Test void appHasAGreeting() { + App classUnderTest = new App(); + assertNotNull(classUnderTest.getGreeting(), "app should have a greeting"); + } +} diff --git a/Test/gradle.properties b/Test/gradle.properties new file mode 100644 index 0000000..377538c --- /dev/null +++ b/Test/gradle.properties @@ -0,0 +1,5 @@ +# This file was generated by the Gradle 'init' task. +# https://docs.gradle.org/current/userguide/build_environment.html#sec:gradle_configuration_properties + +org.gradle.configuration-cache=true + diff --git a/Test/gradle/libs.versions.toml b/Test/gradle/libs.versions.toml new file mode 100644 index 0000000..239e149 --- /dev/null +++ b/Test/gradle/libs.versions.toml @@ -0,0 +1,10 @@ +# This file was generated by the Gradle 'init' task. +# https://docs.gradle.org/current/userguide/platforms.html#sub::toml-dependencies-format + +[versions] +guava = "33.2.1-jre" +junit-jupiter = "5.10.3" + +[libraries] +guava = { module = "com.google.guava:guava", version.ref = "guava" } +junit-jupiter = { module = "org.junit.jupiter:junit-jupiter", version.ref = "junit-jupiter" } diff --git a/Test/gradle/wrapper/gradle-wrapper.jar b/Test/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..a4b76b9530d66f5e68d973ea569d8e19de379189 GIT binary patch literal 43583 zcma&N1CXTcmMvW9vTb(Rwr$&4wr$(C?dmSu>@vG-+vuvg^_??!{yS%8zW-#zn-LkA z5&1^$^{lnmUON?}LBF8_K|(?T0Ra(xUH{($5eN!MR#ZihR#HxkUPe+_R8Cn`RRs(P z_^*#_XlXmGv7!4;*Y%p4nw?{bNp@UZHv1?Um8r6)Fei3p@ClJn0ECfg1hkeuUU@Or zDaPa;U3fE=3L}DooL;8f;P0ipPt0Z~9P0)lbStMS)ag54=uL9ia-Lm3nh|@(Y?B`; zx_#arJIpXH!U{fbCbI^17}6Ri*H<>OLR%c|^mh8+)*h~K8Z!9)DPf zR2h?lbDZQ`p9P;&DQ4F0sur@TMa!Y}S8irn(%d-gi0*WxxCSk*A?3lGh=gcYN?FGl z7D=Js!i~0=u3rox^eO3i@$0=n{K1lPNU zwmfjRVmLOCRfe=seV&P*1Iq=^i`502keY8Uy-WNPwVNNtJFx?IwAyRPZo2Wo1+S(xF37LJZ~%i)kpFQ3Fw=mXfd@>%+)RpYQLnr}B~~zoof(JVm^^&f zxKV^+3D3$A1G;qh4gPVjhrC8e(VYUHv#dy^)(RoUFM?o%W-EHxufuWf(l*@-l+7vt z=l`qmR56K~F|v<^Pd*p~1_y^P0P^aPC##d8+HqX4IR1gu+7w#~TBFphJxF)T$2WEa zxa?H&6=Qe7d(#tha?_1uQys2KtHQ{)Qco)qwGjrdNL7thd^G5i8Os)CHqc>iOidS} z%nFEDdm=GXBw=yXe1W-ShHHFb?Cc70+$W~z_+}nAoHFYI1MV1wZegw*0y^tC*s%3h zhD3tN8b=Gv&rj}!SUM6|ajSPp*58KR7MPpI{oAJCtY~JECm)*m_x>AZEu>DFgUcby z1Qaw8lU4jZpQ_$;*7RME+gq1KySGG#Wql>aL~k9tLrSO()LWn*q&YxHEuzmwd1?aAtI zBJ>P=&$=l1efe1CDU;`Fd+_;&wI07?V0aAIgc(!{a z0Jg6Y=inXc3^n!U0Atk`iCFIQooHqcWhO(qrieUOW8X(x?(RD}iYDLMjSwffH2~tB z)oDgNBLB^AJBM1M^c5HdRx6fBfka`(LD-qrlh5jqH~);#nw|iyp)()xVYak3;Ybik z0j`(+69aK*B>)e_p%=wu8XC&9e{AO4c~O1U`5X9}?0mrd*m$_EUek{R?DNSh(=br# z#Q61gBzEpmy`$pA*6!87 zSDD+=@fTY7<4A?GLqpA?Pb2z$pbCc4B4zL{BeZ?F-8`s$?>*lXXtn*NC61>|*w7J* z$?!iB{6R-0=KFmyp1nnEmLsA-H0a6l+1uaH^g%c(p{iT&YFrbQ$&PRb8Up#X3@Zsk zD^^&LK~111%cqlP%!_gFNa^dTYT?rhkGl}5=fL{a`UViaXWI$k-UcHJwmaH1s=S$4 z%4)PdWJX;hh5UoK?6aWoyLxX&NhNRqKam7tcOkLh{%j3K^4Mgx1@i|Pi&}<^5>hs5 zm8?uOS>%)NzT(%PjVPGa?X%`N2TQCKbeH2l;cTnHiHppPSJ<7y-yEIiC!P*ikl&!B z%+?>VttCOQM@ShFguHVjxX^?mHX^hSaO_;pnyh^v9EumqSZTi+#f&_Vaija0Q-e*| z7ulQj6Fs*bbmsWp{`auM04gGwsYYdNNZcg|ph0OgD>7O}Asn7^Z=eI>`$2*v78;sj-}oMoEj&@)9+ycEOo92xSyY344^ z11Hb8^kdOvbf^GNAK++bYioknrpdN>+u8R?JxG=!2Kd9r=YWCOJYXYuM0cOq^FhEd zBg2puKy__7VT3-r*dG4c62Wgxi52EMCQ`bKgf*#*ou(D4-ZN$+mg&7$u!! z-^+Z%;-3IDwqZ|K=ah85OLwkO zKxNBh+4QHh)u9D?MFtpbl)us}9+V!D%w9jfAMYEb>%$A;u)rrI zuBudh;5PN}_6J_}l55P3l_)&RMlH{m!)ai-i$g)&*M`eN$XQMw{v^r@-125^RRCF0 z^2>|DxhQw(mtNEI2Kj(;KblC7x=JlK$@78`O~>V!`|1Lm-^JR$-5pUANAnb(5}B}JGjBsliK4& zk6y(;$e&h)lh2)L=bvZKbvh@>vLlreBdH8No2>$#%_Wp1U0N7Ank!6$dFSi#xzh|( zRi{Uw%-4W!{IXZ)fWx@XX6;&(m_F%c6~X8hx=BN1&q}*( zoaNjWabE{oUPb!Bt$eyd#$5j9rItB-h*5JiNi(v^e|XKAj*8(k<5-2$&ZBR5fF|JA z9&m4fbzNQnAU}r8ab>fFV%J0z5awe#UZ|bz?Ur)U9bCIKWEzi2%A+5CLqh?}K4JHi z4vtM;+uPsVz{Lfr;78W78gC;z*yTch~4YkLr&m-7%-xc ztw6Mh2d>_iO*$Rd8(-Cr1_V8EO1f*^@wRoSozS) zy1UoC@pruAaC8Z_7~_w4Q6n*&B0AjOmMWa;sIav&gu z|J5&|{=a@vR!~k-OjKEgPFCzcJ>#A1uL&7xTDn;{XBdeM}V=l3B8fE1--DHjSaxoSjNKEM9|U9#m2<3>n{Iuo`r3UZp;>GkT2YBNAh|b z^jTq-hJp(ebZh#Lk8hVBP%qXwv-@vbvoREX$TqRGTgEi$%_F9tZES@z8Bx}$#5eeG zk^UsLBH{bc2VBW)*EdS({yw=?qmevwi?BL6*=12k9zM5gJv1>y#ML4!)iiPzVaH9% zgSImetD@dam~e>{LvVh!phhzpW+iFvWpGT#CVE5TQ40n%F|p(sP5mXxna+Ev7PDwA zamaV4m*^~*xV+&p;W749xhb_X=$|LD;FHuB&JL5?*Y2-oIT(wYY2;73<^#46S~Gx| z^cez%V7x$81}UWqS13Gz80379Rj;6~WdiXWOSsdmzY39L;Hg3MH43o*y8ibNBBH`(av4|u;YPq%{R;IuYow<+GEsf@R?=@tT@!}?#>zIIn0CoyV!hq3mw zHj>OOjfJM3F{RG#6ujzo?y32m^tgSXf@v=J$ELdJ+=5j|=F-~hP$G&}tDZsZE?5rX ztGj`!S>)CFmdkccxM9eGIcGnS2AfK#gXwj%esuIBNJQP1WV~b~+D7PJTmWGTSDrR` zEAu4B8l>NPuhsk5a`rReSya2nfV1EK01+G!x8aBdTs3Io$u5!6n6KX%uv@DxAp3F@{4UYg4SWJtQ-W~0MDb|j-$lwVn znAm*Pl!?Ps&3wO=R115RWKb*JKoexo*)uhhHBncEDMSVa_PyA>k{Zm2(wMQ(5NM3# z)jkza|GoWEQo4^s*wE(gHz?Xsg4`}HUAcs42cM1-qq_=+=!Gk^y710j=66(cSWqUe zklbm8+zB_syQv5A2rj!Vbw8;|$@C!vfNmNV!yJIWDQ>{+2x zKjuFX`~~HKG~^6h5FntRpnnHt=D&rq0>IJ9#F0eM)Y-)GpRjiN7gkA8wvnG#K=q{q z9dBn8_~wm4J<3J_vl|9H{7q6u2A!cW{bp#r*-f{gOV^e=8S{nc1DxMHFwuM$;aVI^ zz6A*}m8N-&x8;aunp1w7_vtB*pa+OYBw=TMc6QK=mbA-|Cf* zvyh8D4LRJImooUaSb7t*fVfih<97Gf@VE0|z>NcBwBQze);Rh!k3K_sfunToZY;f2 z^HmC4KjHRVg+eKYj;PRN^|E0>Gj_zagfRbrki68I^#~6-HaHg3BUW%+clM1xQEdPYt_g<2K+z!$>*$9nQ>; zf9Bei{?zY^-e{q_*|W#2rJG`2fy@{%6u0i_VEWTq$*(ZN37|8lFFFt)nCG({r!q#9 z5VK_kkSJ3?zOH)OezMT{!YkCuSSn!K#-Rhl$uUM(bq*jY? zi1xbMVthJ`E>d>(f3)~fozjg^@eheMF6<)I`oeJYx4*+M&%c9VArn(OM-wp%M<-`x z7sLP1&3^%Nld9Dhm@$3f2}87!quhI@nwd@3~fZl_3LYW-B?Ia>ui`ELg z&Qfe!7m6ze=mZ`Ia9$z|ARSw|IdMpooY4YiPN8K z4B(ts3p%2i(Td=tgEHX z0UQ_>URBtG+-?0E;E7Ld^dyZ;jjw0}XZ(}-QzC6+NN=40oDb2^v!L1g9xRvE#@IBR zO!b-2N7wVfLV;mhEaXQ9XAU+>=XVA6f&T4Z-@AX!leJ8obP^P^wP0aICND?~w&NykJ#54x3_@r7IDMdRNy4Hh;h*!u(Ol(#0bJdwEo$5437-UBjQ+j=Ic>Q2z` zJNDf0yO6@mr6y1#n3)s(W|$iE_i8r@Gd@!DWDqZ7J&~gAm1#~maIGJ1sls^gxL9LLG_NhU!pTGty!TbhzQnu)I*S^54U6Yu%ZeCg`R>Q zhBv$n5j0v%O_j{QYWG!R9W?5_b&67KB$t}&e2LdMvd(PxN6Ir!H4>PNlerpBL>Zvyy!yw z-SOo8caEpDt(}|gKPBd$qND5#a5nju^O>V&;f890?yEOfkSG^HQVmEbM3Ugzu+UtH zC(INPDdraBN?P%kE;*Ae%Wto&sgw(crfZ#Qy(<4nk;S|hD3j{IQRI6Yq|f^basLY; z-HB&Je%Gg}Jt@={_C{L$!RM;$$|iD6vu#3w?v?*;&()uB|I-XqEKqZPS!reW9JkLewLb!70T7n`i!gNtb1%vN- zySZj{8-1>6E%H&=V}LM#xmt`J3XQoaD|@XygXjdZ1+P77-=;=eYpoEQ01B@L*a(uW zrZeZz?HJsw_4g0vhUgkg@VF8<-X$B8pOqCuWAl28uB|@r`19DTUQQsb^pfqB6QtiT z*`_UZ`fT}vtUY#%sq2{rchyfu*pCg;uec2$-$N_xgjZcoumE5vSI{+s@iLWoz^Mf; zuI8kDP{!XY6OP~q5}%1&L}CtfH^N<3o4L@J@zg1-mt{9L`s^z$Vgb|mr{@WiwAqKg zp#t-lhrU>F8o0s1q_9y`gQNf~Vb!F%70f}$>i7o4ho$`uciNf=xgJ>&!gSt0g;M>*x4-`U)ysFW&Vs^Vk6m%?iuWU+o&m(2Jm26Y(3%TL; zA7T)BP{WS!&xmxNw%J=$MPfn(9*^*TV;$JwRy8Zl*yUZi8jWYF>==j~&S|Xinsb%c z2?B+kpet*muEW7@AzjBA^wAJBY8i|#C{WtO_or&Nj2{=6JTTX05}|H>N2B|Wf!*3_ z7hW*j6p3TvpghEc6-wufFiY!%-GvOx*bZrhZu+7?iSrZL5q9}igiF^*R3%DE4aCHZ zqu>xS8LkW+Auv%z-<1Xs92u23R$nk@Pk}MU5!gT|c7vGlEA%G^2th&Q*zfg%-D^=f z&J_}jskj|Q;73NP4<4k*Y%pXPU2Thoqr+5uH1yEYM|VtBPW6lXaetokD0u z9qVek6Q&wk)tFbQ8(^HGf3Wp16gKmr>G;#G(HRBx?F`9AIRboK+;OfHaLJ(P>IP0w zyTbTkx_THEOs%Q&aPrxbZrJlio+hCC_HK<4%f3ZoSAyG7Dn`=X=&h@m*|UYO-4Hq0 z-Bq&+Ie!S##4A6OGoC~>ZW`Y5J)*ouaFl_e9GA*VSL!O_@xGiBw!AF}1{tB)z(w%c zS1Hmrb9OC8>0a_$BzeiN?rkPLc9%&;1CZW*4}CDDNr2gcl_3z+WC15&H1Zc2{o~i) z)LLW=WQ{?ricmC`G1GfJ0Yp4Dy~Ba;j6ZV4r{8xRs`13{dD!xXmr^Aga|C=iSmor% z8hi|pTXH)5Yf&v~exp3o+sY4B^^b*eYkkCYl*T{*=-0HniSA_1F53eCb{x~1k3*`W zr~};p1A`k{1DV9=UPnLDgz{aJH=-LQo<5%+Em!DNN252xwIf*wF_zS^!(XSm(9eoj z=*dXG&n0>)_)N5oc6v!>-bd(2ragD8O=M|wGW z!xJQS<)u70m&6OmrF0WSsr@I%T*c#Qo#Ha4d3COcX+9}hM5!7JIGF>7<~C(Ear^Sn zm^ZFkV6~Ula6+8S?oOROOA6$C&q&dp`>oR-2Ym3(HT@O7Sd5c~+kjrmM)YmgPH*tL zX+znN>`tv;5eOfX?h{AuX^LK~V#gPCu=)Tigtq9&?7Xh$qN|%A$?V*v=&-2F$zTUv z`C#WyIrChS5|Kgm_GeudCFf;)!WH7FI60j^0o#65o6`w*S7R@)88n$1nrgU(oU0M9 zx+EuMkC>(4j1;m6NoGqEkpJYJ?vc|B zOlwT3t&UgL!pX_P*6g36`ZXQ; z9~Cv}ANFnJGp(;ZhS(@FT;3e)0)Kp;h^x;$*xZn*k0U6-&FwI=uOGaODdrsp-!K$Ac32^c{+FhI-HkYd5v=`PGsg%6I`4d9Jy)uW0y%) zm&j^9WBAp*P8#kGJUhB!L?a%h$hJgQrx!6KCB_TRo%9{t0J7KW8!o1B!NC)VGLM5! zpZy5Jc{`r{1e(jd%jsG7k%I+m#CGS*BPA65ZVW~fLYw0dA-H_}O zrkGFL&P1PG9p2(%QiEWm6x;U-U&I#;Em$nx-_I^wtgw3xUPVVu zqSuKnx&dIT-XT+T10p;yjo1Y)z(x1fb8Dzfn8e yu?e%!_ptzGB|8GrCfu%p?(_ zQccdaaVK$5bz;*rnyK{_SQYM>;aES6Qs^lj9lEs6_J+%nIiuQC*fN;z8md>r_~Mfl zU%p5Dt_YT>gQqfr@`cR!$NWr~+`CZb%dn;WtzrAOI>P_JtsB76PYe*<%H(y>qx-`Kq!X_; z<{RpAqYhE=L1r*M)gNF3B8r(<%8mo*SR2hu zccLRZwGARt)Hlo1euqTyM>^!HK*!Q2P;4UYrysje@;(<|$&%vQekbn|0Ruu_Io(w4#%p6ld2Yp7tlA`Y$cciThP zKzNGIMPXX%&Ud0uQh!uQZz|FB`4KGD?3!ND?wQt6!n*f4EmCoJUh&b?;B{|lxs#F- z31~HQ`SF4x$&v00@(P+j1pAaj5!s`)b2RDBp*PB=2IB>oBF!*6vwr7Dp%zpAx*dPr zb@Zjq^XjN?O4QcZ*O+8>)|HlrR>oD*?WQl5ri3R#2?*W6iJ>>kH%KnnME&TT@ZzrHS$Q%LC?n|e>V+D+8D zYc4)QddFz7I8#}y#Wj6>4P%34dZH~OUDb?uP%-E zwjXM(?Sg~1!|wI(RVuxbu)-rH+O=igSho_pDCw(c6b=P zKk4ATlB?bj9+HHlh<_!&z0rx13K3ZrAR8W)!@Y}o`?a*JJsD+twZIv`W)@Y?Amu_u zz``@-e2X}27$i(2=9rvIu5uTUOVhzwu%mNazS|lZb&PT;XE2|B&W1>=B58#*!~D&) zfVmJGg8UdP*fx(>Cj^?yS^zH#o-$Q-*$SnK(ZVFkw+er=>N^7!)FtP3y~Xxnu^nzY zikgB>Nj0%;WOltWIob|}%lo?_C7<``a5hEkx&1ku$|)i>Rh6@3h*`slY=9U}(Ql_< zaNG*J8vb&@zpdhAvv`?{=zDedJ23TD&Zg__snRAH4eh~^oawdYi6A3w8<Ozh@Kw)#bdktM^GVb zrG08?0bG?|NG+w^&JvD*7LAbjED{_Zkc`3H!My>0u5Q}m!+6VokMLXxl`Mkd=g&Xx z-a>m*#G3SLlhbKB!)tnzfWOBV;u;ftU}S!NdD5+YtOjLg?X}dl>7m^gOpihrf1;PY zvll&>dIuUGs{Qnd- zwIR3oIrct8Va^Tm0t#(bJD7c$Z7DO9*7NnRZorrSm`b`cxz>OIC;jSE3DO8`hX955ui`s%||YQtt2 z5DNA&pG-V+4oI2s*x^>-$6J?p=I>C|9wZF8z;VjR??Icg?1w2v5Me+FgAeGGa8(3S z4vg*$>zC-WIVZtJ7}o9{D-7d>zCe|z#<9>CFve-OPAYsneTb^JH!Enaza#j}^mXy1 z+ULn^10+rWLF6j2>Ya@@Kq?26>AqK{A_| zQKb*~F1>sE*=d?A?W7N2j?L09_7n+HGi{VY;MoTGr_)G9)ot$p!-UY5zZ2Xtbm=t z@dpPSGwgH=QtIcEulQNI>S-#ifbnO5EWkI;$A|pxJd885oM+ zGZ0_0gDvG8q2xebj+fbCHYfAXuZStH2j~|d^sBAzo46(K8n59+T6rzBwK)^rfPT+B zyIFw)9YC-V^rhtK`!3jrhmW-sTmM+tPH+;nwjL#-SjQPUZ53L@A>y*rt(#M(qsiB2 zx6B)dI}6Wlsw%bJ8h|(lhkJVogQZA&n{?Vgs6gNSXzuZpEyu*xySy8ro07QZ7Vk1!3tJphN_5V7qOiyK8p z#@jcDD8nmtYi1^l8ml;AF<#IPK?!pqf9D4moYk>d99Im}Jtwj6c#+A;f)CQ*f-hZ< z=p_T86jog%!p)D&5g9taSwYi&eP z#JuEK%+NULWus;0w32-SYFku#i}d~+{Pkho&^{;RxzP&0!RCm3-9K6`>KZpnzS6?L z^H^V*s!8<>x8bomvD%rh>Zp3>Db%kyin;qtl+jAv8Oo~1g~mqGAC&Qi_wy|xEt2iz zWAJEfTV%cl2Cs<1L&DLRVVH05EDq`pH7Oh7sR`NNkL%wi}8n>IXcO40hp+J+sC!W?!krJf!GJNE8uj zg-y~Ns-<~D?yqbzVRB}G>0A^f0!^N7l=$m0OdZuqAOQqLc zX?AEGr1Ht+inZ-Qiwnl@Z0qukd__a!C*CKuGdy5#nD7VUBM^6OCpxCa2A(X;e0&V4 zM&WR8+wErQ7UIc6LY~Q9x%Sn*Tn>>P`^t&idaOEnOd(Ufw#>NoR^1QdhJ8s`h^|R_ zXX`c5*O~Xdvh%q;7L!_!ohf$NfEBmCde|#uVZvEo>OfEq%+Ns7&_f$OR9xsihRpBb z+cjk8LyDm@U{YN>+r46?nn{7Gh(;WhFw6GAxtcKD+YWV?uge>;+q#Xx4!GpRkVZYu zzsF}1)7$?%s9g9CH=Zs+B%M_)+~*j3L0&Q9u7!|+T`^O{xE6qvAP?XWv9_MrZKdo& z%IyU)$Q95AB4!#hT!_dA>4e@zjOBD*Y=XjtMm)V|+IXzjuM;(l+8aA5#Kaz_$rR6! zj>#&^DidYD$nUY(D$mH`9eb|dtV0b{S>H6FBfq>t5`;OxA4Nn{J(+XihF(stSche7$es&~N$epi&PDM_N`As;*9D^L==2Q7Z2zD+CiU(|+-kL*VG+&9!Yb3LgPy?A zm7Z&^qRG_JIxK7-FBzZI3Q<;{`DIxtc48k> zc|0dmX;Z=W$+)qE)~`yn6MdoJ4co;%!`ddy+FV538Y)j(vg}5*k(WK)KWZ3WaOG!8 z!syGn=s{H$odtpqFrT#JGM*utN7B((abXnpDM6w56nhw}OY}0TiTG1#f*VFZr+^-g zbP10`$LPq_;PvrA1XXlyx2uM^mrjTzX}w{yuLo-cOClE8MMk47T25G8M!9Z5ypOSV zAJUBGEg5L2fY)ZGJb^E34R2zJ?}Vf>{~gB!8=5Z) z9y$>5c)=;o0HeHHSuE4U)#vG&KF|I%-cF6f$~pdYJWk_dD}iOA>iA$O$+4%@>JU08 zS`ep)$XLPJ+n0_i@PkF#ri6T8?ZeAot$6JIYHm&P6EB=BiaNY|aA$W0I+nz*zkz_z zkEru!tj!QUffq%)8y0y`T&`fuus-1p>=^hnBiBqD^hXrPs`PY9tU3m0np~rISY09> z`P3s=-kt_cYcxWd{de@}TwSqg*xVhp;E9zCsnXo6z z?f&Sv^U7n4`xr=mXle94HzOdN!2kB~4=%)u&N!+2;z6UYKUDqi-s6AZ!haB;@&B`? z_TRX0%@suz^TRdCb?!vNJYPY8L_}&07uySH9%W^Tc&1pia6y1q#?*Drf}GjGbPjBS zbOPcUY#*$3sL2x4v_i*Y=N7E$mR}J%|GUI(>WEr+28+V z%v5{#e!UF*6~G&%;l*q*$V?&r$Pp^sE^i-0$+RH3ERUUdQ0>rAq2(2QAbG}$y{de( z>{qD~GGuOk559Y@%$?N^1ApVL_a704>8OD%8Y%8B;FCt%AoPu8*D1 zLB5X>b}Syz81pn;xnB}%0FnwazlWfUV)Z-~rZg6~b z6!9J$EcE&sEbzcy?CI~=boWA&eeIa%z(7SE^qgVLz??1Vbc1*aRvc%Mri)AJaAG!p z$X!_9Ds;Zz)f+;%s&dRcJt2==P{^j3bf0M=nJd&xwUGlUFn?H=2W(*2I2Gdu zv!gYCwM10aeus)`RIZSrCK=&oKaO_Ry~D1B5!y0R=%!i2*KfXGYX&gNv_u+n9wiR5 z*e$Zjju&ODRW3phN925%S(jL+bCHv6rZtc?!*`1TyYXT6%Ju=|X;6D@lq$8T zW{Y|e39ioPez(pBH%k)HzFITXHvnD6hw^lIoUMA;qAJ^CU?top1fo@s7xT13Fvn1H z6JWa-6+FJF#x>~+A;D~;VDs26>^oH0EI`IYT2iagy23?nyJ==i{g4%HrAf1-*v zK1)~@&(KkwR7TL}L(A@C_S0G;-GMDy=MJn2$FP5s<%wC)4jC5PXoxrQBFZ_k0P{{s@sz+gX`-!=T8rcB(=7vW}^K6oLWMmp(rwDh}b zwaGGd>yEy6fHv%jM$yJXo5oMAQ>c9j`**}F?MCry;T@47@r?&sKHgVe$MCqk#Z_3S z1GZI~nOEN*P~+UaFGnj{{Jo@16`(qVNtbU>O0Hf57-P>x8Jikp=`s8xWs^dAJ9lCQ z)GFm+=OV%AMVqVATtN@|vp61VVAHRn87}%PC^RAzJ%JngmZTasWBAWsoAqBU+8L8u z4A&Pe?fmTm0?mK-BL9t+{y7o(7jm+RpOhL9KnY#E&qu^}B6=K_dB}*VlSEiC9fn)+V=J;OnN)Ta5v66ic1rG+dGAJ1 z1%Zb_+!$=tQ~lxQrzv3x#CPb?CekEkA}0MYSgx$Jdd}q8+R=ma$|&1a#)TQ=l$1tQ z=tL9&_^vJ)Pk}EDO-va`UCT1m#Uty1{v^A3P~83_#v^ozH}6*9mIjIr;t3Uv%@VeW zGL6(CwCUp)Jq%G0bIG%?{_*Y#5IHf*5M@wPo6A{$Um++Co$wLC=J1aoG93&T7Ho}P z=mGEPP7GbvoG!uD$k(H3A$Z))+i{Hy?QHdk>3xSBXR0j!11O^mEe9RHmw!pvzv?Ua~2_l2Yh~_!s1qS`|0~0)YsbHSz8!mG)WiJE| z2f($6TQtt6L_f~ApQYQKSb=`053LgrQq7G@98#igV>y#i==-nEjQ!XNu9 z~;mE+gtj4IDDNQJ~JVk5Ux6&LCSFL!y=>79kE9=V}J7tD==Ga+IW zX)r7>VZ9dY=V&}DR))xUoV!u(Z|%3ciQi_2jl}3=$Agc(`RPb z8kEBpvY>1FGQ9W$n>Cq=DIpski};nE)`p3IUw1Oz0|wxll^)4dq3;CCY@RyJgFgc# zKouFh!`?Xuo{IMz^xi-h=StCis_M7yq$u) z?XHvw*HP0VgR+KR6wI)jEMX|ssqYvSf*_3W8zVTQzD?3>H!#>InzpSO)@SC8q*ii- z%%h}_#0{4JG;Jm`4zg};BPTGkYamx$Xo#O~lBirRY)q=5M45n{GCfV7h9qwyu1NxOMoP4)jjZMxmT|IQQh0U7C$EbnMN<3)Kk?fFHYq$d|ICu>KbY_hO zTZM+uKHe(cIZfEqyzyYSUBZa8;Fcut-GN!HSA9ius`ltNebF46ZX_BbZNU}}ZOm{M2&nANL9@0qvih15(|`S~z}m&h!u4x~(%MAO$jHRWNfuxWF#B)E&g3ghSQ9|> z(MFaLQj)NE0lowyjvg8z0#m6FIuKE9lDO~Glg}nSb7`~^&#(Lw{}GVOS>U)m8bF}x zVjbXljBm34Cs-yM6TVusr+3kYFjr28STT3g056y3cH5Tmge~ASxBj z%|yb>$eF;WgrcOZf569sDZOVwoo%8>XO>XQOX1OyN9I-SQgrm;U;+#3OI(zrWyow3 zk==|{lt2xrQ%FIXOTejR>;wv(Pb8u8}BUpx?yd(Abh6? zsoO3VYWkeLnF43&@*#MQ9-i-d0t*xN-UEyNKeyNMHw|A(k(_6QKO=nKMCxD(W(Yop zsRQ)QeL4X3Lxp^L%wzi2-WVSsf61dqliPUM7srDB?Wm6Lzn0&{*}|IsKQW;02(Y&| zaTKv|`U(pSzuvR6Rduu$wzK_W-Y-7>7s?G$)U}&uK;<>vU}^^ns@Z!p+9?St1s)dG zK%y6xkPyyS1$~&6v{kl?Md6gwM|>mt6Upm>oa8RLD^8T{0?HC!Z>;(Bob7el(DV6x zi`I)$&E&ngwFS@bi4^xFLAn`=fzTC;aimE^!cMI2n@Vo%Ae-ne`RF((&5y6xsjjAZ zVguVoQ?Z9uk$2ON;ersE%PU*xGO@T*;j1BO5#TuZKEf(mB7|g7pcEA=nYJ{s3vlbg zd4-DUlD{*6o%Gc^N!Nptgay>j6E5;3psI+C3Q!1ZIbeCubW%w4pq9)MSDyB{HLm|k zxv-{$$A*pS@csolri$Ge<4VZ}e~78JOL-EVyrbxKra^d{?|NnPp86!q>t<&IP07?Z z^>~IK^k#OEKgRH+LjllZXk7iA>2cfH6+(e&9ku5poo~6y{GC5>(bRK7hwjiurqAiZ zg*DmtgY}v83IjE&AbiWgMyFbaRUPZ{lYiz$U^&Zt2YjG<%m((&_JUbZcfJ22(>bi5 z!J?<7AySj0JZ&<-qXX;mcV!f~>G=sB0KnjWca4}vrtunD^1TrpfeS^4dvFr!65knK zZh`d;*VOkPs4*-9kL>$GP0`(M!j~B;#x?Ba~&s6CopvO86oM?-? zOw#dIRc;6A6T?B`Qp%^<U5 z19x(ywSH$_N+Io!6;e?`tWaM$`=Db!gzx|lQ${DG!zb1Zl&|{kX0y6xvO1o z220r<-oaS^^R2pEyY;=Qllqpmue|5yI~D|iI!IGt@iod{Opz@*ml^w2bNs)p`M(Io z|E;;m*Xpjd9l)4G#KaWfV(t8YUn@A;nK^#xgv=LtnArX|vWQVuw3}B${h+frU2>9^ z!l6)!Uo4`5k`<<;E(ido7M6lKTgWezNLq>U*=uz&s=cc$1%>VrAeOoUtA|T6gO4>UNqsdK=NF*8|~*sl&wI=x9-EGiq*aqV!(VVXA57 zw9*o6Ir8Lj1npUXvlevtn(_+^X5rzdR>#(}4YcB9O50q97%rW2me5_L=%ffYPUSRc z!vv?Kv>dH994Qi>U(a<0KF6NH5b16enCp+mw^Hb3Xs1^tThFpz!3QuN#}KBbww`(h z7GO)1olDqy6?T$()R7y%NYx*B0k_2IBiZ14&8|JPFxeMF{vW>HF-Vi3+ZOI=+qP}n zw(+!WcTd~4ZJX1!ZM&y!+uyt=&i!+~d(V%GjH;-NsEEv6nS1TERt|RHh!0>W4+4pp z1-*EzAM~i`+1f(VEHI8So`S`akPfPTfq*`l{Fz`hS%k#JS0cjT2mS0#QLGf=J?1`he3W*;m4)ce8*WFq1sdP=~$5RlH1EdWm|~dCvKOi4*I_96{^95p#B<(n!d?B z=o`0{t+&OMwKcxiBECznJcfH!fL(z3OvmxP#oWd48|mMjpE||zdiTBdWelj8&Qosv zZFp@&UgXuvJw5y=q6*28AtxZzo-UUpkRW%ne+Ylf!V-0+uQXBW=5S1o#6LXNtY5!I z%Rkz#(S8Pjz*P7bqB6L|M#Er{|QLae-Y{KA>`^} z@lPjeX>90X|34S-7}ZVXe{wEei1<{*e8T-Nbj8JmD4iwcE+Hg_zhkPVm#=@b$;)h6 z<<6y`nPa`f3I6`!28d@kdM{uJOgM%`EvlQ5B2bL)Sl=|y@YB3KeOzz=9cUW3clPAU z^sYc}xf9{4Oj?L5MOlYxR{+>w=vJjvbyO5}ptT(o6dR|ygO$)nVCvNGnq(6;bHlBd zl?w-|plD8spjDF03g5ip;W3Z z><0{BCq!Dw;h5~#1BuQilq*TwEu)qy50@+BE4bX28+7erX{BD4H)N+7U`AVEuREE8 z;X?~fyhF-x_sRfHIj~6f(+^@H)D=ngP;mwJjxhQUbUdzk8f94Ab%59-eRIq?ZKrwD z(BFI=)xrUlgu(b|hAysqK<}8bslmNNeD=#JW*}^~Nrswn^xw*nL@Tx!49bfJecV&KC2G4q5a!NSv)06A_5N3Y?veAz;Gv+@U3R% z)~UA8-0LvVE{}8LVDOHzp~2twReqf}ODIyXMM6=W>kL|OHcx9P%+aJGYi_Om)b!xe zF40Vntn0+VP>o<$AtP&JANjXBn7$}C@{+@3I@cqlwR2MdwGhVPxlTIcRVu@Ho-wO` z_~Or~IMG)A_`6-p)KPS@cT9mu9RGA>dVh5wY$NM9-^c@N=hcNaw4ITjm;iWSP^ZX| z)_XpaI61<+La+U&&%2a z0za$)-wZP@mwSELo#3!PGTt$uy0C(nTT@9NX*r3Ctw6J~7A(m#8fE)0RBd`TdKfAT zCf@$MAxjP`O(u9s@c0Fd@|}UQ6qp)O5Q5DPCeE6mSIh|Rj{$cAVIWsA=xPKVKxdhg zLzPZ`3CS+KIO;T}0Ip!fAUaNU>++ZJZRk@I(h<)RsJUhZ&Ru9*!4Ptn;gX^~4E8W^TSR&~3BAZc#HquXn)OW|TJ`CTahk+{qe`5+ixON^zA9IFd8)kc%*!AiLu z>`SFoZ5bW-%7}xZ>gpJcx_hpF$2l+533{gW{a7ce^B9sIdmLrI0)4yivZ^(Vh@-1q zFT!NQK$Iz^xu%|EOK=n>ug;(7J4OnS$;yWmq>A;hsD_0oAbLYhW^1Vdt9>;(JIYjf zdb+&f&D4@4AS?!*XpH>8egQvSVX`36jMd>$+RgI|pEg))^djhGSo&#lhS~9%NuWfX zDDH;3T*GzRT@5=7ibO>N-6_XPBYxno@mD_3I#rDD?iADxX`! zh*v8^i*JEMzyN#bGEBz7;UYXki*Xr(9xXax(_1qVW=Ml)kSuvK$coq2A(5ZGhs_pF z$*w}FbN6+QDseuB9=fdp_MTs)nQf!2SlROQ!gBJBCXD&@-VurqHj0wm@LWX-TDmS= z71M__vAok|@!qgi#H&H%Vg-((ZfxPAL8AI{x|VV!9)ZE}_l>iWk8UPTGHs*?u7RfP z5MC&=c6X;XlUzrz5q?(!eO@~* zoh2I*%J7dF!!_!vXoSIn5o|wj1#_>K*&CIn{qSaRc&iFVxt*^20ngCL;QonIS>I5^ zMw8HXm>W0PGd*}Ko)f|~dDd%;Wu_RWI_d;&2g6R3S63Uzjd7dn%Svu-OKpx*o|N>F zZg=-~qLb~VRLpv`k zWSdfHh@?dp=s_X`{yxOlxE$4iuyS;Z-x!*E6eqmEm*j2bE@=ZI0YZ5%Yj29!5+J$4h{s($nakA`xgbO8w zi=*r}PWz#lTL_DSAu1?f%-2OjD}NHXp4pXOsCW;DS@BC3h-q4_l`<))8WgzkdXg3! zs1WMt32kS2E#L0p_|x+x**TFV=gn`m9BWlzF{b%6j-odf4{7a4y4Uaef@YaeuPhU8 zHBvRqN^;$Jizy+ z=zW{E5<>2gp$pH{M@S*!sJVQU)b*J5*bX4h>5VJve#Q6ga}cQ&iL#=(u+KroWrxa%8&~p{WEUF0il=db;-$=A;&9M{Rq`ouZ5m%BHT6%st%saGsD6)fQgLN}x@d3q>FC;=f%O3Cyg=Ke@Gh`XW za@RajqOE9UB6eE=zhG%|dYS)IW)&y&Id2n7r)6p_)vlRP7NJL(x4UbhlcFXWT8?K=%s7;z?Vjts?y2+r|uk8Wt(DM*73^W%pAkZa1Jd zNoE)8FvQA>Z`eR5Z@Ig6kS5?0h;`Y&OL2D&xnnAUzQz{YSdh0k zB3exx%A2TyI)M*EM6htrxSlep!Kk(P(VP`$p0G~f$smld6W1r_Z+o?=IB@^weq>5VYsYZZR@` z&XJFxd5{|KPZmVOSxc@^%71C@;z}}WhbF9p!%yLj3j%YOlPL5s>7I3vj25 z@xmf=*z%Wb4;Va6SDk9cv|r*lhZ`(y_*M@>q;wrn)oQx%B(2A$9(74>;$zmQ!4fN; z>XurIk-7@wZys<+7XL@0Fhe-f%*=(weaQEdR9Eh6>Kl-EcI({qoZqyzziGwpg-GM#251sK_ z=3|kitS!j%;fpc@oWn65SEL73^N&t>Ix37xgs= zYG%eQDJc|rqHFia0!_sm7`@lvcv)gfy(+KXA@E{3t1DaZ$DijWAcA)E0@X?2ziJ{v z&KOYZ|DdkM{}t+@{@*6ge}m%xfjIxi%qh`=^2Rwz@w0cCvZ&Tc#UmCDbVwABrON^x zEBK43FO@weA8s7zggCOWhMvGGE`baZ62cC)VHyy!5Zbt%ieH+XN|OLbAFPZWyC6)p z4P3%8sq9HdS3=ih^0OOlqTPbKuzQ?lBEI{w^ReUO{V?@`ARsL|S*%yOS=Z%sF)>-y z(LAQdhgAcuF6LQjRYfdbD1g4o%tV4EiK&ElLB&^VZHbrV1K>tHTO{#XTo>)2UMm`2 z^t4s;vnMQgf-njU-RVBRw0P0-m#d-u`(kq7NL&2T)TjI_@iKuPAK-@oH(J8?%(e!0Ir$yG32@CGUPn5w4)+9@8c&pGx z+K3GKESI4*`tYlmMHt@br;jBWTei&(a=iYslc^c#RU3Q&sYp zSG){)V<(g7+8W!Wxeb5zJb4XE{I|&Y4UrFWr%LHkdQ;~XU zgy^dH-Z3lmY+0G~?DrC_S4@=>0oM8Isw%g(id10gWkoz2Q%7W$bFk@mIzTCcIB(K8 zc<5h&ZzCdT=9n-D>&a8vl+=ZF*`uTvQviG_bLde*k>{^)&0o*b05x$MO3gVLUx`xZ z43j+>!u?XV)Yp@MmG%Y`+COH2?nQcMrQ%k~6#O%PeD_WvFO~Kct za4XoCM_X!c5vhRkIdV=xUB3xI2NNStK*8_Zl!cFjOvp-AY=D;5{uXj}GV{LK1~IE2 z|KffUiBaStRr;10R~K2VVtf{TzM7FaPm;Y(zQjILn+tIPSrJh&EMf6evaBKIvi42-WYU9Vhj~3< zZSM-B;E`g_o8_XTM9IzEL=9Lb^SPhe(f(-`Yh=X6O7+6ALXnTcUFpI>ekl6v)ZQeNCg2 z^H|{SKXHU*%nBQ@I3It0m^h+6tvI@FS=MYS$ZpBaG7j#V@P2ZuYySbp@hA# ze(kc;P4i_-_UDP?%<6>%tTRih6VBgScKU^BV6Aoeg6Uh(W^#J^V$Xo^4#Ekp ztqQVK^g9gKMTHvV7nb64UU7p~!B?>Y0oFH5T7#BSW#YfSB@5PtE~#SCCg3p^o=NkMk$<8- z6PT*yIKGrvne7+y3}_!AC8NNeI?iTY(&nakN>>U-zT0wzZf-RuyZk^X9H-DT_*wk= z;&0}6LsGtfVa1q)CEUPlx#(ED@-?H<1_FrHU#z5^P3lEB|qsxEyn%FOpjx z3S?~gvoXy~L(Q{Jh6*i~=f%9kM1>RGjBzQh_SaIDfSU_9!<>*Pm>l)cJD@wlyxpBV z4Fmhc2q=R_wHCEK69<*wG%}mgD1=FHi4h!98B-*vMu4ZGW~%IrYSLGU{^TuseqVgV zLP<%wirIL`VLyJv9XG_p8w@Q4HzNt-o;U@Au{7%Ji;53!7V8Rv0^Lu^Vf*sL>R(;c zQG_ZuFl)Mh-xEIkGu}?_(HwkB2jS;HdPLSxVU&Jxy9*XRG~^HY(f0g8Q}iqnVmgjI zfd=``2&8GsycjR?M%(zMjn;tn9agcq;&rR!Hp z$B*gzHsQ~aXw8c|a(L^LW(|`yGc!qOnV(ZjU_Q-4z1&0;jG&vAKuNG=F|H?@m5^N@ zq{E!1n;)kNTJ>|Hb2ODt-7U~-MOIFo%9I)_@7fnX+eMMNh>)V$IXesJpBn|uo8f~#aOFytCT zf9&%MCLf8mp4kwHTcojWmM3LU=#|{3L>E}SKwOd?%{HogCZ_Z1BSA}P#O(%H$;z7XyJ^sjGX;j5 zrzp>|Ud;*&VAU3x#f{CKwY7Vc{%TKKqmB@oTHA9;>?!nvMA;8+Jh=cambHz#J18x~ zs!dF>$*AnsQ{{82r5Aw&^7eRCdvcgyxH?*DV5(I$qXh^zS>us*I66_MbL8y4d3ULj z{S(ipo+T3Ag!+5`NU2sc+@*m{_X|&p#O-SAqF&g_n7ObB82~$p%fXA5GLHMC+#qqL zdt`sJC&6C2)=juQ_!NeD>U8lDVpAOkW*khf7MCcs$A(wiIl#B9HM%~GtQ^}yBPjT@ z+E=|A!Z?A(rwzZ;T}o6pOVqHzTr*i;Wrc%&36kc@jXq~+w8kVrs;%=IFdACoLAcCAmhFNpbP8;s`zG|HC2Gv?I~w4ITy=g$`0qMQdkijLSOtX6xW%Z9Nw<;M- zMN`c7=$QxN00DiSjbVt9Mi6-pjv*j(_8PyV-il8Q-&TwBwH1gz1uoxs6~uU}PrgWB zIAE_I-a1EqlIaGQNbcp@iI8W1sm9fBBNOk(k&iLBe%MCo#?xI$%ZmGA?=)M9D=0t7 zc)Q0LnI)kCy{`jCGy9lYX%mUsDWwsY`;jE(;Us@gmWPqjmXL+Hu#^;k%eT>{nMtzj zsV`Iy6leTA8-PndszF;N^X@CJrTw5IIm!GPeu)H2#FQitR{1p;MasQVAG3*+=9FYK zw*k!HT(YQorfQj+1*mCV458(T5=fH`um$gS38hw(OqVMyunQ;rW5aPbF##A3fGH6h z@W)i9Uff?qz`YbK4c}JzQpuxuE3pcQO)%xBRZp{zJ^-*|oryTxJ-rR+MXJ)!f=+pp z10H|DdGd2exhi+hftcYbM0_}C0ZI-2vh+$fU1acsB-YXid7O|=9L!3e@$H*6?G*Zp z%qFB(sgl=FcC=E4CYGp4CN>=M8#5r!RU!u+FJVlH6=gI5xHVD&k;Ta*M28BsxfMV~ zLz+@6TxnfLhF@5=yQo^1&S}cmTN@m!7*c6z;}~*!hNBjuE>NLVl2EwN!F+)0$R1S! zR|lF%n!9fkZ@gPW|x|B={V6x3`=jS*$Pu0+5OWf?wnIy>Y1MbbGSncpKO0qE(qO=ts z!~@&!N`10S593pVQu4FzpOh!tvg}p%zCU(aV5=~K#bKi zHdJ1>tQSrhW%KOky;iW+O_n;`l9~omqM%sdxdLtI`TrJzN6BQz+7xOl*rM>xVI2~# z)7FJ^Dc{DC<%~VS?@WXzuOG$YPLC;>#vUJ^MmtbSL`_yXtNKa$Hk+l-c!aC7gn(Cg ze?YPYZ(2Jw{SF6MiO5(%_pTo7j@&DHNW`|lD`~{iH+_eSTS&OC*2WTT*a`?|9w1dh zh1nh@$a}T#WE5$7Od~NvSEU)T(W$p$s5fe^GpG+7fdJ9=enRT9$wEk+ZaB>G3$KQO zgq?-rZZnIv!p#>Ty~}c*Lb_jxJg$eGM*XwHUwuQ|o^}b3^T6Bxx{!?va8aC@-xK*H ztJBFvFfsSWu89%@b^l3-B~O!CXs)I6Y}y#0C0U0R0WG zybjroj$io0j}3%P7zADXOwHwafT#uu*zfM!oD$6aJx7+WL%t-@6^rD_a_M?S^>c;z zMK580bZXo1f*L$CuMeM4Mp!;P@}b~$cd(s5*q~FP+NHSq;nw3fbWyH)i2)-;gQl{S zZO!T}A}fC}vUdskGSq&{`oxt~0i?0xhr6I47_tBc`fqaSrMOzR4>0H^;A zF)hX1nfHs)%Zb-(YGX;=#2R6C{BG;k=?FfP?9{_uFLri~-~AJ;jw({4MU7e*d)?P@ zXX*GkNY9ItFjhwgAIWq7Y!ksbMzfqpG)IrqKx9q{zu%Mdl+{Dis#p9q`02pr1LG8R z@As?eG!>IoROgS!@J*to<27coFc1zpkh?w=)h9CbYe%^Q!Ui46Y*HO0mr% zEff-*$ndMNw}H2a5@BsGj5oFfd!T(F&0$<{GO!Qdd?McKkorh=5{EIjDTHU`So>8V zBA-fqVLb2;u7UhDV1xMI?y>fe3~4urv3%PX)lDw+HYa;HFkaLqi4c~VtCm&Ca+9C~ zge+67hp#R9`+Euq59WhHX&7~RlXn=--m8$iZ~~1C8cv^2(qO#X0?vl91gzUKBeR1J z^p4!!&7)3#@@X&2aF2-)1Ffcc^F8r|RtdL2X%HgN&XU-KH2SLCbpw?J5xJ*!F-ypZ zMG%AJ!Pr&}`LW?E!K~=(NJxuSVTRCGJ$2a*Ao=uUDSys!OFYu!Vs2IT;xQ6EubLIl z+?+nMGeQQhh~??0!s4iQ#gm3!BpMpnY?04kK375e((Uc7B3RMj;wE?BCoQGu=UlZt!EZ1Q*auI)dj3Jj{Ujgt zW5hd~-HWBLI_3HuO) zNrb^XzPsTIb=*a69wAAA3J6AAZZ1VsYbIG}a`=d6?PjM)3EPaDpW2YP$|GrBX{q*! z$KBHNif)OKMBCFP5>!1d=DK>8u+Upm-{hj5o|Wn$vh1&K!lVfDB&47lw$tJ?d5|=B z^(_9=(1T3Fte)z^>|3**n}mIX;mMN5v2F#l(q*CvU{Ga`@VMp#%rQkDBy7kYbmb-q z<5!4iuB#Q_lLZ8}h|hPODI^U6`gzLJre9u3k3c#%86IKI*^H-@I48Bi*@avYm4v!n0+v zWu{M{&F8#p9cx+gF0yTB_<2QUrjMPo9*7^-uP#~gGW~y3nfPAoV%amgr>PSyVAd@l)}8#X zR5zV6t*uKJZL}?NYvPVK6J0v4iVpwiN|>+t3aYiZSp;m0!(1`bHO}TEtWR1tY%BPB z(W!0DmXbZAsT$iC13p4f>u*ZAy@JoLAkJhzFf1#4;#1deO8#8d&89}en&z!W&A3++^1(;>0SB1*54d@y&9Pn;^IAf3GiXbfT`_>{R+Xv; zQvgL>+0#8-laO!j#-WB~(I>l0NCMt_;@Gp_f0#^c)t?&#Xh1-7RR0@zPyBz!U#0Av zT?}n({(p?p7!4S2ZBw)#KdCG)uPnZe+U|0{BW!m)9 zi_9$F?m<`2!`JNFv+w8MK_K)qJ^aO@7-Ig>cM4-r0bi=>?B_2mFNJ}aE3<+QCzRr*NA!QjHw# z`1OsvcoD0?%jq{*7b!l|L1+Tw0TTAM4XMq7*ntc-Ived>Sj_ZtS|uVdpfg1_I9knY z2{GM_j5sDC7(W&}#s{jqbybqJWyn?{PW*&cQIU|*v8YGOKKlGl@?c#TCnmnAkAzV- zmK={|1G90zz=YUvC}+fMqts0d4vgA%t6Jhjv?d;(Z}(Ep8fTZfHA9``fdUHkA+z3+ zhh{ohP%Bj?T~{i0sYCQ}uC#5BwN`skI7`|c%kqkyWIQ;!ysvA8H`b-t()n6>GJj6xlYDu~8qX{AFo$Cm3d|XFL=4uvc?Keb zzb0ZmMoXca6Mob>JqkNuoP>B2Z>D`Q(TvrG6m`j}-1rGP!g|qoL=$FVQYxJQjFn33lODt3Wb1j8VR zlR++vIT6^DtYxAv_hxupbLLN3e0%A%a+hWTKDV3!Fjr^cWJ{scsAdfhpI)`Bms^M6 zQG$waKgFr=c|p9Piug=fcJvZ1ThMnNhQvBAg-8~b1?6wL*WyqXhtj^g(Ke}mEfZVM zJuLNTUVh#WsE*a6uqiz`b#9ZYg3+2%=C(6AvZGc=u&<6??!slB1a9K)=VL zY9EL^mfyKnD zSJyYBc_>G;5RRnrNgzJz#Rkn3S1`mZgO`(r5;Hw6MveN(URf_XS-r58Cn80K)ArH4 z#Rrd~LG1W&@ttw85cjp8xV&>$b%nSXH_*W}7Ch2pg$$c0BdEo-HWRTZcxngIBJad> z;C>b{jIXjb_9Jis?NZJsdm^EG}e*pR&DAy0EaSGi3XWTa(>C%tz1n$u?5Fb z1qtl?;_yjYo)(gB^iQq?=jusF%kywm?CJP~zEHi0NbZ);$(H$w(Hy@{i>$wcVRD_X|w-~(0Z9BJyh zhNh;+eQ9BEIs;tPz%jSVnfCP!3L&9YtEP;svoj_bNzeGSQIAjd zBss@A;)R^WAu-37RQrM%{DfBNRx>v!G31Z}8-El9IOJlb_MSoMu2}GDYycNaf>uny z+8xykD-7ONCM!APry_Lw6-yT>5!tR}W;W`C)1>pxSs5o1z#j7%m=&=7O4hz+Lsqm` z*>{+xsabZPr&X=}G@obTb{nPTkccJX8w3CG7X+1+t{JcMabv~UNv+G?txRqXib~c^Mo}`q{$`;EBNJ;#F*{gvS12kV?AZ%O0SFB$^ zn+}!HbmEj}w{Vq(G)OGAzH}R~kS^;(-s&=ectz8vN!_)Yl$$U@HNTI-pV`LSj7Opu zTZ5zZ)-S_{GcEQPIQXLQ#oMS`HPu{`SQiAZ)m1at*Hy%3xma|>o`h%E%8BEbi9p0r zVjcsh<{NBKQ4eKlXU|}@XJ#@uQw*$4BxKn6#W~I4T<^f99~(=}a`&3(ur8R9t+|AQ zWkQx7l}wa48-jO@ft2h+7qn%SJtL%~890FG0s5g*kNbL3I&@brh&f6)TlM`K^(bhr zJWM6N6x3flOw$@|C@kPi7yP&SP?bzP-E|HSXQXG>7gk|R9BTj`e=4de9C6+H7H7n# z#GJeVs1mtHhLDmVO?LkYRQc`DVOJ_vdl8VUihO-j#t=0T3%Fc1f9F73ufJz*adn*p zc%&vi(4NqHu^R>sAT_0EDjVR8bc%wTz#$;%NU-kbDyL_dg0%TFafZwZ?5KZpcuaO54Z9hX zD$u>q!-9`U6-D`E#`W~fIfiIF5_m6{fvM)b1NG3xf4Auw;Go~Fu7cth#DlUn{@~yu z=B;RT*dp?bO}o%4x7k9v{r=Y@^YQ^UUm(Qmliw8brO^=NP+UOohLYiaEB3^DB56&V zK?4jV61B|1Uj_5fBKW;8LdwOFZKWp)g{B%7g1~DgO&N& z#lisxf?R~Z@?3E$Mms$$JK8oe@X`5m98V*aV6Ua}8Xs2#A!{x?IP|N(%nxsH?^c{& z@vY&R1QmQs83BW28qAmJfS7MYi=h(YK??@EhjL-t*5W!p z^gYX!Q6-vBqcv~ruw@oMaU&qp0Fb(dbVzm5xJN%0o_^@fWq$oa3X?9s%+b)x4w-q5Koe(@j6Ez7V@~NRFvd zfBH~)U5!ix3isg`6be__wBJp=1@yfsCMw1C@y+9WYD9_C%{Q~7^0AF2KFryfLlUP# zwrtJEcH)jm48!6tUcxiurAMaiD04C&tPe6DI0#aoqz#Bt0_7_*X*TsF7u*zv(iEfA z;$@?XVu~oX#1YXtceQL{dSneL&*nDug^OW$DSLF0M1Im|sSX8R26&)<0Fbh^*l6!5wfSu8MpMoh=2l z^^0Sr$UpZp*9oqa23fcCfm7`ya2<4wzJ`Axt7e4jJrRFVf?nY~2&tRL* zd;6_njcz01c>$IvN=?K}9ie%Z(BO@JG2J}fT#BJQ+f5LFSgup7i!xWRKw6)iITjZU z%l6hPZia>R!`aZjwCp}I zg)%20;}f+&@t;(%5;RHL>K_&7MH^S+7<|(SZH!u zznW|jz$uA`P9@ZWtJgv$EFp>)K&Gt+4C6#*khZQXS*S~6N%JDT$r`aJDs9|uXWdbg zBwho$phWx}x!qy8&}6y5Vr$G{yGSE*r$^r{}pw zVTZKvikRZ`J_IJrjc=X1uw?estdwm&bEahku&D04HD+0Bm~q#YGS6gp!KLf$A{%Qd z&&yX@Hp>~(wU{|(#U&Bf92+1i&Q*-S+=y=3pSZy$#8Uc$#7oiJUuO{cE6=tsPhwPe| zxQpK>`Dbka`V)$}e6_OXKLB%i76~4N*zA?X+PrhH<&)}prET;kel24kW%+9))G^JI zsq7L{P}^#QsZViX%KgxBvEugr>ZmFqe^oAg?{EI=&_O#e)F3V#rc z8$4}0Zr19qd3tE4#$3_f=Bbx9oV6VO!d3(R===i-7p=Vj`520w0D3W6lQfY48}!D* z&)lZMG;~er2qBoI2gsX+Ts-hnpS~NYRDtPd^FPzn!^&yxRy#CSz(b&E*tL|jIkq|l zf%>)7Dtu>jCf`-7R#*GhGn4FkYf;B$+9IxmqH|lf6$4irg{0ept__%)V*R_OK=T06 zyT_m-o@Kp6U{l5h>W1hGq*X#8*y@<;vsOFqEjTQXFEotR+{3}ODDnj;o0@!bB5x=N z394FojuGOtVKBlVRLtHp%EJv_G5q=AgF)SKyRN5=cGBjDWv4LDn$IL`*=~J7u&Dy5 zrMc83y+w^F&{?X(KOOAl-sWZDb{9X9#jrQtmrEXD?;h-}SYT7yM(X_6qksM=K_a;Z z3u0qT0TtaNvDER_8x*rxXw&C^|h{P1qxK|@pS7vdlZ#P z7PdB7MmC2}%sdzAxt>;WM1s0??`1983O4nFK|hVAbHcZ3x{PzytQLkCVk7hA!Lo` zEJH?4qw|}WH{dc4z%aB=0XqsFW?^p=X}4xnCJXK%c#ItOSjdSO`UXJyuc8bh^Cf}8 z@Ht|vXd^6{Fgai8*tmyRGmD_s_nv~r^Fy7j`Bu`6=G)5H$i7Q7lvQnmea&TGvJp9a|qOrUymZ$6G|Ly z#zOCg++$3iB$!6!>215A4!iryregKuUT344X)jQb3|9qY>c0LO{6Vby05n~VFzd?q zgGZv&FGlkiH*`fTurp>B8v&nSxNz)=5IF$=@rgND4d`!AaaX;_lK~)-U8la_Wa8i?NJC@BURO*sUW)E9oyv3RG^YGfN%BmxzjlT)bp*$<| zX3tt?EAy<&K+bhIuMs-g#=d1}N_?isY)6Ay$mDOKRh z4v1asEGWoAp=srraLW^h&_Uw|6O+r;wns=uwYm=JN4Q!quD8SQRSeEcGh|Eb5Jg8m zOT}u;N|x@aq)=&;wufCc^#)5U^VcZw;d_wwaoh9$p@Xrc{DD6GZUqZ ziC6OT^zSq@-lhbgR8B+e;7_Giv;DK5gn^$bs<6~SUadiosfewWDJu`XsBfOd1|p=q zE>m=zF}!lObA%ePey~gqU8S6h-^J2Y?>7)L2+%8kV}Gp=h`Xm_}rlm)SyUS=`=S7msKu zC|T!gPiI1rWGb1z$Md?0YJQ;%>uPLOXf1Z>N~`~JHJ!^@D5kSXQ4ugnFZ>^`zH8CAiZmp z6Ms|#2gcGsQ{{u7+Nb9sA?U>(0e$5V1|WVwY`Kn)rsnnZ4=1u=7u!4WexZD^IQ1Jk zfF#NLe>W$3m&C^ULjdw+5|)-BSHwpegdyt9NYC{3@QtMfd8GrIWDu`gd0nv-3LpGCh@wgBaG z176tikL!_NXM+Bv#7q^cyn9$XSeZR6#!B4JE@GVH zoobHZN_*RF#@_SVYKkQ_igme-Y5U}cV(hkR#k1c{bQNMji zU7aE`?dHyx=1`kOYZo_8U7?3-7vHOp`Qe%Z*i+FX!s?6huNp0iCEW-Z7E&jRWmUW_ z67j>)Ew!yq)hhG4o?^z}HWH-e=es#xJUhDRc4B51M4~E-l5VZ!&zQq`gWe`?}#b~7w1LH4Xa-UCT5LXkXQWheBa2YJYbyQ zl1pXR%b(KCXMO0OsXgl0P0Og<{(@&z1aokU-Pq`eQq*JYgt8xdFQ6S z6Z3IFSua8W&M#`~*L#r>Jfd6*BzJ?JFdBR#bDv$_0N!_5vnmo@!>vULcDm`MFU823 zpG9pqjqz^FE5zMDoGqhs5OMmC{Y3iVcl>F}5Rs24Y5B^mYQ;1T&ks@pIApHOdrzXF z-SdX}Hf{X;TaSxG_T$0~#RhqKISGKNK47}0*x&nRIPtmdwxc&QT3$8&!3fWu1eZ_P zJveQj^hJL#Sn!*4k`3}(d(aasl&7G0j0-*_2xtAnoX1@9+h zO#c>YQg60Z;o{Bi=3i7S`Ic+ZE>K{(u|#)9y}q*j8uKQ1^>+(BI}m%1v3$=4ojGBc zm+o1*!T&b}-lVvZqIUBc8V}QyFEgm#oyIuC{8WqUNV{Toz`oxhYpP!_p2oHHh5P@iB*NVo~2=GQm+8Yrkm2Xjc_VyHg1c0>+o~@>*Qzo zHVBJS>$$}$_4EniTI;b1WShX<5-p#TPB&!;lP!lBVBbLOOxh6FuYloD%m;n{r|;MU3!q4AVkua~fieeWu2 zQAQ$ue(IklX6+V;F1vCu-&V?I3d42FgWgsb_e^29ol}HYft?{SLf>DrmOp9o!t>I^ zY7fBCk+E8n_|apgM|-;^=#B?6RnFKlN`oR)`e$+;D=yO-(U^jV;rft^G_zl`n7qnM zL z*-Y4Phq+ZI1$j$F-f;`CD#|`-T~OM5Q>x}a>B~Gb3-+9i>Lfr|Ca6S^8g*{*?_5!x zH_N!SoRP=gX1?)q%>QTY!r77e2j9W(I!uAz{T`NdNmPBBUzi2{`XMB^zJGGwFWeA9 z{fk33#*9SO0)DjROug+(M)I-pKA!CX;IY(#gE!UxXVsa)X!UftIN98{pt#4MJHOhY zM$_l}-TJlxY?LS6Nuz1T<44m<4i^8k@D$zuCPrkmz@sdv+{ciyFJG2Zwy&%c7;atIeTdh!a(R^QXnu1Oq1b42*OQFWnyQ zWeQrdvP|w_idy53Wa<{QH^lFmEd+VlJkyiC>6B#s)F;w-{c;aKIm;Kp50HnA-o3lY z9B~F$gJ@yYE#g#X&3ADx&tO+P_@mnQTz9gv30_sTsaGXkfNYXY{$(>*PEN3QL>I!k zp)KibPhrfX3%Z$H6SY`rXGYS~143wZrG2;=FLj50+VM6soI~up_>fU(2Wl@{BRsMi zO%sL3x?2l1cXTF)k&moNsHfQrQ+wu(gBt{sk#CU=UhrvJIncy@tJX5klLjgMn>~h= zg|FR&;@eh|C7`>s_9c~0-{IAPV){l|Ts`i=)AW;d9&KPc3fMeoTS%8@V~D8*h;&(^>yjT84MM}=%#LS7shLAuuj(0VAYoozhWjq z4LEr?wUe2^WGwdTIgWBkDUJa>YP@5d9^Rs$kCXmMRxuF*YMVrn?0NFyPl}>`&dqZb z<5eqR=ZG3>n2{6v6BvJ`YBZeeTtB88TAY(x0a58EWyuf>+^|x8Qa6wA|1Nb_p|nA zWWa}|z8a)--Wj`LqyFk_a3gN2>5{Rl_wbW?#by7&i*^hRknK%jwIH6=dQ8*-_{*x0j^DUfMX0`|K@6C<|1cgZ~D(e5vBFFm;HTZF(!vT8=T$K+|F)x3kqzBV4-=p1V(lzi(s7jdu0>LD#N=$Lk#3HkG!a zIF<7>%B7sRNzJ66KrFV76J<2bdYhxll0y2^_rdG=I%AgW4~)1Nvz=$1UkE^J%BxLo z+lUci`UcU062os*=`-j4IfSQA{w@y|3}Vk?i;&SSdh8n+$iHA#%ERL{;EpXl6u&8@ zzg}?hkEOUOJt?ZL=pWZFJ19mI1@P=$U5*Im1e_8Z${JsM>Ov?nh8Z zP5QvI!{Jy@&BP48%P2{Jr_VgzW;P@7)M9n|lDT|Ep#}7C$&ud&6>C^5ZiwKIg2McPU(4jhM!BD@@L(Gd*Nu$ji(ljZ<{FIeW_1Mmf;76{LU z-ywN~=uNN)Xi6$<12A9y)K%X|(W0p|&>>4OXB?IiYr||WKDOJPxiSe01NSV-h24^L z_>m$;|C+q!Mj**-qQ$L-*++en(g|hw;M!^%_h-iDjFHLo-n3JpB;p?+o2;`*jpvJU zLY^lt)Un4joij^^)O(CKs@7E%*!w>!HA4Q?0}oBJ7Nr8NQ7QmY^4~jvf0-`%waOLn zdNjAPaC0_7c|RVhw)+71NWjRi!y>C+Bl;Z`NiL^zn2*0kmj5gyhCLCxts*cWCdRI| zjsd=sT5BVJc^$GxP~YF$-U{-?kW6r@^vHXB%{CqYzU@1>dzf#3SYedJG-Rm6^RB7s zGM5PR(yKPKR)>?~vpUIeTP7A1sc8-knnJk*9)3t^e%izbdm>Y=W{$wm(cy1RB-19i za#828DMBY+ps#7Y8^6t)=Ea@%Nkt)O6JCx|ybC;Ap}Z@Zw~*}3P>MZLPb4Enxz9Wf zssobT^(R@KuShj8>@!1M7tm|2%-pYYDxz-5`rCbaTCG5{;Uxm z*g=+H1X8{NUvFGzz~wXa%Eo};I;~`37*WrRU&K0dPSB$yk(Z*@K&+mFal^?c zurbqB-+|Kb5|sznT;?Pj!+kgFY1#Dr;_%A(GIQC{3ct|{*Bji%FNa6c-thbpBkA;U zURV!Dr&X{0J}iht#-Qp2=xzuh(fM>zRoiGrYl5ttw2#r34gC41CCOC31m~^UPTK@s z6;A@)7O7_%C)>bnAXerYuAHdE93>j2N}H${zEc6&SbZ|-fiG*-qtGuy-qDelH(|u$ zorf8_T6Zqe#Ub!+e3oSyrskt_HyW_^5lrWt#30l)tHk|j$@YyEkXUOV;6B51L;M@=NIWZXU;GrAa(LGxO%|im%7F<-6N;en0Cr zLH>l*y?pMwt`1*cH~LdBPFY_l;~`N!Clyfr;7w<^X;&(ZiVdF1S5e(+Q%60zgh)s4 zn2yj$+mE=miVERP(g8}G4<85^-5f@qxh2ec?n+$A_`?qN=iyT1?U@t?V6DM~BIlBB z>u~eXm-aE>R0sQy!-I4xtCNi!!qh?R1!kKf6BoH2GG{L4%PAz0{Sh6xpuyI%*~u)s z%rLuFl)uQUCBQAtMyN;%)zFMx4loh7uTfKeB2Xif`lN?2gq6NhWhfz0u5WP9J>=V2 zo{mLtSy&BA!mSzs&CrKWq^y40JF5a&GSXIi2= z{EYb59J4}VwikL4P=>+mc6{($FNE@e=VUwG+KV21;<@lrN`mnz5jYGASyvz7BOG_6(p^eTxD-4O#lROgon;R35=|nj#eHIfJBYPWG>H>`dHKCDZ3`R{-?HO0mE~(5_WYcFmp8sU?wr*UkAQiNDGc6T zA%}GOLXlOWqL?WwfHO8MB#8M8*~Y*gz;1rWWoVSXP&IbKxbQ8+s%4Jnt?kDsq7btI zCDr0PZ)b;B%!lu&CT#RJzm{l{2fq|BcY85`w~3LSK<><@(2EdzFLt9Y_`;WXL6x`0 zDoQ?=?I@Hbr;*VVll1Gmd8*%tiXggMK81a+T(5Gx6;eNb8=uYn z5BG-0g>pP21NPn>$ntBh>`*})Fl|38oC^9Qz>~MAazH%3Q~Qb!ALMf$srexgPZ2@&c~+hxRi1;}+)-06)!#Mq<6GhP z-Q?qmgo${aFBApb5p}$1OJKTClfi8%PpnczyVKkoHw7Ml9e7ikrF0d~UB}i3vizos zXW4DN$SiEV9{faLt5bHy2a>33K%7Td-n5C*N;f&ZqAg#2hIqEb(y<&f4u5BWJ>2^4 z414GosL=Aom#m&=x_v<0-fp1r%oVJ{T-(xnomNJ(Dryv zh?vj+%=II_nV+@NR+(!fZZVM&(W6{6%9cm+o+Z6}KqzLw{(>E86uA1`_K$HqINlb1 zKelh3-jr2I9V?ych`{hta9wQ2c9=MM`2cC{m6^MhlL2{DLv7C^j z$xXBCnDl_;l|bPGMX@*tV)B!c|4oZyftUlP*?$YU9C_eAsuVHJ58?)zpbr30P*C`T z7y#ao`uE-SOG(Pi+`$=e^mle~)pRrdwL5)N;o{gpW21of(QE#U6w%*C~`v-z0QqBML!!5EeYA5IQB0 z^l01c;L6E(iytN!LhL}wfwP7W9PNAkb+)Cst?qg#$n;z41O4&v+8-zPs+XNb-q zIeeBCh#ivnFLUCwfS;p{LC0O7tm+Sf9Jn)~b%uwP{%69;QC)Ok0t%*a5M+=;y8j=v z#!*pp$9@!x;UMIs4~hP#pnfVc!%-D<+wsG@R2+J&%73lK|2G!EQC)O05TCV=&3g)C!lT=czLpZ@Sa%TYuoE?v8T8`V;e$#Zf2_Nj6nvBgh1)2 GZ~q4|mN%#X literal 0 HcmV?d00001 diff --git a/Test/gradle/wrapper/gradle-wrapper.properties b/Test/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..94113f2 --- /dev/null +++ b/Test/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/Test/gradlew b/Test/gradlew new file mode 100755 index 0000000..f5feea6 --- /dev/null +++ b/Test/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/Test/gradlew.bat b/Test/gradlew.bat new file mode 100644 index 0000000..9d21a21 --- /dev/null +++ b/Test/gradlew.bat @@ -0,0 +1,94 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/Test/settings.gradle b/Test/settings.gradle new file mode 100644 index 0000000..d967ff8 --- /dev/null +++ b/Test/settings.gradle @@ -0,0 +1,14 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * The settings file is used to specify which projects to include in your build. + * For more detailed information on multi-project builds, please refer to https://docs.gradle.org/8.11/userguide/multi_project_builds.html in the Gradle documentation. + */ + +plugins { + // Apply the foojay-resolver plugin to allow automatic download of JDKs + id 'org.gradle.toolchains.foojay-resolver-convention' version '0.8.0' +} + +rootProject.name = 'Test' +include('app') diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 552478d..525ce7a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import dev.doglog.DogLog; +import dev.doglog.DogLogOptions; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -26,6 +28,7 @@ public RobotContainer() { SmartDashboard.putData("Main-Controller Mode", main.selector); SmartDashboard.putData("Backup-Controller Mode", main.selector); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); // SmartDashboard.putData("Autonomous", ); // TBD } diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index f573422..fec5f46 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -6,6 +6,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -40,5 +41,6 @@ public boolean grabbed(){ @Override public void periodic() { SmartDashboard.putBoolean("Intake Full", grabbed()); + DogLog.log("Intake/Full", grabbed()); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 984e633..b895fc0 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -2,6 +2,9 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; + +import dev.doglog.DogLog; + import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkBase.ControlType; @@ -110,5 +113,11 @@ public void periodic() { SmartDashboard.putNumber("Elevator Speed", motor.getEncoder().getVelocity()); SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); + + DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); + DogLog.log("Elevator/Target Height", target); + DogLog.log("Elevator/cTarget Height", out.position); + DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); + DogLog.log("Elevator/cTarget Velocity", out.velocity); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 617a2d9..b9ad6d6 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.Pigeon2; + +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -219,5 +221,16 @@ public void periodic() { SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + + DogLog.log("Swerve/current_states", moduleStates(modules)); + DogLog.log("Swerve/target_states", states); + DogLog.log("Swerve/pose", pose); + DogLog.log("Swerve/X_position", pose.getX()); + DogLog.log("Swerve/Y_position", pose.getY()); + DogLog.log("Swerve/Odometry_rotation", rotation().getDegrees()); + DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); + DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); + DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); + DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } } \ No newline at end of file diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 0000000..b410eb1 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2025.3.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2025", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2025.3.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file From 52776ef76d80e5f37a7fa359b0ba95b812ff659b Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sun, 2 Feb 2025 14:10:23 -0500 Subject: [PATCH 12/79] added aimbot and camera offsets for pose estimation --- src/main/java/frc/robot/commands/aimbot.java | 43 +++++++++++ .../java/frc/robot/subsystems/Limelight.java | 15 ++-- src/main/java/frc/robot/utility/IO.java | 23 +++--- vendordeps/photonlib.json | 71 +++++++++++++++++++ 4 files changed, 130 insertions(+), 22 deletions(-) create mode 100644 src/main/java/frc/robot/commands/aimbot.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/commands/aimbot.java b/src/main/java/frc/robot/commands/aimbot.java new file mode 100644 index 0000000..03dfad3 --- /dev/null +++ b/src/main/java/frc/robot/commands/aimbot.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; + +/* 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 aimbot extends Command { + + IO io; + PIDController pid = new PIDController(0.00, 0.00, 0.00); + double tolerance = 5; + + /** Creates a new aimbot. */ + public aimbot(IO io) { + io.chassis.drive(new ChassisSpeeds(0, pid.calculate(io.limelight.x), 0)); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return Math.abs(pid.getError()) < tolerance; + } +} diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index bcd0545..fba6c95 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -12,7 +12,7 @@ public class Limelight extends SubsystemBase { double area; - double x; + public double x; double y; double robotyaw; double latency; @@ -20,15 +20,10 @@ public class Limelight extends SubsystemBase { /** Creates a new Limelight. */ public Limelight() { final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); - NetworkTableEntry tx = table.getEntry("tx"); - NetworkTableEntry ty = table.getEntry("ty"); - NetworkTableEntry ta = table.getEntry("ta"); - NetworkTableEntry tl = table.getEntry("tl"); - - x = tx.getDouble(0.0); - y= ty.getDouble(0.0); - area= ta.getDouble(0.0); - latency = tl.getDouble(0.0); + x = table.getEntry("tx").getDouble(0.0); + y = table.getEntry("ty").getDouble(0.0); + area = table.getEntry("ta").getDouble(0.0); + latency = table.getEntry("tl").getDouble(0.0); } @Override diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index c0bdb7d..59f665c 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -2,6 +2,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; @@ -18,6 +20,7 @@ public class IO extends SubsystemBase { public final Swerve chassis = new Swerve(); public final AlgaeIntake algaeIntake = new AlgaeIntake(); public final Elevator elevator = new Elevator(); + public final Limelight limelight = new Limelight(); public CommandScheduler scheduler = CommandScheduler.getInstance(); @@ -65,7 +68,14 @@ public void configTesting() { @Override public void periodic() { - + + LimelightHelpers.setCameraPose_RobotSpace("limelight", + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0); // field localization robotyaw = chassis.getYaw(); LimelightHelpers.SetRobotOrientation("", robotyaw, 0.0, 0.0, 0.0, 0.0, 0.0); @@ -75,16 +85,5 @@ public void periodic() { chassis.poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); chassis.poseEstimator.addVisionMeasurement(limelightMeasurement.pose, limelightMeasurement.timestampSeconds); - - /* LIMELIGHT OFFSET NEED TODO - LimelightHelpers.setCameraPose_RobotSpace("", - 0.5, // Forward offset (meters) - 0.0, // Side offset (meters) - 0.5, // Height offset (meters) - 0.0, // Roll (degrees) - 30.0, // Pitch (degrees) - 0.0 // Yaw (degrees) - ); - */ } } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file From d0d68138096aa8f5c448d719ad9e91a47b12b03b Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Sun, 2 Feb 2025 17:55:39 -0500 Subject: [PATCH 13/79] adjustments for canencoders --- .../frc/robot/modules/KrakenSwerveModule.java | 25 +++++++++++++------ .../java/frc/robot/subsystems/Swerve.java | 1 + 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/modules/KrakenSwerveModule.java b/src/main/java/frc/robot/modules/KrakenSwerveModule.java index 12c5f90..88e8590 100644 --- a/src/main/java/frc/robot/modules/KrakenSwerveModule.java +++ b/src/main/java/frc/robot/modules/KrakenSwerveModule.java @@ -9,19 +9,26 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import static edu.wpi.first.units.Units.Radians; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; public class KrakenSwerveModule { public final TalonFX driveMotor; public final SparkMax steerMotor; - public final Canandmag steerEncoder; + public final CANcoder steerEncoder; double desiredAngle; @@ -35,7 +42,7 @@ public class KrakenSwerveModule { public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerCANID) { driveMotor = new TalonFX(driveID, "rio"); steerMotor = new SparkMax(steerID, MotorType.kBrushless); - steerEncoder = new Canandmag(steerCANID); + steerEncoder = new CANcoder(steerCANID); SparkMaxConfig steerConfig = new SparkMaxConfig(); @@ -57,11 +64,14 @@ public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int steerConfig.signals.primaryEncoderPositionAlwaysOn(false); steerConfig.signals.primaryEncoderPositionPeriodMs(20); - CanandmagSettings settings = new CanandmagSettings(); - settings.setInvertDirection(true); + // CanandmagSettings settings = new CanandmagSettings(); + // settings.setInvertDirection(true); + + MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs(); + magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; steerEncoder.clearStickyFaults(); - steerEncoder.setSettings(settings); + steerEncoder.getConfigurator().apply(magnetConfig); TalonFXConfiguration config = new TalonFXConfiguration(); @@ -96,7 +106,8 @@ public void syncSteerEncoders() { } public void resetAbsolute() { - steerEncoder.setAbsPosition(0, 250); + // steerEncoder.setAbsPosition(0, 250); + steerEncoder.setPosition(0); } public double drivePosition() { @@ -108,7 +119,7 @@ public double velocity() { } public double angle() { - return (steerEncoder.getAbsPosition() * PI2) % PI2; + return steerEncoder.getAbsolutePosition().getValue().in(Radians); } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index a4b64c0..58175a5 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Timer; From f2e4e6d5a13520a3eda17047909ff6b3f01b4014 Mon Sep 17 00:00:00 2001 From: Red Date: Mon, 3 Feb 2025 07:51:18 +0000 Subject: [PATCH 14/79] Should allow us to configure most things ahead of time --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/modules/KrakenSwerveModule.java | 8 +- .../java/frc/robot/subsystems/Swerve.java | 44 +++---- src/main/java/frc/robot/swerve/Module.java | 116 ++++++++++++++++++ src/main/java/frc/robot/swerve/Swerve.java | 94 ++++++++++++++ .../robot/utility/AutomatedController.java | 4 +- .../frc/robot/utility/SwerveConstants.java | 20 --- 7 files changed, 240 insertions(+), 47 deletions(-) create mode 100644 src/main/java/frc/robot/swerve/Module.java create mode 100644 src/main/java/frc/robot/swerve/Swerve.java delete mode 100644 src/main/java/frc/robot/utility/SwerveConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9550c5..c7294b6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,6 @@ import frc.robot.commands.DefaultDrive; import frc.robot.utility.AutomatedController; import frc.robot.utility.IO; -import frc.robot.utility.SwerveConstants; public class RobotContainer { public IO io = new IO(); diff --git a/src/main/java/frc/robot/modules/KrakenSwerveModule.java b/src/main/java/frc/robot/modules/KrakenSwerveModule.java index 88e8590..1fa5b3f 100644 --- a/src/main/java/frc/robot/modules/KrakenSwerveModule.java +++ b/src/main/java/frc/robot/modules/KrakenSwerveModule.java @@ -61,8 +61,8 @@ public KrakenSwerveModule(ShuffleboardLayout tab, int driveID, int steerID, int // .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pid(0.2, 0.0, 0.0); - steerConfig.signals.primaryEncoderPositionAlwaysOn(false); - steerConfig.signals.primaryEncoderPositionPeriodMs(20); + steerConfig.signals.primaryEncoderPositionAlwaysOn(false); + steerConfig.signals.primaryEncoderPositionPeriodMs(20); // CanandmagSettings settings = new CanandmagSettings(); // settings.setInvertDirection(true); @@ -105,8 +105,10 @@ public void syncSteerEncoders() { steerMotor.getEncoder().setPosition(angle()); } - public void resetAbsolute() { + public void zeroAbsolute() { // steerEncoder.setAbsPosition(0, 250); + + steerEncoder.getConfigurator().apply() steerEncoder.setPosition(0); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 58175a5..465b0de 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -14,7 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.modules.KrakenSwerveModule; -import frc.robot.utility.SwerveConstants; +import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -27,17 +27,9 @@ public class Swerve extends SubsystemBase { public boolean field_oritented = true; public boolean slow_mode = false; - private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( - new Translation2d(SwerveConstants.TRACKWIDTH / 2.0, - SwerveConstants.WHEELBASE / 2.0), - new Translation2d(SwerveConstants.TRACKWIDTH / 2.0, - -SwerveConstants.WHEELBASE / 2.0), - new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0, - SwerveConstants.WHEELBASE / 2.0), - new Translation2d(-SwerveConstants.TRACKWIDTH / 2.0, - -SwerveConstants.WHEELBASE / 2.0)); - - public final Pigeon2 pigeon2 = new Pigeon2(SwerveConstants.PIGEON_ID); + private final SwerveDriveKinematics kinematics; + + public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID); public final Timer syncTimer = new Timer(); StructArrayPublisher current_states = Util.table @@ -50,20 +42,30 @@ public class Swerve extends SubsystemBase { final SwerveDriveOdometry odometry; final KrakenSwerveModule[] modules = new KrakenSwerveModule[4]; ChassisSpeeds speeds = new ChassisSpeeds(); + Constants constants = new Constants(); public boolean active = true; public Swerve() { - new SwerveConstants(); + kinematics = new SwerveDriveKinematics( + new Translation2d(constants.TRACKWIDTH / 2.0, + constants.WHEELBASE / 2.0), + new Translation2d(constants.TRACKWIDTH / 2.0, + -constants.WHEELBASE / 2.0), + new Translation2d(-constants.TRACKWIDTH / 2.0, + constants.WHEELBASE / 2.0), + new Translation2d(-constants.TRACKWIDTH / 2.0, + -constants.WHEELBASE / 2.0)); + ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { modules[i] = new KrakenSwerveModule( - tab.getLayout(SwerveConstants.LAYOUT_TITLE[i], BuiltInLayouts.kList) + tab.getLayout(Constants.LAYOUT_TITLE[i], BuiltInLayouts.kList) .withSize(2, 4) .withPosition(i * 2, 0), - SwerveConstants.CHASSIS_ID[i], - SwerveConstants.CHASSIS_ID[i], - SwerveConstants.ENCODER_ID[i]); + Constants.CHASSIS_ID[i], + Constants.CHASSIS_ID[i], + Constants.ENCODER_ID[i]); } odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), @@ -163,9 +165,9 @@ public void syncEncoders() { mod.syncSteerEncoders(); } - public void resetAbsolute() { + public void zeroAbsolute() { for (KrakenSwerveModule mod : modules) - mod.resetAbsolute(); + mod.zeroAbsolute(); } public void resetSteerPositions() { @@ -174,11 +176,11 @@ public void resetSteerPositions() { } public void setModuleStates(SwerveModuleState[] states) { - SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.MAX_VELOCITY); + SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.MAX_VELOCITY); for (int i = 0; i < modules.length; i++) { states[i].optimize(new Rotation2d(modules[i].angle())); // states[i].cosineScale(new Rotation2d(modules[i].angle())); // Cosine Compensation - modules[i].set((states[i].speedMetersPerSecond / SwerveConstants.MAX_VELOCITY) * .8 , states[i].angle.getRadians()); + modules[i].set((states[i].speedMetersPerSecond / Constants.MAX_VELOCITY) * .8 , states[i].angle.getRadians()); } } diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java new file mode 100644 index 0000000..6079c1f --- /dev/null +++ b/src/main/java/frc/robot/swerve/Module.java @@ -0,0 +1,116 @@ +package frc.robot.swerve; + +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class Module { + public final TalonFX drive; + public final SparkMax steer; + public final Swerve.Encoder encoder; + + double desiredAngle; + + public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); + public static final double DRIVE_REDUCTION = (15.0 / 32.0) * (10.0 / 60.0); + public static final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0); + public static final double DRIVE_CONVERSION_FACTOR = Math.PI * WHEEL_DIAMETER * DRIVE_REDUCTION; + + public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, boolean comp) { + drive = new TalonFX(driveID, "rio"); + steer = new SparkMax(steerID, MotorType.kBrushless); + encoder = (comp) ? new Swerve.Cancoder(encoderID) : new Swerve.Canand(encoderID); + + SparkMaxConfig steerConfig = new SparkMaxConfig(); + + steerConfig + .smartCurrentLimit(20) + .idleMode(IdleMode.kCoast) + .inverted(false); + + steerConfig.encoder + .positionConversionFactor(2 * Math.PI * STEER_REDUCTION) + .velocityConversionFactor(Math.PI * STEER_REDUCTION / 60); + + steerConfig.closedLoop + .positionWrappingEnabled(true) + .positionWrappingMaxInput(Swerve.PI2) + // .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(0.2, 0.0, 0.0); + + steerConfig.signals.primaryEncoderPositionAlwaysOn(false); + steerConfig.signals.primaryEncoderPositionPeriodMs(20); + + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.StatorCurrentLimit = 80; + config.CurrentLimits.StatorCurrentLimitEnable = true; + + config.CurrentLimits.SupplyCurrentLimit = 20; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + steer.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + steer.getEncoder().setPosition(angle()); + + drive.getConfigurator().apply(config); + + tab.addDouble("Absolute Angle", () -> Math.toDegrees(angle())); + // tab.addDouble("Current Angle", () -> Math.toDegrees(steer.getEncoder().getPosition())); + tab.addDouble("Angle Difference", () -> Math.toDegrees(angle() - steer.getEncoder().getPosition())); + tab.addDouble("Target Angle", () -> Math.toDegrees(desiredAngle)); + tab.addBoolean("Active", encoder::connected); + } + + public void resetDrivePosition() { + drive.setPosition(0.0); + } + + public void syncencoders() { + steer.getEncoder().setPosition(encoder.angle()); + } + + public void zeroAbsolute() { + encoder.zero(); + } + + public double drivePosition() { + return drive.getPosition().getValueAsDouble() * .502 * WHEEL_DIAMETER; + } + + public double velocity() { + return drive.getRotorVelocity().getValueAsDouble() * Swerve.PI2 * .502 * Units.inchesToMeters(3); + } + + public double angle() { + return encoder.angle(); + } + + public void stop() { + drive.stopMotor(); + steer.stopMotor(); + } + + public void set(double driveVolts, double targetAngle) { + syncencoders(); + + drive.set(driveVolts); + steer.getClosedLoopController().setReference(targetAngle, ControlType.kPosition); + } + +} diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java new file mode 100644 index 0000000..12f435e --- /dev/null +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -0,0 +1,94 @@ +package frc.robot.swerve; + +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.reduxrobotics.sensors.canandmag.Canandmag; +import com.reduxrobotics.sensors.canandmag.CanandmagSettings; +import static edu.wpi.first.units.Units.Radians; + +public class Swerve { + + public static final double PI2 = 2.0 * Math.PI; + + public interface Encoder { + public void zero(); + public boolean connected(); + public double angle(); + } + + public static class Constants { + boolean compChassis = false; + + public double TRACKWIDTH = 19.5; // 30.0 for MKi + public double WHEELBASE = 21.5; // 30.0 for MKi + public double GEAR_RATIO; + public static final double MAX_VELOCITY = 5.4; + public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; + public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR + public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR + public static final int PIGEON_ID = 6; + + public Constants(){ + // compChassis = (System.getenv("BLUEBOT") == null); // Determine if we're comp based on an enviornmental variable like in 2023 + if (compChassis) { + TRACKWIDTH = 30.0; + WHEELBASE = 30.0; + GEAR_RATIO = 8.14; // L1 + } else { + TRACKWIDTH = 19.5; + WHEELBASE = 21.5; + GEAR_RATIO = 6.12; // L3 + } + } + } + + public static class Cancoder implements Encoder { + + CANcoder encoder; + MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs(); + + public Cancoder(int id) { + encoder = new CANcoder(id); + magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + encoder.getConfigurator().apply(magnetConfig); + } + + public void zero() { + magnetConfig.MagnetOffset = -encoder.getAbsolutePosition().getValueAsDouble(); + encoder.getConfigurator().apply(magnetConfig); + } + + public boolean connected(){ + return encoder.isConnected(); + } + + public double angle() { + return ((encoder.getAbsolutePosition().getValue().in(Radians) + PI2) % PI2); + } + } + + public static class Canand implements Encoder { + Canandmag encoder; + + public Canand(int id) { + encoder = new Canandmag(id); + CanandmagSettings settings = new CanandmagSettings(); + settings.setInvertDirection(true); + encoder.clearStickyFaults(); + Encoder.setSettings(settings); + } + + public void zero() { + encoder.setAbsPosition(0, 250); + } + + public boolean connected(){ + return encoder.isConnected(); + } + + public double angle() { + return (encoder.getAbsPosition() * PI2) % PI2; + } + } +} diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 0805aef..9bf9eed 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -103,8 +103,8 @@ public void configure(){ // controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::resetAbsolute)); // Add the Rumble effect + // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::resetAbsolute)); // Add the Rumble effect + // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } } diff --git a/src/main/java/frc/robot/utility/SwerveConstants.java b/src/main/java/frc/robot/utility/SwerveConstants.java deleted file mode 100644 index fafa34e..0000000 --- a/src/main/java/frc/robot/utility/SwerveConstants.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.utility; - -public class SwerveConstants { - boolean compChassis = false; - - public static double TRACKWIDTH = 19.5; // 30.0 for MKi - public static double WHEELBASE = 21.5; // 30.0 for MKi - - public static double GEAR_RATIO; - - public static final double MAX_VELOCITY = 5.4; - - public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; - - public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR - public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR - - public static final int PIGEON_ID = 6; - -} \ No newline at end of file From 86ee60cca281a072ba2dcb205e8dbf544e0eb19c Mon Sep 17 00:00:00 2001 From: Red Date: Mon, 3 Feb 2025 08:00:35 +0000 Subject: [PATCH 15/79] Should be good now --- .../java/frc/robot/commands/DefaultDrive.java | 16 +++++++--------- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- src/main/java/frc/robot/swerve/Swerve.java | 12 ++++++++++++ 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index b631bc0..01015da 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -3,9 +3,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.utility.Util; +import frc.robot.swerve.Swerve; import frc.robot.utility.IO; -import frc.robot.utility.SwerveConstants; import java.util.function.DoubleSupplier; @@ -22,9 +21,9 @@ public DefaultDrive(IO io, ChassisSpeeds chassisSpeeds) { } public DefaultDrive(IO io, CommandXboxController controller) { - this(io, () -> modifyAxis(controller.getLeftY()) * SwerveConstants.MAX_VELOCITY, - () -> modifyAxis(controller.getLeftX()) * SwerveConstants.MAX_VELOCITY, - () -> modifyAxis(controller.getRightX()) * SwerveConstants.MAX_VELOCITY); + this(io, () -> modifyAxis(controller.getLeftY()) * Swerve.Constants.MAX_VELOCITY, + () -> modifyAxis(controller.getLeftX()) * Swerve.Constants.MAX_VELOCITY, + () -> modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_VELOCITY); this.controller = controller; } @@ -44,16 +43,15 @@ public DefaultDrive(IO io, @Override public void execute() { double down_scale = 1.25 - modifyAxis(controller.getLeftTriggerAxis()); - double up_scale = (SwerveConstants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis()); + double up_scale = (Swerve.Constants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis()); - double scale = 0.8 * down_scale + up_scale; - double rot_scale = .48 * down_scale + up_scale; //0.48 for Shaan. 0.6 for Tristan. + double scale = io.chassis.constants.transFactor * down_scale + up_scale; + double rot_scale = io.chassis.constants.rotFactor * down_scale + up_scale; double xSpeed = x_supplier.getAsDouble() * scale; double ySpeed = y_supplier.getAsDouble() * scale; double rotationSpeed = rotation_supplier.getAsDouble() * down_scale * rot_scale; - ChassisSpeeds output = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); if (io.chassis.field_oritented) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 465b0de..7c442b4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -42,7 +42,7 @@ public class Swerve extends SubsystemBase { final SwerveDriveOdometry odometry; final KrakenSwerveModule[] modules = new KrakenSwerveModule[4]; ChassisSpeeds speeds = new ChassisSpeeds(); - Constants constants = new Constants(); + public final Constants constants = new Constants(); public boolean active = true; diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 12f435e..7ab5320 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -23,6 +23,11 @@ public static class Constants { public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi public double GEAR_RATIO; + + public int driver = 0; + public double transFactor = .8; // factor = x/125, with x being the percentage of our max speed, same for the thing below + public double rotFactor = .48; // .6 for tristan + public static final double MAX_VELOCITY = 5.4; public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR @@ -40,6 +45,13 @@ public Constants(){ WHEELBASE = 21.5; GEAR_RATIO = 6.12; // L3 } + + switch (driver) { + default: // Shaan + transFactor = .8; + rotFactor = .48; + break; + } } } From c191703839ee10c390d8595676cedcbcacb7c3cc Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Mon, 3 Feb 2025 16:18:26 -0500 Subject: [PATCH 16/79] Configurations fully working --- .../frc/robot/modules/KrakenSwerveModule.java | 7 ----- .../java/frc/robot/subsystems/Swerve.java | 27 ++++++++++--------- src/main/java/frc/robot/swerve/Module.java | 12 ++++----- src/main/java/frc/robot/swerve/Swerve.java | 21 ++++++++------- 4 files changed, 32 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/modules/KrakenSwerveModule.java b/src/main/java/frc/robot/modules/KrakenSwerveModule.java index 1fa5b3f..516ccd9 100644 --- a/src/main/java/frc/robot/modules/KrakenSwerveModule.java +++ b/src/main/java/frc/robot/modules/KrakenSwerveModule.java @@ -9,12 +9,10 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.AngleUnit; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import static edu.wpi.first.units.Units.Radians; -import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; @@ -22,8 +20,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.reduxrobotics.sensors.canandmag.Canandmag; -import com.reduxrobotics.sensors.canandmag.CanandmagSettings; public class KrakenSwerveModule { public final TalonFX driveMotor; @@ -107,9 +103,6 @@ public void syncSteerEncoders() { public void zeroAbsolute() { // steerEncoder.setAbsPosition(0, 250); - - steerEncoder.getConfigurator().apply() - steerEncoder.setPosition(0); } public double drivePosition() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 7c442b4..1b99df1 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -13,7 +13,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.modules.KrakenSwerveModule; +import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -40,7 +40,7 @@ public class Swerve extends SubsystemBase { .getStructTopic("Current pose", Pose2d.struct).publish(); final SwerveDriveOdometry odometry; - final KrakenSwerveModule[] modules = new KrakenSwerveModule[4]; + final Module[] modules = new Module[4]; ChassisSpeeds speeds = new ChassisSpeeds(); public final Constants constants = new Constants(); @@ -59,13 +59,14 @@ public Swerve() { ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { - modules[i] = new KrakenSwerveModule( + modules[i] = new Module( tab.getLayout(Constants.LAYOUT_TITLE[i], BuiltInLayouts.kList) .withSize(2, 4) .withPosition(i * 2, 0), Constants.CHASSIS_ID[i], Constants.CHASSIS_ID[i], - Constants.ENCODER_ID[i]); + Constants.ENCODER_ID[i], + constants.comp); } odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), @@ -103,11 +104,11 @@ public double distance(double[] reference_point) { return distance(new Pose2d(reference_point[0], reference_point[2], new Rotation2d(reference_point[3]))); } - private SwerveModulePosition modulePosition(KrakenSwerveModule module) { + private SwerveModulePosition modulePosition(Module module) { return new SwerveModulePosition(module.drivePosition(), Rotation2d.fromRadians(module.angle())); } - private SwerveModuleState moduleState(KrakenSwerveModule module) { + private SwerveModuleState moduleState(Module module) { return new SwerveModuleState(module.velocity(), new Rotation2d(module.angle())); } @@ -118,7 +119,7 @@ public SwerveModulePosition[] modulePositions() { return pos; } - public SwerveModuleState[] moduleStates(KrakenSwerveModule[] modules) { + public SwerveModuleState[] moduleStates(Module[] modules) { SwerveModuleState[] state = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) state[i] = moduleState(modules[i]); @@ -156,22 +157,22 @@ public void setOdometry(Pose2d pose) { } public void resetPosition() { - for (KrakenSwerveModule mod : modules) + for (Module mod : modules) mod.resetDrivePosition(); } public void syncEncoders() { - for (KrakenSwerveModule mod : modules) - mod.syncSteerEncoders(); + for (Module mod : modules) + mod.syncEncoders(); } public void zeroAbsolute() { - for (KrakenSwerveModule mod : modules) + for (Module mod : modules) mod.zeroAbsolute(); } public void resetSteerPositions() { - for (KrakenSwerveModule mod : modules) + for (Module mod : modules) mod.set(0, 0); } @@ -199,7 +200,7 @@ public void enable() { public void disable() { active = false; - for (KrakenSwerveModule mod : modules) + for (Module mod : modules) mod.stop(); } diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 6079c1f..49cded1 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -37,11 +37,11 @@ public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, b steerConfig .smartCurrentLimit(20) - .idleMode(IdleMode.kCoast) - .inverted(false); + .idleMode(IdleMode.kBrake) + .inverted(true); steerConfig.encoder - .positionConversionFactor(2 * Math.PI * STEER_REDUCTION) + .positionConversionFactor(Math.PI * STEER_REDUCTION) .velocityConversionFactor(Math.PI * STEER_REDUCTION / 60); steerConfig.closedLoop @@ -71,7 +71,7 @@ public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, b drive.getConfigurator().apply(config); tab.addDouble("Absolute Angle", () -> Math.toDegrees(angle())); - // tab.addDouble("Current Angle", () -> Math.toDegrees(steer.getEncoder().getPosition())); + tab.addDouble("Current Angle", () -> Math.toDegrees(steer.getEncoder().getPosition())); tab.addDouble("Angle Difference", () -> Math.toDegrees(angle() - steer.getEncoder().getPosition())); tab.addDouble("Target Angle", () -> Math.toDegrees(desiredAngle)); tab.addBoolean("Active", encoder::connected); @@ -81,7 +81,7 @@ public void resetDrivePosition() { drive.setPosition(0.0); } - public void syncencoders() { + public void syncEncoders() { steer.getEncoder().setPosition(encoder.angle()); } @@ -107,7 +107,7 @@ public void stop() { } public void set(double driveVolts, double targetAngle) { - syncencoders(); + syncEncoders(); drive.set(driveVolts); steer.getClosedLoopController().setReference(targetAngle, ControlType.kPosition); diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 7ab5320..083566b 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -11,14 +11,8 @@ public class Swerve { public static final double PI2 = 2.0 * Math.PI; - public interface Encoder { - public void zero(); - public boolean connected(); - public double angle(); - } - public static class Constants { - boolean compChassis = false; + public boolean comp = true; public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi @@ -32,11 +26,12 @@ public static class Constants { public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR + public static double[] ENCODER_OFFSETS = {-0.8823, -0.8371, -0.6311, -0.7314}; public static final int PIGEON_ID = 6; public Constants(){ // compChassis = (System.getenv("BLUEBOT") == null); // Determine if we're comp based on an enviornmental variable like in 2023 - if (compChassis) { + if (comp) { TRACKWIDTH = 30.0; WHEELBASE = 30.0; GEAR_RATIO = 8.14; // L1 @@ -55,6 +50,12 @@ public Constants(){ } } + public interface Encoder { + public void zero(); + public boolean connected(); + public double angle(); + } + public static class Cancoder implements Encoder { CANcoder encoder; @@ -63,6 +64,8 @@ public static class Cancoder implements Encoder { public Cancoder(int id) { encoder = new CANcoder(id); magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + magnetConfig.AbsoluteSensorDiscontinuityPoint = 1.0; + magnetConfig.withMagnetOffset(Swerve.Constants.ENCODER_OFFSETS[id-7]); encoder.getConfigurator().apply(magnetConfig); } @@ -88,7 +91,7 @@ public Canand(int id) { CanandmagSettings settings = new CanandmagSettings(); settings.setInvertDirection(true); encoder.clearStickyFaults(); - Encoder.setSettings(settings); + encoder.setSettings(settings); } public void zero() { From 795728d33a8e065b61c986cc35e5d28d913050a3 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:30:33 -0500 Subject: [PATCH 17/79] profile switching working --- src/main/java/frc/robot/subsystems/Hang.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 20 ++++----- src/main/java/frc/robot/swerve/Module.java | 2 +- src/main/java/frc/robot/swerve/Swerve.java | 2 +- .../robot/utility/AutomatedController.java | 44 +------------------ src/main/java/frc/robot/utility/IO.java | 12 +++-- 6 files changed, 23 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 728aae5..ea06033 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { - public TalonFX hang = new TalonFX(0, "rio"); + public TalonFX hang = new TalonFX(10, "rio"); public static final double HANG_MAX_ANGLE = 0; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6b1301e..b373755 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -211,22 +211,22 @@ public void periodic() { if (active && speeds != new ChassisSpeeds()) setModuleStates(states); - current_states.set(moduleStates(modules)); - target_states.set(states); + // current_states.set(moduleStates(modules)); + // target_states.set(states); Pose2d pose = odometry.update(rotation(), modulePositions()); - posePublisher.set(pose); + // posePublisher.set(pose); - SmartDashboard.putNumber("X position", pose.getX()); - SmartDashboard.putNumber("Y position", pose.getY()); + // SmartDashboard.putNumber("X position", pose.getX()); + // SmartDashboard.putNumber("Y position", pose.getY()); - SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); - SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); - SmartDashboard.putNumber("Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); - SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); + // SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); + // SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); + // SmartDashboard.putNumber("Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); + // SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); - SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); DogLog.log("Swerve/current_states", moduleStates(modules)); DogLog.log("Swerve/target_states", states); diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 49cded1..f45e8ec 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -38,7 +38,7 @@ public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, b steerConfig .smartCurrentLimit(20) .idleMode(IdleMode.kBrake) - .inverted(true); + .inverted(comp); steerConfig.encoder .positionConversionFactor(Math.PI * STEER_REDUCTION) diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 083566b..60297cb 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -12,7 +12,7 @@ public class Swerve { public static final double PI2 = 2.0 * Math.PI; public static class Constants { - public boolean comp = true; + public boolean comp = false; public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 9bf9eed..f7efd51 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -57,49 +57,7 @@ public void configure(){ // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - controller.y().and( automated() ).onTrue(Util.Do(io.elevator::L4)); - controller.b().and( automated() ).onTrue(Util.Do(io.elevator::L3)); - controller.a().and( automated() ).onTrue(Util.Do(io.elevator::L2)); - controller.x().and( automated() ) - .onTrue(Util.Do( - new ReleaseAlgae(io, false), - new GrabAlgae(io), - io.intake::grabbed)); - - controller.povUp().and( automated() ) - .onTrue(Util.Do(() -> io.elevator.speed(0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - - controller.povDown().and( automated() ) - .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - - controller.povLeft().and( automated() ) - .onTrue(Util.Do(() -> io.intake.speed(-0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); - - controller.povRight().and( automated() ) - .onTrue(Util.Do(() -> io.intake.speed(0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); - - // MANUAL - - controller.rightTrigger().and( manual() ) - .onTrue(Util.Do(() -> io.elevator.speed(0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - - controller.leftTrigger().and( manual() ) - .onTrue(Util.Do(() -> io.elevator.speed(-0.25))) - .onFalse(Util.Do(() -> io.elevator.speed(0.0))); - - controller.leftBumper().and( manual() ) - .onTrue(Util.Do(() -> io.intake.speed(-0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); - - controller.rightBumper().and( manual() ) - .onTrue(Util.Do(() -> io.intake.speed(0.5))) - .onFalse(Util.Do(() -> io.intake.speed(0.0))); - + // controller.y().and( automated() ).onTrue(Util.D // controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index 29f1c26..bbfb92d 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -6,9 +6,15 @@ public class IO extends SubsystemBase { public final Swerve chassis = new Swerve(); - public final AlgaeIntake intake = new AlgaeIntake(); - public final Elevator elevator = new Elevator(); - public final Hang hang = new Hang(); + // public final AlgaeIntake intake = new AlgaeIntake(); + // public final Elevator elevator = new Elevator(); + // public final Hang hang = new Hang(); + + public final AlgaeIntake intake = null; + public final Elevator elevator = null; + public final Hang hang = null; + + public CommandScheduler scheduler = CommandScheduler.getInstance(); From a1b4c077cd0f33dc19951f8f901a29ad1e268547 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Wed, 5 Feb 2025 16:01:45 -0500 Subject: [PATCH 18/79] Running paths --- .../pathplanner/autos/Straight Test.auto | 19 +++++++ .../paths/Straight And Rotate.path | 54 +++++++++++++++++++ .../pathplanner/paths/Straight Test.path | 54 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/subsystems/Swerve.java | 37 +++++++++---- src/main/java/frc/robot/swerve/Swerve.java | 27 +++++++++- 6 files changed, 182 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Straight Test.auto create mode 100644 src/main/deploy/pathplanner/paths/Straight And Rotate.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Test.path diff --git a/src/main/deploy/pathplanner/autos/Straight Test.auto b/src/main/deploy/pathplanner/autos/Straight Test.auto new file mode 100644 index 0000000..70b6480 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Straight Test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path new file mode 100644 index 0000000..b4bdd4c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1720034246575342, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.172003424657536, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.482791095890411, + "y": 6.0 + }, + "prevControl": { + "x": 6.482791095890411, + "y": 6.0 + }, + "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": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path new file mode 100644 index 0000000..30f11b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1720034246575342, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.172003424657536, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.482791095890411, + "y": 6.0 + }, + "prevControl": { + "x": 6.482791095890411, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 584b1c5..70c56ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ 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.PrintCommand; import dev.doglog.DogLog; import dev.doglog.DogLogOptions; import frc.robot.commands.DefaultDrive; @@ -24,7 +25,7 @@ public class RobotContainer { private final SendableChooser auto_selector; - private Command current_auto; + private Command current_auto = new PrintCommand("Temp"); public RobotContainer() { main = new AutomatedController(0, io); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index b2295c7..99c8632 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,28 +1,31 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; 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.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; - -import edu.wpi.first.wpilibj.Timer; public class Swerve extends SubsystemBase { @@ -31,7 +34,6 @@ public class Swerve extends SubsystemBase { private final SwerveDriveKinematics kinematics; public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID); - public final Timer syncTimer = new Timer(); StructArrayPublisher current_states = Util.table .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish(); @@ -70,9 +72,24 @@ public Swerve() { constants.comp); } + AutoBuilder.configure( + this::pose, + this::resetOdometry, + () -> speeds, + (speeds, feedforwards) -> drive(speeds), + new PPHolonomicDriveController( + new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID constants + new PIDConstants(constants.ThetaControllerP, 0, constants.XControllerD, 0.0) // Rotation PID constants + ), + constants.autoConfig, + () -> { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; + }, + this); + odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), new Pose2d(0, 0, new Rotation2d())); - syncTimer.start(); } public void zeroGyro() { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 60297cb..e534268 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -3,25 +3,42 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.path.PathPlannerPath; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; import static edu.wpi.first.units.Units.Radians; +import java.io.IOException; + +import org.json.simple.parser.ParseException; + public class Swerve { public static final double PI2 = 2.0 * Math.PI; public static class Constants { - public boolean comp = false; + // BOT SWITCHING + public boolean comp = true; public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi public double GEAR_RATIO; + // DRIVER SETTINGS public int driver = 0; public double transFactor = .8; // factor = x/125, with x being the percentage of our max speed, same for the thing below public double rotFactor = .48; // .6 for tristan + // AUTON CONSTANTS + public double XControllerP = 0.0; + public double XControllerD = 0.0; + public double ThetaControllerP = 0.0; + public double ThetaControllerD = 0.0; + public RobotConfig autoConfig; + + // BASE CHASSIS CONFIGURATION public static final double MAX_VELOCITY = 5.4; public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR @@ -47,6 +64,14 @@ public Constants(){ rotFactor = .48; break; } + + try { + autoConfig = RobotConfig.fromGUISettings(); + } catch (IOException e) { + e.printStackTrace(); + } catch (ParseException e) { + e.printStackTrace(); + } } } From dbbcc872c4ff9fe46b969ee2c83d60fc20adb426 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Wed, 5 Feb 2025 16:03:10 -0500 Subject: [PATCH 19/79] Push --- src/main/deploy/pathplanner/paths/Straight Test.path | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index 30f11b9..0067703 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.1720034246575342, + "x": 1.0, "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 2.172003424657536, + "x": 2.0000000000000018, "y": 6.0 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.482791095890411, + "x": 3.0, "y": 6.0 }, "prevControl": { - "x": 6.482791095890411, + "x": 2.0, "y": 6.0 }, "nextControl": null, From f8398a4af74f3f2a246e9d7e19a066a95f0a40e7 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Thu, 6 Feb 2025 15:24:00 -0500 Subject: [PATCH 20/79] Updabte --- .../deploy/pathplanner/autos/New Auto.auto | 19 ++++ .../paths/J,I to Station and back.path | 4 +- .../deploy/pathplanner/paths/Triangle.path | 91 +++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 2 +- src/main/java/frc/robot/swerve/Module.java | 2 +- src/main/java/frc/robot/swerve/Swerve.java | 8 +- 6 files changed, 118 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Triangle.path diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..63af25f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Triangle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J,I to Station and back.path b/src/main/deploy/pathplanner/paths/J,I to Station and back.path index 6861d61..bb5f03f 100644 --- a/src/main/deploy/pathplanner/paths/J,I to Station and back.path +++ b/src/main/deploy/pathplanner/paths/J,I to Station and back.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 1.231399772034817, - "y": 7.532716062218043 + "y": 7.532716062218042 }, "isLocked": false, "linkedName": null @@ -36,7 +36,7 @@ "y": 5.207857863967483 }, "prevControl": { - "x": 1.7386036147673978, + "x": 1.7386036147673982, "y": 7.246865032045945 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path new file mode 100644 index 0000000..97a2d4d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Triangle.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.138574759002216, + "y": 7.0 + }, + "prevControl": { + "x": 4.138574759002216, + "y": 7.0 + }, + "nextControl": { + "x": 6.138574759002216, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 4.955063886077153 + }, + "prevControl": { + "x": 2.154192543186652, + "y": 4.703218396657915 + }, + "nextControl": { + "x": 1.8694606562423997, + "y": 5.1682761740235446 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.0255228704471047, + "y": 6.423228095526727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 99c8632..f7f9fbb 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -79,7 +79,7 @@ public Swerve() { (speeds, feedforwards) -> drive(speeds), new PPHolonomicDriveController( new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID constants - new PIDConstants(constants.ThetaControllerP, 0, constants.XControllerD, 0.0) // Rotation PID constants + new PIDConstants(constants.ThetaControllerP, 0, constants.ThetaControllerD, 0.0) // Rotation PID constants ), constants.autoConfig, () -> { diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index f45e8ec..1d866d4 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -90,7 +90,7 @@ public void zeroAbsolute() { } public double drivePosition() { - return drive.getPosition().getValueAsDouble() * .502 * WHEEL_DIAMETER; + return drive.getPosition().getValueAsDouble() * .432 * WHEEL_DIAMETER; } public double velocity() { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index e534268..71813ce 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -20,7 +20,7 @@ public class Swerve { public static class Constants { // BOT SWITCHING - public boolean comp = true; + public boolean comp = false; public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi @@ -32,10 +32,10 @@ public static class Constants { public double rotFactor = .48; // .6 for tristan // AUTON CONSTANTS - public double XControllerP = 0.0; + public double XControllerP = 0.1; public double XControllerD = 0.0; - public double ThetaControllerP = 0.0; - public double ThetaControllerD = 0.0; + public double ThetaControllerP = 0.05; + public double ThetaControllerD = 0.005; public RobotConfig autoConfig; // BASE CHASSIS CONFIGURATION From 47b43214dafeca7094504102117ebe54b6844d1a Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 8 Feb 2025 12:20:27 -0500 Subject: [PATCH 21/79] added function to check location in the field with the limelight --- .../java/frc/robot/subsystems/Limelight.java | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index fba6c95..e81845a 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -16,7 +16,9 @@ public class Limelight extends SubsystemBase { double y; double robotyaw; double latency; - + int tagNumber; + + int[] tagLocation; /** Creates a new Limelight. */ public Limelight() { final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); @@ -24,6 +26,7 @@ public Limelight() { y = table.getEntry("ty").getDouble(0.0); area = table.getEntry("ta").getDouble(0.0); latency = table.getEntry("tl").getDouble(0.0); + tagNumber = (int) table.getEntry("tid").getInteger(0); } @Override @@ -34,4 +37,40 @@ public void periodic() { SmartDashboard.putNumber("LimelightArea", area); SmartDashboard.putNumber("Limelight Latency", latency); } + + public int[] TagLocationProcessing() { + /*{red/blue, zone} + { 1/2, 1-3} + zone 1 = coral + zone 2 = coral station + zone 3 = processor + + ** if returns 0 and 0 there is no tag in view ** + */ + // return red or blue + if (tagNumber > 0){ + if (tagNumber <= 11){ + tagLocation[0] = 1; + } + else { + tagLocation[0] = 2; + } + + // return zone + if (tagNumber >=6 && tagNumber <= 11 || tagNumber >= 17 && tagNumber <= 22){ + tagLocation[1] = 1; + } + else if (tagNumber >=1 && tagNumber <= 2 || tagNumber >= 12 && tagNumber <= 13) { + tagLocation[1] = 2; + } + else { + tagLocation[1] = 3; + } + } + else { + tagLocation[0] = 0; + tagLocation[1] = 0; + } + return tagLocation; + } } From 541f6767cc80fe1f18a36dff39272b589d5aa551 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Sat, 8 Feb 2025 19:35:33 -0500 Subject: [PATCH 22/79] make characterization code later --- build.gradle | 2 +- .../deploy/pathplanner/autos/New Auto.auto | 2 +- .../pathplanner/paths/Forward and Back.path | 70 +++++++++++++++++++ .../paths/Straight Line Rotate 90.path | 54 ++++++++++++++ .../pathplanner/paths/Straight Test.path | 4 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/AlgaeIntake.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 10 +-- .../java/frc/robot/subsystems/Swerve.java | 44 ++++++++---- src/main/java/frc/robot/swerve/Swerve.java | 12 ++-- .../robot/utility/AutomatedController.java | 8 +-- 12 files changed, 177 insertions(+), 35 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Forward and Back.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path diff --git a/build.gradle b/build.gradle index 11d95e6..76cbd04 100644 --- a/build.gradle +++ b/build.gradle @@ -101,4 +101,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 63af25f..e70990e 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Triangle" + "pathName": "Straight Line Rotate 90" } } ] diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path new file mode 100644 index 0000000..8703b60 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Forward and Back.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.499651445630644, + "y": 7.0 + }, + "prevControl": { + "x": 3.4996514456306436, + "y": 7.0 + }, + "nextControl": { + "x": 5.499651445630644, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 3.5072811929471364, + "y": 6.980586730595545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path new file mode 100644 index 0000000..d9ae114 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 7.0 + }, + "prevControl": { + "x": 5.0, + "y": 7.0 + }, + "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": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index 0067703..2ec6b22 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.0, + "x": 6.0, "y": 6.0 }, "prevControl": { - "x": 2.0, + "x": 5.0, "y": 6.0 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index c03e6b1..67333ae 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -14,7 +14,7 @@ "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 5.4, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70c56ef..467e220 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public RobotContainer() { SmartDashboard.putData("Main-Controller Mode", main.selector); SmartDashboard.putData("Backup-Controller Mode", main.selector); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); - DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); + // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); // SmartDashboard.putData("Autonomous", ); // TBD } diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index fec5f46..1f0829d 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -41,6 +41,6 @@ public boolean grabbed(){ @Override public void periodic() { SmartDashboard.putBoolean("Intake Full", grabbed()); - DogLog.log("Intake/Full", grabbed()); + // DogLog.log("Intake/Full", grabbed()); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b895fc0..a10e74e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -114,10 +114,10 @@ public void periodic() { SmartDashboard.putNumber("Elevator Speed", motor.getEncoder().getVelocity()); SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); - DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); - DogLog.log("Elevator/Target Height", target); - DogLog.log("Elevator/cTarget Height", out.position); - DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); - DogLog.log("Elevator/cTarget Velocity", out.velocity); + // DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); + // DogLog.log("Elevator/Target Height", target); + // DogLog.log("Elevator/cTarget Height", out.position); + // DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); + // DogLog.log("Elevator/cTarget Velocity", out.velocity); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f7f9fbb..5efb3d4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -18,11 +18,14 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; @@ -226,17 +229,32 @@ public void disable() { mod.stop(); } + // public void characterizeDrive(double volts){ + // for(int i = 0; i < modules.length; i++){ + + // } + // } + + // private final SysIdRoutine driveCharacterization = new SysIdRoutine( + // new SysIdRoutine.Config(), + // new SysIdRoutine.Mechanism( + // (Measure volts) -> { + + // }, + // ) + // ); + public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) setModuleStates(states); - // current_states.set(moduleStates(modules)); - // target_states.set(states); + current_states.set(moduleStates(modules)); + target_states.set(states); Pose2d pose = odometry.update(rotation(), modulePositions()); - // posePublisher.set(pose); + posePublisher.set(pose); // SmartDashboard.putNumber("X position", pose.getX()); // SmartDashboard.putNumber("Y position", pose.getY()); @@ -248,15 +266,15 @@ public void periodic() { // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); - DogLog.log("Swerve/current_states", moduleStates(modules)); - DogLog.log("Swerve/target_states", states); - DogLog.log("Swerve/pose", pose); - DogLog.log("Swerve/X_position", pose.getX()); - DogLog.log("Swerve/Y_position", pose.getY()); - DogLog.log("Swerve/Odometry_rotation", rotation().getDegrees()); - DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); - DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); - DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); - DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + // DogLog.log("Swerve/current_states", moduleStates(modules)); + // DogLog.log("Swerve/target_states", states); + // DogLog.log("Swerve/pose", pose); + // DogLog.log("Swerve/X_position", pose.getX()); + // DogLog.log("Swerve/Y_position", pose.getY()); + // DogLog.log("Swerve/Odometry_rotation", rotation().getDegrees()); + // DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); + // DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); + // DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); + // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 71813ce..02599f8 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -28,14 +28,14 @@ public static class Constants { // DRIVER SETTINGS public int driver = 0; - public double transFactor = .8; // factor = x/125, with x being the percentage of our max speed, same for the thing below - public double rotFactor = .48; // .6 for tristan + public double transFactor = .65; // factor = x/125, with x being the percentage of our max speed, same for the thing below + public double rotFactor = .30; // .6 for tristan // AUTON CONSTANTS - public double XControllerP = 0.1; + public double XControllerP = 3.35; public double XControllerD = 0.0; - public double ThetaControllerP = 0.05; - public double ThetaControllerD = 0.005; + public double ThetaControllerP = 0; + public double ThetaControllerD = 0; public RobotConfig autoConfig; // BASE CHASSIS CONFIGURATION @@ -60,7 +60,7 @@ public Constants(){ switch (driver) { default: // Shaan - transFactor = .8; + transFactor = .65; rotFactor = .48; break; } diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index f7efd51..4accf18 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -58,10 +58,10 @@ public void configure(){ // RB align Right and Score Coral & Score Processor // controller.y().and( automated() ).onTrue(Util.D - // controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); - // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); - // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect + controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); + controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); + controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); + controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } From 788974d648aacfb3083438170590c4c347a7e4aa Mon Sep 17 00:00:00 2001 From: Evan Date: Sat, 8 Feb 2025 22:41:53 -0500 Subject: [PATCH 23/79] refactored swerve --- simgui-ds.json | 92 ++++++++++ .../deploy/pathplanner/paths/New Path.path | 166 ++++++++++++++++++ .../java/frc/robot/commands/DefaultDrive.java | 1 - .../java/frc/robot/subsystems/Swerve.java | 102 ++++------- src/main/java/frc/robot/swerve/Swerve.java | 2 - .../robot/utility/AutomatedController.java | 5 +- 6 files changed, 294 insertions(+), 74 deletions(-) create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..a8099ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,166 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.79424342105263, + "y": 6.170641447368421 + }, + "prevControl": null, + "nextControl": { + "x": 7.1296875, + "y": 6.132154605263157 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.435608552631579, + "y": 5.978207236842105 + }, + "prevControl": { + "x": 3.435608552631579, + "y": 5.978207236842105 + }, + "nextControl": { + "x": 5.435608552631579, + "y": 5.978207236842105 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.994325657894737, + "y": 4.8139802631578945 + }, + "prevControl": { + "x": 5.714967105263158, + "y": 5.39609375 + }, + "nextControl": { + "x": 6.273684210526316, + "y": 4.231866776315789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.042434210526316, + "y": 2.9377467105263158 + }, + "prevControl": { + "x": 6.158059210526316, + "y": 3.584806743421052 + }, + "nextControl": { + "x": 5.926809210526315, + "y": 2.2906866776315793 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.63766447368421, + "y": 2.360444078947369 + }, + "prevControl": { + "x": 5.282236842105263, + "y": 2.325565378289474 + }, + "nextControl": { + "x": 3.993092105263157, + "y": 2.3953227796052636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1930921052631578, + "y": 2.244983552631579 + }, + "prevControl": { + "x": 2.5930921052631577, + "y": 2.3201531661184216 + }, + "nextControl": { + "x": -0.20690789473684212, + "y": 2.1698139391447366 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.746134868421052, + "y": 6.228371710526315 + }, + "prevControl": { + "x": 4.269613486842105, + "y": 4.199092824835526 + }, + "nextControl": { + "x": 13.22265625, + "y": 8.257650596217104 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4631578947368418, + "y": 5.429769736842105 + }, + "prevControl": { + "x": 7.842907072368421, + "y": 6.843710166529604 + }, + "nextControl": { + "x": -2.9165912828947373, + "y": 4.015829307154607 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.367598684210526, + "y": 7.007730263157895 + }, + "prevControl": { + "x": 0.31690995065789496, + "y": 5.497347219366777 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index aa1c40a..01015da 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.swerve.Swerve; import frc.robot.utility.IO; -import frc.robot.utility.SwerveConstants; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 5efb3d4..548a0b5 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import dev.doglog.DogLog; @@ -18,14 +17,11 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; @@ -33,7 +29,6 @@ public class Swerve extends SubsystemBase { public boolean field_oritented = true; - public boolean slow_mode = false; private final SwerveDriveKinematics kinematics; public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID); @@ -44,7 +39,7 @@ public class Swerve extends SubsystemBase { .getStructArrayTopic("Target Module States", SwerveModuleState.struct).publish(); StructPublisher posePublisher = NetworkTableInstance.getDefault().getTable("Debug") .getStructTopic("Current pose", Pose2d.struct).publish(); - + final SwerveDriveOdometry odometry; final Module[] modules = new Module[4]; ChassisSpeeds speeds = new ChassisSpeeds(); @@ -54,15 +49,11 @@ public class Swerve extends SubsystemBase { public Swerve() { kinematics = new SwerveDriveKinematics( - new Translation2d(constants.TRACKWIDTH / 2.0, - constants.WHEELBASE / 2.0), - new Translation2d(constants.TRACKWIDTH / 2.0, - -constants.WHEELBASE / 2.0), - new Translation2d(-constants.TRACKWIDTH / 2.0, - constants.WHEELBASE / 2.0), - new Translation2d(-constants.TRACKWIDTH / 2.0, - -constants.WHEELBASE / 2.0)); - + createTranslation(constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), + createTranslation(constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0), + createTranslation(-constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), + createTranslation(-constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0)); + ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { modules[i] = new Module( @@ -75,14 +66,16 @@ public Swerve() { constants.comp); } - AutoBuilder.configure( + AutoBuilder.configure( this::pose, this::resetOdometry, () -> speeds, (speeds, feedforwards) -> drive(speeds), new PPHolonomicDriveController( - new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID constants - new PIDConstants(constants.ThetaControllerP, 0, constants.ThetaControllerD, 0.0) // Rotation PID constants + new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID + // constants + new PIDConstants(constants.ThetaControllerP, 0, constants.ThetaControllerD, 0.0) // Rotation PID + // constants ), constants.autoConfig, () -> { @@ -95,6 +88,10 @@ public Swerve() { new Pose2d(0, 0, new Rotation2d())); } + private Translation2d createTranslation(double x, double y) { + return new Translation2d(x, y); + } + public void zeroGyro() { pigeon2.setYaw(0); } @@ -146,11 +143,9 @@ public SwerveModuleState[] moduleStates(Module[] modules) { state[i] = moduleState(modules[i]); return state; } - + public void adjustRotation() { - double rotation = (absoluteRotation() - 180) % 360; - rotation += (rotation < 0) ? 360 : 0; - pigeon2.setYaw(rotation); + pigeon2.setYaw((absoluteRotation() + 180) % 360); } public Pose2d pose() { @@ -158,27 +153,14 @@ public Pose2d pose() { } public void resetOdometry() { - setOdometry(new Pose2d()); + resetOdometry(new Pose2d()); } public void resetOdometry(Pose2d pose) { - pigeon2.setYaw(0); - resetPosition(); - - odometry.resetPosition(rotation(), modulePositions(), pose); - odometry.resetPosition(rotation(), modulePositions(), pose); - } - - public void setOdometry(Pose2d pose) { zeroGyro(); resetPosition(); odometry.resetPosition(rotation(), modulePositions(), pose); - odometry.resetPosition(rotation(), modulePositions(), pose); - } - - public void zeroHeading(){ - pigeon2.setYaw(0); } public void resetPosition() { @@ -201,12 +183,12 @@ public void resetSteerPositions() { mod.set(0, 0); } - public void setModuleStates(SwerveModuleState[] states) { + public void setModuleStates(SwerveModuleState[] states) { SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.MAX_VELOCITY); for (int i = 0; i < modules.length; i++) { states[i].optimize(new Rotation2d(modules[i].angle())); - // states[i].cosineScale(new Rotation2d(modules[i].angle())); // Cosine Compensation - modules[i].set((states[i].speedMetersPerSecond / Constants.MAX_VELOCITY) * .8 , states[i].angle.getRadians()); + modules[i].set((states[i].speedMetersPerSecond / Constants.MAX_VELOCITY) * .8, + states[i].angle.getRadians()); } } @@ -218,32 +200,15 @@ public double getRoll() { return pigeon2.getRoll().getValueAsDouble(); } - public void enable() { - active = true; - } - - public void disable() { - active = false; + public void toggle() { + active = !active; - for (Module mod : modules) - mod.stop(); + if (!active) { + for (Module mod : modules) + mod.stop(); + } } - // public void characterizeDrive(double volts){ - // for(int i = 0; i < modules.length; i++){ - - // } - // } - - // private final SysIdRoutine driveCharacterization = new SysIdRoutine( - // new SysIdRoutine.Config(), - // new SysIdRoutine.Mechanism( - // (Measure volts) -> { - - // }, - // ) - // ); - public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) @@ -252,7 +217,6 @@ public void periodic() { current_states.set(moduleStates(modules)); target_states.set(states); - Pose2d pose = odometry.update(rotation(), modulePositions()); posePublisher.set(pose); @@ -261,10 +225,13 @@ public void periodic() { // SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); // SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); + // SmartDashboard.putNumber("Pigeon Pitch", + // pigeon2.getPitch().getValueAsDouble()); + // SmartDashboard.putNumber("Pigeon Roll", + // pigeon2.getRoll().getValueAsDouble()); - // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : + // "Robot-Oriented"); // DogLog.log("Swerve/current_states", moduleStates(modules)); // DogLog.log("Swerve/target_states", states); @@ -275,6 +242,7 @@ public void periodic() { // DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); // DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); // DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); - // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : + // "Robot-Oriented"); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 02599f8..ccbf04f 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -3,9 +3,7 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.path.PathPlannerPath; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; import static edu.wpi.first.units.Units.Radians; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 4accf18..307607e 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,8 +5,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.GrabAlgae; -import frc.robot.commands.ReleaseAlgae; public class AutomatedController { public final CommandXboxController controller; @@ -58,8 +56,7 @@ public void configure(){ // RB align Right and Score Coral & Score Processor // controller.y().and( automated() ).onTrue(Util.D - controller.povUp().and( manual() ).onTrue(Util.Do(io.chassis::enable)); - controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::disable)).debounce(1.5); + controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect From 893c140d704c89e6389fd8e40414056c40211ab5 Mon Sep 17 00:00:00 2001 From: Evan Date: Sun, 9 Feb 2025 00:20:09 -0500 Subject: [PATCH 24/79] set all the pathplanner values to how they should be and added the config into the code so swapping between comp and test bots is easier --- src/main/deploy/pathplanner/settings.json | 26 +++++----- .../java/frc/robot/subsystems/Swerve.java | 2 +- src/main/java/frc/robot/swerve/Swerve.java | 48 ++++++++++++++----- .../frc/robot/utility/SwerveConstants.java | 20 -------- 4 files changed, 51 insertions(+), 45 deletions(-) delete mode 100644 src/main/java/frc/robot/utility/SwerveConstants.java diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 67333ae..b481774 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 45.35924, - "robotMOI": 6.883, + "robotMass": 47.0, + "robotMOI": 2.12, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.12, "maxDriveSpeed": 5.4, "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 59.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.5461, + "flModuleY": 0.4953, + "frModuleX": 0.5461, + "frModuleY": -0.4953, + "blModuleX": -0.5461, + "blModuleY": 0.495, + "brModuleX": -0.5461, + "brModuleY": -0.4953, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 548a0b5..15e3d9d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -143,7 +143,7 @@ public SwerveModuleState[] moduleStates(Module[] modules) { state[i] = moduleState(modules[i]); return state; } - + public void adjustRotation() { pigeon2.setYaw((absoluteRotation() + 180) % 360); } diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index ccbf04f..732abb1 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -3,14 +3,15 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; -import static edu.wpi.first.units.Units.Radians; -import java.io.IOException; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; -import org.json.simple.parser.ParseException; +import static edu.wpi.first.units.Units.Radians; public class Swerve { @@ -23,6 +24,7 @@ public static class Constants { public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi public double GEAR_RATIO; + public double WHEEL_RADIUS; // DRIVER SETTINGS public int driver = 0; @@ -35,6 +37,7 @@ public static class Constants { public double ThetaControllerP = 0; public double ThetaControllerD = 0; public RobotConfig autoConfig; + public double MASS = 47; // BASE CHASSIS CONFIGURATION public static final double MAX_VELOCITY = 5.4; @@ -50,26 +53,49 @@ public Constants(){ TRACKWIDTH = 30.0; WHEELBASE = 30.0; GEAR_RATIO = 8.14; // L1 + WHEEL_RADIUS = 0.1143; + MASS = 60.0; } else { TRACKWIDTH = 19.5; WHEELBASE = 21.5; GEAR_RATIO = 6.12; // L3 + WHEEL_RADIUS = 0.1016; + MASS = 47.0; } + double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); + double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); + Translation2d[] swerve_offsets = + new Translation2d[] { + new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), + new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), + new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), + new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) + }; + + double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi + ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); + autoConfig = new RobotConfig( + MASS, + moi, + moduleConfigs, + swerve_offsets + ); + + // try { + // autoConfig = RobotConfig.fromGUISettings(); + // } catch (IOException e) { + // e.printStackTrace(); + // } catch (ParseException e) { + // e.printStackTrace(); + // } + switch (driver) { default: // Shaan transFactor = .65; rotFactor = .48; break; } - - try { - autoConfig = RobotConfig.fromGUISettings(); - } catch (IOException e) { - e.printStackTrace(); - } catch (ParseException e) { - e.printStackTrace(); - } } } diff --git a/src/main/java/frc/robot/utility/SwerveConstants.java b/src/main/java/frc/robot/utility/SwerveConstants.java deleted file mode 100644 index fafa34e..0000000 --- a/src/main/java/frc/robot/utility/SwerveConstants.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.utility; - -public class SwerveConstants { - boolean compChassis = false; - - public static double TRACKWIDTH = 19.5; // 30.0 for MKi - public static double WHEELBASE = 21.5; // 30.0 for MKi - - public static double GEAR_RATIO; - - public static final double MAX_VELOCITY = 5.4; - - public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; - - public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR - public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR - - public static final int PIGEON_ID = 6; - -} \ No newline at end of file From fb006f63cf1d46553ddce9c1463c05659ca41eb3 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 9 Feb 2025 11:02:51 -0500 Subject: [PATCH 25/79] fixces for alignment --- src/main/java/frc/robot/commands/aimbot.java | 14 +++++--------- src/main/java/frc/robot/subsystems/Limelight.java | 3 +-- src/main/java/frc/robot/utility/IO.java | 2 -- 3 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/commands/aimbot.java b/src/main/java/frc/robot/commands/aimbot.java index 03dfad3..64c92bf 100644 --- a/src/main/java/frc/robot/commands/aimbot.java +++ b/src/main/java/frc/robot/commands/aimbot.java @@ -13,29 +13,25 @@ public class aimbot extends Command { IO io; - PIDController pid = new PIDController(0.00, 0.00, 0.00); + PIDController pid = new PIDController(0.01, 0.00, 0.00); double tolerance = 5; - /** Creates a new aimbot. */ public aimbot(IO io) { - io.chassis.drive(new ChassisSpeeds(0, pid.calculate(io.limelight.x), 0)); - // Use addRequirements() here to declare subsystem dependencies. + addRequirements(io.limelight, io.chassis); } - // Called when the command is initially scheduled. @Override public void initialize() {} - // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + io.chassis.drive(new ChassisSpeeds(0, pid.calculate(io.limelight.x), 0)); + } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { } - // Returns true when the command should end. @Override public boolean isFinished() { return Math.abs(pid.getError()) < tolerance; diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index e81845a..32d8a05 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -5,7 +5,6 @@ package frc.robot.subsystems; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,7 +18,7 @@ public class Limelight extends SubsystemBase { int tagNumber; int[] tagLocation; - /** Creates a new Limelight. */ + public Limelight() { final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); x = table.getEntry("tx").getDouble(0.0); diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index 59f665c..99ce7b1 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -2,8 +2,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; From 64eed84a4c7c39c5e32e5215d78c274852ba4b55 Mon Sep 17 00:00:00 2001 From: Evan Date: Sun, 9 Feb 2025 19:10:42 -0500 Subject: [PATCH 26/79] fixed offsets in pathplanner --- src/main/deploy/pathplanner/settings.json | 16 +++--- src/main/java/frc/robot/swerve/Swerve.java | 58 +++++++++++----------- 2 files changed, 36 insertions(+), 38 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index b481774..0ed0eaf 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -18,14 +18,14 @@ "driveMotorType": "krakenX60", "driveCurrentLimit": 59.0, "wheelCOF": 1.2, - "flModuleX": 0.5461, - "flModuleY": 0.4953, - "frModuleX": 0.5461, - "frModuleY": -0.4953, - "blModuleX": -0.5461, - "blModuleY": 0.495, - "brModuleX": -0.5461, - "brModuleY": -0.4953, + "flModuleX": 0.27305, + "flModuleY": 0.24765, + "frModuleX": 0.27305, + "frModuleY": -0.24765, + "blModuleX": -0.27305, + "blModuleY": 0.24765, + "brModuleX": -0.27305, + "brModuleY": -0.24765, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 732abb1..82f6e84 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -3,16 +3,16 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; - import static edu.wpi.first.units.Units.Radians; +import java.io.IOException; + +import org.json.simple.parser.ParseException; + public class Swerve { public static final double PI2 = 2.0 * Math.PI; @@ -63,32 +63,30 @@ public Constants(){ MASS = 47.0; } - double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); - double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); - Translation2d[] swerve_offsets = - new Translation2d[] { - new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), - new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), - new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), - new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) - }; - - double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi - ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); - autoConfig = new RobotConfig( - MASS, - moi, - moduleConfigs, - swerve_offsets - ); - - // try { - // autoConfig = RobotConfig.fromGUISettings(); - // } catch (IOException e) { - // e.printStackTrace(); - // } catch (ParseException e) { - // e.printStackTrace(); - // } + // double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); + // double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); + // Translation2d[] swerve_offsets = + // new Translation2d[] { + // new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), + // new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), + // new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), + // new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) + // }; + + // double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi + // ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); + // autoConfig = new RobotConfig( + // MASS, + // moi, + // moduleConfigs, + // swerve_offsets + // ); + + try { + autoConfig = RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + e.printStackTrace(); + } switch (driver) { default: // Shaan From 6d12e65cecc52f0fb6f9cc0b527c347c56f54ca0 Mon Sep 17 00:00:00 2001 From: Evan Date: Mon, 10 Feb 2025 19:19:45 -0500 Subject: [PATCH 27/79] pivot code --- .../java/frc/robot/commands/GrabAlgae.java | 2 +- .../java/frc/robot/commands/ReleaseAlgae.java | 2 +- .../frc/robot/subsystems/AlgaeIntake.java | 33 ++++++++++++++++--- 3 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/GrabAlgae.java b/src/main/java/frc/robot/commands/GrabAlgae.java index 6a9cf60..51221bb 100644 --- a/src/main/java/frc/robot/commands/GrabAlgae.java +++ b/src/main/java/frc/robot/commands/GrabAlgae.java @@ -16,7 +16,7 @@ public void initialize() {} @Override public void execute() { - io.intake.volts(12); + io.intake.IntakeVolts(12); } @Override diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java index a532b8b..a26d45a 100644 --- a/src/main/java/frc/robot/commands/ReleaseAlgae.java +++ b/src/main/java/frc/robot/commands/ReleaseAlgae.java @@ -20,7 +20,7 @@ public void initialize() {} @Override public void execute() { - io.intake.volts(speed); + io.intake.IntakeVolts(speed); } @Override diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index 1f0829d..4221a08 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -8,26 +8,45 @@ import dev.doglog.DogLog; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class AlgaeIntake extends SubsystemBase { public SparkMax roller = new SparkMax(9, MotorType.kBrushless); + public SparkMax pivot = new SparkMax(11, MotorType.kBrushless); public DigitalInput beamBreak = new DigitalInput(0); SparkMaxConfig rollerConfig = new SparkMaxConfig(); + SparkMaxConfig pivotConfig = new SparkMaxConfig(); + public DutyCycleEncoder encoder = new DutyCycleEncoder(0); + + public static final double CLOSED_ANGLE = 0; + public boolean closed; public AlgaeIntake() { rollerConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); - roller.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotConfig.idleMode(SparkMaxConfig.IdleMode.kCoast); + pivot.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + closed = (angle() > CLOSED_ANGLE); } - public void volts(double volts){ + public void IntakeVolts(double volts){ roller.setVoltage(volts); } - public void speed(double speed){ - roller.setVoltage(speed); + public void intakeSpeed(double speed){ + roller.set(speed); + } + + public void pivotSpeed(double speed){ + pivot.set(speed); + } + + public void pivotVolts(double volts){ + pivot.setVoltage(volts); } public void stop(){ @@ -38,9 +57,15 @@ public boolean grabbed(){ return beamBreak.get(); } + public double angle(){ + return encoder.get() * 360; + } + @Override public void periodic() { SmartDashboard.putBoolean("Intake Full", grabbed()); + closed = (angle() < CLOSED_ANGLE); + SmartDashboard.putNumber("Pivot Angle", angle()); // DogLog.log("Intake/Full", grabbed()); } } From 47a8b4f6d56327543d9f083f05bfb10f2bc093ac Mon Sep 17 00:00:00 2001 From: Evan Date: Tue, 11 Feb 2025 22:18:52 -0500 Subject: [PATCH 28/79] Pivot Intake Command --- .../java/frc/robot/commands/GrabAlgae.java | 2 +- .../java/frc/robot/commands/PivotIntake.java | 35 +++++++++++++++++++ .../java/frc/robot/commands/ReleaseAlgae.java | 2 +- .../frc/robot/subsystems/AlgaeIntake.java | 12 ++++--- 4 files changed, 45 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/commands/PivotIntake.java diff --git a/src/main/java/frc/robot/commands/GrabAlgae.java b/src/main/java/frc/robot/commands/GrabAlgae.java index 51221bb..6299067 100644 --- a/src/main/java/frc/robot/commands/GrabAlgae.java +++ b/src/main/java/frc/robot/commands/GrabAlgae.java @@ -21,7 +21,7 @@ public void execute() { @Override public void end(boolean interrupted) { - io.intake.stop(); + io.intake.stopIntake(); } @Override diff --git a/src/main/java/frc/robot/commands/PivotIntake.java b/src/main/java/frc/robot/commands/PivotIntake.java new file mode 100644 index 0000000..0d9aa1d --- /dev/null +++ b/src/main/java/frc/robot/commands/PivotIntake.java @@ -0,0 +1,35 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; + +public class PivotIntake extends Command { + IO io; + boolean origin; + + public PivotIntake(IO io) { + this.io = io; + addRequirements(io.intake); + } + + @Override + public void initialize() { + origin = (io.intake.angle() > io.intake.closedAngle); + io.intake.pivotVolts(6 * (origin ? 1 : -1)); + } + + @Override + public void execute() { + io.intake.closed = (io.intake.angle() > io.intake.closedAngle); + } + + @Override + public void end(boolean interrupted) { + io.intake.stopPivot(); + } + + @Override + public boolean isFinished() { + return io.intake.closed != origin; + } +} diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java index a26d45a..7356519 100644 --- a/src/main/java/frc/robot/commands/ReleaseAlgae.java +++ b/src/main/java/frc/robot/commands/ReleaseAlgae.java @@ -25,7 +25,7 @@ public void execute() { @Override public void end(boolean interrupted) { - io.intake.stop(); + io.intake.stopIntake(); } @Override diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index 4221a08..9be68a8 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -20,7 +20,7 @@ public class AlgaeIntake extends SubsystemBase { SparkMaxConfig pivotConfig = new SparkMaxConfig(); public DutyCycleEncoder encoder = new DutyCycleEncoder(0); - public static final double CLOSED_ANGLE = 0; + public final double closedAngle = 0; public boolean closed; public AlgaeIntake() { @@ -30,7 +30,7 @@ public AlgaeIntake() { pivotConfig.idleMode(SparkMaxConfig.IdleMode.kCoast); pivot.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - closed = (angle() > CLOSED_ANGLE); + closed = (angle() > closedAngle); } public void IntakeVolts(double volts){ @@ -49,10 +49,14 @@ public void pivotVolts(double volts){ pivot.setVoltage(volts); } - public void stop(){ + public void stopIntake(){ roller.stopMotor(); } + public void stopPivot(){ + pivot.stopMotor(); + } + public boolean grabbed(){ return beamBreak.get(); } @@ -64,7 +68,7 @@ public double angle(){ @Override public void periodic() { SmartDashboard.putBoolean("Intake Full", grabbed()); - closed = (angle() < CLOSED_ANGLE); + closed = (angle() < closedAngle); SmartDashboard.putNumber("Pivot Angle", angle()); // DogLog.log("Intake/Full", grabbed()); } From 3035d2f3915aa0f841caa1053057bf562790365b Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 13 Feb 2025 17:49:30 -0500 Subject: [PATCH 29/79] kj fdaboijfnbpdaiufnb --- build.gradle | 2 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 40 +++++---- .../java/frc/robot/commands/ReleaseAlgae.java | 5 +- .../java/frc/robot/commands/ScoreBarge.java | 22 +++++ .../java/frc/robot/commands/ScoreReef.java | 20 +++++ src/main/java/frc/robot/commands/aimbot.java | 16 ++-- .../java/frc/robot/subsystems/Limelight.java | 82 +++++++++---------- .../java/frc/robot/subsystems/Swerve.java | 4 +- src/main/java/frc/robot/swerve/Swerve.java | 57 ++++++------- .../robot/utility/AutomatedController.java | 15 +++- 11 files changed, 163 insertions(+), 102 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ScoreBarge.java create mode 100644 src/main/java/frc/robot/commands/ScoreReef.java diff --git a/build.gradle b/build.gradle index 76cbd04..f37724f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 409c8c3..3b57bf9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,7 +45,7 @@ public void autonomousPeriodic() {} @Override public void autonomousExit() { - m_robotContainer.io.chassis.adjustRotation(); + // m_robotContainer.io.chassis.adjustRotation(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 467e220..643ec23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,8 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -19,32 +21,36 @@ public class RobotContainer { - public IO io = new IO(); - public final AutomatedController main; - public final AutomatedController backup; + // public IO io = new IO(); + // public final AutomatedController main; + // public final AutomatedController backup; - private final SendableChooser auto_selector; - private Command current_auto = new PrintCommand("Temp"); + // private final SendableChooser auto_selector; + // private Command current_auto = new PrintCommand("Temp"); + + private SparkMax s = new SparkMax(2, MotorType.kBrushed); public RobotContainer() { - main = new AutomatedController(0, io); - backup = new AutomatedController(1, io); + s.set(1); + // main = new AutomatedController(0, io); + // backup = new AutomatedController(1, io); - auto_selector = AutoBuilder.buildAutoChooser(); - auto_selector.onChange((command) -> {current_auto = command;}); + // auto_selector = AutoBuilder.buildAutoChooser(); + // auto_selector.onChange((command) -> {current_auto = command;}); - SmartDashboard.putData("Autos",auto_selector); - SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); + // SmartDashboard.putData("Autos",auto_selector); + // SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); - SmartDashboard.putData("Main-Controller Mode", main.selector); - SmartDashboard.putData("Backup-Controller Mode", main.selector); - io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); - // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); - // SmartDashboard.putData("Autonomous", ); // TBD + // SmartDashboard.putData("Main-Controller Mode", main.selector); + // SmartDashboard.putData("Backup-Controller Mode", main.selector); + // io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + // // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); + // // SmartDashboard.putData("Autonomous", ); // TBD } public Command getAutonomousCommand() { - return auto_selector.getSelected(); + // return auto_selector.getSelected(); + return new PrintCommand(""); } } diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java index a532b8b..ef84797 100644 --- a/src/main/java/frc/robot/commands/ReleaseAlgae.java +++ b/src/main/java/frc/robot/commands/ReleaseAlgae.java @@ -8,10 +8,11 @@ public class ReleaseAlgae extends Command { double speed; - public ReleaseAlgae(IO io, boolean spit_out) { + public ReleaseAlgae(IO io) { this.io = io; - speed = (spit_out) ? -12 : 12; + speed = 12; + addRequirements(io.intake); } diff --git a/src/main/java/frc/robot/commands/ScoreBarge.java b/src/main/java/frc/robot/commands/ScoreBarge.java new file mode 100644 index 0000000..d08e587 --- /dev/null +++ b/src/main/java/frc/robot/commands/ScoreBarge.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.utility.IO; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ScoreBarge extends ParallelCommandGroup { + /** Creates a new ScoreBarge. */ + public ScoreBarge(IO io) { + addCommands( + // TODO: Move to Height + // TODO: Release Algae + // TODO: Move back to Resting Position + ); + } +} diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java new file mode 100644 index 0000000..2f9bd41 --- /dev/null +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.utility.IO; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ScoreReef extends SequentialCommandGroup { + + public ScoreReef(IO io, boolean score_right) { + addCommands( + + ); + } +} diff --git a/src/main/java/frc/robot/commands/aimbot.java b/src/main/java/frc/robot/commands/aimbot.java index 64c92bf..ccdbdcd 100644 --- a/src/main/java/frc/robot/commands/aimbot.java +++ b/src/main/java/frc/robot/commands/aimbot.java @@ -6,17 +6,20 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.utility.IO; +import frc.robot.utility.LimelightHelpers; /* 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 aimbot extends Command { IO io; - PIDController pid = new PIDController(0.01, 0.00, 0.00); - double tolerance = 5; + PIDController pid = new PIDController(0.1, 0.00, 0.00); public aimbot(IO io) { + this.io = io; + pid.setTolerance(10); addRequirements(io.limelight, io.chassis); } @@ -25,15 +28,16 @@ public void initialize() {} @Override public void execute() { - io.chassis.drive(new ChassisSpeeds(0, pid.calculate(io.limelight.x), 0)); + double output = -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0); + SmartDashboard.putNumber("PID Output", output); + io.chassis.drive(new ChassisSpeeds(0, output, 0)); } @Override - public void end(boolean interrupted) { - } + public void end(boolean interrupted) {} @Override public boolean isFinished() { - return Math.abs(pid.getError()) < tolerance; + return pid.atSetpoint() || !LimelightHelpers.getTV("limelight-main"); } } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 32d8a05..388bf3e 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -4,8 +4,13 @@ package frc.robot.subsystems; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -15,61 +20,52 @@ public class Limelight extends SubsystemBase { double y; double robotyaw; double latency; - int tagNumber; + int tag; + + public static final int LEFT = 0; + public static final int CENTRE = 1; + public static final int RIGHT = 2; + + public static final int X = 0; + public static final int Y = 1; + public static final int ROTATION = 2; + public static final double[][][] TagPose = { + { // TAG 1 **All locations in meters**All rotations in radians** + // LEFT POLE // CENTRE TAG // RIGHT POLE + {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0} + }, + { // TAG 2 + {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0} + } + }; + + // EXAMPLE: TagPOse[1][RIGHT][X] int[] tagLocation; - public Limelight() { - final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); - x = table.getEntry("tx").getDouble(0.0); - y = table.getEntry("ty").getDouble(0.0); - area = table.getEntry("ta").getDouble(0.0); - latency = table.getEntry("tl").getDouble(0.0); - tagNumber = (int) table.getEntry("tid").getInteger(0); - } + final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); + + public Limelight() {} @Override public void periodic() { - // This method will be called once per scheduler run + // x = table.getEntry("tx").getDouble(0.0); + y = table.getEntry("ty").getDouble(0.0); + area = table.getEntry("ta").getDouble(0.0); + latency = table.getEntry("tl").getDouble(0.0); + tag = (int) table.getEntry("tid").getInteger(0); + SmartDashboard.putNumber("LimelightX", x); SmartDashboard.putNumber("LimelightY", y); //MIGHT NEED TO CHANGE THIS SmartDashboard.putNumber("LimelightArea", area); SmartDashboard.putNumber("Limelight Latency", latency); } - public int[] TagLocationProcessing() { - /*{red/blue, zone} - { 1/2, 1-3} - zone 1 = coral - zone 2 = coral station - zone 3 = processor + public boolean zoned(){ + return ((tag > 11) ? Alliance.Blue : Alliance.Red) == DriverStation.getAlliance().get(); + } - ** if returns 0 and 0 there is no tag in view ** - */ - // return red or blue - if (tagNumber > 0){ - if (tagNumber <= 11){ - tagLocation[0] = 1; - } - else { - tagLocation[0] = 2; - } - - // return zone - if (tagNumber >=6 && tagNumber <= 11 || tagNumber >= 17 && tagNumber <= 22){ - tagLocation[1] = 1; - } - else if (tagNumber >=1 && tagNumber <= 2 || tagNumber >= 12 && tagNumber <= 13) { - tagLocation[1] = 2; - } - else { - tagLocation[1] = 3; - } - } - else { - tagLocation[0] = 0; - tagLocation[1] = 0; - } - return tagLocation; + public boolean reefZone(){ + return (tag >=6 && tag <= 11 || tag >= 17 && tag <= 22); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ac602cf..2ab7530 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -34,7 +34,7 @@ public class Swerve extends SubsystemBase { public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID); - public SwerveDrivePoseEstimator poseEstimator; + // public SwerveDrivePoseEstimator poseEstimator; StructArrayPublisher current_states = Util.table .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish(); StructArrayPublisher target_states = Util.table @@ -56,7 +56,7 @@ public Swerve() { createTranslation(-constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), createTranslation(-constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0)); - poseEstimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), pose()); + // poseEstimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), pose()); ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 732abb1..424f491 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -10,16 +10,19 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.struct.parser.ParseException; import static edu.wpi.first.units.Units.Radians; +import java.io.IOException; + public class Swerve { public static final double PI2 = 2.0 * Math.PI; public static class Constants { // BOT SWITCHING - public boolean comp = false; + public boolean comp = true; public double TRACKWIDTH = 19.5; // 30.0 for MKi public double WHEELBASE = 21.5; // 30.0 for MKi @@ -63,33 +66,31 @@ public Constants(){ MASS = 47.0; } - double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); - double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); - Translation2d[] swerve_offsets = - new Translation2d[] { - new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), - new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), - new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), - new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) - }; - - double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi - ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); - autoConfig = new RobotConfig( - MASS, - moi, - moduleConfigs, - swerve_offsets - ); - - // try { - // autoConfig = RobotConfig.fromGUISettings(); - // } catch (IOException e) { - // e.printStackTrace(); - // } catch (ParseException e) { - // e.printStackTrace(); - // } - + // double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); + // double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); + // Translation2d[] swerve_offsets = + // new Translation2d[] { + // new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), + // new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), + // new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), + // new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) + // }; + + // double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi + // ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); + // autoConfig = new RobotConfig( + // MASS, + // moi, + // moduleConfigs, + // swerve_offsets + // ); + + try { + autoConfig = RobotConfig.fromGUISettings(); + } catch (IOException | org.json.simple.parser.ParseException e) { + e.printStackTrace(); + } + switch (driver) { default: // Shaan transFactor = .65; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 307607e..ef4fa5f 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,6 +5,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.ReleaseAlgae; +import frc.robot.commands.ScoreBarge; +import frc.robot.commands.ScoreReef; +import frc.robot.commands.aimbot; public class AutomatedController { public final CommandXboxController controller; @@ -53,9 +57,16 @@ public void configure(){ // Based on the nearest element and our field orientation // LB align Left and Score Coral & Score Barge - // RB align Right and Score Coral & Score Processor + // RB align Right and Score Coral & Score Processor + + // controller.leftBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, false), new ScoreBarge(io), + // io.limelight::reefZone)); + + // controller.rightBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, true), new ReleaseAlgae(io), + // io.limelight::reefZone)); + + controller.a().toggleOnTrue(new aimbot(io)); - // controller.y().and( automated() ).onTrue(Util.D controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect From 71537990ffbdc17690161e101021f3a00ebc0a3c Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sat, 15 Feb 2025 10:25:02 -0500 Subject: [PATCH 30/79] Removed 550 Testing --- src/main/java/frc/robot/RobotContainer.java | 38 +++++++++------------ 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 643ec23..4fe0e1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,36 +21,32 @@ public class RobotContainer { - // public IO io = new IO(); - // public final AutomatedController main; - // public final AutomatedController backup; + public IO io = new IO(); + public final AutomatedController main; + public final AutomatedController backup; - // private final SendableChooser auto_selector; - // private Command current_auto = new PrintCommand("Temp"); - - private SparkMax s = new SparkMax(2, MotorType.kBrushed); + private final SendableChooser auto_selector; + private Command current_auto = new PrintCommand("Temp"); public RobotContainer() { - s.set(1); - // main = new AutomatedController(0, io); - // backup = new AutomatedController(1, io); + main = new AutomatedController(0, io); + backup = new AutomatedController(1, io); - // auto_selector = AutoBuilder.buildAutoChooser(); - // auto_selector.onChange((command) -> {current_auto = command;}); + auto_selector = AutoBuilder.buildAutoChooser(); + auto_selector.onChange((command) -> {current_auto = command;}); - // SmartDashboard.putData("Autos",auto_selector); - // SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); + SmartDashboard.putData("Autos",auto_selector); + SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); - // SmartDashboard.putData("Main-Controller Mode", main.selector); - // SmartDashboard.putData("Backup-Controller Mode", main.selector); - // io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); - // // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); - // // SmartDashboard.putData("Autonomous", ); // TBD + SmartDashboard.putData("Main-Controller Mode", main.selector); + SmartDashboard.putData("Backup-Controller Mode", main.selector); + io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); + // SmartDashboard.putData("Autonomous", ); // TBD } public Command getAutonomousCommand() { - // return auto_selector.getSelected(); - return new PrintCommand(""); + return auto_selector.getSelected(); } } From a72f776fa4d1d89ac99e405d8d166404490c2ad8 Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sat, 15 Feb 2025 15:06:03 -0500 Subject: [PATCH 31/79] added cordinates for all april tags and all reef levels. also edited comments to take up less space and be more descriptive --- .../java/frc/robot/subsystems/Limelight.java | 67 ++++++++++++++++++- 1 file changed, 64 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 388bf3e..f287cbc 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -32,10 +32,71 @@ public class Limelight extends SubsystemBase { public static final double[][][] TagPose = { { // TAG 1 **All locations in meters**All rotations in radians** // LEFT POLE // CENTRE TAG // RIGHT POLE - {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0} + {0.0, 0.0, 0.0}, {16.6972, 0.65532, 0.01745329}, {0.0, 0.0, 0.0} }, { // TAG 2 - {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0} + // x y rotation + {0.0, 0.0, 0.0}, {16.6972, 7.39648, 4.08406986}, {0.0, 0.0, 0.0} + }, + { // TAG 3 + {0.0, 0.0, 0.0}, {11.56081, 8.05561, 4.7123883 }, {0.0, 0.0, 0.0} + }, + { // TAG 4 + {0.0, 0.0, 0.0}, {9.27608, 6.137656, 0.0 }, {0.0, 0.0, 0.0} + }, + { // TAG 5 + {0.0, 0.0, 0.0}, {9.27608, 1.914906, 0.0 }, {0.0, 0.0, 0.0} + }, + { // TAG 6 + {13.5914892, 4.620133, 0.0}, {13.47445, 3.306318, 5.235987 }, {13.3068568, 4.784471, 0.0} + }, + { // TAG 7 + {13.839698, 3.861562, 0.0}, {13.8905, 4.0259, 0.0 }, {13.839698, 4.190238, 0.0} + }, + { // TAG 8 + {13.3068568, 3.2674306, 0.0}, {13.47445, 4.745482, 1.0471974 }, {13.5914892, 3.4317686, 0.0} + }, + { // TAG 9 + {12.5253496, 3.4317686, 0.0}, {12.64336, 4.745482, 2.0943948 }, {12.8104392, 3.2674306, 0.0} + }, + { // TAG 10 + {12.277598, 4.190238, 0.0}, {12.22731, 4.0259, 3.1415922 }, {12.277598, 3.861562, 0.0} + }, + { // TAG 11 + {12.8104392, 4.784471, 0.0}, {12.64336, 3.306318, 4.1887896 }, {12.5258068, 4.620133, 0.0} + }, + { // TAG 12 + {0.0, 0.0, 0.0}, {0.851154, 0.65532, 0.94247766}, {0.0, 0.0, 0.0} + }, + { // TAG 13 + {0.0, 0.0, 0.0}, {0.851154, 7.39648, 5.34070674}, {0.0, 0.0, 0.0} + }, + { // TAG 14 + {0.0, 0.0, 0.0}, {8.272272, 6.137656, 3.1415922 }, {0.0, 0.0, 0.0} + }, + { // TAG 15 + {0.0, 0.0, 0.0}, {8.272272, 1.914906, 3.1415922 }, {0.0, 0.0, 0.0} + }, + { // TAG 16 + {0.0, 0.0, 0.0}, {5.987542, -0.00381, 1.5707961 }, {0.0, 0.0, 0.0} + }, + { // TAG 17 + {3.9572184, 3.4317686, 0.0}, {4.073906, 3.306318, 4.1887896 }, {4.2411904, 3.2674306, 0.0} + }, + { // TAG 18 + {3.7084, 4.190238, 0.0 }, {3.6576, 4.0259, 3.1415922 }, {3.7084, 3.861562, 0.0} + }, + { // TAG 19 + {4.2411904, 4.784471, 0.0}, {4.073906, 4.745482, 2.0943948 }, {3.9572184, 4.620133, 0.0} + }, + { // TAG 20 + {5.0220372, 4.620133, 0.0 }, {4.90474, 4.745482, 1.0471974 }, {4.7374048, 4.784471, 0.0} + }, + { // TAG 21 + {5.269738, 3.861562, 0.0 }, {5.321046, 4.0259, 0.0 }, {5.269738, 4.190238, 0.0} + }, + { // TAG 22 + {4.7374048, 3.2674306, 0.0}, {4.90474, 3.306318, 5.235987 }, {5.0214276, 3.4317686, 0.0} } }; @@ -49,7 +110,7 @@ public Limelight() {} @Override public void periodic() { - // x = table.getEntry("tx").getDouble(0.0); + x = table.getEntry("tx").getDouble(0.0); y = table.getEntry("ty").getDouble(0.0); area = table.getEntry("ta").getDouble(0.0); latency = table.getEntry("tl").getDouble(0.0); From deccaf3b1df0ae8c70dd0224d962896f6131f06b Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Sun, 16 Feb 2025 16:54:58 -0500 Subject: [PATCH 32/79] Added offsets to all of the positions of the poles to make the pid going to the setpoint actually work --- .../robot/commands/aimbot_two_point_o.java | 86 +++++++++++++++ .../java/frc/robot/subsystems/Limelight.java | 104 ++++-------------- 2 files changed, 108 insertions(+), 82 deletions(-) create mode 100644 src/main/java/frc/robot/commands/aimbot_two_point_o.java diff --git a/src/main/java/frc/robot/commands/aimbot_two_point_o.java b/src/main/java/frc/robot/commands/aimbot_two_point_o.java new file mode 100644 index 0000000..2d914f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/aimbot_two_point_o.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; +import frc.robot.utility.LimelightHelpers; + +/* 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 aimbot_two_point_o extends Command { + IO io; + + public static final int LEFT = 0; + public static final int CENTRE = 1; + public static final int RIGHT = 2; + + public static final int X = 0; + public static final int Y = 1; + public static final int ROTATION = 2; + public static final double[][][] TagPose = { + // all units in meters and all rotations in degrees + // {x, y, rotation} {left pole}, {center tag}, {right pole} + + {{0.0, 0.0, 0.0}, {16.6972, 0.65532, 126}, {0.0, 0.0, 0.0}}, // TAG 1 + {{0.0, 0.0, 0.0}, {16.6972, 7.39648, 234}, {0.0, 0.0, 0.0}}, // TAG 2 + {{0.0, 0.0, 0.0}, {11.56081, 8.05561, 270}, {0.0, 0.0, 0.0}}, // TAG 3 + {{0.0, 0.0, 0.0}, {9.27608, 6.137656, 0.0}, {0.0, 0.0, 0.0}}, // TAG 4 + {{0.0, 0.0, 0.0}, {9.27608, 1.914906, 0.0}, {0.0, 0.0, 0.0}}, // TAG 5 + {{13.6319768, 2.7594306, 0.0}, {13.47445, 3.306318, 300}, {13.9166092, 2.9237686, 0.0}}, // TAG 6 + {{14.347698, 3.861562, 0.0}, {13.8905, 4.0259, 0.0}, {14.347698, 4.190238, 0.0}}, // TAG 7 + {{13.9166092, 5.128133, 0.0}, {13.47445, 4.745482, 60 }, {13.6319768, 5.292471, 0.0}}, // TAG 8 + {{12.4853192, 5.292471, 0.0}, {12.64336, 4.745482, 120}, {12.2006868, 5.128133, 0.0}}, // TAG 9 + {{11.769598, 4.190238, 0.0}, {12.22731, 4.0259, 180}, {11.769598, 3.861562, 0.0}}, // TAG 10 + {{12.2002296, 2.9247686, 0.0}, {12.64336, 3.306318, 240}, {12.4853192, 2.7594306, 0.0}}, // TAG 11 + {{0.0, 0.0, 0.0}, {0.851154, 0.65532, 54 }, {0.0, 0.0, 0.0}}, // TAG 12 + {{0.0, 0.0, 0.0}, {0.851154, 7.39648, 306}, {0.0, 0.0, 0.0}}, // TAG 13 + {{0.0, 0.0, 0.0}, {8.272272, 6.137656, 180}, {0.0, 0.0, 0.0}}, // TAG 14 + {{0.0, 0.0, 0.0}, {8.272272, 1.914906, 180}, {0.0, 0.0, 0.0}}, // TAG 15 + {{0.0, 0.0, 0.0}, {5.987542, -0.00381, 90 }, {0.0, 0.0, 0.0}}, // TAG 16 + {{3.6320984, 2.9237686, 0.0}, {4.073906, 3.306318, 240}, {3.9160704, 2.7594306, 0.0}}, // TAG 17 + {{3.2004, 4.190238, 0.0}, {3.6576, 4.0259, 180}, {3.2004, 3.861562, 0.0}}, // TAG 18 + {{3.9160704, 5.292471, 0.0}, {4.073906, 4.745482, 120}, {3.6320984, 5.128133, 0.0}}, // TAG 19 + {{5.3471572, 5.128133, 0.0}, {4.90474, 4.745482, 60 }, {5.0625248, 5.292471, 0.0}}, // TAG 20 + {{5.777738, 3.861562, 0.0}, {5.321046, 4.0259, 0.0}, {5.777738, 4.190238, 0.0}}, // TAG 21 + {{5.0625248, 2.7594306, 0.0}, {4.90474, 3.306318, 300}, {5.3465476, 2.9237686, 0.0}} // TAG 22 + }; + + // will eventually be seperatly adjusted + PIDController pidY = new PIDController(0.10, 0.00, 0.00); + PIDController pidX = new PIDController(0.10, 0.00, 0.00); + PIDController pidR = new PIDController(0.10, 0.00, 0.00); + + public double poleX; + public double poleY; + public double tagRotation; + /** Creates a new aimbot_two_point_o. */ + public aimbot_two_point_o(int side /** make sure that the number is either 1 or 3*/) { + poleX = TagPose[io.limelight.tag-1][side][X]; + poleY = TagPose[io.limelight.tag-1][side][Y]; + tagRotation = TagPose[io.limelight.tag][CENTRE][ROTATION]; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + double currentRotation = io.chassis.getYaw(); + double currentPositionX = io.chassis.pose().getX(); + double currentPositionY = io.chassis.pose().getY(); + + + io.chassis.drive(new ChassisSpeeds(pidX.calculate(currentPositionX, poleX), pidY.calculate(currentPositionY, poleY), pidR.calculate(currentRotation, tagRotation))); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return pidX.atSetpoint() && pidY.atSetpoint() && pidR.atSetpoint() || !LimelightHelpers.getTV("limelight-main"); + } +} diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index f287cbc..1bf46e8 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -4,15 +4,21 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Rotation; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; 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.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utility.IO; + public class Limelight extends SubsystemBase { double area; @@ -20,93 +26,16 @@ public class Limelight extends SubsystemBase { double y; double robotyaw; double latency; - int tag; - - public static final int LEFT = 0; - public static final int CENTRE = 1; - public static final int RIGHT = 2; - - public static final int X = 0; - public static final int Y = 1; - public static final int ROTATION = 2; - public static final double[][][] TagPose = { - { // TAG 1 **All locations in meters**All rotations in radians** - // LEFT POLE // CENTRE TAG // RIGHT POLE - {0.0, 0.0, 0.0}, {16.6972, 0.65532, 0.01745329}, {0.0, 0.0, 0.0} - }, - { // TAG 2 - // x y rotation - {0.0, 0.0, 0.0}, {16.6972, 7.39648, 4.08406986}, {0.0, 0.0, 0.0} - }, - { // TAG 3 - {0.0, 0.0, 0.0}, {11.56081, 8.05561, 4.7123883 }, {0.0, 0.0, 0.0} - }, - { // TAG 4 - {0.0, 0.0, 0.0}, {9.27608, 6.137656, 0.0 }, {0.0, 0.0, 0.0} - }, - { // TAG 5 - {0.0, 0.0, 0.0}, {9.27608, 1.914906, 0.0 }, {0.0, 0.0, 0.0} - }, - { // TAG 6 - {13.5914892, 4.620133, 0.0}, {13.47445, 3.306318, 5.235987 }, {13.3068568, 4.784471, 0.0} - }, - { // TAG 7 - {13.839698, 3.861562, 0.0}, {13.8905, 4.0259, 0.0 }, {13.839698, 4.190238, 0.0} - }, - { // TAG 8 - {13.3068568, 3.2674306, 0.0}, {13.47445, 4.745482, 1.0471974 }, {13.5914892, 3.4317686, 0.0} - }, - { // TAG 9 - {12.5253496, 3.4317686, 0.0}, {12.64336, 4.745482, 2.0943948 }, {12.8104392, 3.2674306, 0.0} - }, - { // TAG 10 - {12.277598, 4.190238, 0.0}, {12.22731, 4.0259, 3.1415922 }, {12.277598, 3.861562, 0.0} - }, - { // TAG 11 - {12.8104392, 4.784471, 0.0}, {12.64336, 3.306318, 4.1887896 }, {12.5258068, 4.620133, 0.0} - }, - { // TAG 12 - {0.0, 0.0, 0.0}, {0.851154, 0.65532, 0.94247766}, {0.0, 0.0, 0.0} - }, - { // TAG 13 - {0.0, 0.0, 0.0}, {0.851154, 7.39648, 5.34070674}, {0.0, 0.0, 0.0} - }, - { // TAG 14 - {0.0, 0.0, 0.0}, {8.272272, 6.137656, 3.1415922 }, {0.0, 0.0, 0.0} - }, - { // TAG 15 - {0.0, 0.0, 0.0}, {8.272272, 1.914906, 3.1415922 }, {0.0, 0.0, 0.0} - }, - { // TAG 16 - {0.0, 0.0, 0.0}, {5.987542, -0.00381, 1.5707961 }, {0.0, 0.0, 0.0} - }, - { // TAG 17 - {3.9572184, 3.4317686, 0.0}, {4.073906, 3.306318, 4.1887896 }, {4.2411904, 3.2674306, 0.0} - }, - { // TAG 18 - {3.7084, 4.190238, 0.0 }, {3.6576, 4.0259, 3.1415922 }, {3.7084, 3.861562, 0.0} - }, - { // TAG 19 - {4.2411904, 4.784471, 0.0}, {4.073906, 4.745482, 2.0943948 }, {3.9572184, 4.620133, 0.0} - }, - { // TAG 20 - {5.0220372, 4.620133, 0.0 }, {4.90474, 4.745482, 1.0471974 }, {4.7374048, 4.784471, 0.0} - }, - { // TAG 21 - {5.269738, 3.861562, 0.0 }, {5.321046, 4.0259, 0.0 }, {5.269738, 4.190238, 0.0} - }, - { // TAG 22 - {4.7374048, 3.2674306, 0.0}, {4.90474, 3.306318, 5.235987 }, {5.0214276, 3.4317686, 0.0} - } - }; + public int tag; - // EXAMPLE: TagPOse[1][RIGHT][X] + // EXAMPLE: TagPose[1][RIGHT][X] int[] tagLocation; final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); - - public Limelight() {} + IO io; + public Limelight() { + } @Override public void periodic() { @@ -122,6 +51,17 @@ public void periodic() { SmartDashboard.putNumber("Limelight Latency", latency); } + // outputs a pid value to fix the rotation of the robot towards the intended tag + // public void fixRotationAndLocation(PIDController pid,double poleX, double poleY) { + // double currentRotation = io.chassis.getYaw(); + // double currentPositionX = io.chassis.pose().getX(); + // double currentPositionY = io.chassis.pose().getY(); + // double tagRotation = TagPose[io.limelight.tag][CENTRE][ROTATION]; + + // // sets the chassis moving towards the target pole + // io.chassis.drive(new ChassisSpeeds(pid.calculate(currentPositionX, poleX),pid.calculate(currentPositionY, poleY), pid.calculate(currentRotation, tagRotation))); + // } + public boolean zoned(){ return ((tag > 11) ? Alliance.Blue : Alliance.Red) == DriverStation.getAlliance().get(); } From 080be22f250983254cfb1e754b1a8f65ef9650c6 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 16 Feb 2025 17:59:58 -0500 Subject: [PATCH 33/79] characterization --- src/main/java/frc/robot/RobotContainer.java | 5 ++++ .../java/frc/robot/subsystems/Swerve.java | 29 +++++++++++++++++++ src/main/java/frc/robot/swerve/Module.java | 4 +++ .../robot/utility/AutomatedController.java | 9 ++++-- 4 files changed, 45 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 467e220..93b3c6c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -27,6 +28,7 @@ public class RobotContainer { private final SendableChooser auto_selector; private Command current_auto = new PrintCommand("Temp"); + public RobotContainer() { main = new AutomatedController(0, io); backup = new AutomatedController(1, io); @@ -40,6 +42,9 @@ public RobotContainer() { SmartDashboard.putData("Main-Controller Mode", main.selector); SmartDashboard.putData("Backup-Controller Mode", main.selector); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + + SignalLogger.setPath("/media/sda1/ctre-logs/"); + // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); // SmartDashboard.putData("Autonomous", ); // TBD } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 15e3d9d..058db6d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,5 +1,10 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; @@ -17,11 +22,17 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.Util; @@ -192,6 +203,24 @@ public void setModuleStates(SwerveModuleState[] states) { } } + final MutVoltage appliedVoltage = Volts.mutable(0); + final MutDistance distance = Meters.mutable(0); + final MutLinearVelocity velocity = MetersPerSecond.mutable(0); + + + public final SysIdRoutine routine = new SysIdRoutine(new Config(), + new SysIdRoutine.Mechanism(voltage -> { + for (Module mod : modules) + mod.set(voltage.magnitude(), 0); + }, log -> { + for (int i = 0; i < 4; i++){ + log.motor(frc.robot.swerve.Swerve.Constants.LAYOUT_TITLE[i]) + .voltage(appliedVoltage.mut_replace(modules[i].voltage(), Volts)) + .linearPosition(distance.mut_replace(modules[i].drivePosition(), Meters)) + .linearVelocity(velocity.mut_replace(modules[i].velocity(), MetersPerSecond)); + } + }, this)); + public ChassisSpeeds getSpeeds() { return speeds; } diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 1d866d4..cb8fe7b 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -97,6 +97,10 @@ public double velocity() { return drive.getRotorVelocity().getValueAsDouble() * Swerve.PI2 * .502 * Units.inchesToMeters(3); } + public double voltage(){ + return drive.getMotorVoltage().getValueAsDouble(); + } + public double angle() { return encoder.angle(); } diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 307607e..67cb204 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class AutomatedController { public final CommandXboxController controller; @@ -45,7 +46,6 @@ public BooleanSupplier toggleMode(){ } public void configure(){ - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::toggleMode)); controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); @@ -55,7 +55,12 @@ public void configure(){ // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - // controller.y().and( automated() ).onTrue(Util.D + controller.x().onTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kForward)); + controller.a().onTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kReverse)); + controller.y().onTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kForward)); + controller.b().onTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kReverse)); + + controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect From adb35477e6af76b7d5477b2d41ee88d22cba34b1 Mon Sep 17 00:00:00 2001 From: Red Date: Mon, 17 Feb 2025 19:56:24 +0000 Subject: [PATCH 34/79] Characterises both Steer And Drive Motors now, will need to fix imports --- .../java/frc/robot/subsystems/Swerve.java | 27 ++++++++++++++----- src/main/java/frc/robot/swerve/Module.java | 10 +++++++ 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 058db6d..9a00b4a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,6 +3,8 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; @@ -24,6 +26,8 @@ import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DriverStation; @@ -203,9 +207,13 @@ public void setModuleStates(SwerveModuleState[] states) { } } - final MutVoltage appliedVoltage = Volts.mutable(0); - final MutDistance distance = Meters.mutable(0); - final MutLinearVelocity velocity = MetersPerSecond.mutable(0); + final MutVoltage[] driveVoltage = {Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0)}; + final MutDistance[] distance = {Meters.mutable(0), Meters.mutable(0), Meters.mutable(0), Meters.mutable(0)}; + final MutLinearVelocity[] velocity = {MetersPerSecond.mutable(0),MetersPerSecond.mutable(0),MetersPerSecond.mutable(0),MetersPerSecond.mutable(0)}; + + final MutVoltage[] steerVoltage = {Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0)}; + final MutAngle[] angle = {Radians.mutable(0),Radians.mutable(0),Radians.mutable(0),Radians.mutable(0)}; + final MutAngularVelocity[] angularVelocity = {RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0)}; public final SysIdRoutine routine = new SysIdRoutine(new Config(), @@ -214,10 +222,15 @@ public void setModuleStates(SwerveModuleState[] states) { mod.set(voltage.magnitude(), 0); }, log -> { for (int i = 0; i < 4; i++){ - log.motor(frc.robot.swerve.Swerve.Constants.LAYOUT_TITLE[i]) - .voltage(appliedVoltage.mut_replace(modules[i].voltage(), Volts)) - .linearPosition(distance.mut_replace(modules[i].drivePosition(), Meters)) - .linearVelocity(velocity.mut_replace(modules[i].velocity(), MetersPerSecond)); + log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + .voltage(driveVoltage[i].mut_replace(modules[i].voltage(), Volts)) + .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) + .linearVelocity(velocity[i].mut_replace(modules[i].velocity(), MetersPerSecond)); + + log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + .voltage(steerVoltage[i].mut_replace(modules[i].steerVoltage(), Volts)) + .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + .angularVelocity(angularVelocity[i].mut_replace(modules[i].steerVelocity(), RadiansPerSecond)); } }, this)); diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index cb8fe7b..fdb0381 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -9,6 +9,7 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.RadiansPerSecond; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -16,6 +17,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; + public class Module { public final TalonFX drive; public final SparkMax steer; @@ -105,6 +107,14 @@ public double angle() { return encoder.angle(); } + public double steerVelocity(){ + return steer.getVelocity().getValue().in(RadiansPerSecond); + } + + public double steerVoltage(){ + return steer.getMotorVoltage().getValueAsDouble(); + } + public void stop() { drive.stopMotor(); steer.stopMotor(); From 4bf5d405d56e6e660bc2c91f7ae6df885300c1ea Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Mon, 17 Feb 2025 16:21:27 -0500 Subject: [PATCH 35/79] Added Rumble --- .../robot/utility/AutomatedController.java | 9 +++ src/main/java/frc/robot/utility/Rumble.java | 80 ++++++++----------- .../frc/robot/utility/RumblePatterns.java | 25 ------ src/main/java/frc/robot/utility/Util.java | 9 +++ 4 files changed, 50 insertions(+), 73 deletions(-) delete mode 100644 src/main/java/frc/robot/utility/RumblePatterns.java diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index ef4fa5f..cd5532b 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -15,6 +15,7 @@ public class AutomatedController { public final SendableChooser selector = new SendableChooser(); public boolean manual = true; + Rumble rumble; IO io; public AutomatedController(int port, IO io){ @@ -26,6 +27,8 @@ public AutomatedController(int port, IO io){ selector.onChange((x) -> {x.run();}); // TODO: See if this allows us to have the selector always be up-to-date and add the same for auton testing controller = new CommandXboxController(port); + rumble = new Rumble(controller.getHID()); + controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry)); // controller.back().onTrue(Util.Do(io.elevator::rest)); @@ -71,6 +74,12 @@ public void configure(){ controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect + // controller.povRight().and(manual()).and(() -> {return !io.chassis.active;}).onTrue(Util.DoUntil( + // () -> { + // rumble.Run(.5, 0); + // io.chassis.zeroAbsolute(); + // }, rumble::End, rumble::finished, + // io.chassis)); // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } } diff --git a/src/main/java/frc/robot/utility/Rumble.java b/src/main/java/frc/robot/utility/Rumble.java index cc9037e..b0126a4 100644 --- a/src/main/java/frc/robot/utility/Rumble.java +++ b/src/main/java/frc/robot/utility/Rumble.java @@ -1,54 +1,38 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.utility; - import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj2.command.*; - -public class Rumble extends Command { - -Timer time = new Timer(); -double duration = 0; -boolean condition = false; -Runnable action; -GenericHID controller; -RumblePatterns pattern = new RumblePatterns(); -int type; - - public Rumble(int type, double duration, GenericHID controller, Runnable action) { - time.start(); - this.type = type; - this.controller = controller; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - // pattern.timer.start(); - action.run(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - pattern.Run(type, controller); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - controller.setRumble(RumbleType.kBothRumble, 0); - action.run(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return time.hasElapsed(duration) || condition; - } + +public class Rumble { + + public Timer timer = new Timer(); + + double duration; + GenericHID controller; + + public Rumble(GenericHID controller){ + this.controller = controller; + + } + + Runnable[] patterns = { + () -> { // Basic Shaking + controller.setRumble(RumbleType.kBothRumble, .25); + } + }; + + public void Run(double duration, int pattern){ + this.duration = duration; + timer.start(); + patterns[pattern].run(); + } + + public boolean finished(){ + return timer.hasElapsed(duration); + } + + public void End(){ + controller.setRumble(RumbleType.kBothRumble,0); + } } diff --git a/src/main/java/frc/robot/utility/RumblePatterns.java b/src/main/java/frc/robot/utility/RumblePatterns.java deleted file mode 100644 index 4994638..0000000 --- a/src/main/java/frc/robot/utility/RumblePatterns.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.utility; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; - -public class RumblePatterns { - - public Timer timer = new Timer(); - - public void Run(int pattern, GenericHID controller){ - switch(pattern){ - default: - controller.setRumble(RumbleType.kBothRumble, .5); - break; - - case 1: - if (timer.get()/5.0 % 2.0 == 0.0) - controller.setRumble(RumbleType.kBothRumble, .5); - else - controller.setRumble(RumbleType.kBothRumble, 0.0); - break; - } - } -} diff --git a/src/main/java/frc/robot/utility/Util.java b/src/main/java/frc/robot/utility/Util.java index a869e37..430961b 100644 --- a/src/main/java/frc/robot/utility/Util.java +++ b/src/main/java/frc/robot/utility/Util.java @@ -6,6 +6,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -40,6 +41,14 @@ public static InstantCommand Do(Runnable action, Subsystem... systems) { return new InstantCommand(action, systems); } + public static Command Do(Runnable action, Runnable end, Subsystem... systems) { + return Commands.runEnd(action, end, systems); + } + + public static Command DoUntil(Runnable action, Runnable end, BooleanSupplier Condition, Subsystem... systems) { + return Commands.runEnd(action, end, systems).until(Condition); + } + public static ConditionalCommand Do(Command onTrue, Command onFalse, BooleanSupplier condition){ return new ConditionalCommand(onTrue, onFalse, condition); } From 0d61bbd1107894c28379682cd6e282c71c595ce0 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Mon, 17 Feb 2025 22:55:17 -0500 Subject: [PATCH 36/79] elevator characterisation code ready --- .../java/frc/robot/subsystems/Elevator.java | 100 ++++++++++++------ .../java/frc/robot/subsystems/Swerve.java | 22 ++-- src/main/java/frc/robot/swerve/Module.java | 24 +++-- src/main/java/frc/robot/swerve/Swerve.java | 13 +++ 4 files changed, 108 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a10e74e..44e08c6 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,13 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; @@ -13,13 +21,21 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.*; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; public class Elevator extends SubsystemBase { - public SparkMax motor = new SparkMax(7, MotorType.kBrushless);; - public SparkMax follower = new SparkMax(8, MotorType.kBrushless); + // public SparkMax motor = new SparkMax(7, MotorType.kBrushless);; + // public SparkMax follower = new SparkMax(8, MotorType.kBrushless); + + TalonFX lead = new TalonFX(7); + TalonFX follow = new TalonFX(8); + SparkMaxConfig config = new SparkMaxConfig(); TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); @@ -27,6 +43,8 @@ public class Elevator extends SubsystemBase { double target = 0; boolean stopped = true; + PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); + // Target Heights public final double Rest = 0; public final double L2 = 0; @@ -37,49 +55,53 @@ public class Elevator extends SubsystemBase { public Elevator() { - config - .idleMode(SparkMaxConfig.IdleMode.kBrake) - .closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(0, 0, 0); + // config + // .idleMode(SparkMaxConfig.IdleMode.kBrake) + // .closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder) + // .pid(0, 0, 0); - motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - config.follow(motor, true); + // config.follow(motor, true); - follower.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // follower.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - zero(); + Slot0Configs config = new Slot0Configs(); + config.kP = 0.0; + config.kI = 0.0; + config.kD = 0.0; + + lead.getConfigurator().apply(config); + follow.setControl(new Follower(lead.getDeviceID(), false)); // TODO: Check if we need to invert } public void speed(double speed) { - motor.set(speed); + lead.set(speed); } public void volts(double volts) { - motor.setVoltage(volts); + lead.setVoltage(volts); } public void stop(){ stopped = true; - motor.stopMotor(); - } - - public void movePID(double height) { - motor.getClosedLoopController().setReference(height, ControlType.kPosition); + lead.stopMotor(); } public void move(double height){ // TODO: Checkout how adding a feedforward affects the results stopped = false; - target = Math.max( Math.min( height, 0 ), MAX_HEIGHT ); + target = Math.max( Math.min( height, 0 ), MAX_HEIGHT ); // TODO: Should be able to remove later because we know what heights we set the bot to time.restart(); } - public double elevatorPos() { - return motor.getEncoder().getPosition(); + public double position() { + // return motor.getEncoder().getPosition(); + return lead.getPosition().getValueAsDouble(); // TODO: Convert to metres or inches } public void zero(){ - motor.getEncoder().setPosition(0); + // motor.getEncoder().setPosition(0); + lead.setPosition(0); } public void rest(){ @@ -98,26 +120,42 @@ public void L4(){ move(L4); } + private Voltage leaderVoltage(){ + return lead.getMotorVoltage().getValue(); + } + + public final SysIdRoutine routine = new SysIdRoutine(new Config( + null, + Volts.of(4), + null, + (state) -> SignalLogger.writeString("state", state.toString()) + ), new Mechanism( + (volts) -> lead.setControl(new VoltageOut(volts.in(Volts))), + null, + this)); + @Override public void periodic() { if (stopped) return; State out = profile.calculate(time.get(), new State(L2, Barge), new State(target, 0)); - motor.getClosedLoopController().setReference(out.position, ControlType.kPosition); + // motor.getClosedLoopController().setReference(out.position, ControlType.kPosition); + lead.setControl(positionRequest.withPosition(out.position)); - // TODO: Maybe log Supplied Volts - SmartDashboard.putNumber("Elevator Height", motor.getEncoder().getPosition()); + SmartDashboard.putNumber("Elevator Voltage [Lead]", leaderVoltage().magnitude()); + SmartDashboard.putNumber("Elevator Height", position()); SmartDashboard.putNumber("Elevator Target Height", target); - SmartDashboard.putNumber("Elevator cTarget Height", out.position); + SmartDashboard.putNumber("Elevator Profile Target Height", out.position); - SmartDashboard.putNumber("Elevator Speed", motor.getEncoder().getVelocity()); - SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); + SmartDashboard.putNumber("Elevator Speed",lead.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Elevator Profile Target Velocity", out.velocity); - // DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); + // DogLog.log("Elevator/Voltage [Lead]", leaderVoltage().magnitude()) + // DogLog.log("Elevator/Height", position()); // DogLog.log("Elevator/Target Height", target); - // DogLog.log("Elevator/cTarget Height", out.position); - // DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); - // DogLog.log("Elevator/cTarget Velocity", out.velocity); + // DogLog.log("Elevator/Profiled Target Height", out.position); + // DogLog.log("Elevator/Profiled Speed", lead.getVelocity().getValueAsDouble()); + // DogLog.log("Elevator/Profiled Target Velocity", out.velocity); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 9a00b4a..5a5a760 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -221,17 +221,17 @@ public void setModuleStates(SwerveModuleState[] states) { for (Module mod : modules) mod.set(voltage.magnitude(), 0); }, log -> { - for (int i = 0; i < 4; i++){ - log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") - .voltage(driveVoltage[i].mut_replace(modules[i].voltage(), Volts)) - .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) - .linearVelocity(velocity[i].mut_replace(modules[i].velocity(), MetersPerSecond)); - - log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") - .voltage(steerVoltage[i].mut_replace(modules[i].steerVoltage(), Volts)) - .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - .angularVelocity(angularVelocity[i].mut_replace(modules[i].steerVelocity(), RadiansPerSecond)); - } + // for (int i = 0; i < 4; i++){ + // log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + // .voltage(modules[i].voltage()) + // .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) + // .linearVelocity(modules[i].velocity()); + + // log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + // .voltage(modules[i].steerVoltage()) + // .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + // .angularVelocity(modules[i].steerVelocity()); + // } }, this)); public ChassisSpeeds getSpeeds() { diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index fdb0381..526a2f0 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -9,7 +9,13 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; -import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -95,24 +101,24 @@ public double drivePosition() { return drive.getPosition().getValueAsDouble() * .432 * WHEEL_DIAMETER; } - public double velocity() { - return drive.getRotorVelocity().getValueAsDouble() * Swerve.PI2 * .502 * Units.inchesToMeters(3); + public LinearVelocity velocity() { + return MetersPerSecond.of(drive.getVelocity().getValueAsDouble() * Swerve.PI2 * .432 * WHEEL_DIAMETER); } - public double voltage(){ - return drive.getMotorVoltage().getValueAsDouble(); + public Voltage voltage(){ + return drive.getMotorVoltage().getValue(); } public double angle() { return encoder.angle(); } - public double steerVelocity(){ - return steer.getVelocity().getValue().in(RadiansPerSecond); + public AngularVelocity steerVelocity(){ + return encoder.velocity(); } - public double steerVoltage(){ - return steer.getMotorVoltage().getValueAsDouble(); + public Voltage steerVoltage(){ + return Volts.of(steer.getBusVoltage()); } public void stop() { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 82f6e84..01ce310 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -7,7 +7,11 @@ import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; +import edu.wpi.first.units.AngularAccelerationUnit; +import edu.wpi.first.units.measure.AngularVelocity; + import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import java.io.IOException; @@ -101,6 +105,7 @@ public interface Encoder { public void zero(); public boolean connected(); public double angle(); + public AngularVelocity velocity(); } public static class Cancoder implements Encoder { @@ -128,6 +133,10 @@ public boolean connected(){ public double angle() { return ((encoder.getAbsolutePosition().getValue().in(Radians) + PI2) % PI2); } + + public AngularVelocity velocity(){ + return encoder.getVelocity().getValue(); + } } public static class Canand implements Encoder { @@ -152,5 +161,9 @@ public boolean connected(){ public double angle() { return (encoder.getAbsPosition() * PI2) % PI2; } + + public AngularVelocity velocity(){ + return RadiansPerSecond.of(encoder.getVelocity()); + } } } From cd816237b4943403628b8e835dda8cd43c1c658e Mon Sep 17 00:00:00 2001 From: willtreaty21 Date: Tue, 18 Feb 2025 15:35:10 -0500 Subject: [PATCH 37/79] Finished Auto-Align setup, PID tuning next --- .../commands/{aimbot.java => Aimbot.java} | 4 +- ...aimbot_two_point_o.java => AutoAlign.java} | 63 +++++++++++++++---- .../robot/utility/AutomatedController.java | 8 ++- 3 files changed, 60 insertions(+), 15 deletions(-) rename src/main/java/frc/robot/commands/{aimbot.java => Aimbot.java} (95%) rename src/main/java/frc/robot/commands/{aimbot_two_point_o.java => AutoAlign.java} (71%) diff --git a/src/main/java/frc/robot/commands/aimbot.java b/src/main/java/frc/robot/commands/Aimbot.java similarity index 95% rename from src/main/java/frc/robot/commands/aimbot.java rename to src/main/java/frc/robot/commands/Aimbot.java index ccdbdcd..7e5cbda 100644 --- a/src/main/java/frc/robot/commands/aimbot.java +++ b/src/main/java/frc/robot/commands/Aimbot.java @@ -12,12 +12,12 @@ import frc.robot.utility.LimelightHelpers; /* 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 aimbot extends Command { +public class Aimbot extends Command { IO io; PIDController pid = new PIDController(0.1, 0.00, 0.00); - public aimbot(IO io) { + public Aimbot(IO io) { this.io = io; pid.setTolerance(10); addRequirements(io.limelight, io.chassis); diff --git a/src/main/java/frc/robot/commands/aimbot_two_point_o.java b/src/main/java/frc/robot/commands/AutoAlign.java similarity index 71% rename from src/main/java/frc/robot/commands/aimbot_two_point_o.java rename to src/main/java/frc/robot/commands/AutoAlign.java index 2d914f0..5f043a5 100644 --- a/src/main/java/frc/robot/commands/aimbot_two_point_o.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -5,12 +5,13 @@ package frc.robot.commands; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.utility.IO; -import frc.robot.utility.LimelightHelpers; /* 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 aimbot_two_point_o extends Command { +public class AutoAlign extends Command { IO io; public static final int LEFT = 0; @@ -22,7 +23,7 @@ public class aimbot_two_point_o extends Command { public static final int ROTATION = 2; public static final double[][][] TagPose = { // all units in meters and all rotations in degrees - // {x, y, rotation} {left pole}, {center tag}, {right pole} + // {x, y, rotation} {left pole}, {center tag}, {right pole} **origin of blue reef is (4.48932, 4.0259), origin of red reef is (13.05831, 4.0259) {{0.0, 0.0, 0.0}, {16.6972, 0.65532, 126}, {0.0, 0.0, 0.0}}, // TAG 1 {{0.0, 0.0, 0.0}, {16.6972, 7.39648, 234}, {0.0, 0.0, 0.0}}, // TAG 2 @@ -56,15 +57,53 @@ public class aimbot_two_point_o extends Command { public double poleX; public double poleY; public double tagRotation; - /** Creates a new aimbot_two_point_o. */ - public aimbot_two_point_o(int side /** make sure that the number is either 1 or 3*/) { - poleX = TagPose[io.limelight.tag-1][side][X]; - poleY = TagPose[io.limelight.tag-1][side][Y]; - tagRotation = TagPose[io.limelight.tag][CENTRE][ROTATION]; + int side; + int tag; + int zone; + + public AutoAlign(int side /** make sure that the number is either 0 or 2*/) { + this.side = side; + pidR.enableContinuousInput(0.0, 360.0); // CHECK IF THIS ACTUALLY ALLOWS WRAPPING } @Override - public void initialize() {} + public void initialize() { + // find zone first + Boolean cZone = DriverStation.getAlliance().get() == Alliance.Blue; + zone = (cZone)? + (int) Math.atan((io.chassis.pose().getY()- 4.0259)/(io.chassis.pose().getX()-4.48932)): + (int) Math.atan((io.chassis.pose().getY()- 4.0259)/(io.chassis.pose().getX()-13.05831)); + zone = zone/60; + + switch (zone) { + case 1: + tag = (cZone)? 20: 8; + break; + case 2: + tag = (cZone)? 19: 9; + break; + case 3: + tag = (cZone)? 18: 10; + break; + case 4: + tag = (cZone)? 17: 11; + break; + case 5: + tag = (cZone)? 22: 6; + break; + default: + tag = (cZone)? 21: 7; + break; + } + // then check if the zone + + // tag = zone + // poleX = TagPose[tag][side][X]; + // poleY = TagPose[tag][side][Y]; + poleX = TagPose[tag - 1][side][X]; + poleY = TagPose[tag - 1][side][Y]; + tagRotation = TagPose[tag - 1][CENTRE][ROTATION]; + } @Override public void execute() { @@ -77,10 +116,12 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + io.chassis.drive(new ChassisSpeeds()); + } @Override public boolean isFinished() { - return pidX.atSetpoint() && pidY.atSetpoint() && pidR.atSetpoint() || !LimelightHelpers.getTV("limelight-main"); + return (pidX.atSetpoint() && pidY.atSetpoint() && pidR.atSetpoint()); } } diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index ef4fa5f..ee7ca8c 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -8,7 +8,8 @@ import frc.robot.commands.ReleaseAlgae; import frc.robot.commands.ScoreBarge; import frc.robot.commands.ScoreReef; -import frc.robot.commands.aimbot; +import frc.robot.commands.Aimbot; +import frc.robot.commands.AutoAlign; public class AutomatedController { public final CommandXboxController controller; @@ -62,10 +63,13 @@ public void configure(){ // controller.leftBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, false), new ScoreBarge(io), // io.limelight::reefZone)); + controller.leftBumper().onTrue(new AutoAlign(0)); + controller.rightBumper().onTrue(new AutoAlign(2)); + // controller.rightBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, true), new ReleaseAlgae(io), // io.limelight::reefZone)); - controller.a().toggleOnTrue(new aimbot(io)); + controller.a().toggleOnTrue(new Aimbot(io)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); From 6895e00414b182cbd8bd25638ff22b208a86c0a9 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Wed, 19 Feb 2025 17:47:31 -0500 Subject: [PATCH 38/79] Push --- .../java/frc/robot/subsystems/Elevator.java | 20 ++++------- src/main/java/frc/robot/subsystems/Hang.java | 4 +-- .../java/frc/robot/subsystems/Limelight.java | 7 ---- .../java/frc/robot/subsystems/Swerve.java | 36 +++++++------------ 4 files changed, 20 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a10e74e..d7edba2 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.*; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Elevator extends SubsystemBase { @@ -106,18 +105,11 @@ public void periodic() { motor.getClosedLoopController().setReference(out.position, ControlType.kPosition); // TODO: Maybe log Supplied Volts - SmartDashboard.putNumber("Elevator Height", motor.getEncoder().getPosition()); - - SmartDashboard.putNumber("Elevator Target Height", target); - SmartDashboard.putNumber("Elevator cTarget Height", out.position); - - SmartDashboard.putNumber("Elevator Speed", motor.getEncoder().getVelocity()); - SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); - - // DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); - // DogLog.log("Elevator/Target Height", target); - // DogLog.log("Elevator/cTarget Height", out.position); - // DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); - // DogLog.log("Elevator/cTarget Velocity", out.velocity); + DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); + DogLog.log("Elevator/Target Height", target); + DogLog.log("Elevator/Profiled Target Height", out.position); + DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); + DogLog.log("Elevator/Profiled Target Velocity", out.velocity); + // DogLog.log("Elevator/Supplied Voltage [Lead]", ); } } diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index ea06033..7fbe228 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -3,7 +3,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { @@ -29,6 +29,6 @@ public void stopHang() { @Override public void periodic() { - SmartDashboard.putNumber("Hang Pos", hang.getPosition().getValueAsDouble()); + DogLog.log("Hang/Position", hang.getPosition().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 1bf46e8..be613fb 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -4,13 +4,6 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Rotation; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -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.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 2ab7530..ae9b544 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,7 +6,6 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import dev.doglog.DogLog; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -228,28 +227,17 @@ public void periodic() { Pose2d pose = odometry.update(rotation(), modulePositions()); posePublisher.set(pose); - // SmartDashboard.putNumber("X position", pose.getX()); - // SmartDashboard.putNumber("Y position", pose.getY()); - - // SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); - // SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Pitch", - // pigeon2.getPitch().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Roll", - // pigeon2.getRoll().getValueAsDouble()); - - // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); - - // DogLog.log("Swerve/current_states", moduleStates(modules)); - // DogLog.log("Swerve/target_states", states); - // DogLog.log("Swerve/pose", pose); - // DogLog.log("Swerve/X_position", pose.getX()); - // DogLog.log("Swerve/Y_position", pose.getY()); - // DogLog.log("Swerve/Odometry_rotation", rotation().getDegrees()); - // DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); - // DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); - // DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); - // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : - // "Robot-Oriented"); + DogLog.log("Swerve/Current States", moduleStates(modules)); + DogLog.log("Swerve/Target States", states); + + DogLog.log("Swerve/X Position", pose.getX()); + DogLog.log("Swerve/Y Position", pose.getY()); + DogLog.log("Swerve/Pose", pose); + DogLog.log("Swerve/Odometry Rotation", rotation().getDegrees()); + + DogLog.log("Swerve/Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); + DogLog.log("Swerve/Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); + DogLog.log("Swerve/Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); + DogLog.log("Swerve/Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } } \ No newline at end of file From b3510d4c0b068c89f09204a87640c73aaefabb8e Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Wed, 19 Feb 2025 22:23:50 -0500 Subject: [PATCH 39/79] Updating Claw Code, Dog Log Uncommented and actively disabled, Added scoring commands and will need to add them to controller soon --- src/main/java/frc/robot/RobotContainer.java | 13 +-- .../java/frc/robot/commands/GrabAlgae.java | 31 ------- src/main/java/frc/robot/commands/Intake.java | 63 ++++++++++++++ .../java/frc/robot/commands/ReleaseAlgae.java | 36 -------- .../{ScoreBarge.java => ScoreAlgae.java} | 20 +++-- .../java/frc/robot/commands/ScoreReef.java | 13 +-- .../frc/robot/subsystems/AlgaeIntake.java | 46 ---------- src/main/java/frc/robot/subsystems/Claw.java | 86 +++++++++++++++++++ .../java/frc/robot/subsystems/Elevator.java | 26 ++++++ src/main/java/frc/robot/swerve/Swerve.java | 5 -- .../robot/utility/AutomatedController.java | 3 - src/main/java/frc/robot/utility/IO.java | 2 +- src/main/java/frc/robot/utility/Util.java | 6 ++ 13 files changed, 209 insertions(+), 141 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/GrabAlgae.java create mode 100644 src/main/java/frc/robot/commands/Intake.java delete mode 100644 src/main/java/frc/robot/commands/ReleaseAlgae.java rename src/main/java/frc/robot/commands/{ScoreBarge.java => ScoreAlgae.java} (51%) delete mode 100644 src/main/java/frc/robot/subsystems/AlgaeIntake.java create mode 100644 src/main/java/frc/robot/subsystems/Claw.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4fe0e1b..6616da6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,8 +5,6 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -27,7 +25,7 @@ public class RobotContainer { private final SendableChooser auto_selector; - private Command current_auto = new PrintCommand("Temp"); + private Command current_auto = new PrintCommand(""); public RobotContainer() { main = new AutomatedController(0, io); @@ -42,8 +40,13 @@ public RobotContainer() { SmartDashboard.putData("Main-Controller Mode", main.selector); SmartDashboard.putData("Backup-Controller Mode", main.selector); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); - // DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); - // SmartDashboard.putData("Autonomous", ); // TBD + + DogLog.setOptions(new DogLogOptions() + .withCaptureDs(true) + .withCaptureConsole(true) + .withLogExtras(true)); + + DogLog.setEnabled( false); // TODO: Turn back on when we're testing proper } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/GrabAlgae.java b/src/main/java/frc/robot/commands/GrabAlgae.java deleted file mode 100644 index 6a9cf60..0000000 --- a/src/main/java/frc/robot/commands/GrabAlgae.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utility.IO; - -public class GrabAlgae extends Command { - IO io; - - public GrabAlgae(IO io) { - this.io = io; - addRequirements(io.intake); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - io.intake.volts(12); - } - - @Override - public void end(boolean interrupted) { - io.intake.stop(); - } - - @Override - public boolean isFinished() { - return io.intake.grabbed(); - } -} diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java new file mode 100644 index 0000000..7e4af40 --- /dev/null +++ b/src/main/java/frc/robot/commands/Intake.java @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; + +/* 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 Intake extends Command { + + Runnable intake; + Runnable stop; + BooleanSupplier holding; + double angle; + + // public static int INTAKE_CORAL = -1; + // public static int INTAKE_ALGAE = -2; + // public static int SCORE_CORAL = 1; + // public static int SCORE_ALGAE = 2; + // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP + + public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio + + if (coral){ + intake = () -> io.claw.speedCoral( (release) ? .4 : -.4); + stop = io.claw::stopCoral; + holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); + } else { // Algae + intake = () -> { + io.claw.speedAlgae(1); + // TODO: Set Claw to Reef intake Angle or Ground pickup if we + }; + stop = io.claw::stopAlgae; + holding = () -> (release) ? io.claw.hasAlgae() : !io.claw.hasAlgae(); + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intake.run(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + stop.run(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return holding.getAsBoolean(); + } +} diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java deleted file mode 100644 index ef84797..0000000 --- a/src/main/java/frc/robot/commands/ReleaseAlgae.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utility.IO; - -public class ReleaseAlgae extends Command { - IO io; - - double speed; - - public ReleaseAlgae(IO io) { - this.io = io; - - speed = 12; - - addRequirements(io.intake); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - io.intake.volts(speed); - } - - @Override - public void end(boolean interrupted) { - io.intake.stop(); - } - - @Override - public boolean isFinished() { - return !io.intake.grabbed(); - } -} diff --git a/src/main/java/frc/robot/commands/ScoreBarge.java b/src/main/java/frc/robot/commands/ScoreAlgae.java similarity index 51% rename from src/main/java/frc/robot/commands/ScoreBarge.java rename to src/main/java/frc/robot/commands/ScoreAlgae.java index d08e587..0932d02 100644 --- a/src/main/java/frc/robot/commands/ScoreBarge.java +++ b/src/main/java/frc/robot/commands/ScoreAlgae.java @@ -5,18 +5,20 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class ScoreBarge extends ParallelCommandGroup { - /** Creates a new ScoreBarge. */ - public ScoreBarge(IO io) { - addCommands( - // TODO: Move to Height - // TODO: Release Algae - // TODO: Move back to Resting Position +public class ScoreAlgae extends ParallelCommandGroup { + + public ScoreAlgae(IO io, boolean barge) { + addCommands( + //TODO: Check if we have Algae + io.elevator.move((barge) ? 5 : 0), + new AutoAlign(1), + new WaitUntilCommand(io.elevator::atPosition), + new Intake(io, false, true), + io.elevator.move(0) ); } } diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 2f9bd41..4c73d43 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -5,16 +5,19 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ScoreReef extends SequentialCommandGroup { - public ScoreReef(IO io, boolean score_right) { + public ScoreReef(IO io, boolean score_right, int Level) { addCommands( - + // TODO: Check if we have coral & if we don't have + io.elevator.move(Level), + new AutoAlign((score_right) ? 2 : 1), + new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height + new Intake(io, true, true), // TODO: Set to Reef Scoring Angle + io.elevator.move(0) ); } } diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java deleted file mode 100644 index 1f0829d..0000000 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; - -import dev.doglog.DogLog; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class AlgaeIntake extends SubsystemBase { - public SparkMax roller = new SparkMax(9, MotorType.kBrushless); - public DigitalInput beamBreak = new DigitalInput(0); - SparkMaxConfig rollerConfig = new SparkMaxConfig(); - - public AlgaeIntake() { - rollerConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); - - roller.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void volts(double volts){ - roller.setVoltage(volts); - } - - public void speed(double speed){ - roller.setVoltage(speed); - } - - public void stop(){ - roller.stopMotor(); - } - - public boolean grabbed(){ - return beamBreak.get(); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Intake Full", grabbed()); - // DogLog.log("Intake/Full", grabbed()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java new file mode 100644 index 0000000..9172c1a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; + +import dev.doglog.DogLog; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Claw extends SubsystemBase { + SparkMax algaeIntake = new SparkMax(9, MotorType.kBrushless); + TalonFX coralIntake = new TalonFX(9); + SparkMax pivot = new SparkMax(10, MotorType.kBrushless); + + DigitalInput algaeBreak = new DigitalInput(0); + DigitalInput coralBreak = new DigitalInput(1); + SparkMaxConfig rollerConfig = new SparkMaxConfig(); + + TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); + Timer time = new Timer(); + double target = 0; + boolean stopped = true; + + public Claw() { + rollerConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); + + algaeIntake.configure(rollerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void voltsAlgae(double volts){ + algaeIntake.setVoltage(volts); + } + + public void speedAlgae(double speed){ + algaeIntake.setVoltage(speed); + } + + public void voltsCoral(double volts){ + coralIntake.setVoltage(volts); + } + + public void speedCoral(double speed){ + coralIntake.setVoltage(speed); + } + + public void scoreCoral(){ + coralIntake.set(.4); + } + + public void stopCoral(){ + coralIntake.set(0); + } + + public void scoreAlgae(){ + algaeIntake.set(1); + } + + public void stopAlgae(){ + algaeIntake.set(0); + } + + public void stop(){ + algaeIntake.stopMotor(); + } + + public boolean hasAlgae(){ + return algaeBreak.get(); + } + + public boolean hasCoral(){ + return coralBreak.get(); + } + + @Override + public void periodic() { + DogLog.log("Claw/Algae Full", hasAlgae()); + DogLog.log("Claw/Coral Full", hasCoral()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index d7edba2..2f63440 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.*; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Elevator extends SubsystemBase { @@ -73,6 +74,23 @@ public void move(double height){ // TODO: Checkout how adding a feedforward affe time.restart(); } + public InstantCommand move(int level){ + return new InstantCommand(() -> { + switch (level) { + case 2: L2(); + break; + case 3: L3(); + break; + case 4: L4(); + break; + case 5: Barge(); + break; + default: rest(); // LEVEL 1 // TODO: See if we need to change the height we go to + break; + } + },this); + } + public double elevatorPos() { return motor.getEncoder().getPosition(); } @@ -97,6 +115,14 @@ public void L4(){ move(L4); } + public void Barge(){ + move(Barge); + } + + public boolean atPosition(){ + return profile.isFinished(time.get()); + } + @Override public void periodic() { if (stopped) return; diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 424f491..2b300df 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -3,15 +3,10 @@ import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; import com.reduxrobotics.sensors.canandmag.Canandmag; import com.reduxrobotics.sensors.canandmag.CanandmagSettings; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.struct.parser.ParseException; - import static edu.wpi.first.units.Units.Radians; import java.io.IOException; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index b33cd1c..37c0a4f 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,9 +5,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.ReleaseAlgae; -import frc.robot.commands.ScoreBarge; -import frc.robot.commands.ScoreReef; import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index b777fe6..e4f6713 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -11,7 +11,7 @@ public class IO extends SubsystemBase { // public final Hang hang = new Hang(); public final Limelight limelight = new Limelight(); - public final AlgaeIntake intake = null; + public final Claw claw = null; public final Elevator elevator = null; public final Hang hang = null; diff --git a/src/main/java/frc/robot/utility/Util.java b/src/main/java/frc/robot/utility/Util.java index 430961b..df4e543 100644 --- a/src/main/java/frc/robot/utility/Util.java +++ b/src/main/java/frc/robot/utility/Util.java @@ -45,6 +45,12 @@ public static Command Do(Runnable action, Runnable end, Subsystem... systems) { return Commands.runEnd(action, end, systems); } + public static InstantCommand blank = new InstantCommand(); + + public static Command DoCheck(Command cmd, BooleanSupplier condition, Subsystem... systems){ + return new ConditionalCommand(cmd, blank, condition); + } + public static Command DoUntil(Runnable action, Runnable end, BooleanSupplier Condition, Subsystem... systems) { return Commands.runEnd(action, end, systems).until(Condition); } From 7e9fab9f4710f6130f6779f120614b9e81ab2b26 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Wed, 19 Feb 2025 22:25:09 -0500 Subject: [PATCH 40/79] X and Y are swapped in WPILib coordinates --- src/main/java/frc/robot/commands/AutoAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java index 5f043a5..2aab8b9 100644 --- a/src/main/java/frc/robot/commands/AutoAlign.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -112,7 +112,7 @@ public void execute() { double currentPositionY = io.chassis.pose().getY(); - io.chassis.drive(new ChassisSpeeds(pidX.calculate(currentPositionX, poleX), pidY.calculate(currentPositionY, poleY), pidR.calculate(currentRotation, tagRotation))); + io.chassis.drive(new ChassisSpeeds(pidY.calculate(currentPositionY, poleY), pidX.calculate(currentPositionX, poleX), pidR.calculate(currentRotation, tagRotation))); } @Override From ccceaa4a722df425b4a60f2d3236bd27b2e74c9c Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 20 Feb 2025 15:44:07 -0500 Subject: [PATCH 41/79] Pose Estimation --- build.gradle | 2 +- .../java/frc/robot/commands/AutoAlign.java | 3 +- .../java/frc/robot/subsystems/Swerve.java | 35 ++++++++++++++----- .../robot/utility/AutomatedController.java | 5 +-- 4 files changed, 33 insertions(+), 12 deletions(-) diff --git a/build.gradle b/build.gradle index f37724f..b25fd8e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" } java { diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java index 2aab8b9..a2b9859 100644 --- a/src/main/java/frc/robot/commands/AutoAlign.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -61,8 +61,9 @@ public class AutoAlign extends Command { int tag; int zone; - public AutoAlign(int side /** make sure that the number is either 0 or 2*/) { + public AutoAlign(int side /** make sure that the number is either 0 or 2*/, IO io) { this.side = side; + this.io = io; pidR.enableContinuousInput(0.0, 360.0); // CHECK IF THIS ACTUALLY ALLOWS WRAPPING } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ae9b544..36f90bc 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,6 +6,8 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import dev.doglog.DogLog; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -24,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.swerve.Module; import frc.robot.swerve.Swerve.Constants; +import frc.robot.utility.LimelightHelpers; import frc.robot.utility.Util; public class Swerve extends SubsystemBase { @@ -33,14 +36,17 @@ public class Swerve extends SubsystemBase { public final Pigeon2 pigeon2 = new Pigeon2(Constants.PIGEON_ID); - // public SwerveDrivePoseEstimator poseEstimator; StructArrayPublisher current_states = Util.table .getStructArrayTopic("Current Module States", SwerveModuleState.struct).publish(); StructArrayPublisher target_states = Util.table .getStructArrayTopic("Target Module States", SwerveModuleState.struct).publish(); StructPublisher posePublisher = NetworkTableInstance.getDefault().getTable("Debug") .getStructTopic("Current pose", Pose2d.struct).publish(); + StructPublisher estimatedPosePublisher = Util.table + .getStructTopic("Estimated Pose", Pose2d.struct).publish(); + SwerveDrivePoseEstimator estimator; + final SwerveDriveOdometry odometry; final Module[] modules = new Module[4]; ChassisSpeeds speeds = new ChassisSpeeds(); @@ -55,8 +61,6 @@ public Swerve() { createTranslation(-constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), createTranslation(-constants.TRACKWIDTH / 2.0, -constants.WHEELBASE / 2.0)); - // poseEstimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), pose()); - ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); for (int i = 0; i < modules.length; i++) { modules[i] = new Module( @@ -69,6 +73,11 @@ public Swerve() { constants.comp); } + odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), + new Pose2d(0, 0, new Rotation2d())); + + estimator = new SwerveDrivePoseEstimator(kinematics, rotation(), modulePositions(), odometry.getPoseMeters()); + AutoBuilder.configure( this::pose, this::resetOdometry, @@ -76,9 +85,7 @@ public Swerve() { (speeds, feedforwards) -> drive(speeds), new PPHolonomicDriveController( new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID - // constants new PIDConstants(constants.ThetaControllerP, 0, constants.ThetaControllerD, 0.0) // Rotation PID - // constants ), constants.autoConfig, () -> { @@ -86,9 +93,6 @@ public Swerve() { return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; }, this); - - odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), - new Pose2d(0, 0, new Rotation2d())); } private Translation2d createTranslation(double x, double y) { @@ -157,6 +161,7 @@ public void adjustRotation() { public Pose2d pose() { return odometry.getPoseMeters(); + // return estimatePose(); } public void resetOdometry() { @@ -216,6 +221,18 @@ public void toggle() { } } + public Pose2d estimatePose() { + LimelightHelpers.SetRobotOrientation("limelight-main", absoluteRotation(), 0,0,0,0,0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); + if (mt2 == null) return new Pose2d(); + if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ + estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); + estimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); + estimatedPosePublisher.set(mt2.pose); + } + return mt2.pose; + } + public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) @@ -225,8 +242,10 @@ public void periodic() { target_states.set(states); Pose2d pose = odometry.update(rotation(), modulePositions()); + estimatePose(); posePublisher.set(pose); + DogLog.log("Swerve/Current States", moduleStates(modules)); DogLog.log("Swerve/Target States", states); diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 37c0a4f..e8ac168 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -63,13 +63,14 @@ public void configure(){ // controller.leftBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, false), new ScoreBarge(io), // io.limelight::reefZone)); - controller.leftBumper().onTrue(new AutoAlign(0)); - controller.rightBumper().onTrue(new AutoAlign(2)); + controller.leftBumper().onTrue(new AutoAlign(0, io)); + controller.rightBumper().onTrue(new AutoAlign(2, io)); // controller.rightBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, true), new ReleaseAlgae(io), // io.limelight::reefZone)); controller.a().toggleOnTrue(new Aimbot(io)); + controller.b().toggleOnTrue(new AutoAlign(0, io)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); From 3afc6aa482ceacc74770955def8d9ff0ff4f7fb7 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 20 Feb 2025 17:52:53 -0500 Subject: [PATCH 42/79] Tuning Positioning --- src/main/java/frc/robot/commands/AutoAlign.java | 2 +- src/main/java/frc/robot/commands/ScoreAlgae.java | 2 +- src/main/java/frc/robot/commands/ScoreReef.java | 2 +- src/main/java/frc/robot/subsystems/Swerve.java | 6 ++++-- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoAlign.java b/src/main/java/frc/robot/commands/AutoAlign.java index a2b9859..2a69041 100644 --- a/src/main/java/frc/robot/commands/AutoAlign.java +++ b/src/main/java/frc/robot/commands/AutoAlign.java @@ -52,7 +52,7 @@ public class AutoAlign extends Command { // will eventually be seperatly adjusted PIDController pidY = new PIDController(0.10, 0.00, 0.00); PIDController pidX = new PIDController(0.10, 0.00, 0.00); - PIDController pidR = new PIDController(0.10, 0.00, 0.00); + PIDController pidR = new PIDController(0.01, 0.00, 0.00); public double poleX; public double poleY; diff --git a/src/main/java/frc/robot/commands/ScoreAlgae.java b/src/main/java/frc/robot/commands/ScoreAlgae.java index 0932d02..9b50e50 100644 --- a/src/main/java/frc/robot/commands/ScoreAlgae.java +++ b/src/main/java/frc/robot/commands/ScoreAlgae.java @@ -15,7 +15,7 @@ public ScoreAlgae(IO io, boolean barge) { addCommands( //TODO: Check if we have Algae io.elevator.move((barge) ? 5 : 0), - new AutoAlign(1), + new AutoAlign(1, io), new WaitUntilCommand(io.elevator::atPosition), new Intake(io, false, true), io.elevator.move(0) diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 4c73d43..9bd44bc 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -14,7 +14,7 @@ public ScoreReef(IO io, boolean score_right, int Level) { addCommands( // TODO: Check if we have coral & if we don't have io.elevator.move(Level), - new AutoAlign((score_right) ? 2 : 1), + new AutoAlign((score_right) ? 2 : 1, io), new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height new Intake(io, true, true), // TODO: Set to Reef Scoring Angle io.elevator.move(0) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 36f90bc..f8e310d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -222,13 +222,15 @@ public void toggle() { } public Pose2d estimatePose() { - LimelightHelpers.SetRobotOrientation("limelight-main", absoluteRotation(), 0,0,0,0,0); + estimator.update(rotation(), modulePositions()); + LimelightHelpers.SetRobotOrientation("limelight-main", estimator.getEstimatedPosition().getRotation().getRotations(), 0,0,0,0,0); LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); if (mt2 == null) return new Pose2d(); if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); estimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); - estimatedPosePublisher.set(mt2.pose); + // estimatedPosePublisher.set(mt2.pose); + estimatedPosePublisher.set(estimator.getEstimatedPosition()); } return mt2.pose; } From 094946e2f06207629d5b5a89e1541e3421e7954b Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Thu, 20 Feb 2025 17:59:45 -0500 Subject: [PATCH 43/79] Slowed down --- .../java/frc/robot/subsystems/Elevator.java | 3 +- .../java/frc/robot/subsystems/Swerve.java | 30 +++++++++++-------- src/main/java/frc/robot/swerve/Swerve.java | 6 ++-- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 44e08c6..eb5af88 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; @@ -127,7 +128,7 @@ private Voltage leaderVoltage(){ public final SysIdRoutine routine = new SysIdRoutine(new Config( null, Volts.of(4), - null, + Seconds.of(5), (state) -> SignalLogger.writeString("state", state.toString()) ), new Mechanism( (volts) -> lead.setControl(new VoltageOut(volts.in(Volts))), diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 5a5a760..424c0e9 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -215,23 +215,27 @@ public void setModuleStates(SwerveModuleState[] states) { final MutAngle[] angle = {Radians.mutable(0),Radians.mutable(0),Radians.mutable(0),Radians.mutable(0)}; final MutAngularVelocity[] angularVelocity = {RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0)}; - - public final SysIdRoutine routine = new SysIdRoutine(new Config(), + public final SysIdRoutine routine = new SysIdRoutine(new Config( + null, + Volts.of(4), + null, + null + ), new SysIdRoutine.Mechanism(voltage -> { for (Module mod : modules) mod.set(voltage.magnitude(), 0); }, log -> { - // for (int i = 0; i < 4; i++){ - // log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") - // .voltage(modules[i].voltage()) - // .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) - // .linearVelocity(modules[i].velocity()); - - // log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") - // .voltage(modules[i].steerVoltage()) - // .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - // .angularVelocity(modules[i].steerVelocity()); - // } + for (int i = 0; i < 4; i++){ + log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + .voltage(modules[i].voltage()) + .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) + .linearVelocity(modules[i].velocity()); + + log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + .voltage(modules[i].steerVoltage()) + .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + .angularVelocity(modules[i].steerVelocity()); + } }, this)); public ChassisSpeeds getSpeeds() { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 01ce310..9749ec4 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -36,8 +36,10 @@ public static class Constants { public double rotFactor = .30; // .6 for tristan // AUTON CONSTANTS - public double XControllerP = 3.35; - public double XControllerD = 0.0; + public double XControllerP = 6.6544; + public double XControllerD = 4.833; + public double kV = 1; + public double kA = 1; public double ThetaControllerP = 0; public double ThetaControllerD = 0; public RobotConfig autoConfig; From 8f53743a28b52975bdc4ad713f605fe5df1e59a5 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Sat, 22 Feb 2025 18:33:47 -0500 Subject: [PATCH 44/79] Updates --- src/main/java/frc/robot/subsystems/Swerve.java | 6 +++++- src/main/java/frc/robot/utility/AutomatedController.java | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f8e310d..99784a6 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -148,6 +148,10 @@ public double getYaw(){ return pigeon2.getYaw().getValueAsDouble(); } + public void resetAngle(){ + pigeon2.setYaw(180); + } + public SwerveModuleState[] moduleStates(Module[] modules) { SwerveModuleState[] state = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) @@ -165,7 +169,7 @@ public Pose2d pose() { } public void resetOdometry() { - resetOdometry(new Pose2d()); + resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(180))); } public void resetOdometry(Pose2d pose) { diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index e8ac168..1947aa7 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -71,6 +71,7 @@ public void configure(){ controller.a().toggleOnTrue(new Aimbot(io)); controller.b().toggleOnTrue(new AutoAlign(0, io)); + controller.x().onTrue(Util.Do(io.chassis::resetAngle)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); From ec3f7e9e6243b2836e8232646aff484217e30dc9 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Sat, 22 Feb 2025 20:04:13 -0500 Subject: [PATCH 45/79] Updated X PID Constants. Now working on theta characterising --- .../autos/{New Auto.auto => 90 Auto.auto} | 0 .../pathplanner/paths/Forward and Back.path | 2 +- .../paths/J,I to Station and back.path | 2 +- .../paths/J,I to Station to L, K.path | 2 +- .../paths/L, K to Station and back.path | 2 +- .../deploy/pathplanner/paths/New Path.path | 2 +- .../paths/Straight And Rotate.path | 2 +- .../paths/Straight Line Rotate 90.path | 6 ++-- .../pathplanner/paths/Straight Test.path | 6 ++-- .../deploy/pathplanner/paths/Top to J,I.path | 2 +- .../deploy/pathplanner/paths/Triangle.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/subsystems/Swerve.java | 29 +++++++++++++++++-- src/main/java/frc/robot/swerve/Swerve.java | 10 +++---- .../robot/utility/AutomatedController.java | 8 ++--- 16 files changed, 50 insertions(+), 28 deletions(-) rename src/main/deploy/pathplanner/autos/{New Auto.auto => 90 Auto.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/90 Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/90 Auto.auto diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path index 8703b60..9001e38 100644 --- a/src/main/deploy/pathplanner/paths/Forward and Back.path +++ b/src/main/deploy/pathplanner/paths/Forward and Back.path @@ -49,7 +49,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/J,I to Station and back.path b/src/main/deploy/pathplanner/paths/J,I to Station and back.path index bb5f03f..a2ecf71 100644 --- a/src/main/deploy/pathplanner/paths/J,I to Station and back.path +++ b/src/main/deploy/pathplanner/paths/J,I to Station and back.path @@ -75,7 +75,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path b/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path index fc3e888..2e93682 100644 --- a/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path +++ b/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path @@ -79,7 +79,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/L, K to Station and back.path b/src/main/deploy/pathplanner/paths/L, K to Station and back.path index 9d157a6..bb98543 100644 --- a/src/main/deploy/pathplanner/paths/L, K to Station and back.path +++ b/src/main/deploy/pathplanner/paths/L, K to Station and back.path @@ -75,7 +75,7 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index a8099ef..f2908c5 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -145,7 +145,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path index b4bdd4c..5b84969 100644 --- a/src/main/deploy/pathplanner/paths/Straight And Rotate.path +++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path index d9ae114..5dcb531 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 6.0, + "x": 4.0, "y": 7.0 }, "prevControl": { - "x": 5.0, + "x": 3.0, "y": 7.0 }, "nextControl": null, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index 2ec6b22..ea9c0a9 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 6.0, + "x": 4.0, "y": 6.0 }, "prevControl": { - "x": 5.0, + "x": 3.0, "y": 6.0 }, "nextControl": null, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/Top to J,I.path b/src/main/deploy/pathplanner/paths/Top to J,I.path index decb39d..b3f3aff 100644 --- a/src/main/deploy/pathplanner/paths/Top to J,I.path +++ b/src/main/deploy/pathplanner/paths/Top to J,I.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path index 97a2d4d..10643aa 100644 --- a/src/main/deploy/pathplanner/paths/Triangle.path +++ b/src/main/deploy/pathplanner/paths/Triangle.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 0ed0eaf..00d9d11 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,7 +4,7 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, + "defaultMaxVel": 2.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93b3c6c..36ec8aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,7 +24,6 @@ public class RobotContainer { public final AutomatedController main; public final AutomatedController backup; - private final SendableChooser auto_selector; private Command current_auto = new PrintCommand("Temp"); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 424c0e9..71c8bed 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.VoltsPerMeterPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; @@ -97,7 +98,7 @@ public Swerve() { var alliance = DriverStation.getAlliance(); return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; }, - this); + this); odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), new Pose2d(0, 0, new Rotation2d())); @@ -217,7 +218,31 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine routine = new SysIdRoutine(new Config( null, - Volts.of(4), + Volts.of(2), + null, + null + ), + new SysIdRoutine.Mechanism(voltage -> { + for (Module mod : modules) + mod.set(voltage.magnitude(), 0); + }, log -> { + for (int i = 0; i < 4; i++){ + log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + .voltage(modules[i].voltage()) + .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) + .linearVelocity(modules[i].velocity()); + + log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + .voltage(modules[i].steerVoltage()) + .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + .angularVelocity(modules[i].steerVelocity()); + } + }, this)); + + + public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( + null, + Volts.of(2), null, null ), diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 9749ec4..65dcddf 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -36,12 +36,10 @@ public static class Constants { public double rotFactor = .30; // .6 for tristan // AUTON CONSTANTS - public double XControllerP = 6.6544; - public double XControllerD = 4.833; - public double kV = 1; - public double kA = 1; - public double ThetaControllerP = 0; - public double ThetaControllerD = 0; + public double XControllerP = 5.1555; + public double XControllerD = 0.61072; + public double ThetaControllerP = 0.0000001; + public double ThetaControllerD = 0.00000001; public RobotConfig autoConfig; public double MASS = 47; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 67cb204..f535f10 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -55,10 +55,10 @@ public void configure(){ // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - controller.x().onTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kForward)); - controller.a().onTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kReverse)); - controller.y().onTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kForward)); - controller.b().onTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kReverse)); + // controller.x().toggleOnTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kForward)); + // controller.a().toggleOnTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kReverse)); + // controller.y().toggleOnTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kForward)); + // controller.b().toggleOnTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kReverse)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); From 5da88646524a78b5e629d8903116ceb808ce1790 Mon Sep 17 00:00:00 2001 From: Evan Date: Sat, 22 Feb 2025 21:15:53 -0500 Subject: [PATCH 46/79] steer characterization --- .../java/frc/robot/subsystems/Swerve.java | 100 ++++++++---------- src/main/java/frc/robot/swerve/Module.java | 6 ++ 2 files changed, 53 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 71c8bed..3c2055e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -98,7 +98,7 @@ public Swerve() { var alliance = DriverStation.getAlliance(); return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; }, - this); + this); odometry = new SwerveDriveOdometry(kinematics, rotation(), modulePositions(), new Pose2d(0, 0, new Rotation2d())); @@ -208,60 +208,54 @@ public void setModuleStates(SwerveModuleState[] states) { } } - final MutVoltage[] driveVoltage = {Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0)}; - final MutDistance[] distance = {Meters.mutable(0), Meters.mutable(0), Meters.mutable(0), Meters.mutable(0)}; - final MutLinearVelocity[] velocity = {MetersPerSecond.mutable(0),MetersPerSecond.mutable(0),MetersPerSecond.mutable(0),MetersPerSecond.mutable(0)}; - - final MutVoltage[] steerVoltage = {Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0)}; - final MutAngle[] angle = {Radians.mutable(0),Radians.mutable(0),Radians.mutable(0),Radians.mutable(0)}; - final MutAngularVelocity[] angularVelocity = {RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0),RadiansPerSecond.mutable(0)}; - - public final SysIdRoutine routine = new SysIdRoutine(new Config( - null, - Volts.of(2), - null, - null - ), - new SysIdRoutine.Mechanism(voltage -> { - for (Module mod : modules) - mod.set(voltage.magnitude(), 0); - }, log -> { - for (int i = 0; i < 4; i++){ - log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") - .voltage(modules[i].voltage()) - .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) - .linearVelocity(modules[i].velocity()); - - log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") - .voltage(modules[i].steerVoltage()) - .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - .angularVelocity(modules[i].steerVelocity()); - } - }, this)); - + final MutVoltage[] driveVoltage = { Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0) }; + final MutDistance[] distance = { Meters.mutable(0), Meters.mutable(0), Meters.mutable(0), Meters.mutable(0) }; + final MutLinearVelocity[] velocity = { MetersPerSecond.mutable(0), MetersPerSecond.mutable(0), + MetersPerSecond.mutable(0), MetersPerSecond.mutable(0) }; + + final MutVoltage[] steerVoltage = { Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0) }; + final MutAngle[] angle = { Radians.mutable(0), Radians.mutable(0), Radians.mutable(0), Radians.mutable(0) }; + final MutAngularVelocity[] angularVelocity = { RadiansPerSecond.mutable(0), RadiansPerSecond.mutable(0), + RadiansPerSecond.mutable(0), RadiansPerSecond.mutable(0) }; + + public final SysIdRoutine driveRoutine = new SysIdRoutine(new Config( + null, + Volts.of(2), + null, + null), + new SysIdRoutine.Mechanism(voltage -> { + for (Module mod : modules) + mod.set(voltage.magnitude(), 0); + }, log -> { + for (int i = 0; i < 4; i++) { + log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + .voltage(modules[i].voltage()) + .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) + .linearVelocity(modules[i].velocity()); + + log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + .voltage(modules[i].steerVoltage()) + .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + .angularVelocity(modules[i].steerVelocity()); + } + }, this)); public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( - null, - Volts.of(2), - null, - null - ), - new SysIdRoutine.Mechanism(voltage -> { - for (Module mod : modules) - mod.set(voltage.magnitude(), 0); - }, log -> { - for (int i = 0; i < 4; i++){ - log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") - .voltage(modules[i].voltage()) - .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) - .linearVelocity(modules[i].velocity()); - - log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") - .voltage(modules[i].steerVoltage()) - .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - .angularVelocity(modules[i].steerVelocity()); - } - }, this)); + null, + Volts.of(2), + null, + null), + new SysIdRoutine.Mechanism(voltage -> { + for (Module mod : modules) + mod.setSteer(voltage.magnitude()); // Applies voltage to the steer motors + }, log -> { + for (int i = 0; i < 4; i++) { + log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + .voltage(modules[i].steerVoltage()) + .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + .angularVelocity(modules[i].steerVelocity()); + } + }, this)); public ChassisSpeeds getSpeeds() { return speeds; diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 526a2f0..029729b 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -133,4 +133,10 @@ public void set(double driveVolts, double targetAngle) { steer.getClosedLoopController().setReference(targetAngle, ControlType.kPosition); } + public void setSteer(double steerVolts){ + syncEncoders(); + drive.set(0); + steer.set(steerVolts); + } + } From 68c0b2f0b13ed8f7fe44de2b7324271c5102b165 Mon Sep 17 00:00:00 2001 From: Evan Date: Sat, 22 Feb 2025 21:16:41 -0500 Subject: [PATCH 47/79] steer characterization --- src/main/java/frc/robot/utility/AutomatedController.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index f535f10..7ae0ad8 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -55,10 +55,10 @@ public void configure(){ // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - // controller.x().toggleOnTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kForward)); - // controller.a().toggleOnTrue(io.chassis.routine.quasistatic(SysIdRoutine.Direction.kReverse)); - // controller.y().toggleOnTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kForward)); - // controller.b().toggleOnTrue(io.chassis.routine.dynamic(SysIdRoutine.Direction.kReverse)); + controller.x().toggleOnTrue(io.chassis.steerRoutine.quasistatic(SysIdRoutine.Direction.kForward)); + controller.a().toggleOnTrue(io.chassis.steerRoutine.quasistatic(SysIdRoutine.Direction.kReverse)); + controller.y().toggleOnTrue(io.chassis.steerRoutine.dynamic(SysIdRoutine.Direction.kForward)); + controller.b().toggleOnTrue(io.chassis.steerRoutine.dynamic(SysIdRoutine.Direction.kReverse)); controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); From 61a776be225976d71a5f126587b35821eac600c8 Mon Sep 17 00:00:00 2001 From: Evan Date: Sat, 22 Feb 2025 22:28:46 -0500 Subject: [PATCH 48/79] changed volts --- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3c2055e..6ab804d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -242,7 +242,7 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( null, - Volts.of(2), + Volts.of(1), null, null), new SysIdRoutine.Mechanism(voltage -> { From 280e74ee7867a0ea3b20cb267b9843d7f3f324a3 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Mon, 24 Feb 2025 22:59:40 -0500 Subject: [PATCH 49/79] Finally finished the characterisation for pivot and elevator, added driver profile switching, updated controller mode switching, added softlimit placeholders elevator, claw, and hang --- src/main/java/frc/robot/RobotContainer.java | 11 +++ src/main/java/frc/robot/subsystems/Claw.java | 15 ++- .../java/frc/robot/subsystems/Elevator.java | 92 +++++++++++-------- src/main/java/frc/robot/subsystems/Hang.java | 5 +- src/main/java/frc/robot/swerve/Swerve.java | 28 ++++-- .../robot/utility/AutomatedController.java | 38 +++++--- 6 files changed, 125 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f71c9da..177e03e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -14,6 +15,7 @@ import dev.doglog.DogLog; import dev.doglog.DogLogOptions; import frc.robot.commands.DefaultDrive; +import frc.robot.swerve.Swerve; import frc.robot.utility.AutomatedController; import frc.robot.utility.IO; import frc.robot.utility.Util; @@ -27,6 +29,8 @@ public class RobotContainer { private final SendableChooser auto_selector; private Command current_auto = new PrintCommand(""); + final SendableChooser driver_selector = new SendableChooser(); + public RobotContainer() { main = new AutomatedController(0, io); backup = new AutomatedController(1, io); @@ -39,8 +43,15 @@ public RobotContainer() { SmartDashboard.putData("Main-Controller Mode", main.selector); SmartDashboard.putData("Backup-Controller Mode", main.selector); + SmartDashboard.putString("Driver", Swerve.Constants.drivers[Swerve.Constants.driver]); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); + driver_selector.setDefaultOption("Shaan", 0); + driver_selector.addOption("Norah", 1); + driver_selector.addOption("Jason", 2); + driver_selector.addOption("Uriel", 3); + driver_selector.onChange( (driver) -> Swerve.Constants.SwitchDriver(driver)); + DogLog.setOptions(new DogLogOptions() .withCaptureDs(true) .withCaptureConsole(true) diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index f7bf1ad..f08608b 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -7,6 +9,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.TalonFX; +import com.fasterxml.jackson.databind.util.Converter; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -20,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; @@ -49,6 +53,10 @@ public Claw() { config.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + // config.softLimit.forwardSoftLimitEnabled(true); + // config.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit + // config.softLimit.reverseSoftLimitEnabled(true); + // config.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @@ -114,18 +122,19 @@ public void setAngle(double target_angle){ time.restart(); } + public final SysIdRoutine routine = new SysIdRoutine(new Config( null, Volts.of(4), Seconds.of(5), - (state) -> SignalLogger.writeString("state", state.toString()) + null ), new Mechanism( volts -> pivot.setVoltage(volts), log -> { log.motor("Claw Pivot") .voltage(voltage()) - .linearPosition(Meters.of(0)) // TODO: Replace rotations with meters for the KRakens - .linearVelocity(MetersPerSecond.of(0)); // TODO: Replace rotations per second to meters per second for the KRakens + .angularPosition(Degree.of(angle())) + .angularVelocity(DegreesPerSecond.of(pivot.getAbsoluteEncoder().getVelocity())); }, this)); @Override diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index ff67625..8756ea7 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; @@ -15,6 +16,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.*; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Timer; @@ -28,7 +30,7 @@ public class Elevator extends SubsystemBase { TalonFX lead = new TalonFX(11); TalonFX follow = new TalonFX(12); - + SparkMaxConfig config = new SparkMaxConfig(); TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); @@ -46,7 +48,6 @@ public class Elevator extends SubsystemBase { public final double Barge = 0; public final double MAX_HEIGHT = 0; - public Elevator() { Slot0Configs config = new Slot0Configs(); config.kP = 0.0; @@ -54,109 +55,120 @@ public Elevator() { config.kD = 0.0; lead.getConfigurator().apply(config); + // lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(true).withReverseSoftLimitEnable(true) + // .withForwardSoftLimitThreshold(0).withReverseSoftLimitThreshold(0)); // TODO: Find the forward and reverse Soft limit switches follow.setControl(new Follower(lead.getDeviceID(), false)); // TODO: Check if we need to invert } public void speed(double speed) { lead.set(speed); } - + public void volts(double volts) { lead.setVoltage(volts); } - public void stop(){ + public void stop() { stopped = true; lead.stopMotor(); } - public void move(double height){ // TODO: Checkout how adding a feedforward affects the results + public void move(double height) { // TODO: Checkout how adding a feedforward affects the results stopped = false; - target = Math.max( Math.min( height, 0 ), MAX_HEIGHT ); // TODO: Should be able to remove later because we know what heights we set the bot to + target = Math.max(Math.min(height, 0), MAX_HEIGHT); // TODO: Should be able to remove later because we know what + // heights we set the bot to time.restart(); } - public InstantCommand move(int level){ + public InstantCommand move(int level) { return new InstantCommand(() -> { switch (level) { - case 2: L2(); + case 2: + L2(); + break; + case 3: + L3(); break; - case 3: L3(); + case 4: + L4(); break; - case 4: L4(); + case 5: + Barge(); break; - case 5: Barge(); - break; - default: rest(); // LEVEL 1 // TODO: See if we need to change the height we go to + default: + rest(); // LEVEL 1 // TODO: See if we need to change the height we go to break; } - },this); - } - - public double position() { - return lead.getPosition().getValueAsDouble(); // TODO: Convert to metres or inches + }, this); } - public void zero(){ + public void zero() { lead.setPosition(0); } - public void rest(){ + public void rest() { move(Rest); } - public void L2(){ + public void L2() { move(L2); } - public void L3(){ + public void L3() { move(L3); } - public void L4(){ + public void L4() { move(L4); } - public void Barge(){ + public void Barge() { move(Barge); } - public boolean atPosition(){ + public boolean atPosition() { return profile.isFinished(time.get()); } - public Voltage voltage(){ + public Voltage voltage() { return lead.getMotorVoltage().getValue(); } - public LinearVelocity velocity(){ - return MetersPerSecond.of(0); // TODO: Do conversions later + public double position() { + return lead.getPosition().getValueAsDouble() * conversion; + } + + public LinearVelocity velocity() { + return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * conversion); } + final double conversion = Math.PI * (Units.inchesToMeters(2) / 60); // TODO: Replace 2 with the diameter of the drum + public final SysIdRoutine routine = new SysIdRoutine(new Config( null, Volts.of(4), Seconds.of(5), - (state) -> SignalLogger.writeString("state", state.toString()) - ), new Mechanism( - volts -> lead.setControl(new VoltageOut(volts.in(Volts))), - log -> { - log.motor("Elevator") - .voltage(voltage()) - .linearPosition(Meters.of(0)) // TODO: Replace rotations with meters for the KRakens - .linearVelocity(MetersPerSecond.of(0)); // TODO: Replace rotations per second to meters per second for the KRakens - }, this)); + (state) -> SignalLogger.writeString("state", state.toString())), + new Mechanism( + volts -> lead.setControl(new VoltageOut(volts.in(Volts))), + log -> { + log.motor("Elevator") + .voltage(voltage()) + .linearPosition(Meters.of(position())) + .linearVelocity(velocity()); + }, this)); @Override public void periodic() { - if (stopped) return; + if (stopped) + return; State out = profile.calculate(time.get(), new State(L2, Barge), new State(target, 0)); lead.setControl(positionRequest.withPosition(out.position)); - + SmartDashboard.putNumber("Elevator Motor Voltage", voltage().magnitude()); SmartDashboard.putNumber("Elevator Height", position()); - + SmartDashboard.putNumber("Elevator Target Height", target); SmartDashboard.putNumber("Elevator cTarget Height", out.position); diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 7fbe228..70f1e60 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -1,17 +1,20 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.config.SoftLimitConfig; import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { - public TalonFX hang = new TalonFX(10, "rio"); + public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet public static final double HANG_MAX_ANGLE = 0; public Hang() { + // TODO: Configure a soft limit switch hang.setNeutralMode(NeutralModeValue.Brake); } diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 3a19469..4b9f3da 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -28,9 +28,10 @@ public static class Constants { public double WHEEL_RADIUS; // DRIVER SETTINGS - public int driver = 0; - public double transFactor = .65; // factor = x/125, with x being the percentage of our max speed, same for the thing below - public double rotFactor = .30; // .6 for tristan + public static int driver = 0; + public static double transFactor = .65; // factor = x/125, with x being the percentage of our max speed, same for the thing below + public static double rotFactor = .30; // .6 for tristan + public static String[] drivers = {"Shaan", "Norah", "Jason", "Uriel"}; // AUTON CONSTANTS public double XControllerP = 5.1555; @@ -53,13 +54,13 @@ public Constants(){ if (comp) { TRACKWIDTH = 30.0; WHEELBASE = 30.0; - GEAR_RATIO = 8.14; // L1 + GEAR_RATIO = 8.14; // L1 Ratio WHEEL_RADIUS = 0.1143; MASS = 60.0; } else { TRACKWIDTH = 19.5; WHEELBASE = 21.5; - GEAR_RATIO = 6.12; // L3 + GEAR_RATIO = 6.12; // L3 Ratio WHEEL_RADIUS = 0.1016; MASS = 47.0; } @@ -88,8 +89,23 @@ public Constants(){ } catch (IOException | org.json.simple.parser.ParseException e) { e.printStackTrace(); } - + SwitchDriver(driver); + } + + public static void SwitchDriver(int driver){ switch (driver) { + case 4: // DEBUG + break; + + case 3: // Uriel + break; + + case 2: // Jason + break; + + case 1: // Norah + break; + default: // Shaan transFactor = .65; rotFactor = .48; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 40c9e82..3cfb21c 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -11,7 +11,11 @@ public class AutomatedController { public final CommandXboxController controller; public final SendableChooser selector = new SendableChooser(); - public boolean manual = true; + public int mode = 0; + + final int AUTOMATED = 0; + final int MANUAL = 1; + final int DEBUG = 2; Rumble rumble; IO io; @@ -19,10 +23,11 @@ public class AutomatedController { public AutomatedController(int port, IO io){ this.io = io; - selector.setDefaultOption("Automated", () -> {manual = false;}); - selector.addOption("Manual", () -> {manual = true;}); + selector.setDefaultOption("Automated", () -> {mode = 0;}); + selector.addOption("Manual", () -> {mode = 1;}); + selector.addOption("Debug", () -> {mode = 2;}); - selector.onChange((x) -> {x.run();}); // TODO: See if this allows us to have the selector always be up-to-date and add the same for auton testing + selector.onChange((x) -> {x.run();}); controller = new CommandXboxController(port); rumble = new Rumble(controller.getHID()); @@ -31,26 +36,31 @@ public AutomatedController(int port, IO io){ controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry)); // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); - controller.start().onTrue(Util.Do( () -> manual = !manual)).debounce(3); configure(); } - public BooleanSupplier automated(){ - return () -> { return !manual; }; + + public BooleanSupplier mode(int targetMode){ + return () -> {return mode == targetMode;}; } + public BooleanSupplier automated(){ + return mode(0); + } + public BooleanSupplier manual(){ - return () -> { return manual; }; + return mode(1); + } + + public BooleanSupplier debug(){ + return mode(2); } - public BooleanSupplier toggleMode(){ - return () -> { - manual = !manual; - return manual; - }; + public void switchMode(){ + mode = (mode + 1) % 3; } public void configure(){ - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::toggleMode)); + controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); // AUTOMATED From 08524f72bb8a08db56f2364c8311b0135b692d85 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 25 Feb 2025 17:24:21 -0500 Subject: [PATCH 50/79] Cleaned up swerve code --- .../java/frc/robot/subsystems/Swerve.java | 98 +++++-------------- 1 file changed, 27 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ab804d..c198f77 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,19 +1,15 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.VoltsPerMeterPerSecond; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,15 +22,12 @@ import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.MutDistance; -import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -86,7 +79,7 @@ public Swerve() { this::pose, this::resetOdometry, () -> speeds, - (speeds, feedforwards) -> drive(speeds), + this::drive, new PPHolonomicDriveController( new PIDConstants(constants.XControllerP, 0.0, constants.XControllerD), // Translation PID // constants @@ -113,57 +106,45 @@ public void zeroGyro() { } public Rotation2d rotation() { - return new Rotation2d(absoluteRotation()); - } - - public double absoluteRotation() { double rotation = pigeon2.getYaw().getValueAsDouble() % 360; rotation += (rotation < 0) ? 360 : 0; - return Math.toRadians(rotation); + return new Rotation2d(Degree.of(rotation)); + } + + public void adjustRotation() { + pigeon2.setYaw((rotation().getDegrees() + 180) % 360); } public void drive(ChassisSpeeds speeds) { this.speeds = speeds; } + // public void drive(ChassisSpeeds speeds, DriveFeedforwards feedforwards){ + // this.speeds = speeds; + // } + public void stop() { speeds = new ChassisSpeeds(); } public double distance(Pose2d reference_point) { - return odometry.getPoseMeters().getTranslation().getDistance(reference_point.getTranslation()); - } - - public double distance(double[] reference_point) { - return distance(new Pose2d(reference_point[0], reference_point[2], new Rotation2d(reference_point[3]))); - } - - private SwerveModulePosition modulePosition(Module module) { - return new SwerveModulePosition(module.drivePosition(), Rotation2d.fromRadians(module.angle())); - } - - private SwerveModuleState moduleState(Module module) { - return new SwerveModuleState(module.velocity(), new Rotation2d(module.angle())); + return pose().getTranslation().getDistance(reference_point.getTranslation()); } public SwerveModulePosition[] modulePositions() { SwerveModulePosition[] pos = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) - pos[i] = modulePosition(modules[i]); + pos[i] = new SwerveModulePosition(modules[i].drivePosition(), Rotation2d.fromRadians(modules[i].angle())); return pos; } public SwerveModuleState[] moduleStates(Module[] modules) { SwerveModuleState[] state = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) - state[i] = moduleState(modules[i]); + state[i] = new SwerveModuleState(modules[i].velocity(), new Rotation2d(modules[i].angle())); return state; } - public void adjustRotation() { - pigeon2.setYaw((absoluteRotation() + 180) % 360); - } - public Pose2d pose() { return odometry.getPoseMeters(); } @@ -194,11 +175,6 @@ public void zeroAbsolute() { mod.zeroAbsolute(); } - public void resetSteerPositions() { - for (Module mod : modules) - mod.set(0, 0); - } - public void setModuleStates(SwerveModuleState[] states) { SwerveDriveKinematics.desaturateWheelSpeeds(states, Constants.MAX_VELOCITY); for (int i = 0; i < modules.length; i++) { @@ -208,15 +184,8 @@ public void setModuleStates(SwerveModuleState[] states) { } } - final MutVoltage[] driveVoltage = { Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0) }; final MutDistance[] distance = { Meters.mutable(0), Meters.mutable(0), Meters.mutable(0), Meters.mutable(0) }; - final MutLinearVelocity[] velocity = { MetersPerSecond.mutable(0), MetersPerSecond.mutable(0), - MetersPerSecond.mutable(0), MetersPerSecond.mutable(0) }; - - final MutVoltage[] steerVoltage = { Volts.mutable(0), Volts.mutable(0), Volts.mutable(0), Volts.mutable(0) }; final MutAngle[] angle = { Radians.mutable(0), Radians.mutable(0), Radians.mutable(0), Radians.mutable(0) }; - final MutAngularVelocity[] angularVelocity = { RadiansPerSecond.mutable(0), RadiansPerSecond.mutable(0), - RadiansPerSecond.mutable(0), RadiansPerSecond.mutable(0) }; public final SysIdRoutine driveRoutine = new SysIdRoutine(new Config( null, @@ -232,11 +201,6 @@ public void setModuleStates(SwerveModuleState[] states) { .voltage(modules[i].voltage()) .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) .linearVelocity(modules[i].velocity()); - - log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") - .voltage(modules[i].steerVoltage()) - .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - .angularVelocity(modules[i].steerVelocity()); } }, this)); @@ -257,10 +221,6 @@ public void setModuleStates(SwerveModuleState[] states) { } }, this)); - public ChassisSpeeds getSpeeds() { - return speeds; - } - public double getRoll() { return pigeon2.getRoll().getValueAsDouble(); } @@ -268,10 +228,8 @@ public double getRoll() { public void toggle() { active = !active; - if (!active) { - for (Module mod : modules) - mod.stop(); - } + for (Module mod : modules) + mod.stop(); } public void periodic() { @@ -285,18 +243,17 @@ public void periodic() { Pose2d pose = odometry.update(rotation(), modulePositions()); posePublisher.set(pose); - // SmartDashboard.putNumber("X position", pose.getX()); - // SmartDashboard.putNumber("Y position", pose.getY()); + SmartDashboard.putNumber("X position", pose.getX()); + SmartDashboard.putNumber("Y position", pose.getY()); - // SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); - // SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Pitch", - // pigeon2.getPitch().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Roll", - // pigeon2.getRoll().getValueAsDouble()); + SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); + SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); + SmartDashboard.putNumber("Pigeon Pitch", + pigeon2.getPitch().getValueAsDouble()); + SmartDashboard.putNumber("Pigeon Roll", + pigeon2.getRoll().getValueAsDouble()); - // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : - // "Robot-Oriented"); + SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); // DogLog.log("Swerve/current_states", moduleStates(modules)); // DogLog.log("Swerve/target_states", states); @@ -307,7 +264,6 @@ public void periodic() { // DogLog.log("Swerve/Pigeon_Yaw", pigeon2.getYaw().getValueAsDouble()); // DogLog.log("Swerve/Pigeon_Pitch", pigeon2.getPitch().getValueAsDouble()); // DogLog.log("Swerve/Pigeon_Roll", pigeon2.getRoll().getValueAsDouble()); - // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : - // "Robot-Oriented"); + // DogLog.log("Swerve/Drive_Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } } \ No newline at end of file From e37e40584c56b9bb2b1583b4e2ce1f75f286576b Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 25 Feb 2025 21:01:32 -0500 Subject: [PATCH 51/79] Moved stuff to debug --- .../java/frc/robot/utility/AutomatedController.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 3cfb21c..49da0b5 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -37,6 +37,7 @@ public AutomatedController(int port, IO io){ // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); configure(); + configureDebug(); } public BooleanSupplier mode(int targetMode){ @@ -78,9 +79,7 @@ public void configure(){ // controller.rightBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, true), new ReleaseAlgae(io), // io.limelight::reefZone)); - controller.a().toggleOnTrue(new Aimbot(io)); - controller.b().toggleOnTrue(new AutoAlign(0, io)); - controller.x().onTrue(Util.Do(io.chassis::resetAngle)); + controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); @@ -94,4 +93,10 @@ public void configure(){ // io.chassis)); // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } + + private void configureDebug(){ + controller.a().and(debug()).toggleOnTrue(new Aimbot(io)); + controller.b().and(debug()).toggleOnTrue(new AutoAlign(0, io)); + controller.x().and(debug()).onTrue(Util.Do(io.chassis::resetAngle)); + } } From b94681bb13469bafda968c532a82ad85b1332807 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 25 Feb 2025 21:27:41 -0500 Subject: [PATCH 52/79] Added debug bindings --- .../java/frc/robot/subsystems/Elevator.java | 7 +++- .../java/frc/robot/subsystems/Swerve.java | 22 +---------- .../robot/utility/AutomatedController.java | 38 +++++++++++++------ 3 files changed, 34 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8756ea7..ea7a7c5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -42,6 +42,7 @@ public class Elevator extends SubsystemBase { // Target Heights public final double Rest = 0; + public final double L1 = 0; public final double L2 = 0; public final double L3 = 0; public final double L4 = 0; @@ -109,6 +110,9 @@ public void zero() { public void rest() { move(Rest); } + public void L1(){ + move(L1); + } public void L2() { move(L2); @@ -142,7 +146,8 @@ public LinearVelocity velocity() { return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * conversion); } - final double conversion = Math.PI * (Units.inchesToMeters(2) / 60); // TODO: Replace 2 with the diameter of the drum + final double gearReduction = 1/17; + final double conversion = Math.PI * gearReduction *(Units.inchesToMeters(2) / 60); public final SysIdRoutine routine = new SysIdRoutine(new Config( null, diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0eff28d..0c0f947 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -109,7 +109,7 @@ private Translation2d createTranslation(double x, double y) { return new Translation2d(x, y); } - public void zeroGyro() { + public void resetAngle() { pigeon2.setYaw(0); } @@ -150,10 +150,6 @@ public double getYaw(){ return pigeon2.getYaw().getValueAsDouble(); } - public void resetAngle(){ - pigeon2.setYaw(180); - } - public SwerveModuleState[] moduleStates(Module[] modules) { SwerveModuleState[] state = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) @@ -171,7 +167,7 @@ public void resetOdometry() { } public void resetOdometry(Pose2d pose) { - zeroGyro(); + resetAngle(); resetPosition(); odometry.resetPosition(rotation(), modulePositions(), pose); @@ -263,20 +259,6 @@ public Pose2d estimatePose() { return mt2.pose; } - public Pose2d estimatePose() { - estimator.update(rotation(), modulePositions()); - LimelightHelpers.SetRobotOrientation("limelight-main", estimator.getEstimatedPosition().getRotation().getRotations(), 0,0,0,0,0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); - if (mt2 == null) return new Pose2d(); - if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ - estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); - estimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); - // estimatedPosePublisher.set(mt2.pose); - estimatedPosePublisher.set(estimator.getEstimatedPosition()); - } - return mt2.pose; - } - public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 49da0b5..867690a 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; @@ -33,7 +34,7 @@ public AutomatedController(int port, IO io){ rumble = new Rumble(controller.getHID()); controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); - controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry)); + controller.leftStick().debounce(2).onTrue(new InstantCommand(io.chassis::resetAngle)); // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); configure(); @@ -62,7 +63,7 @@ public void switchMode(){ public void configure(){ controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); - controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + controller.back().onTrue(Util.Do(io.elevator::rest, io.elevator)); // AUTOMATED @@ -70,16 +71,19 @@ public void configure(){ // LB align Left and Score Coral & Score Barge // RB align Right and Score Coral & Score Processor - // controller.leftBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, false), new ScoreBarge(io), - // io.limelight::reefZone)); + controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); + controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); - controller.leftBumper().onTrue(new AutoAlign(0, io)); - controller.rightBumper().onTrue(new AutoAlign(2, io)); + // [DEBUG] FOR TESTING for rn + controller.y().and(automated()).onTrue(Util.Do(io.elevator::L4,io.elevator)); + controller.b().and(automated()).onTrue(Util.Do(io.elevator::L3,io.elevator)); + controller.x().and(automated()).onTrue(Util.Do(io.elevator::L2,io.elevator)); + controller.a().and(automated()).onTrue(Util.Do(io.elevator::L1,io.elevator)); - // controller.rightBumper().and(automated()).onTrue(new ConditionalCommand(new ScoreReef(io, true), new ReleaseAlgae(io), - // io.limelight::reefZone)); + controller.povLeft().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); + controller.povDown().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); - + // MANUAL controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); @@ -95,8 +99,18 @@ public void configure(){ } private void configureDebug(){ - controller.a().and(debug()).toggleOnTrue(new Aimbot(io)); - controller.b().and(debug()).toggleOnTrue(new AutoAlign(0, io)); - controller.x().and(debug()).onTrue(Util.Do(io.chassis::resetAngle)); + controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); + controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); + controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); + + controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + + controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); + controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); + controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); + controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); } } From e644dc846471d2f92204cf17802ecfbfb6b6c202 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 25 Feb 2025 21:31:37 -0500 Subject: [PATCH 53/79] Added room for feedforward gains --- src/main/java/frc/robot/subsystems/Elevator.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index ea7a7c5..393ad8c 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -54,6 +54,7 @@ public Elevator() { config.kP = 0.0; config.kI = 0.0; config.kD = 0.0; + config.kG = 0.0; lead.getConfigurator().apply(config); // lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(true).withReverseSoftLimitEnable(true) @@ -74,10 +75,10 @@ public void stop() { lead.stopMotor(); } - public void move(double height) { // TODO: Checkout how adding a feedforward affects the results + public void move(double height) { stopped = false; - target = Math.max(Math.min(height, 0), MAX_HEIGHT); // TODO: Should be able to remove later because we know what - // heights we set the bot to + // target = Math.max(Math.min(height, 0), MAX_HEIGHT); + target = height; time.restart(); } From a145b7406c75434963941e6725aacf15d3644b4b Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 25 Feb 2025 21:40:47 -0500 Subject: [PATCH 54/79] Added barge test binding --- src/main/java/frc/robot/utility/AutomatedController.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 867690a..c94eeb7 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -79,7 +79,8 @@ public void configure(){ controller.b().and(automated()).onTrue(Util.Do(io.elevator::L3,io.elevator)); controller.x().and(automated()).onTrue(Util.Do(io.elevator::L2,io.elevator)); controller.a().and(automated()).onTrue(Util.Do(io.elevator::L1,io.elevator)); - + controller.povUp().and(automated()).onTrue(Util.Do(io.elevator::Barge,io.elevator)); + controller.povLeft().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); controller.povDown().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); From 9bb10748276e1bdfdd97351c009a6a169c72f495 Mon Sep 17 00:00:00 2001 From: Evan Date: Wed, 26 Feb 2025 20:42:29 -0500 Subject: [PATCH 55/79] claw characterization routine --- src/main/java/frc/robot/subsystems/Claw.java | 89 ++++++++++++-------- 1 file changed, 56 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index f08608b..eb399e9 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -2,14 +2,10 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.TalonFX; -import com.fasterxml.jackson.databind.util.Converter; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -23,7 +19,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; @@ -36,7 +31,7 @@ public class Claw extends SubsystemBase { SparkMax algaeIntake = new SparkMax(13, MotorType.kBrushless); TalonFX coralIntake = new TalonFX(14); SparkMax pivot = new SparkMax(15, MotorType.kBrushless); - + DigitalInput algaeBreak = new DigitalInput(0); DigitalInput coralBreak = new DigitalInput(1); @@ -60,82 +55,109 @@ public Claw() { pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - public void voltsAlgae(double volts){ + public void voltsAlgae(double volts) { algaeIntake.setVoltage(volts); } - public void speedAlgae(double speed){ + public void speedAlgae(double speed) { algaeIntake.setVoltage(speed); } - public void voltsCoral(double volts){ + public void voltsCoral(double volts) { coralIntake.setVoltage(volts); } - public void speedCoral(double speed){ + public void speedCoral(double speed) { coralIntake.setVoltage(speed); } - public void scoreCoral(){ + public void scoreCoral() { coralIntake.set(.4); } - public void stopCoral(){ + public void stopCoral() { coralIntake.set(0); } - public void scoreAlgae(){ + public void scoreAlgae() { algaeIntake.set(1); } - public void stopAlgae(){ + public void stopAlgae() { algaeIntake.set(0); } - public void stop(){ + public void stop() { algaeIntake.stopMotor(); } - public boolean hasAlgae(){ + public boolean hasAlgae() { return algaeBreak.get(); } - public boolean hasCoral(){ + public boolean hasCoral() { return coralBreak.get(); } - public Voltage voltage(){ + public Voltage voltage() { return Volts.of(pivot.getBusVoltage()); // TODO: Return Bus voltage } - public double angle(){ + public double angle() { return pivot.getAbsoluteEncoder().getPosition(); } - public void pivotVolts(double volts){ + public void pivotVolts(double volts) { pivot.setVoltage(volts); } - public void setAngle(double target_angle){ + public void setAngle(double target_angle) { target = target_angle; stopped = false; time.restart(); } - - public final SysIdRoutine routine = new SysIdRoutine(new Config( + public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config( + null, + Volts.of(4), + Seconds.of(5), + null), + new Mechanism( + volts -> pivot.setVoltage(volts), + log -> { + log.motor("Claw Pivot") + .voltage(voltage()) + .angularPosition(Degree.of(angle())) + .angularVelocity(DegreesPerSecond.of(pivot.getAbsoluteEncoder().getVelocity())); + }, this)); + + public final SysIdRoutine algaeIntakeRoutine = new SysIdRoutine(new Config( + null, + Volts.of(4), + Seconds.of(5), + null), + new Mechanism( + volts -> algaeIntake.setVoltage(volts), + log -> { + log.motor("Algae Intake") + .voltage(voltage()) + .angularPosition(Degree.of(angle())) + .angularVelocity(DegreesPerSecond.of(algaeIntake.getAbsoluteEncoder().getVelocity())); + }, this)); + + public final SysIdRoutine coralIntakeRoutine = new SysIdRoutine(new Config( null, Volts.of(4), Seconds.of(5), - null - ), new Mechanism( - volts -> pivot.setVoltage(volts), - log -> { - log.motor("Claw Pivot") - .voltage(voltage()) - .angularPosition(Degree.of(angle())) - .angularVelocity(DegreesPerSecond.of(pivot.getAbsoluteEncoder().getVelocity())); - }, this)); + null), + new Mechanism( + volts -> coralIntake.setVoltage(volts.magnitude()), + log -> { + log.motor("Coral Intake") + .voltage(voltage()) + .angularPosition(Degree.of(angle())) + .angularVelocity(DegreesPerSecond.of(coralIntake.getVelocity().getValueAsDouble())); + }, this)); @Override public void periodic() { @@ -144,7 +166,8 @@ public void periodic() { DogLog.log("Claw/Pivot Angle", angle()); double cTime = time.get(); - if (stopped) return; + if (stopped) + return; State out = profile.calculate(cTime, new State(angle(), 0), new State(target, 0)); pivot.getClosedLoopController().setReference(out.position, ControlType.kPosition); From 662101f29b5fe10f39fd0c3145e607db4971be35 Mon Sep 17 00:00:00 2001 From: 312Evan Date: Thu, 27 Feb 2025 17:14:13 -0500 Subject: [PATCH 56/79] Busss --- .../java/frc/robot/subsystems/Swerve.java | 3 +- .../robot/utility/AutomatedController.java | 89 ++++--------------- 2 files changed, 20 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0c0f947..177fd02 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -223,8 +223,7 @@ public void setModuleStates(SwerveModuleState[] states) { null, null), new SysIdRoutine.Mechanism(voltage -> { - for (Module mod : modules) - mod.setSteer(voltage.magnitude()); // Applies voltage to the steer motors + speeds = new ChassisSpeeds(0, 0, (voltage/16.0) * Constants.MAX_VELOCITY); }, log -> { for (int i = 0; i < 4; i++) { log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 867690a..307607e 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,112 +5,61 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.commands.Aimbot; -import frc.robot.commands.AutoAlign; public class AutomatedController { public final CommandXboxController controller; public final SendableChooser selector = new SendableChooser(); - public int mode = 0; + public boolean manual = true; - final int AUTOMATED = 0; - final int MANUAL = 1; - final int DEBUG = 2; - - Rumble rumble; IO io; public AutomatedController(int port, IO io){ this.io = io; - selector.setDefaultOption("Automated", () -> {mode = 0;}); - selector.addOption("Manual", () -> {mode = 1;}); - selector.addOption("Debug", () -> {mode = 2;}); + selector.setDefaultOption("Automated", () -> {manual = false;}); + selector.addOption("Manual", () -> {manual = true;}); - selector.onChange((x) -> {x.run();}); + selector.onChange((x) -> {x.run();}); // TODO: See if this allows us to have the selector always be up-to-date and add the same for auton testing controller = new CommandXboxController(port); - rumble = new Rumble(controller.getHID()); - controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); - controller.leftStick().debounce(2).onTrue(new InstantCommand(io.chassis::resetAngle)); + controller.leftStick().onTrue(new InstantCommand(io.chassis::resetOdometry)); // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); + controller.start().onTrue(Util.Do( () -> manual = !manual)).debounce(3); configure(); - configureDebug(); - } - - public BooleanSupplier mode(int targetMode){ - return () -> {return mode == targetMode;}; } - public BooleanSupplier automated(){ - return mode(0); - } - - public BooleanSupplier manual(){ - return mode(1); + return () -> { return !manual; }; } - public BooleanSupplier debug(){ - return mode(2); + public BooleanSupplier manual(){ + return () -> { return manual; }; } - public void switchMode(){ - mode = (mode + 1) % 3; + public BooleanSupplier toggleMode(){ + return () -> { + manual = !manual; + return manual; + }; } public void configure(){ - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); - controller.back().onTrue(Util.Do(io.elevator::rest, io.elevator)); + + controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::toggleMode)); + controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); // AUTOMATED // Based on the nearest element and our field orientation // LB align Left and Score Coral & Score Barge - // RB align Right and Score Coral & Score Processor - - controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); - controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); - - // [DEBUG] FOR TESTING for rn - controller.y().and(automated()).onTrue(Util.Do(io.elevator::L4,io.elevator)); - controller.b().and(automated()).onTrue(Util.Do(io.elevator::L3,io.elevator)); - controller.x().and(automated()).onTrue(Util.Do(io.elevator::L2,io.elevator)); - controller.a().and(automated()).onTrue(Util.Do(io.elevator::L1,io.elevator)); - - controller.povLeft().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); - controller.povDown().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); - - // MANUAL + // RB align Right and Score Coral & Score Processor + // controller.y().and( automated() ).onTrue(Util.D controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect - // controller.povRight().and(manual()).and(() -> {return !io.chassis.active;}).onTrue(Util.DoUntil( - // () -> { - // rumble.Run(.5, 0); - // io.chassis.zeroAbsolute(); - // }, rumble::End, rumble::finished, - // io.chassis)); // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } - - private void configureDebug(){ - controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); - controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); - controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - - controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); - - controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); - controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); - controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); - controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); - } } From 02e01afcc125b52b73c0a886f9934ee0d4da3ffa Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 27 Feb 2025 20:03:18 -0500 Subject: [PATCH 57/79] Claw Fixes and adjustments to pose estimation --- src/main/java/frc/robot/commands/Intake.java | 8 +-- .../frc/robot/commands/LimelightAlign.java | 60 ++++++++++++++++ src/main/java/frc/robot/subsystems/Claw.java | 71 +++---------------- .../java/frc/robot/subsystems/Swerve.java | 10 +-- 4 files changed, 79 insertions(+), 70 deletions(-) create mode 100644 src/main/java/frc/robot/commands/LimelightAlign.java diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index 7e4af40..f7d095a 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -25,18 +25,18 @@ public class Intake extends Command { public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio + // TODO: Review and see if we can optimise it if (coral){ - intake = () -> io.claw.speedCoral( (release) ? .4 : -.4); - stop = io.claw::stopCoral; + intake = () -> io.claw.speed( (release) ? .4 : -.4); holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); } else { // Algae intake = () -> { - io.claw.speedAlgae(1); + io.claw.speed(1); // TODO: Set Claw to Reef intake Angle or Ground pickup if we }; - stop = io.claw::stopAlgae; holding = () -> (release) ? io.claw.hasAlgae() : !io.claw.hasAlgae(); } + stop = io.claw::stop; } // Called when the command is initially scheduled. diff --git a/src/main/java/frc/robot/commands/LimelightAlign.java b/src/main/java/frc/robot/commands/LimelightAlign.java new file mode 100644 index 0000000..5c871a2 --- /dev/null +++ b/src/main/java/frc/robot/commands/LimelightAlign.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; +import frc.robot.utility.LimelightHelpers; + +/* 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 LimelightAlign extends Command { + + IO io; + int positions; + PIDController pid = new PIDController(.2, 0, 0); + + public LimelightAlign(IO io, int positions) { + this.io = io; + this.positions = positions; + // pid.setTolerance(5); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + switch (positions) { + case 2: // Right + LimelightHelpers.SetFidcuial3DOffset("limelight-main", 0.2,0, 0); + break; + case 1: // Centre + LimelightHelpers.SetFidcuial3DOffset("limelight-main", 0, 0, 0); + break; + default:// Left + LimelightHelpers.SetFidcuial3DOffset("limelight-main", -0.2, 0, 0); + break; + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // io.chassis.drive(new ChassisSpeeds(0, pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // io.chassis.drive(new ChassisSpeeds()); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // return pid.atSetpoint(); + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index eb399e9..bb51552 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -28,9 +28,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; public class Claw extends SubsystemBase { - SparkMax algaeIntake = new SparkMax(13, MotorType.kBrushless); - TalonFX coralIntake = new TalonFX(14); - SparkMax pivot = new SparkMax(15, MotorType.kBrushless); + SparkMax intake = new SparkMax(13, MotorType.kBrushless); + SparkMax pivot = new SparkMax(14, MotorType.kBrushless); DigitalInput algaeBreak = new DigitalInput(0); DigitalInput coralBreak = new DigitalInput(1); @@ -44,7 +43,7 @@ public Claw() { SparkMaxConfig config = new SparkMaxConfig(); config.idleMode(SparkMaxConfig.IdleMode.kBrake); - algaeIntake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + intake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); config.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); @@ -55,40 +54,16 @@ public Claw() { pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - public void voltsAlgae(double volts) { - algaeIntake.setVoltage(volts); + public void volts(double volts) { + intake.setVoltage(volts); } - public void speedAlgae(double speed) { - algaeIntake.setVoltage(speed); - } - - public void voltsCoral(double volts) { - coralIntake.setVoltage(volts); - } - - public void speedCoral(double speed) { - coralIntake.setVoltage(speed); - } - - public void scoreCoral() { - coralIntake.set(.4); - } - - public void stopCoral() { - coralIntake.set(0); - } - - public void scoreAlgae() { - algaeIntake.set(1); - } - - public void stopAlgae() { - algaeIntake.set(0); + public void speed(double speed) { + intake.setVoltage(speed); } public void stop() { - algaeIntake.stopMotor(); + intake.stopMotor(); } public boolean hasAlgae() { @@ -100,7 +75,7 @@ public boolean hasCoral() { } public Voltage voltage() { - return Volts.of(pivot.getBusVoltage()); // TODO: Return Bus voltage + return Volts.of(pivot.getBusVoltage()); } public double angle() { @@ -131,34 +106,6 @@ public void setAngle(double target_angle) { .angularVelocity(DegreesPerSecond.of(pivot.getAbsoluteEncoder().getVelocity())); }, this)); - public final SysIdRoutine algaeIntakeRoutine = new SysIdRoutine(new Config( - null, - Volts.of(4), - Seconds.of(5), - null), - new Mechanism( - volts -> algaeIntake.setVoltage(volts), - log -> { - log.motor("Algae Intake") - .voltage(voltage()) - .angularPosition(Degree.of(angle())) - .angularVelocity(DegreesPerSecond.of(algaeIntake.getAbsoluteEncoder().getVelocity())); - }, this)); - - public final SysIdRoutine coralIntakeRoutine = new SysIdRoutine(new Config( - null, - Volts.of(4), - Seconds.of(5), - null), - new Mechanism( - volts -> coralIntake.setVoltage(volts.magnitude()), - log -> { - log.motor("Coral Intake") - .voltage(voltage()) - .angularPosition(Degree.of(angle())) - .angularVelocity(DegreesPerSecond.of(coralIntake.getVelocity().getValueAsDouble())); - }, this)); - @Override public void periodic() { DogLog.log("Claw/Algae Full", hasAlgae()); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 177fd02..1efe210 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -223,7 +223,7 @@ public void setModuleStates(SwerveModuleState[] states) { null, null), new SysIdRoutine.Mechanism(voltage -> { - speeds = new ChassisSpeeds(0, 0, (voltage/16.0) * Constants.MAX_VELOCITY); + speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_VELOCITY); }, log -> { for (int i = 0; i < 4; i++) { log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") @@ -246,13 +246,15 @@ public void toggle() { public Pose2d estimatePose() { estimator.update(rotation(), modulePositions()); - LimelightHelpers.SetRobotOrientation("limelight-main", estimator.getEstimatedPosition().getRotation().getRotations(), 0,0,0,0,0); + double tagAngle = (LimelightHelpers.getTargetPose_CameraSpace("limelight-main")[3] + 360) %360; + double ourPos = tagAngle - LimelightHelpers.getTX("limelight-main") + 130; + SmartDashboard.putNumber("Our Angle", ourPos); + LimelightHelpers.SetRobotOrientation("limelight-main", ourPos, 0,0,0,0,0); LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); if (mt2 == null) return new Pose2d(); - if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ + else if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); estimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); - // estimatedPosePublisher.set(mt2.pose); estimatedPosePublisher.set(estimator.getEstimatedPosition()); } return mt2.pose; From 20dcdea630606e44da1b63ad05e9fa92d61967c0 Mon Sep 17 00:00:00 2001 From: Evan Date: Fri, 28 Feb 2025 21:21:12 -0500 Subject: [PATCH 58/79] Added a function to manually rotate the chassis incase characterization doesn't work --- .../java/frc/robot/commands/DefaultDrive.java | 5 +++-- .../java/frc/robot/subsystems/Swerve.java | 20 ++++++++++++++++--- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index 01015da..0978b25 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.swerve.Swerve; +import frc.robot.swerve.Swerve.Constants; import frc.robot.utility.IO; import java.util.function.DoubleSupplier; @@ -45,8 +46,8 @@ public void execute() { double down_scale = 1.25 - modifyAxis(controller.getLeftTriggerAxis()); double up_scale = (Swerve.Constants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis()); - double scale = io.chassis.constants.transFactor * down_scale + up_scale; - double rot_scale = io.chassis.constants.rotFactor * down_scale + up_scale; + double scale = Constants.transFactor * down_scale + up_scale; + double rot_scale = Constants.rotFactor * down_scale + up_scale; double xSpeed = x_supplier.getAsDouble() * scale; double ySpeed = y_supplier.getAsDouble() * scale; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 1efe210..c23c8e3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -12,6 +12,7 @@ import dev.doglog.DogLog; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -64,6 +65,8 @@ public class Swerve extends SubsystemBase { public boolean active = true; + private final PIDController rotationPID = new PIDController(0.0, 0.0, 0.0); // TODO tune these if Characterization fails for Theta + public Swerve() { kinematics = new SwerveDriveKinematics( createTranslation(constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), @@ -210,7 +213,7 @@ public void setModuleStates(SwerveModuleState[] states) { mod.set(voltage.magnitude(), 0); }, log -> { for (int i = 0; i < 4; i++) { - log.motor(constants.LAYOUT_TITLE[i] + " [Drive]") + log.motor(Constants.LAYOUT_TITLE[i] + " [Drive]") .voltage(modules[i].voltage()) .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) .linearVelocity(modules[i].velocity()); @@ -226,7 +229,7 @@ public void setModuleStates(SwerveModuleState[] states) { speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_VELOCITY); }, log -> { for (int i = 0; i < 4; i++) { - log.motor(constants.LAYOUT_TITLE[i] + " [Steer]") + log.motor(Constants.LAYOUT_TITLE[i] + " [Steer]") .voltage(modules[i].steerVoltage()) .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) .angularVelocity(modules[i].steerVelocity()); @@ -260,6 +263,17 @@ else if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720 return mt2.pose; } + public void rotateChassis(double targetAngle){ + double yaw = getYaw(); + double error = ((targetAngle - yaw + 180) % 360) - 180; + + drive(new ChassisSpeeds(0, 0, rotationPID.calculate(yaw, targetAngle))); + + if(Math.abs(error) < 1.0){ + stop(); + } + } + public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) @@ -292,7 +306,7 @@ public void periodic() { DogLog.log("Swerve/Pose", pose); DogLog.log("Swerve/Odometry Rotation", rotation().getDegrees()); - DogLog.log("Swerve/Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); + DogLog.log("Swerve/Pigeon Yaw", getYaw()); DogLog.log("Swerve/Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); DogLog.log("Swerve/Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); DogLog.log("Swerve/Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); From da73b9b393e49424c11f0f741c85ae9c3ed462a1 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sat, 1 Mar 2025 15:50:26 -0500 Subject: [PATCH 59/79] auton paths --- .../Bottom 4 Coral Cleanout + Algae.auto | 79 +++++++++ .../pathplanner/autos/Slide and Drop.auto | 25 +++ .../autos/Slide and Watch Deluxe.auto | 55 ++++++ .../pathplanner/autos/Slide and Watch.auto | 25 +++ .../Top 4 Coral Cleanout + Algae Deluxe.auto | 115 ++++++++++++ .../autos/Top 4 Coral Cleanout + Algae.auto | 91 ++++++++++ ...al Auto.auto => Top 4 Coral Cleanout.auto} | 12 +- .../deploy/pathplanner/paths/AB to Barge.path | 54 ++++++ .../deploy/pathplanner/paths/Barge to AB.path | 54 ++++++ .../deploy/pathplanner/paths/Barge to HG.path | 54 ++++++ .../{Top to J,I.path => Barge to IJ.path} | 22 +-- .../paths/Barge to Station to CD.path | 54 ++++++ .../paths/Barge to Station to LK.path | 70 ++++++++ .../pathplanner/paths/Bottom to CD.path | 54 ++++++ .../deploy/pathplanner/paths/CD to Barge.path | 54 ++++++ .../deploy/pathplanner/paths/Cycle CD.path | 70 ++++++++ ...to Station and back.path => Cycle LK.path} | 56 +++--- .../pathplanner/paths/Forward and Back.path | 6 +- .../deploy/pathplanner/paths/HG to Barge.path | 54 ++++++ .../deploy/pathplanner/paths/IJ to Barge.path | 54 ++++++ .../paths/J,I to Station and back.path | 96 ---------- .../paths/J,I to Station to L, K.path | 100 ----------- .../deploy/pathplanner/paths/LK to Barge.path | 54 ++++++ .../pathplanner/paths/Middle to HG.path | 54 ++++++ .../deploy/pathplanner/paths/New Path.path | 166 ------------------ .../deploy/pathplanner/paths/Return Home.path | 54 ++++++ .../paths/Straight And Rotate.path | 6 +- .../paths/Straight Line Rotate 90.path | 6 +- .../pathplanner/paths/Straight Test.path | 6 +- .../deploy/pathplanner/paths/Top to LK.path | 54 ++++++ .../deploy/pathplanner/paths/Triangle.path | 6 +- src/main/deploy/pathplanner/settings.json | 11 +- .../frc/robot/commands/RotateChassis.java | 40 +++++ .../java/frc/robot/subsystems/Swerve.java | 13 +- 34 files changed, 1282 insertions(+), 442 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto create mode 100644 src/main/deploy/pathplanner/autos/Slide and Drop.auto create mode 100644 src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto create mode 100644 src/main/deploy/pathplanner/autos/Slide and Watch.auto create mode 100644 src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto create mode 100644 src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto rename src/main/deploy/pathplanner/autos/{Top - 4 Coral Auto.auto => Top 4 Coral Cleanout.auto} (77%) create mode 100644 src/main/deploy/pathplanner/paths/AB to Barge.path create mode 100644 src/main/deploy/pathplanner/paths/Barge to AB.path create mode 100644 src/main/deploy/pathplanner/paths/Barge to HG.path rename src/main/deploy/pathplanner/paths/{Top to J,I.path => Barge to IJ.path} (71%) create mode 100644 src/main/deploy/pathplanner/paths/Barge to Station to CD.path create mode 100644 src/main/deploy/pathplanner/paths/Barge to Station to LK.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom to CD.path create mode 100644 src/main/deploy/pathplanner/paths/CD to Barge.path create mode 100644 src/main/deploy/pathplanner/paths/Cycle CD.path rename src/main/deploy/pathplanner/paths/{L, K to Station and back.path => Cycle LK.path} (55%) create mode 100644 src/main/deploy/pathplanner/paths/HG to Barge.path create mode 100644 src/main/deploy/pathplanner/paths/IJ to Barge.path delete mode 100644 src/main/deploy/pathplanner/paths/J,I to Station and back.path delete mode 100644 src/main/deploy/pathplanner/paths/J,I to Station to L, K.path create mode 100644 src/main/deploy/pathplanner/paths/LK to Barge.path create mode 100644 src/main/deploy/pathplanner/paths/Middle to HG.path delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/Return Home.path create mode 100644 src/main/deploy/pathplanner/paths/Top to LK.path create mode 100644 src/main/java/frc/robot/commands/RotateChassis.java diff --git a/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto new file mode 100644 index 0000000..9d5f618 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom to CD" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle CD" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle CD" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L3" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle CD" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L3" + } + }, + { + "type": "named", + "data": { + "name": "Clear Low Alage" + } + }, + { + "type": "path", + "data": { + "pathName": "CD to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Slide and Drop.auto b/src/main/deploy/pathplanner/autos/Slide and Drop.auto new file mode 100644 index 0000000..f8d9b2e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Slide and Drop.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle to HG" + } + }, + { + "type": "named", + "data": { + "name": "Score L1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto new file mode 100644 index 0000000..cf22abd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle to HG" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "named", + "data": { + "name": "Clear Low Alage" + } + }, + { + "type": "path", + "data": { + "pathName": "HG to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Face Barge" + } + }, + { + "type": "named", + "data": { + "name": "Score Barge" + } + }, + { + "type": "path", + "data": { + "pathName": "Return Home" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Slide and Watch.auto b/src/main/deploy/pathplanner/autos/Slide and Watch.auto new file mode 100644 index 0000000..ca2226e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Slide and Watch.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle to HG" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto new file mode 100644 index 0000000..58d65a5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto @@ -0,0 +1,115 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L3" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L3" + } + }, + { + "type": "named", + "data": { + "name": "Clear Low Alage" + } + }, + { + "type": "path", + "data": { + "pathName": "LK to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Score Barge" + } + }, + { + "type": "named", + "data": { + "name": "Rotate to IJ" + } + }, + { + "type": "path", + "data": { + "pathName": "Barge to IJ" + } + }, + { + "type": "named", + "data": { + "name": "Clear High Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "IJ to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Face Barge" + } + }, + { + "type": "named", + "data": { + "name": "Score Barge" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto new file mode 100644 index 0000000..53facf5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L3" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L3" + } + }, + { + "type": "named", + "data": { + "name": "Clear Low Alage" + } + }, + { + "type": "path", + "data": { + "pathName": "LK to Barge" + } + }, + { + "type": "named", + "data": { + "name": "Score Barge" + } + }, + { + "type": "path", + "data": { + "pathName": "Barge to Station to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto similarity index 77% rename from src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto rename to src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto index f08796f..b0abb49 100644 --- a/src/main/deploy/pathplanner/autos/Top - 4 Coral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Top to J,I" + "pathName": "Top to LK" } }, { @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "J,I to Station and back" + "pathName": "Cycle LK" } }, { @@ -31,25 +31,25 @@ { "type": "path", "data": { - "pathName": "J,I to Station to L, K" + "pathName": "Cycle LK" } }, { "type": "named", "data": { - "name": "Score L-L4" + "name": "Score L-L3" } }, { "type": "path", "data": { - "pathName": "L, K to Station and back" + "pathName": "Cycle LK" } }, { "type": "named", "data": { - "name": "Score R-L4" + "name": "Score R-L3" } } ] diff --git a/src/main/deploy/pathplanner/paths/AB to Barge.path b/src/main/deploy/pathplanner/paths/AB to Barge.path new file mode 100644 index 0000000..6f2ec09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AB to Barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8672697368421054, + "y": 4.111595394736842 + }, + "prevControl": null, + "nextControl": { + "x": 2.8961348684210524, + "y": 6.007072368421052 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.524177631578947, + "y": 5.583717105263157 + }, + "prevControl": { + "x": 4.753125, + "y": 5.622203947368421 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Barge to AB.path b/src/main/deploy/pathplanner/paths/Barge to AB.path new file mode 100644 index 0000000..078b1ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Barge to AB.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.553042763157894, + "y": 5.2195419520547945 + }, + "prevControl": null, + "nextControl": { + "x": 4.377878289473684, + "y": 6.122532894736842 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0115953947368417, + "y": 3.9865131578947364 + }, + "prevControl": { + "x": 2.3958059210526317, + "y": 5.6703125 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Barge to HG.path b/src/main/deploy/pathplanner/paths/Barge to HG.path new file mode 100644 index 0000000..b607895 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Barge to HG.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.553042763157894, + "y": 5.2195419520547945 + }, + "prevControl": null, + "nextControl": { + "x": 7.177092905565743, + "y": 4.92330746482436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.955838815789473, + "y": 4.005756578947368 + }, + "prevControl": { + "x": 6.831269269372734, + "y": 4.6711797130262935 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top to J,I.path b/src/main/deploy/pathplanner/paths/Barge to IJ.path similarity index 71% rename from src/main/deploy/pathplanner/paths/Top to J,I.path rename to src/main/deploy/pathplanner/paths/Barge to IJ.path index b3f3aff..f6bbbb2 100644 --- a/src/main/deploy/pathplanner/paths/Top to J,I.path +++ b/src/main/deploy/pathplanner/paths/Barge to IJ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.894500732421875, - "y": 5.207857863967483 + "x": 7.6, + "y": 5.7 }, "prevControl": null, "nextControl": { - "x": 5.698075452302135, - "y": 5.2101679886381485 + "x": 7.301390224789032, + "y": 5.638897810070038 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2857169486380915, - "y": 5.207857863967483 + "x": 5.19888698630137, + "y": 5.2195419520547945 }, "prevControl": { - "x": 6.056471022591409, - "y": 5.212898489585596 + "x": 5.557876031958477, + "y": 5.288158179669997 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,7 +45,7 @@ "rotation": -119.99999999999999 }, "reversed": false, - "folder": null, + "folder": "Algae", "idealStartingState": { "velocity": 0, "rotation": -119.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path new file mode 100644 index 0000000..7a5e94e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.75 + }, + "prevControl": null, + "nextControl": { + "x": 4.958476027397261, + "y": 1.8537885273972599 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.557919520547944, + "y": 3.9123073630136984 + }, + "prevControl": { + "x": 5.168835616438354, + "y": 1.8838398972602746 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path new file mode 100644 index 0000000..2b402f3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.6, + "y": 5.7 + }, + "prevControl": null, + "nextControl": { + "x": 6.820069045466418, + "y": 5.8536552289261135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.712928082191781, + "y": 7.3081121575342465 + }, + "prevControl": { + "x": 1.9235838724413903, + "y": 7.173486238654141 + }, + "nextControl": { + "x": 1.5022722919421716, + "y": 7.442738076414352 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8, + "y": 5.3 + }, + "prevControl": { + "x": 2.8920089922838375, + "y": 6.273030788421103 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom to CD.path b/src/main/deploy/pathplanner/paths/Bottom to CD.path new file mode 100644 index 0000000..f840403 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom to CD.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.693150684931506, + "y": 2.2144049657534244 + }, + "prevControl": null, + "nextControl": { + "x": 4.282320205479451, + "y": 2.3195847602739734 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6, + "y": 2.75 + }, + "prevControl": { + "x": 4.201027397260273, + "y": 2.3593321917808225 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Starting", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CD to Barge.path b/src/main/deploy/pathplanner/paths/CD to Barge.path new file mode 100644 index 0000000..7a5e94e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CD to Barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.75 + }, + "prevControl": null, + "nextControl": { + "x": 4.958476027397261, + "y": 1.8537885273972599 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.557919520547944, + "y": 3.9123073630136984 + }, + "prevControl": { + "x": 5.168835616438354, + "y": 1.8838398972602746 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle CD.path b/src/main/deploy/pathplanner/paths/Cycle CD.path new file mode 100644 index 0000000..c5182a7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cycle CD.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6, + "y": 2.75 + }, + "prevControl": null, + "nextControl": { + "x": 2.8076413111349265, + "y": 2.2095878457116576 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2771832191780823, + "y": 0.9822988013698626 + }, + "prevControl": { + "x": 1.469285804381291, + "y": 1.1422881658858952 + }, + "nextControl": { + "x": 1.0609783711466783, + "y": 0.8022362779777934 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6, + "y": 2.75 + }, + "prevControl": { + "x": 3.0821909027253858, + "y": 2.4810005631499648 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L, K to Station and back.path b/src/main/deploy/pathplanner/paths/Cycle LK.path similarity index 55% rename from src/main/deploy/pathplanner/paths/L, K to Station and back.path rename to src/main/deploy/pathplanner/paths/Cycle LK.path index bb98543..3ed2f94 100644 --- a/src/main/deploy/pathplanner/paths/L, K to Station and back.path +++ b/src/main/deploy/pathplanner/paths/Cycle LK.path @@ -3,61 +3,56 @@ "waypoints": [ { "anchor": { - "x": 3.779771959459459, - "y": 5.207857863967483 + "x": 3.8, + "y": 5.3 }, "prevControl": null, "nextControl": { - "x": 1.8725507069059188, - "y": 7.013568682904109 + "x": 2.9060695947806816, + "y": 5.981127203067267 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.607935609206641, - "y": 7.156955540435973 + "x": 1.6828767123287671, + "y": 7.353189212328767 }, "prevControl": { - "x": 1.4089293212996126, - "y": 7.319139314026312 + "x": 1.5172681656002416, + "y": 7.54046924129733 }, "nextControl": { - "x": 3.8299091553650393, - "y": 5.346118012014594 + "x": 1.9544850896817896, + "y": 7.046038248301759 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.779771959459459, - "y": 5.207857863967483 + "x": 3.8, + "y": 5.3 }, "prevControl": { - "x": 3.627209394964983, - "y": 5.4059100386964385 + "x": 3.365004409549383, + "y": 5.774787496649876 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 130.0 - } - ], + "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.5515762061403513, - "maxWaypointRelativePos": 0.7087719298245614, + "minWaypointRelativePos": 0.9612403100775195, + "maxWaypointRelativePos": 1.1369509043927652, "constraints": { - "maxVelocity": 0.3, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -66,17 +61,10 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Raise L4", - "waypointRelativePos": 1.2336759868421046, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path index 9001e38..b427204 100644 --- a/src/main/deploy/pathplanner/paths/Forward and Back.path +++ b/src/main/deploy/pathplanner/paths/Forward and Back.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -61,7 +61,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/HG to Barge.path b/src/main/deploy/pathplanner/paths/HG to Barge.path new file mode 100644 index 0000000..64f8cba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HG to Barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.955838815789473, + "y": 4.005756578947368 + }, + "prevControl": null, + "nextControl": { + "x": 6.934552068302233, + "y": 4.621997023584086 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.5337993421052625, + "y": 5.035279605263158 + }, + "prevControl": { + "x": 7.212622173286315, + "y": 4.842237224982527 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/IJ to Barge.path b/src/main/deploy/pathplanner/paths/IJ to Barge.path new file mode 100644 index 0000000..74d6340 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/IJ to Barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.19888698630137, + "y": 5.2195419520547945 + }, + "prevControl": null, + "nextControl": { + "x": 6.23486049929036, + "y": 5.253929907898961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.553042763157894, + "y": 5.2195419520547945 + }, + "prevControl": { + "x": 6.29554586930132, + "y": 5.195869163139581 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "Algae", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J,I to Station and back.path b/src/main/deploy/pathplanner/paths/J,I to Station and back.path deleted file mode 100644 index a2ecf71..0000000 --- a/src/main/deploy/pathplanner/paths/J,I to Station and back.path +++ /dev/null @@ -1,96 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.2857169486380915, - "y": 5.207857863967483 - }, - "prevControl": null, - "nextControl": { - "x": 1.231399772034817, - "y": 7.532716062218042 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8349959089949324, - "y": 7.200054766680743 - }, - "prevControl": { - "x": 5.73826015519869, - "y": 4.891424218864765 - }, - "nextControl": { - "x": 1.6198163567610897, - "y": 7.327325188619397 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.2857169486380915, - "y": 5.207857863967483 - }, - "prevControl": { - "x": 1.7386036147673982, - "y": 7.246865032045945 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 130.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.070874451754386, - "maxWaypointRelativePos": 1.4, - "constraints": { - "maxVelocity": 0.3, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Raise L4", - "waypointRelativePos": 1.8484100877192986, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.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 -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path b/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path deleted file mode 100644 index 2e93682..0000000 --- a/src/main/deploy/pathplanner/paths/J,I to Station to L, K.path +++ /dev/null @@ -1,100 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.2857169486380915, - "y": 5.207857863967483 - }, - "prevControl": null, - "nextControl": { - "x": 0.8694549570056491, - "y": 7.655828173694941 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8973330233959442, - "y": 7.240962381565823 - }, - "prevControl": { - "x": 5.693393352468902, - "y": 5.037692013550632 - }, - "nextControl": { - "x": 1.6811136855615914, - "y": 7.366458190047517 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.779771959459459, - "y": 5.207857863967483 - }, - "prevControl": { - "x": 1.7201828394381518, - "y": 7.591910063856189 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 130.0 - }, - { - "waypointRelativePos": 1.8369041582150103, - "rotationDegrees": -59.99999999999999 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.0019248188405792, - "maxWaypointRelativePos": 1.5275324577294696, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Raise L4", - "waypointRelativePos": 1.9, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.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": -119.99999999999999 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to Barge.path b/src/main/deploy/pathplanner/paths/LK to Barge.path new file mode 100644 index 0000000..9fbef1d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LK to Barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8, + "y": 5.3 + }, + "prevControl": null, + "nextControl": { + "x": 4.224264068711928, + "y": 5.724264068711928 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6, + "y": 5.7 + }, + "prevControl": { + "x": 4.252268835616438, + "y": 5.7153895547945215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle to HG.path b/src/main/deploy/pathplanner/paths/Middle to HG.path new file mode 100644 index 0000000..424993e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle to HG.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.978638698630138, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 7.2403670394959025, + "y": 3.9896688626101993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.9651969178082185, + "y": 4.0 + }, + "prevControl": { + "x": 6.570306276096605, + "y": 3.9986716795875528 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Starting", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index f2908c5..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,166 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.79424342105263, - "y": 6.170641447368421 - }, - "prevControl": null, - "nextControl": { - "x": 7.1296875, - "y": 6.132154605263157 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.435608552631579, - "y": 5.978207236842105 - }, - "prevControl": { - "x": 3.435608552631579, - "y": 5.978207236842105 - }, - "nextControl": { - "x": 5.435608552631579, - "y": 5.978207236842105 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.994325657894737, - "y": 4.8139802631578945 - }, - "prevControl": { - "x": 5.714967105263158, - "y": 5.39609375 - }, - "nextControl": { - "x": 6.273684210526316, - "y": 4.231866776315789 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.042434210526316, - "y": 2.9377467105263158 - }, - "prevControl": { - "x": 6.158059210526316, - "y": 3.584806743421052 - }, - "nextControl": { - "x": 5.926809210526315, - "y": 2.2906866776315793 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.63766447368421, - "y": 2.360444078947369 - }, - "prevControl": { - "x": 5.282236842105263, - "y": 2.325565378289474 - }, - "nextControl": { - "x": 3.993092105263157, - "y": 2.3953227796052636 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1930921052631578, - "y": 2.244983552631579 - }, - "prevControl": { - "x": 2.5930921052631577, - "y": 2.3201531661184216 - }, - "nextControl": { - "x": -0.20690789473684212, - "y": 2.1698139391447366 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.746134868421052, - "y": 6.228371710526315 - }, - "prevControl": { - "x": 4.269613486842105, - "y": 4.199092824835526 - }, - "nextControl": { - "x": 13.22265625, - "y": 8.257650596217104 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4631578947368418, - "y": 5.429769736842105 - }, - "prevControl": { - "x": 7.842907072368421, - "y": 6.843710166529604 - }, - "nextControl": { - "x": -2.9165912828947373, - "y": 4.015829307154607 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.367598684210526, - "y": 7.007730263157895 - }, - "prevControl": { - "x": 0.31690995065789496, - "y": 5.497347219366777 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Return Home.path b/src/main/deploy/pathplanner/paths/Return Home.path new file mode 100644 index 0000000..bff9875 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Return Home.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5337993421052625, + "y": 5.035279605263158 + }, + "prevControl": null, + "nextControl": { + "x": 7.530894149921405, + "y": 4.273840679843942 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.5337993421052625, + "y": 4.015378289473684 + }, + "prevControl": { + "x": 7.514580154845477, + "y": 4.651815443008323 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Return", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path index 5b84969..8f8b2ca 100644 --- a/src/main/deploy/pathplanner/paths/Straight And Rotate.path +++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path index 5dcb531..fe55b85 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,7 +45,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index ea9c0a9..f42e030 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Top to LK.path b/src/main/deploy/pathplanner/paths/Top to LK.path new file mode 100644 index 0000000..a8556d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top to LK.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5, + "y": 5.7 + }, + "prevControl": null, + "nextControl": { + "x": 4.1, + "y": 5.7 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8, + "y": 5.3 + }, + "prevControl": { + "x": 4.12139380484327, + "y": 5.683022221559489 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "Starting", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path index 10643aa..faf267e 100644 --- a/src/main/deploy/pathplanner/paths/Triangle.path +++ b/src/main/deploy/pathplanner/paths/Triangle.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -82,7 +82,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "Test", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 00d9d11..6644d20 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,10 +2,15 @@ "robotWidth": 0.762, "robotLength": 0.762, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Algae", + "Return", + "Starting", + "Test" + ], "autoFolders": [], - "defaultMaxVel": 2.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 5.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/commands/RotateChassis.java b/src/main/java/frc/robot/commands/RotateChassis.java new file mode 100644 index 0000000..7b87ade --- /dev/null +++ b/src/main/java/frc/robot/commands/RotateChassis.java @@ -0,0 +1,40 @@ +package frc.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; + +public class RotateChassis extends Command { + IO io; + private final PIDController rotationPID = new PIDController(0.0, 0.0, 0.0); + + private double targetAngle; + double yaw; + double error; + + public RotateChassis(IO io, double targetAngle) { + this.io = io; + addRequirements(io.chassis); + this.targetAngle = targetAngle; + rotationPID.setTolerance(5); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + yaw = io.chassis.getYaw(); + io.chassis.drive(new ChassisSpeeds(0, 0, rotationPID.calculate(yaw, targetAngle))); + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return rotationPID.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c23c8e3..0692397 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -254,6 +254,8 @@ public Pose2d estimatePose() { SmartDashboard.putNumber("Our Angle", ourPos); LimelightHelpers.SetRobotOrientation("limelight-main", ourPos, 0,0,0,0,0); LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); + + if (mt2 == null) return new Pose2d(); else if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720|| mt2.tagCount == 0)){ estimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); @@ -263,17 +265,6 @@ else if (!(Math.abs(pigeon2.getAngularVelocityXWorld().getValueAsDouble()) > 720 return mt2.pose; } - public void rotateChassis(double targetAngle){ - double yaw = getYaw(); - double error = ((targetAngle - yaw + 180) % 360) - 180; - - drive(new ChassisSpeeds(0, 0, rotationPID.calculate(yaw, targetAngle))); - - if(Math.abs(error) < 1.0){ - stop(); - } - } - public void periodic() { SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); if (active && speeds != new ChassisSpeeds()) From a0d76ffc8ee2b10e0c5cb601397a8b9a39373139 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 2 Mar 2025 11:00:44 -0500 Subject: [PATCH 60/79] paths --- ...de and Watch.auto => Slide and Score.auto} | 0 .../autos/Top 4 Coral Cleanout Rotations.auto | 73 ++++++++++++++++ .../deploy/pathplanner/paths/Cycle JI.path | 84 +++++++++++++++++++ .../deploy/pathplanner/paths/LK to JI.path | 84 +++++++++++++++++++ 4 files changed, 241 insertions(+) rename src/main/deploy/pathplanner/autos/{Slide and Watch.auto => Slide and Score.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto create mode 100644 src/main/deploy/pathplanner/paths/Cycle JI.path create mode 100644 src/main/deploy/pathplanner/paths/LK to JI.path diff --git a/src/main/deploy/pathplanner/autos/Slide and Watch.auto b/src/main/deploy/pathplanner/autos/Slide and Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Slide and Watch.auto rename to src/main/deploy/pathplanner/autos/Slide and Score.auto diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto new file mode 100644 index 0000000..4a2de67 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L3" + } + }, + { + "type": "path", + "data": { + "pathName": "LK to JI" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle JI" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle JI.path b/src/main/deploy/pathplanner/paths/Cycle JI.path new file mode 100644 index 0000000..b9bb27b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cycle JI.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.176, + "y": 5.3 + }, + "prevControl": null, + "nextControl": { + "x": 4.282069594780682, + "y": 5.981127203067267 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6828767123287671, + "y": 7.353189212328767 + }, + "prevControl": { + "x": 1.5172681656002416, + "y": 7.54046924129733 + }, + "nextControl": { + "x": 1.9544850896817896, + "y": 7.046038248301759 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.176, + "y": 5.3 + }, + "prevControl": { + "x": 4.741004409549384, + "y": 5.774787496649876 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9612403100775195, + "maxWaypointRelativePos": 1.1369509043927652, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to JI.path b/src/main/deploy/pathplanner/paths/LK to JI.path new file mode 100644 index 0000000..9471adb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LK to JI.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8, + "y": 5.3 + }, + "prevControl": null, + "nextControl": { + "x": 2.9060695947806816, + "y": 5.981127203067267 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6828767123287671, + "y": 7.353189212328767 + }, + "prevControl": { + "x": 1.5172681656002416, + "y": 7.54046924129733 + }, + "nextControl": { + "x": 1.9544850896817896, + "y": 7.046038248301759 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.176, + "y": 5.3 + }, + "prevControl": { + "x": 4.741004409549383, + "y": 5.774787496649876 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9612403100775195, + "maxWaypointRelativePos": 1.1369509043927652, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.5, + "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": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 3e98f98b3316871c930ee0ac3c83331503261f37 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 2 Mar 2025 15:31:06 -0500 Subject: [PATCH 61/79] Auton Commands and Simple Alignment --- src/main/java/frc/robot/RobotContainer.java | 47 ++++++++++++++---- src/main/java/frc/robot/commands/Intake.java | 6 ++- .../frc/robot/commands/RotateChassis.java | 7 +-- .../java/frc/robot/commands/ScoreAlgae.java | 4 +- .../java/frc/robot/commands/ScoreReef.java | 4 +- .../java/frc/robot/commands/SimpleAlign.java | 47 ++++++++++++++++++ .../java/frc/robot/subsystems/Elevator.java | 49 +++++++------------ .../java/frc/robot/subsystems/Swerve.java | 48 +++++++++--------- src/main/java/frc/robot/swerve/Swerve.java | 2 +- .../robot/utility/AutomatedController.java | 9 +++- 10 files changed, 148 insertions(+), 75 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SimpleAlign.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 177e03e..d57efc9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,15 +6,17 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.util.sendable.Sendable; 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.PrintCommand; -import dev.doglog.DogLog; -import dev.doglog.DogLogOptions; import frc.robot.commands.DefaultDrive; +import frc.robot.commands.Intake; +import frc.robot.commands.RotateChassis; +import frc.robot.commands.ScoreAlgae; +import frc.robot.commands.ScoreReef; import frc.robot.swerve.Swerve; import frc.robot.utility.AutomatedController; import frc.robot.utility.IO; @@ -27,8 +29,7 @@ public class RobotContainer { public final AutomatedController backup; private final SendableChooser auto_selector; - private Command current_auto = new PrintCommand(""); - + Command current_auto = new PrintCommand(""); final SendableChooser driver_selector = new SendableChooser(); public RobotContainer() { @@ -52,15 +53,41 @@ public RobotContainer() { driver_selector.addOption("Uriel", 3); driver_selector.onChange( (driver) -> Swerve.Constants.SwitchDriver(driver)); - DogLog.setOptions(new DogLogOptions() - .withCaptureDs(true) - .withCaptureConsole(true) - .withLogExtras(true)); + // DogLog.setOptions(new DogLogOptions() + // .withCaptureDs(true) + // .withCaptureConsole(true) + // .withLogExtras(true)); - DogLog.setEnabled( false); // TODO: Turn back on when we're testing proper + // DogLog.setEnabled( false); // TODO: Turn back on when we're testing proper SignalLogger.setPath("/media/sda1/ctre-logs/"); } + + public void configureAuton(){ + NamedCommands.registerCommand("Face Barge", new RotateChassis(io, 0)); + NamedCommands.registerCommand("Rotate IJ", new RotateChassis(io, -120)); + NamedCommands.registerCommand("Rotate LK", new RotateChassis(io, -60)); + NamedCommands.registerCommand("Rotate HG", new RotateChassis(io, 180)); + NamedCommands.registerCommand("Rotate CD", new RotateChassis(io, 60)); + NamedCommands.registerCommand("Rotate EF", new RotateChassis(io, 120)); + + NamedCommands.registerCommand("Score L-L4", new ScoreReef(io, false, 4)); + NamedCommands.registerCommand("Score L-L3", new ScoreReef(io, false, 3)); + NamedCommands.registerCommand("Score L-L2", new ScoreReef(io, false, 2)); + NamedCommands.registerCommand("Score L-L1", new ScoreReef(io, false, 1)); + + NamedCommands.registerCommand("Score R-L4", new ScoreReef(io, true, 4)); + NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, true, 3)); + NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, true, 2)); + NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); + + NamedCommands.registerCommand("Score Barge", new ScoreAlgae(io, true)); + + NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, false, 6)); + NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, false, 7)); + + } + public Command getAutonomousCommand() { return auto_selector.getSelected(); } diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index f7d095a..47f7008 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -24,7 +24,6 @@ public class Intake extends Command { // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio - // TODO: Review and see if we can optimise it if (coral){ intake = () -> io.claw.speed( (release) ? .4 : -.4); @@ -39,6 +38,11 @@ public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 stop = io.claw::stop; } + public Intake(IO io, boolean coral, boolean release, int level) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio + this(io, coral, release); + io.elevator.move(level); + } + // Called when the command is initially scheduled. @Override public void initialize() { diff --git a/src/main/java/frc/robot/commands/RotateChassis.java b/src/main/java/frc/robot/commands/RotateChassis.java index 7b87ade..8f404d3 100644 --- a/src/main/java/frc/robot/commands/RotateChassis.java +++ b/src/main/java/frc/robot/commands/RotateChassis.java @@ -7,7 +7,7 @@ public class RotateChassis extends Command { IO io; - private final PIDController rotationPID = new PIDController(0.0, 0.0, 0.0); + private final PIDController rotationPID = new PIDController(0.01, 0.0, 0.001); private double targetAngle; double yaw; @@ -17,7 +17,8 @@ public RotateChassis(IO io, double targetAngle) { this.io = io; addRequirements(io.chassis); this.targetAngle = targetAngle; - rotationPID.setTolerance(5); + rotationPID.setTolerance(1); + rotationPID.enableContinuousInput(0, 360); } @Override @@ -35,6 +36,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return rotationPID.atSetpoint(); + return Math.abs(rotationPID.getError()) < 1; } } diff --git a/src/main/java/frc/robot/commands/ScoreAlgae.java b/src/main/java/frc/robot/commands/ScoreAlgae.java index 9b50e50..c394264 100644 --- a/src/main/java/frc/robot/commands/ScoreAlgae.java +++ b/src/main/java/frc/robot/commands/ScoreAlgae.java @@ -14,11 +14,11 @@ public class ScoreAlgae extends ParallelCommandGroup { public ScoreAlgae(IO io, boolean barge) { addCommands( //TODO: Check if we have Algae - io.elevator.move((barge) ? 5 : 0), + io.elevator.moveCommand((barge) ? 5 : 0), new AutoAlign(1, io), new WaitUntilCommand(io.elevator::atPosition), new Intake(io, false, true), - io.elevator.move(0) + io.elevator.moveCommand(0) ); } } diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 9bd44bc..1693db6 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -13,11 +13,11 @@ public class ScoreReef extends SequentialCommandGroup { public ScoreReef(IO io, boolean score_right, int Level) { addCommands( // TODO: Check if we have coral & if we don't have - io.elevator.move(Level), + io.elevator.moveCommand(Level), new AutoAlign((score_right) ? 2 : 1, io), new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height new Intake(io, true, true), // TODO: Set to Reef Scoring Angle - io.elevator.move(0) + io.elevator.moveCommand(0) ); } } diff --git a/src/main/java/frc/robot/commands/SimpleAlign.java b/src/main/java/frc/robot/commands/SimpleAlign.java new file mode 100644 index 0000000..68d9cb5 --- /dev/null +++ b/src/main/java/frc/robot/commands/SimpleAlign.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utility.IO; +import frc.robot.utility.Util; + +/* 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 SimpleAlign extends Command { + IO io; + boolean right; + Timer time = new Timer(); + + public SimpleAlign(IO io, boolean right) { + this.io = io; + this.right = right; + addRequirements(io.chassis); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + time.restart(); + io.chassis.drive(new ChassisSpeeds(0, ((right) ? 1 : -1) * (double) Util.get("Align Speed", 2.5), 0)); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + io.chassis.drive(new ChassisSpeeds()); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return time.hasElapsed((double) Util.get("Align Time", .2)); + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 393ad8c..3e0fe7f 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -47,6 +47,8 @@ public class Elevator extends SubsystemBase { public final double L3 = 0; public final double L4 = 0; public final double Barge = 0; + public final double Low_Algae = 0; + public final double High_Algae = 0; public final double MAX_HEIGHT = 0; public Elevator() { @@ -82,53 +84,38 @@ public void move(double height) { time.restart(); } - public InstantCommand move(int level) { - return new InstantCommand(() -> { + public void move(int level) { switch (level) { case 2: - L2(); + move(L2); break; case 3: - L3(); + move(L3); break; case 4: - L4(); + move(L4); break; case 5: - Barge(); + move(Barge); + break; + case 6: + move(Low_Algae); + break; + case 7: + move(High_Algae); break; default: - rest(); // LEVEL 1 // TODO: See if we need to change the height we go to + move(Rest); // LEVEL 1 // TODO: See if we need to change the height we go to break; } - }, this); - } - - public void zero() { - lead.setPosition(0); } - public void rest() { - move(Rest); - } - public void L1(){ - move(L1); + public InstantCommand moveCommand(int level){ + return new InstantCommand(() -> this.move(level), this); } - public void L2() { - move(L2); - } - - public void L3() { - move(L3); - } - - public void L4() { - move(L4); - } - - public void Barge() { - move(Barge); + public void zero() { + lead.setPosition(0); } public boolean atPosition() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0692397..278b4f6 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -277,29 +277,29 @@ public void periodic() { estimatePose(); posePublisher.set(pose); - // SmartDashboard.putNumber("X position", pose.getX()); - // SmartDashboard.putNumber("Y position", pose.getY()); - - // SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); - // SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Pitch", - // pigeon2.getPitch().getValueAsDouble()); - // SmartDashboard.putNumber("Pigeon Roll", - // pigeon2.getRoll().getValueAsDouble()); - - // SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); - - DogLog.log("Swerve/Current States", moduleStates(modules)); - DogLog.log("Swerve/Target States", states); - - DogLog.log("Swerve/X Position", pose.getX()); - DogLog.log("Swerve/Y Position", pose.getY()); - DogLog.log("Swerve/Pose", pose); - DogLog.log("Swerve/Odometry Rotation", rotation().getDegrees()); - - DogLog.log("Swerve/Pigeon Yaw", getYaw()); - DogLog.log("Swerve/Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); - DogLog.log("Swerve/Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); - DogLog.log("Swerve/Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + SmartDashboard.putNumber("X position", pose.getX()); + SmartDashboard.putNumber("Y position", pose.getY()); + + SmartDashboard.putNumber("Odometry rotation", rotation().getDegrees()); + SmartDashboard.putNumber("Pigeon Yaw", pigeon2.getYaw().getValueAsDouble()); + SmartDashboard.putNumber("Pigeon Pitch", + pigeon2.getPitch().getValueAsDouble()); + SmartDashboard.putNumber("Pigeon Roll", + pigeon2.getRoll().getValueAsDouble()); + + SmartDashboard.putString("Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); + + // DogLog.log("Swerve/Current States", moduleStates(modules)); + // DogLog.log("Swerve/Target States", states); + + // DogLog.log("Swerve/X Position", pose.getX()); + // DogLog.log("Swerve/Y Position", pose.getY()); + // DogLog.log("Swerve/Pose", pose); + // DogLog.log("Swerve/Odometry Rotation", rotation().getDegrees()); + + // DogLog.log("Swerve/Pigeon Yaw", getYaw()); + // DogLog.log("Swerve/Pigeon Pitch", pigeon2.getPitch().getValueAsDouble()); + // DogLog.log("Swerve/Pigeon Roll", pigeon2.getRoll().getValueAsDouble()); + // DogLog.log("Swerve/Drive Mode", (field_oritented) ? "Field-Oriented" : "Robot-Oriented"); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 4b9f3da..c05c61b 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -46,7 +46,7 @@ public static class Constants { public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR - public static double[] ENCODER_OFFSETS = {-0.8823, -0.8371, -0.6311, -0.7314}; + public static double[] ENCODER_OFFSETS = {-0.87890625, -0.996337890625, -0.638427734375, -0.892822265625}; public static final int PIGEON_ID = 6; public Constants(){ diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 307607e..663150d 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -3,8 +3,10 @@ import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.RotateChassis; +import frc.robot.commands.SimpleAlign; public class AutomatedController { public final CommandXboxController controller; @@ -49,6 +51,11 @@ public void configure(){ controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::toggleMode)); controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + // controller.leftBumper().onTrue(new SimpleAlign(io, false)); + // controller.rightBumper().onTrue(new SimpleAlign(io, true)); + + controller.leftBumper().onTrue(new RotateChassis(io, 45)); + // AUTOMATED // Based on the nearest element and our field orientation From f3f718c9068ef14ee445d2c2d2bec4169c8dffe8 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 2 Mar 2025 17:13:02 -0500 Subject: [PATCH 62/79] Aligning with the limelight functional --- .../frc/robot/commands/LimelightAlign.java | 23 +++++++++++-------- .../java/frc/robot/commands/SimpleAlign.java | 1 - src/main/java/frc/robot/subsystems/Claw.java | 11 ++++++--- src/main/java/frc/robot/subsystems/Hang.java | 4 ++-- .../robot/utility/AutomatedController.java | 8 ++++--- 5 files changed, 28 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/LimelightAlign.java b/src/main/java/frc/robot/commands/LimelightAlign.java index 5c871a2..97cd07c 100644 --- a/src/main/java/frc/robot/commands/LimelightAlign.java +++ b/src/main/java/frc/robot/commands/LimelightAlign.java @@ -15,12 +15,14 @@ public class LimelightAlign extends Command { IO io; int positions; - PIDController pid = new PIDController(.2, 0, 0); + PIDController pid = new PIDController(.1, 0, 0); + String limelight = "limelight-main"; public LimelightAlign(IO io, int positions) { this.io = io; this.positions = positions; - // pid.setTolerance(5); + pid.setTolerance(1); + addRequirements(io.chassis); } // Called when the command is initially scheduled. @@ -28,33 +30,34 @@ public LimelightAlign(IO io, int positions) { public void initialize() { switch (positions) { case 2: // Right - LimelightHelpers.SetFidcuial3DOffset("limelight-main", 0.2,0, 0); + LimelightHelpers.SetFidcuial3DOffset(limelight, 0,0.2, 0); break; - case 1: // Centre - LimelightHelpers.SetFidcuial3DOffset("limelight-main", 0, 0, 0); + case 1: // Center + LimelightHelpers.SetFidcuial3DOffset(limelight, 0, 0, 0); break; default:// Left - LimelightHelpers.SetFidcuial3DOffset("limelight-main", -0.2, 0, 0); + LimelightHelpers.SetFidcuial3DOffset(limelight, 0, -0.2, 0); break; } + LimelightHelpers.Flush(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // io.chassis.drive(new ChassisSpeeds(0, pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0)); + io.chassis.drive(new ChassisSpeeds(0, -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0)); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // io.chassis.drive(new ChassisSpeeds()); + io.chassis.drive(new ChassisSpeeds()); } // Returns true when the command should end. @Override public boolean isFinished() { - // return pid.atSetpoint(); - return false; + // return pid.atSetpoint() || LimelightHelpers.getTV("limelight-main"); + return pid.atSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/SimpleAlign.java b/src/main/java/frc/robot/commands/SimpleAlign.java index 68d9cb5..7e7f1f9 100644 --- a/src/main/java/frc/robot/commands/SimpleAlign.java +++ b/src/main/java/frc/robot/commands/SimpleAlign.java @@ -10,7 +10,6 @@ import frc.robot.utility.IO; import frc.robot.utility.Util; -/* 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 SimpleAlign extends Command { IO io; boolean right; diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index bb51552..cbc1c46 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -22,6 +22,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -108,9 +109,13 @@ public void setAngle(double target_angle) { @Override public void periodic() { - DogLog.log("Claw/Algae Full", hasAlgae()); - DogLog.log("Claw/Coral Full", hasCoral()); - DogLog.log("Claw/Pivot Angle", angle()); + // DogLog.log("Claw/Algae Full", hasAlgae()); + // DogLog.log("Claw/Coral Full", hasCoral()); + // DogLog.log("Claw/Pivot Angle", angle()); + + SmartDashboard.putBoolean("Have Algae", hasAlgae()); + SmartDashboard.putBoolean("Have Algae", hasAlgae()); + SmartDashboard.putNumber("Claw Pivot", angle()); double cTime = time.get(); if (stopped) diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 70f1e60..517ea84 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -6,12 +6,12 @@ import com.revrobotics.spark.config.SoftLimitConfig; import dev.doglog.DogLog; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet - public static final double HANG_MAX_ANGLE = 0; public Hang() { // TODO: Configure a soft limit switch @@ -32,6 +32,6 @@ public void stopHang() { @Override public void periodic() { - DogLog.log("Hang/Position", hang.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Hang Position", hang.getPosition().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 663150d..83bbd43 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -5,8 +5,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.RotateChassis; -import frc.robot.commands.SimpleAlign; +import frc.robot.commands.LimelightAlign; public class AutomatedController { public final CommandXboxController controller; @@ -54,7 +53,10 @@ public void configure(){ // controller.leftBumper().onTrue(new SimpleAlign(io, false)); // controller.rightBumper().onTrue(new SimpleAlign(io, true)); - controller.leftBumper().onTrue(new RotateChassis(io, 45)); + // controller.leftBumper().onTrue(new RotateChassis(io, 45)); + controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); + controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); + controller.x().toggleOnTrue(new LimelightAlign(io, 1)); // AUTOMATED From a514c8f59150b94f0e967bd1e85584dd74e999ed Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Sun, 2 Mar 2025 20:00:56 -0500 Subject: [PATCH 63/79] Working Comp Bot --- .../java/frc/robot/subsystems/Elevator.java | 6 ++++-- src/main/java/frc/robot/subsystems/Swerve.java | 8 ++++---- src/main/java/frc/robot/swerve/Module.java | 6 ++---- src/main/java/frc/robot/swerve/Swerve.java | 18 ++++++------------ .../frc/robot/utility/AutomatedController.java | 7 +++++-- src/main/java/frc/robot/utility/IO.java | 4 ++-- 6 files changed, 23 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 3e0fe7f..2acb7ee 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -61,7 +61,7 @@ public Elevator() { lead.getConfigurator().apply(config); // lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(true).withReverseSoftLimitEnable(true) // .withForwardSoftLimitThreshold(0).withReverseSoftLimitThreshold(0)); // TODO: Find the forward and reverse Soft limit switches - follow.setControl(new Follower(lead.getDeviceID(), false)); // TODO: Check if we need to invert + follow.setControl(new Follower(lead.getDeviceID(), true)); // TODO: Check if we need to invert } public void speed(double speed) { @@ -153,6 +153,8 @@ public LinearVelocity velocity() { @Override public void periodic() { + SmartDashboard.putNumber("Elevator Height", position()); + if (stopped) return; @@ -160,7 +162,7 @@ public void periodic() { lead.setControl(positionRequest.withPosition(out.position)); SmartDashboard.putNumber("Elevator Motor Voltage", voltage().magnitude()); - SmartDashboard.putNumber("Elevator Height", position()); + // SmartDashboard.putNumber("Elevator Height", position()); SmartDashboard.putNumber("Elevator Target Height", target); SmartDashboard.putNumber("Elevator cTarget Height", out.position); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 278b4f6..d778b73 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -249,10 +249,10 @@ public void toggle() { public Pose2d estimatePose() { estimator.update(rotation(), modulePositions()); - double tagAngle = (LimelightHelpers.getTargetPose_CameraSpace("limelight-main")[3] + 360) %360; - double ourPos = tagAngle - LimelightHelpers.getTX("limelight-main") + 130; - SmartDashboard.putNumber("Our Angle", ourPos); - LimelightHelpers.SetRobotOrientation("limelight-main", ourPos, 0,0,0,0,0); + // double tagAngle = (LimelightHelpers.getTargetPose_CameraSpace("limelight-main")[3] + 360) %360; + // double ourPos = tagAngle - LimelightHelpers.getTX("limelight-main") + 130; + // SmartDashboard.putNumber("Our Angle", ourPos); + LimelightHelpers.SetRobotOrientation("limelight-main", 0, 0,0,0,0,0); LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-main"); diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 029729b..908d022 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -32,21 +32,19 @@ public class Module { double desiredAngle; public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); - public static final double DRIVE_REDUCTION = (15.0 / 32.0) * (10.0 / 60.0); public static final double STEER_REDUCTION = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0); - public static final double DRIVE_CONVERSION_FACTOR = Math.PI * WHEEL_DIAMETER * DRIVE_REDUCTION; public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, boolean comp) { drive = new TalonFX(driveID, "rio"); steer = new SparkMax(steerID, MotorType.kBrushless); - encoder = (comp) ? new Swerve.Cancoder(encoderID) : new Swerve.Canand(encoderID); + encoder = (comp) ? new Swerve.Canand(encoderID) : new Swerve.Cancoder(encoderID); SparkMaxConfig steerConfig = new SparkMaxConfig(); steerConfig .smartCurrentLimit(20) .idleMode(IdleMode.kBrake) - .inverted(comp); + .inverted(true); steerConfig.encoder .positionConversionFactor(Math.PI * STEER_REDUCTION) diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index c05c61b..51ac9fb 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -22,10 +22,10 @@ public static class Constants { // BOT SWITCHING public boolean comp = true; - public double TRACKWIDTH = 19.5; // 30.0 for MKi - public double WHEELBASE = 21.5; // 30.0 for MKi - public double GEAR_RATIO; - public double WHEEL_RADIUS; + public double TRACKWIDTH = 30; // 30.0 for MKi + public double WHEELBASE = 30; // 30.0 for MKi + public double GEAR_RATIO = 8.14; + public double WHEEL_RADIUS = .1143; // DRIVER SETTINGS public static int driver = 0; @@ -52,19 +52,13 @@ public static class Constants { public Constants(){ // compChassis = (System.getenv("BLUEBOT") == null); // Determine if we're comp based on an enviornmental variable like in 2023 if (comp) { - TRACKWIDTH = 30.0; - WHEELBASE = 30.0; - GEAR_RATIO = 8.14; // L1 Ratio - WHEEL_RADIUS = 0.1143; MASS = 60.0; } else { - TRACKWIDTH = 19.5; - WHEELBASE = 21.5; - GEAR_RATIO = 6.12; // L3 Ratio - WHEEL_RADIUS = 0.1016; MASS = 47.0; } + MASS = (comp) ? 60.0 : 47.0; + // double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); // double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); // Translation2d[] swerve_offsets = diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 83bbd43..7b1b32d 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -54,8 +54,11 @@ public void configure(){ // controller.rightBumper().onTrue(new SimpleAlign(io, true)); // controller.leftBumper().onTrue(new RotateChassis(io, 45)); - controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); - controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); + // controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); + // controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); + + controller.leftBumper().onTrue(Util.Do(() -> io.elevator.volts(1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.rightBumper().onTrue(Util.Do(() -> io.elevator.volts(-1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.x().toggleOnTrue(new LimelightAlign(io, 1)); // AUTOMATED diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index e4f6713..a0115b7 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -7,12 +7,12 @@ public class IO extends SubsystemBase { public final Swerve chassis = new Swerve(); // public final AlgaeIntake intake = new AlgaeIntake(); - // public final Elevator elevator = new Elevator(); + public final Elevator elevator = new Elevator(); // public final Hang hang = new Hang(); public final Limelight limelight = new Limelight(); public final Claw claw = null; - public final Elevator elevator = null; + // public final Elevator elevator = null; public final Hang hang = null; public CommandScheduler scheduler = CommandScheduler.getInstance(); From e916f03b4605bcbe8bdb347ce47318b3d2918b5f Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Tue, 4 Mar 2025 15:35:20 -0500 Subject: [PATCH 64/79] tuning --- .../deploy/pathplanner/autos/Hot Reload.auto | 19 ++++++++ .../pathplanner/paths/Straight Test.path | 8 ++-- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/subsystems/Elevator.java | 46 +++++++++++++------ src/main/java/frc/robot/swerve/Module.java | 4 +- src/main/java/frc/robot/swerve/Swerve.java | 5 +- .../robot/utility/AutomatedController.java | 21 +++++++-- vendordeps/DogLog.json | 20 -------- 8 files changed, 76 insertions(+), 51 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Hot Reload.auto delete mode 100644 vendordeps/DogLog.json diff --git a/src/main/deploy/pathplanner/autos/Hot Reload.auto b/src/main/deploy/pathplanner/autos/Hot Reload.auto new file mode 100644 index 0000000..6968d89 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Hot Reload.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "XXX" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index f42e030..6b7c7ec 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 4.0, + "x": 3.0, "y": 6.0 }, "prevControl": { - "x": 3.0, + "x": 2.0, "y": 6.0 }, "nextControl": null, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 0.75, "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d57efc9..cf86ef9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,8 +43,8 @@ public RobotContainer() { SmartDashboard.putData("Run Test Auto", Util.Do(current_auto::schedule)); SmartDashboard.putData("Main-Controller Mode", main.selector); - SmartDashboard.putData("Backup-Controller Mode", main.selector); - SmartDashboard.putString("Driver", Swerve.Constants.drivers[Swerve.Constants.driver]); + SmartDashboard.putData("Backup-Controller Mode", backup.selector); + SmartDashboard.putData("Driver", driver_selector); io.chassis.setDefaultCommand(new DefaultDrive(io, main.controller)); driver_selector.setDefaultOption("Shaan", 0); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 2acb7ee..a2d2858 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -6,16 +6,18 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.*; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -40,11 +42,13 @@ public class Elevator extends SubsystemBase { PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); + final String[] level_layout = {"Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae"}; + // Target Heights public final double Rest = 0; - public final double L1 = 0; - public final double L2 = 0; - public final double L3 = 0; + public final double L1 = 25; + public final double L2 = 43.5; + public final double L3 = 76; public final double L4 = 0; public final double Barge = 0; public final double Low_Algae = 0; @@ -52,15 +56,21 @@ public class Elevator extends SubsystemBase { public final double MAX_HEIGHT = 0; public Elevator() { - Slot0Configs config = new Slot0Configs(); - config.kP = 0.0; - config.kI = 0.0; - config.kD = 0.0; - config.kG = 0.0; + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.0; + config.Slot0.kI = 0.0; + config.Slot0.kD = 0.0; + config.Slot0.kG = 0.0; + + config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.SoftwareLimitSwitch + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(300.0) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(0.0); lead.getConfigurator().apply(config); - // lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(true).withReverseSoftLimitEnable(true) - // .withForwardSoftLimitThreshold(0).withReverseSoftLimitThreshold(0)); // TODO: Find the forward and reverse Soft limit switches follow.setControl(new Follower(lead.getDeviceID(), true)); // TODO: Check if we need to invert } @@ -86,6 +96,9 @@ public void move(double height) { public void move(int level) { switch (level) { + case 1: + move(L1); + break; case 2: move(L2); break; @@ -108,6 +121,7 @@ public void move(int level) { move(Rest); // LEVEL 1 // TODO: See if we need to change the height we go to break; } + SmartDashboard.putString("Target Level", level_layout[level]); } public InstantCommand moveCommand(int level){ @@ -127,11 +141,13 @@ public Voltage voltage() { } public double position() { - return lead.getPosition().getValueAsDouble() * conversion; + // return lead.getPosition().getValueAsDouble() * conversion; + return lead.getPosition().getValueAsDouble(); } public LinearVelocity velocity() { - return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * conversion); + // return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * conversion); + return MetersPerSecond.of(lead.getVelocity().getValueAsDouble()); } final double gearReduction = 1/17; diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 908d022..f3ddf8a 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -96,11 +96,11 @@ public void zeroAbsolute() { } public double drivePosition() { - return drive.getPosition().getValueAsDouble() * .432 * WHEEL_DIAMETER; + return drive.getPosition().getValueAsDouble() * .623 * WHEEL_DIAMETER; } public LinearVelocity velocity() { - return MetersPerSecond.of(drive.getVelocity().getValueAsDouble() * Swerve.PI2 * .432 * WHEEL_DIAMETER); + return MetersPerSecond.of(drive.getVelocity().getValueAsDouble() * Swerve.PI2 * .623 * WHEEL_DIAMETER); } public Voltage voltage(){ diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 51ac9fb..7957373 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -31,13 +31,12 @@ public static class Constants { public static int driver = 0; public static double transFactor = .65; // factor = x/125, with x being the percentage of our max speed, same for the thing below public static double rotFactor = .30; // .6 for tristan - public static String[] drivers = {"Shaan", "Norah", "Jason", "Uriel"}; // AUTON CONSTANTS public double XControllerP = 5.1555; public double XControllerD = 0.61072; - public double ThetaControllerP = 0.0000001; - public double ThetaControllerD = 0.00000001; + public double ThetaControllerP = 0; + public double ThetaControllerD = 0; public RobotConfig autoConfig; public double MASS = 47; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index ec192db..83a4534 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -13,7 +13,7 @@ public class AutomatedController { public final CommandXboxController controller; public final SendableChooser selector = new SendableChooser(); - public int mode = 0; + public int mode = 3; IO io; @@ -24,6 +24,7 @@ public AutomatedController(int port, IO io){ selector.setDefaultOption("Automated", () -> {mode = 0;}); selector.addOption("Manual", () -> {mode = 1;}); selector.addOption("Debug", () -> {mode = 2;}); + selector.addOption("Debug Setting", () -> {mode = 3;}); selector.onChange((x) -> {x.run();}); @@ -53,6 +54,9 @@ public BooleanSupplier manual(){ public BooleanSupplier debug(){ return mode(2); } + public BooleanSupplier debug_setting(){ + return mode(3); + } public void switchMode(){ mode = (mode + 1) % 3; @@ -70,10 +74,6 @@ public void configure(){ // controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); // controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); - controller.leftBumper().onTrue(Util.Do(() -> io.elevator.volts(1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.rightBumper().onTrue(Util.Do(() -> io.elevator.volts(-1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.x().toggleOnTrue(new LimelightAlign(io, 1)); - // AUTOMATED // Based on the nearest element and our field orientation @@ -117,5 +117,16 @@ private void configureDebug(){ controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + + controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1)); + + controller.povUp().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); + controller.povDown().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); + controller.povRight().and(debug()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); + controller.povLeft().and(debug()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + } + } diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json deleted file mode 100644 index b410eb1..0000000 --- a/vendordeps/DogLog.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "javaDependencies": [ - { - "groupId": "com.github.jonahsnider", - "artifactId": "doglog", - "version": "2025.3.0" - } - ], - "fileName": "DogLog.json", - "frcYear": "2025", - "jsonUrl": "https://doglog.dev/vendordep.json", - "name": "DogLog", - "jniDependencies": [], - "mavenUrls": [ - "https://jitpack.io" - ], - "cppDependencies": [], - "version": "2025.3.0", - "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" -} \ No newline at end of file From a784033101cbe653e4b2ca0cde4823eb980a0f26 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Tue, 4 Mar 2025 20:21:41 -0500 Subject: [PATCH 65/79] Basics ready --- src/main/java/frc/robot/RobotContainer.java | 7 ++-- src/main/java/frc/robot/commands/Intake.java | 33 ++++++++++++++++++- .../frc/robot/commands/LimelightAlign.java | 6 +++- .../java/frc/robot/commands/ScoreAlgae.java | 16 +++++++++ .../java/frc/robot/commands/ScoreReef.java | 12 +++++++ src/main/java/frc/robot/subsystems/Claw.java | 1 - .../java/frc/robot/subsystems/Elevator.java | 4 +-- src/main/java/frc/robot/subsystems/Hang.java | 1 - .../java/frc/robot/subsystems/Swerve.java | 1 - .../robot/utility/AutomatedController.java | 2 +- 10 files changed, 73 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf86ef9..73ebabc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,14 @@ package frc.robot; +import java.util.function.Consumer; + import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.Pair; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -51,6 +55,7 @@ public RobotContainer() { driver_selector.addOption("Norah", 1); driver_selector.addOption("Jason", 2); driver_selector.addOption("Uriel", 3); + driver_selector.addOption("Debug", 4); driver_selector.onChange( (driver) -> Swerve.Constants.SwitchDriver(driver)); // DogLog.setOptions(new DogLogOptions() @@ -80,12 +85,10 @@ public void configureAuton(){ NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, true, 3)); NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, true, 2)); NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); - NamedCommands.registerCommand("Score Barge", new ScoreAlgae(io, true)); NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, false, 6)); NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, false, 7)); - } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index 47f7008..7fc11b0 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -6,6 +6,8 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.utility.IO; @@ -26,7 +28,9 @@ public class Intake extends Command { public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio // TODO: Review and see if we can optimise it if (coral){ - intake = () -> io.claw.speed( (release) ? .4 : -.4); + intake = () -> { + io.claw.speed( (release) ? .4 : -.4); + }; holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); } else { // Algae intake = () -> { @@ -38,6 +42,33 @@ public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 stop = io.claw::stop; } + public Intake(IO io, boolean coral, boolean release, GenericHID controller) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio + // TODO: Review and see if we can optimise it + if (coral){ + intake = () -> { + io.claw.speed( (release) ? .4 : -.4); + controller.setRumble(RumbleType.kBothRumble, .25); + }; + holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); + } else { // Algae + intake = () -> { + io.claw.speed(1); + controller.setRumble(RumbleType.kBothRumble, .25); + // TODO: Set Claw to Reef intake Angle or Ground pickup if we + }; + holding = () -> (release) ? io.claw.hasAlgae() : !io.claw.hasAlgae(); + } + stop = () -> { + io.claw.stop(); + controller.setRumble(RumbleType.kBothRumble, 0.0); + }; + } + + public Intake(IO io, boolean coral, boolean release, int level, GenericHID controller) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio + this(io, coral, release,controller); + io.elevator.move(level); + } + public Intake(IO io, boolean coral, boolean release, int level) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio this(io, coral, release); io.elevator.move(level); diff --git a/src/main/java/frc/robot/commands/LimelightAlign.java b/src/main/java/frc/robot/commands/LimelightAlign.java index 97cd07c..dd21e1c 100644 --- a/src/main/java/frc/robot/commands/LimelightAlign.java +++ b/src/main/java/frc/robot/commands/LimelightAlign.java @@ -17,10 +17,14 @@ public class LimelightAlign extends Command { int positions; PIDController pid = new PIDController(.1, 0, 0); String limelight = "limelight-main"; + Runnable exec; - public LimelightAlign(IO io, int positions) { + public LimelightAlign(IO io, int positions, Boolean rotation) { this.io = io; this.positions = positions; + + exec = (rotation) ? () -> {io.chassis.drive(new ChassisSpeeds(0, 0, -pid.calculate(LimelightHelpers.getTX("limelight-main"))));} : () -> io.chassis.drive(new ChassisSpeeds(0, -pid.calculate(LimelightHelpers.getTX("limelight-main"), 0), 0)); + pid.setTolerance(1); addRequirements(io.chassis); } diff --git a/src/main/java/frc/robot/commands/ScoreAlgae.java b/src/main/java/frc/robot/commands/ScoreAlgae.java index c394264..5c29f8c 100644 --- a/src/main/java/frc/robot/commands/ScoreAlgae.java +++ b/src/main/java/frc/robot/commands/ScoreAlgae.java @@ -4,6 +4,11 @@ package frc.robot.commands; +import java.util.function.Consumer; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; @@ -11,6 +16,17 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ScoreAlgae extends ParallelCommandGroup { + public ScoreAlgae(IO io, boolean barge, GenericHID controller) { + addCommands( + //TODO: Check if we have Algae + io.elevator.moveCommand((barge) ? 5 : 0), + new AutoAlign(1, io), + new WaitUntilCommand(io.elevator::atPosition), + new Intake(io, false, true, controller), + io.elevator.moveCommand(0) + ); + } + public ScoreAlgae(IO io, boolean barge) { addCommands( //TODO: Check if we have Algae diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 1693db6..c978c09 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -4,12 +4,24 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; public class ScoreReef extends SequentialCommandGroup { + public ScoreReef(IO io, boolean score_right, int Level, GenericHID controller) { + addCommands( + // TODO: Check if we have coral & if we don't have + io.elevator.moveCommand(Level), + new AutoAlign((score_right) ? 2 : 1, io), + new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height + new Intake(io, true, true, controller), // TODO: Set to Reef Scoring Angle + io.elevator.moveCommand(0) + ); + } + public ScoreReef(IO io, boolean score_right, int Level) { addCommands( // TODO: Check if we have coral & if we don't have diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index cbc1c46..68230e0 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -15,7 +15,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import dev.doglog.DogLog; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a2d2858..860b1b7 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -63,10 +63,10 @@ public Elevator() { config.Slot0.kG = 0.0; config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.SoftwareLimitSwitch .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(300.0) + .withForwardSoftLimitThreshold(130.0) .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(0.0); diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 517ea84..38b442e 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -5,7 +5,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.spark.config.SoftLimitConfig; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d778b73..d6fd2e6 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -10,7 +10,6 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import dev.doglog.DogLog; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 83a4534..736db33 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -120,7 +120,7 @@ private void configureDebug(){ controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1)); + controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); controller.povUp().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); controller.povDown().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); From eaeeafcf2c7fb6283e3aed039ca94aceb4493925 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Wed, 5 Mar 2025 15:41:37 -0500 Subject: [PATCH 66/79] Small Cleanup --- src/main/java/frc/robot/commands/Intake.java | 47 ++++++------------- src/main/java/frc/robot/subsystems/Claw.java | 13 +++-- .../java/frc/robot/subsystems/Elevator.java | 14 +++--- .../robot/utility/AutomatedController.java | 23 ++++----- 4 files changed, 42 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index 7fc11b0..cb89cfa 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Claw; import frc.robot.utility.IO; /* 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 */ @@ -16,6 +17,7 @@ public class Intake extends Command { Runnable intake; Runnable stop; + Runnable rumble; BooleanSupplier holding; double angle; @@ -25,45 +27,25 @@ public class Intake extends Command { // public static int SCORE_ALGAE = 2; // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP - public Intake(IO io, boolean coral, boolean release) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio - // TODO: Review and see if we can optimise it - if (coral){ - intake = () -> { - io.claw.speed( (release) ? .4 : -.4); - }; - holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); - } else { // Algae - intake = () -> { - io.claw.speed(1); - // TODO: Set Claw to Reef intake Angle or Ground pickup if we - }; - holding = () -> (release) ? io.claw.hasAlgae() : !io.claw.hasAlgae(); - } - stop = io.claw::stop; - } + public Intake(IO io, boolean coral, boolean release, GenericHID controller) { + rumble = (controller != null) ? () -> controller.setRumble(RumbleType.kBothRumble, .25) : () -> {}; // TODO: Check if it's fine + holding = () -> (coral) ? ((release) ? io.claw.hasCoral() : !io.claw.hasCoral()) : ((release) ? io.claw.hasAlgae() : !io.claw.hasAlgae()); + + intake = () -> { + io.claw.speed((release) ? 1 : .4); + io.claw.angle((release) ? Claw.REEF_ANGLE : Claw.INTAKE_ANGLE); // TODO: See if we need to add an angle for scoring on L4 & Barge + }; - public Intake(IO io, boolean coral, boolean release, GenericHID controller) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio - // TODO: Review and see if we can optimise it - if (coral){ - intake = () -> { - io.claw.speed( (release) ? .4 : -.4); - controller.setRumble(RumbleType.kBothRumble, .25); - }; - holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); - } else { // Algae - intake = () -> { - io.claw.speed(1); - controller.setRumble(RumbleType.kBothRumble, .25); - // TODO: Set Claw to Reef intake Angle or Ground pickup if we - }; - holding = () -> (release) ? io.claw.hasAlgae() : !io.claw.hasAlgae(); - } stop = () -> { io.claw.stop(); controller.setRumble(RumbleType.kBothRumble, 0.0); }; } + public Intake(IO io, boolean coral, boolean release){ + this(io, coral, release, null); + } + public Intake(IO io, boolean coral, boolean release, int level, GenericHID controller) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio this(io, coral, release,controller); io.elevator.move(level); @@ -78,6 +60,7 @@ public Intake(IO io, boolean coral, boolean release, int level) { // negative is @Override public void initialize() { intake.run(); + rumble.run(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 68230e0..dd27e28 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -33,6 +33,9 @@ public class Claw extends SubsystemBase { DigitalInput algaeBreak = new DigitalInput(0); DigitalInput coralBreak = new DigitalInput(1); + + public final static double INTAKE_ANGLE = 0; + public final static double REEF_ANGLE = 0; TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); @@ -82,16 +85,16 @@ public double angle() { return pivot.getAbsoluteEncoder().getPosition(); } - public void pivotVolts(double volts) { - pivot.setVoltage(volts); - } - - public void setAngle(double target_angle) { + public void angle(double target_angle) { target = target_angle; stopped = false; time.restart(); } + public void pivotVolts(double volts) { + pivot.setVoltage(volts); + } + public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config( null, Volts.of(4), diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 860b1b7..06ea3a3 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -50,14 +50,14 @@ public class Elevator extends SubsystemBase { public final double L2 = 43.5; public final double L3 = 76; public final double L4 = 0; - public final double Barge = 0; - public final double Low_Algae = 0; - public final double High_Algae = 0; - public final double MAX_HEIGHT = 0; + public final double Barge = 0; // TODO: FIND BARGE + public final double Low_Algae = 0; // TODO: FIND + public final double High_Algae = 0; // TODO: FIND + public final double MAX_HEIGHT = 0; // TODO: FIND public Elevator() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.0; + config.Slot0.kP = 0.01; config.Slot0.kI = 0.0; config.Slot0.kD = 0.0; config.Slot0.kG = 0.0; @@ -65,9 +65,9 @@ public Elevator() { config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.SoftwareLimitSwitch - .withForwardSoftLimitEnable(true) + .withForwardSoftLimitEnable(false) .withForwardSoftLimitThreshold(130.0) - .withReverseSoftLimitEnable(true) + .withReverseSoftLimitEnable(false) .withReverseSoftLimitThreshold(0.0); lead.getConfigurator().apply(config); diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 736db33..822522f 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -83,16 +83,6 @@ public void configure(){ controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); - // [DEBUG] FOR TESTING for rn - controller.y().and(automated()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); - controller.b().and(automated()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); - controller.x().and(automated()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); - controller.a().and(automated()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); - controller.povUp().and(automated()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); - - controller.povLeft().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); - controller.povDown().and(automated()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); - // MANUAL // RB align Right and Score Coral & Score Processor // controller.y().and( automated() ).onTrue(Util.D @@ -120,13 +110,24 @@ private void configureDebug(){ controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); + controller.x().and(debug_setting()).onTrue(Util.Do(io.elevator::zero, io.elevator)); + // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); controller.povUp().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); controller.povDown().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); controller.povRight().and(debug()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); controller.povLeft().and(debug()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + controller.y().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); + controller.b().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); + controller.x().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); + controller.a().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); + controller.povUp().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); + + controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); + controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); + + } } From 6ec91895eba328a45ad3fce29ee0906130415b25 Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 6 Mar 2025 15:32:17 -0500 Subject: [PATCH 67/79] bindings --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/commands/ScoreAlgae.java | 4 +- .../java/frc/robot/commands/ScoreReef.java | 5 +- src/main/java/frc/robot/subsystems/Claw.java | 19 ++++- .../java/frc/robot/subsystems/Elevator.java | 77 ++++++++++--------- .../robot/utility/AutomatedController.java | 32 +++++++- 6 files changed, 94 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73ebabc..f0312bf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,8 +87,9 @@ public void configureAuton(){ NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); NamedCommands.registerCommand("Score Barge", new ScoreAlgae(io, true)); - NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, false, 6)); - NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, false, 7)); + // TODO: SEE IF WE CAN GET AWAY WITH THESE LEVELS + NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, false, 2)); + NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, false, 3)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/ScoreAlgae.java b/src/main/java/frc/robot/commands/ScoreAlgae.java index 5c29f8c..5749567 100644 --- a/src/main/java/frc/robot/commands/ScoreAlgae.java +++ b/src/main/java/frc/robot/commands/ScoreAlgae.java @@ -20,7 +20,7 @@ public ScoreAlgae(IO io, boolean barge, GenericHID controller) { addCommands( //TODO: Check if we have Algae io.elevator.moveCommand((barge) ? 5 : 0), - new AutoAlign(1, io), + // new AutoAlign(1, io), new WaitUntilCommand(io.elevator::atPosition), new Intake(io, false, true, controller), io.elevator.moveCommand(0) @@ -31,7 +31,7 @@ public ScoreAlgae(IO io, boolean barge) { addCommands( //TODO: Check if we have Algae io.elevator.moveCommand((barge) ? 5 : 0), - new AutoAlign(1, io), + // new AutoAlign(1, io), new WaitUntilCommand(io.elevator::atPosition), new Intake(io, false, true), io.elevator.moveCommand(0) diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index c978c09..3caef7f 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -8,14 +8,14 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; +import frc.robot.utility.Util; public class ScoreReef extends SequentialCommandGroup { - public ScoreReef(IO io, boolean score_right, int Level, GenericHID controller) { + public ScoreReef(IO io, int Level, GenericHID controller) { addCommands( // TODO: Check if we have coral & if we don't have io.elevator.moveCommand(Level), - new AutoAlign((score_right) ? 2 : 1, io), new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height new Intake(io, true, true, controller), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) @@ -28,6 +28,7 @@ public ScoreReef(IO io, boolean score_right, int Level) { io.elevator.moveCommand(Level), new AutoAlign((score_right) ? 2 : 1, io), new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height + Util.Do(() -> io.claw.angle(Level), io.claw), new Intake(io, true, true), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index dd27e28..2f22fd7 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -34,14 +34,17 @@ public class Claw extends SubsystemBase { DigitalInput algaeBreak = new DigitalInput(0); DigitalInput coralBreak = new DigitalInput(1); - public final static double INTAKE_ANGLE = 0; - public final static double REEF_ANGLE = 0; + public final static double INTAKE_ANGLE = 0; // TODO: FIND INTAKE ANGLE + public final static double BARGE_ANGLE = 0; // TODO: FIND BARGE ANGLE + public final static double REEF_ANGLE = 0; // TODO: FIND REEF ANGLE TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); - double target = 0; + double target = 0.0; boolean stopped = true; + public boolean algaeIntaking = false; + public Claw() { SparkMaxConfig config = new SparkMaxConfig(); @@ -91,6 +94,16 @@ public void angle(double target_angle) { time.restart(); } + public void angle(int level){ + if (level == 0){ // Rest + angle(INTAKE_ANGLE); + } else if (level < 3 || level > 5) { + angle(REEF_ANGLE); + } else { + angle(BARGE_ANGLE); + } + } + public void pivotVolts(double volts) { pivot.setVoltage(volts); } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 06ea3a3..8ac2248 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -42,10 +42,11 @@ public class Elevator extends SubsystemBase { PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); - final String[] level_layout = {"Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae"}; + final String[] level_layout = { "Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae" }; // Target Heights public final double Rest = 0; + public final double L0 = 0; public final double L1 = 25; public final double L2 = 43.5; public final double L3 = 76; @@ -65,10 +66,10 @@ public Elevator() { config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.SoftwareLimitSwitch - .withForwardSoftLimitEnable(false) - .withForwardSoftLimitThreshold(130.0) - .withReverseSoftLimitEnable(false) - .withReverseSoftLimitThreshold(0.0); + .withForwardSoftLimitEnable(false) + .withForwardSoftLimitThreshold(130.0) + .withReverseSoftLimitEnable(false) + .withReverseSoftLimitThreshold(0.0); lead.getConfigurator().apply(config); follow.setControl(new Follower(lead.getDeviceID(), true)); // TODO: Check if we need to invert @@ -95,36 +96,39 @@ public void move(double height) { } public void move(int level) { - switch (level) { - case 1: - move(L1); - break; - case 2: - move(L2); - break; - case 3: - move(L3); - break; - case 4: - move(L4); - break; - case 5: - move(Barge); - break; - case 6: - move(Low_Algae); - break; - case 7: - move(High_Algae); - break; - default: - move(Rest); // LEVEL 1 // TODO: See if we need to change the height we go to - break; - } - SmartDashboard.putString("Target Level", level_layout[level]); + switch (level) { + case 0: + move(L0); + break; + case 1: + move(L1); + break; + case 2: + move(L2); + break; + case 3: + move(L3); + break; + case 4: + move(L4); + break; + case 5: + move(Barge); + break; + case 6: + move(Low_Algae); + break; + case 7: + move(High_Algae); + break; + default: + move(Rest); // LEVEL 1 // TODO: See if we need to change the height we go to + break; + } + SmartDashboard.putString("Target Level", level_layout[level]); } - public InstantCommand moveCommand(int level){ + public InstantCommand moveCommand(int level) { return new InstantCommand(() -> this.move(level), this); } @@ -146,12 +150,13 @@ public double position() { } public LinearVelocity velocity() { - // return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * conversion); + // return MetersPerSecond.of(lead.getVelocity().getValueAsDouble() * + // conversion); return MetersPerSecond.of(lead.getVelocity().getValueAsDouble()); } - final double gearReduction = 1/17; - final double conversion = Math.PI * gearReduction *(Units.inchesToMeters(2) / 60); + final double gearReduction = 1 / 17; + final double conversion = Math.PI * gearReduction * (Units.inchesToMeters(2) / 60); public final SysIdRoutine routine = new SysIdRoutine(new Config( null, diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 822522f..6295435 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -2,18 +2,24 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; +import frc.robot.commands.Intake; import frc.robot.commands.LimelightAlign; +import frc.robot.commands.ScoreAlgae; +import frc.robot.commands.ScoreReef; public class AutomatedController { public final CommandXboxController controller; public final SendableChooser selector = new SendableChooser(); public int mode = 3; + public GenericHID rumble = new GenericHID(0); IO io; @@ -54,6 +60,7 @@ public BooleanSupplier manual(){ public BooleanSupplier debug(){ return mode(2); } + public BooleanSupplier debug_setting(){ return mode(3); } @@ -93,7 +100,28 @@ public void configure(){ // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect } - private void configureDebug(){ + void configureAutomated(){ + controller.leftBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 1, false))); + controller.rightBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 2, false))); + + controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(Util.Do(() -> new Intake(io, true, false, rumble))); + + controller.a().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 1, rumble))); + controller.b().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 3, rumble))); + controller.x().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 2, rumble))); + controller.y().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 4, rumble))); + + controller.x().and(automated()).and(() -> io.claw.hasAlgae()).onTrue(io.elevator.moveCommand(7)); + controller.a().and(automated()).and(() -> io.claw.hasAlgae()).onTrue(io.elevator.moveCommand(6)); + + controller.povLeft().and(automated()).onTrue(Util.Do(() -> new Intake(io, false, true, rumble))); + controller.povUp().and(automated()).onTrue(new ConditionalCommand(new ScoreAlgae(io, true, rumble), new Intake(io, false, false, rumble), io.claw::hasAlgae)); + + controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0))); + controller.back().and(automated()).onTrue(Util.Do(() -> io.chassis.resetOdometry())); + } + + void configureDebug(){ controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); @@ -126,8 +154,6 @@ private void configureDebug(){ controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); - - } } From fef3b8245fec51592cdb44cfbc6f63b26eb02aff Mon Sep 17 00:00:00 2001 From: TheIceCreamMan10000000 Date: Thu, 6 Mar 2025 16:52:19 -0500 Subject: [PATCH 68/79] sparkflex for pivot --- src/main/java/frc/robot/subsystems/Claw.java | 28 +++++++++++-------- .../robot/utility/AutomatedController.java | 2 +- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 2f22fd7..93be333 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -11,7 +11,9 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; @@ -29,11 +31,11 @@ public class Claw extends SubsystemBase { SparkMax intake = new SparkMax(13, MotorType.kBrushless); - SparkMax pivot = new SparkMax(14, MotorType.kBrushless); + SparkFlex pivot = new SparkFlex(14, MotorType.kBrushless); DigitalInput algaeBreak = new DigitalInput(0); DigitalInput coralBreak = new DigitalInput(1); - + public final static double INTAKE_ANGLE = 0; // TODO: FIND INTAKE ANGLE public final static double BARGE_ANGLE = 0; // TODO: FIND BARGE ANGLE public final static double REEF_ANGLE = 0; // TODO: FIND REEF ANGLE @@ -51,13 +53,17 @@ public Claw() { config.idleMode(SparkMaxConfig.IdleMode.kBrake); intake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - config.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); - config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - // config.softLimit.forwardSoftLimitEnabled(true); - // config.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit - // config.softLimit.reverseSoftLimitEnabled(true); - // config.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit - pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + SparkFlexConfig pivotConfig = new SparkFlexConfig(); + pivotConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); + pivotConfig.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); + pivotConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + + // pivotConfig.softLimit.forwardSoftLimitEnabled(true); + // pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit + // pivotConfig.softLimit.reverseSoftLimitEnabled(true); + // pivotConfig.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit + + pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void volts(double volts) { @@ -94,8 +100,8 @@ public void angle(double target_angle) { time.restart(); } - public void angle(int level){ - if (level == 0){ // Rest + public void angle(int level) { + if (level == 0) { // Rest angle(INTAKE_ANGLE); } else if (level < 3 || level > 5) { angle(REEF_ANGLE); diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 6295435..e8df264 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -125,7 +125,7 @@ void configureDebug(){ controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - + controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); From f6b05049e2820d8e9555d9e61e80f33209b6753b Mon Sep 17 00:00:00 2001 From: Evan Date: Tue, 11 Mar 2025 20:44:37 -0400 Subject: [PATCH 69/79] inverted drive motor --- src/main/java/frc/robot/swerve/Module.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index f3ddf8a..15ac8d4 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -67,7 +67,7 @@ public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, b config.CurrentLimits.SupplyCurrentLimit = 20; config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; steer.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); From deac49c7edab7a8fb5bdb9405c2f306d1b6bd74c Mon Sep 17 00:00:00 2001 From: Ayidana Date: Thu, 13 Mar 2025 19:41:57 -0400 Subject: [PATCH 70/79] Made path Adjustments, fixed running paths backwards during auto, starting to tune rotation PID --- .../deploy/pathplanner/autos/90 Auto.auto | 2 +- .../Bottom 4 Coral Cleanout + Algae.auto | 2 +- .../deploy/pathplanner/autos/Hot Reload.auto | 4 +- .../autos/Slide and Watch Deluxe.auto | 2 +- .../pathplanner/autos/Straight Test.auto | 2 +- .../Top 4 Coral Cleanout + Algae Deluxe.auto | 2 +- .../autos/Top 4 Coral Cleanout + Algae.auto | 2 +- .../autos/Top 4 Coral Cleanout Rotations.auto | 2 +- .../autos/Top 6 Coral Cleanout.auto | 91 +++++++++++++++++++ .../deploy/pathplanner/autos/Top Ring.auto | 73 +++++++++++++++ .../deploy/pathplanner/autos/Triangle.auto | 19 ++++ .../deploy/pathplanner/paths/AB to Barge.path | 8 +- .../deploy/pathplanner/paths/Barge to AB.path | 8 +- .../deploy/pathplanner/paths/Barge to HG.path | 8 +- .../deploy/pathplanner/paths/Barge to IJ.path | 8 +- .../paths/Barge to Station to CD.path | 14 +-- .../paths/Barge to Station to LK.path | 36 ++++---- .../pathplanner/paths/Bottom to CD.path | 28 +++--- .../deploy/pathplanner/paths/CD to Barge.path | 16 ++-- .../deploy/pathplanner/paths/Cycle AB.path | 89 ++++++++++++++++++ .../deploy/pathplanner/paths/Cycle CD.path | 36 ++++---- .../deploy/pathplanner/paths/Cycle JI.path | 55 ++++++----- .../deploy/pathplanner/paths/Cycle LK.path | 46 +++++----- .../pathplanner/paths/Forward and Back.path | 8 +- .../deploy/pathplanner/paths/HG to Barge.path | 8 +- .../deploy/pathplanner/paths/IJ to Barge.path | 8 +- .../deploy/pathplanner/paths/IJ to LK.path | 84 +++++++++++++++++ .../deploy/pathplanner/paths/LK to AB.path | 84 +++++++++++++++++ .../deploy/pathplanner/paths/LK to Barge.path | 24 ++--- .../deploy/pathplanner/paths/LK to JI.path | 32 +++---- .../pathplanner/paths/Middle to HG.path | 22 ++--- .../deploy/pathplanner/paths/Return Home.path | 8 +- .../paths/Straight And Rotate.path | 8 +- .../paths/Straight Line Rotate 90.path | 8 +- .../deploy/pathplanner/paths/Top to IJ.path | 54 +++++++++++ .../deploy/pathplanner/paths/Top to LK.path | 30 +++--- .../deploy/pathplanner/paths/Triangle.path | 8 +- src/main/deploy/pathplanner/settings.json | 24 +++-- src/main/java/frc/robot/RobotContainer.java | 10 +- .../java/frc/robot/commands/DefaultDrive.java | 4 +- src/main/java/frc/robot/commands/Intake.java | 17 +--- .../java/frc/robot/commands/ScoreAlgae.java | 40 -------- .../java/frc/robot/commands/ScoreReef.java | 17 ++-- src/main/java/frc/robot/subsystems/Claw.java | 36 ++------ src/main/java/frc/robot/subsystems/Hang.java | 2 - .../java/frc/robot/subsystems/Limelight.java | 2 - .../java/frc/robot/subsystems/Swerve.java | 12 +-- src/main/java/frc/robot/swerve/Swerve.java | 6 +- .../robot/utility/AutomatedController.java | 37 +++----- 49 files changed, 782 insertions(+), 364 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto create mode 100644 src/main/deploy/pathplanner/autos/Top Ring.auto create mode 100644 src/main/deploy/pathplanner/autos/Triangle.auto create mode 100644 src/main/deploy/pathplanner/paths/Cycle AB.path create mode 100644 src/main/deploy/pathplanner/paths/IJ to LK.path create mode 100644 src/main/deploy/pathplanner/paths/LK to AB.path create mode 100644 src/main/deploy/pathplanner/paths/Top to IJ.path delete mode 100644 src/main/java/frc/robot/commands/ScoreAlgae.java diff --git a/src/main/deploy/pathplanner/autos/90 Auto.auto b/src/main/deploy/pathplanner/autos/90 Auto.auto index e70990e..28dcc7f 100644 --- a/src/main/deploy/pathplanner/autos/90 Auto.auto +++ b/src/main/deploy/pathplanner/autos/90 Auto.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Testing", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto index 9d5f618..151b9b3 100644 --- a/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto +++ b/src/main/deploy/pathplanner/autos/Bottom 4 Coral Cleanout + Algae.auto @@ -74,6 +74,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Intake", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hot Reload.auto b/src/main/deploy/pathplanner/autos/Hot Reload.auto index 6968d89..b8296ca 100644 --- a/src/main/deploy/pathplanner/autos/Hot Reload.auto +++ b/src/main/deploy/pathplanner/autos/Hot Reload.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "XXX" + "pathName": null } } ] } }, "resetOdom": true, - "folder": null, + "folder": "Testing", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto index cf22abd..3fbe6c3 100644 --- a/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto +++ b/src/main/deploy/pathplanner/autos/Slide and Watch Deluxe.auto @@ -50,6 +50,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Intake", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Straight Test.auto b/src/main/deploy/pathplanner/autos/Straight Test.auto index 70b6480..2c0acd5 100644 --- a/src/main/deploy/pathplanner/autos/Straight Test.auto +++ b/src/main/deploy/pathplanner/autos/Straight Test.auto @@ -14,6 +14,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Testing", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto index 58d65a5..9cdcd5a 100644 --- a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae Deluxe.auto @@ -110,6 +110,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Intake", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto index 53facf5..cc63167 100644 --- a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout + Algae.auto @@ -86,6 +86,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Intake", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto index 4a2de67..34bc30e 100644 --- a/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto +++ b/src/main/deploy/pathplanner/autos/Top 4 Coral Cleanout Rotations.auto @@ -68,6 +68,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Algae Intake", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto new file mode 100644 index 0000000..3030b34 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L3" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L3" + } + }, + { + "type": "named", + "data": { + "name": "Clear Low Alage" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L2" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "score R-L2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top Ring.auto b/src/main/deploy/pathplanner/autos/Top Ring.auto new file mode 100644 index 0000000..5c236be --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Top Ring.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top to IJ" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle JI" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "IJ to LK" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "Cycle LK" + } + }, + { + "type": "named", + "data": { + "name": "Score R-L4" + } + }, + { + "type": "path", + "data": { + "pathName": "LK to AB" + } + }, + { + "type": "named", + "data": { + "name": "Score L-L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Triangle.auto b/src/main/deploy/pathplanner/autos/Triangle.auto new file mode 100644 index 0000000..57cb1e9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Triangle.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Triangle" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Testing", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AB to Barge.path b/src/main/deploy/pathplanner/paths/AB to Barge.path index 6f2ec09..0a9c107 100644 --- a/src/main/deploy/pathplanner/paths/AB to Barge.path +++ b/src/main/deploy/pathplanner/paths/AB to Barge.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Barge to AB.path b/src/main/deploy/pathplanner/paths/Barge to AB.path index 078b1ca..68fe398 100644 --- a/src/main/deploy/pathplanner/paths/Barge to AB.path +++ b/src/main/deploy/pathplanner/paths/Barge to AB.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Barge to HG.path b/src/main/deploy/pathplanner/paths/Barge to HG.path index b607895..07b75eb 100644 --- a/src/main/deploy/pathplanner/paths/Barge to HG.path +++ b/src/main/deploy/pathplanner/paths/Barge to HG.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Barge to IJ.path b/src/main/deploy/pathplanner/paths/Barge to IJ.path index f6bbbb2..ffcc170 100644 --- a/src/main/deploy/pathplanner/paths/Barge to IJ.path +++ b/src/main/deploy/pathplanner/paths/Barge to IJ.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path index 7a5e94e..9c63ca4 100644 --- a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path +++ b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path @@ -12,7 +12,7 @@ "y": 1.8537885273972599 }, "isLocked": false, - "linkedName": null + "linkedName": "CD" }, { "anchor": { @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path index 2b402f3..c6ebb3c 100644 --- a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path +++ b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path @@ -16,32 +16,32 @@ }, { "anchor": { - "x": 1.712928082191781, - "y": 7.3081121575342465 + "x": 1.7649715909090908, + "y": 7.220894886363636 }, "prevControl": { - "x": 1.9235838724413903, - "y": 7.173486238654141 + "x": 1.9756273811587002, + "y": 7.086268967483531 }, "nextControl": { - "x": 1.5022722919421716, - "y": 7.442738076414352 + "x": 1.5543158006594815, + "y": 7.355520805243741 }, "isLocked": false, - "linkedName": null + "linkedName": "Top Coral Station" }, { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": { - "x": 2.8920089922838375, - "y": 6.273030788421103 + "x": 2.761554446829292, + "y": 6.179664311148375 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "LK" } ], "rotationTargets": [], @@ -49,22 +49,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom to CD.path b/src/main/deploy/pathplanner/paths/Bottom to CD.path index f840403..05f1fbb 100644 --- a/src/main/deploy/pathplanner/paths/Bottom to CD.path +++ b/src/main/deploy/pathplanner/paths/Bottom to CD.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 7.693150684931506, - "y": 2.2144049657534244 + "x": 7.082471026490066, + "y": 2.14360513245033 }, "prevControl": null, "nextControl": { - "x": 4.282320205479451, - "y": 2.3195847602739734 + "x": 4.2422185430463575, + "y": 2.252566225165561 }, "isLocked": false, - "linkedName": null + "linkedName": "Bottom Starting Line" }, { "anchor": { @@ -20,12 +20,12 @@ "y": 2.75 }, "prevControl": { - "x": 4.201027397260273, - "y": 2.3593321917808225 + "x": 3.8790149006622516, + "y": 2.485016556291391 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "CD" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CD to Barge.path b/src/main/deploy/pathplanner/paths/CD to Barge.path index 7a5e94e..3043ec9 100644 --- a/src/main/deploy/pathplanner/paths/CD to Barge.path +++ b/src/main/deploy/pathplanner/paths/CD to Barge.path @@ -12,7 +12,7 @@ "y": 1.8537885273972599 }, "isLocked": false, - "linkedName": null + "linkedName": "CD" }, { "anchor": { @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 59.99999999999999 + "velocity": 0.0, + "rotation": -119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle AB.path b/src/main/deploy/pathplanner/paths/Cycle AB.path new file mode 100644 index 0000000..17e7c0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Cycle AB.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9615625, + "y": 3.9801278409090908 + }, + "prevControl": null, + "nextControl": { + "x": 2.878389547719681, + "y": 4.21588674319907 + }, + "isLocked": false, + "linkedName": "AB" + }, + { + "anchor": { + "x": 1.7649715909090908, + "y": 7.220894886363636 + }, + "prevControl": { + "x": 1.8537693216543827, + "y": 6.987196441750741 + }, + "nextControl": { + "x": 1.608924084897191, + "y": 7.6315817263833186 + }, + "isLocked": false, + "linkedName": "Top Coral Station" + }, + { + "anchor": { + "x": 2.9615625, + "y": 3.9801278409090908 + }, + "prevControl": { + "x": 2.8440210650126843, + "y": 4.398962959529563 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AB" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9, + "maxWaypointRelativePos": 1.1, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle CD.path b/src/main/deploy/pathplanner/paths/Cycle CD.path index c5182a7..8db5514 100644 --- a/src/main/deploy/pathplanner/paths/Cycle CD.path +++ b/src/main/deploy/pathplanner/paths/Cycle CD.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 2.8076413111349265, - "y": 2.2095878457116576 + "x": 3.2075563954930058, + "y": 2.324270648118099 }, "isLocked": false, - "linkedName": null + "linkedName": "CD" }, { "anchor": { - "x": 1.2771832191780823, - "y": 0.9822988013698626 + "x": 1.6562086092715231, + "y": 0.7852235099337759 }, "prevControl": { - "x": 1.469285804381291, - "y": 1.1422881658858952 + "x": 1.8315296882327805, + "y": 0.9634439332074864 }, "nextControl": { - "x": 1.0609783711466783, - "y": 0.8022362779777934 + "x": 1.4808875303102658, + "y": 0.6070030866600653 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 2.75 }, "prevControl": { - "x": 3.0821909027253858, - "y": 2.4810005631499648 + "x": 3.385559124014756, + "y": 2.5320786312706076 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "CD" } ], "rotationTargets": [], @@ -49,22 +49,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle JI.path b/src/main/deploy/pathplanner/paths/Cycle JI.path index b9bb27b..1a80066 100644 --- a/src/main/deploy/pathplanner/paths/Cycle JI.path +++ b/src/main/deploy/pathplanner/paths/Cycle JI.path @@ -3,53 +3,58 @@ "waypoints": [ { "anchor": { - "x": 5.176, - "y": 5.3 + "x": 5.344772727272727, + "y": 5.206633522727272 }, "prevControl": null, "nextControl": { - "x": 4.282069594780682, - "y": 5.981127203067267 + "x": 4.7642249354128365, + "y": 5.548440560420403 }, "isLocked": false, - "linkedName": null + "linkedName": "Starting IJ" }, { "anchor": { - "x": 1.6828767123287671, - "y": 7.353189212328767 + "x": 1.7649715909090908, + "y": 7.220894886363636 }, "prevControl": { - "x": 1.5172681656002416, - "y": 7.54046924129733 + "x": 1.5412515749121463, + "y": 7.342700736229868 }, "nextControl": { - "x": 1.9544850896817896, - "y": 7.046038248301759 + "x": 1.9845375267660235, + "y": 7.101350753043471 }, "isLocked": false, - "linkedName": null + "linkedName": "Top Coral Station" }, { "anchor": { - "x": 5.176, - "y": 5.3 + "x": 5.344772727272727, + "y": 5.206633522727272 }, "prevControl": { - "x": 4.741004409549384, - "y": 5.774787496649876 + "x": 4.8663102932710895, + "y": 5.487137469543703 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Starting IJ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 119.99999999999999 } ], - "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", "minWaypointRelativePos": 0.9612403100775195, - "maxWaypointRelativePos": 1.1369509043927652, + "maxWaypointRelativePos": 1.05, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.5, @@ -63,22 +68,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle LK.path b/src/main/deploy/pathplanner/paths/Cycle LK.path index 3ed2f94..57ba928 100644 --- a/src/main/deploy/pathplanner/paths/Cycle LK.path +++ b/src/main/deploy/pathplanner/paths/Cycle LK.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": null, "nextControl": { - "x": 2.9060695947806816, - "y": 5.981127203067267 + "x": 3.357804178610237, + "y": 5.486653629540659 }, "isLocked": false, - "linkedName": null + "linkedName": "LK" }, { "anchor": { - "x": 1.6828767123287671, - "y": 7.353189212328767 + "x": 1.7649715909090908, + "y": 7.220894886363636 }, "prevControl": { - "x": 1.5172681656002416, - "y": 7.54046924129733 + "x": 1.5966241435231892, + "y": 7.40571679242093 }, "nextControl": { - "x": 1.9544850896817896, - "y": 7.046038248301759 + "x": 1.9333190382949925, + "y": 7.036072980306343 }, "isLocked": false, - "linkedName": null + "linkedName": "Top Coral Station" }, { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": { - "x": 3.365004409549383, - "y": 5.774787496649876 + "x": 3.500993559994922, + "y": 5.391268998309367 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "LK" } ], "rotationTargets": [], @@ -63,22 +63,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path index b427204..8db3403 100644 --- a/src/main/deploy/pathplanner/paths/Forward and Back.path +++ b/src/main/deploy/pathplanner/paths/Forward and Back.path @@ -49,10 +49,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HG to Barge.path b/src/main/deploy/pathplanner/paths/HG to Barge.path index 64f8cba..6810a81 100644 --- a/src/main/deploy/pathplanner/paths/HG to Barge.path +++ b/src/main/deploy/pathplanner/paths/HG to Barge.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/IJ to Barge.path b/src/main/deploy/pathplanner/paths/IJ to Barge.path index 74d6340..8536217 100644 --- a/src/main/deploy/pathplanner/paths/IJ to Barge.path +++ b/src/main/deploy/pathplanner/paths/IJ to Barge.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/IJ to LK.path b/src/main/deploy/pathplanner/paths/IJ to LK.path new file mode 100644 index 0000000..8e07a5f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/IJ to LK.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.344772727272727, + "y": 5.206633522727272 + }, + "prevControl": null, + "nextControl": { + "x": 4.599760694017934, + "y": 5.624873196871553 + }, + "isLocked": false, + "linkedName": "Starting IJ" + }, + { + "anchor": { + "x": 1.755, + "y": 7.171036931818182 + }, + "prevControl": { + "x": 1.954912240657937, + "y": 6.979269733268736 + }, + "nextControl": { + "x": 1.564159066139434, + "y": 7.354102416625691 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6695454545454544, + "y": 5.206633522727272 + }, + "prevControl": { + "x": 3.490908924519007, + "y": 5.381530607151542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LK" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9344909234412, + "maxWaypointRelativePos": 1.3480662983425347, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to AB.path b/src/main/deploy/pathplanner/paths/LK to AB.path new file mode 100644 index 0000000..7df363c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LK to AB.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6695454545454544, + "y": 5.206633522727272 + }, + "prevControl": null, + "nextControl": { + "x": 3.442902780309121, + "y": 5.472347910370522 + }, + "isLocked": false, + "linkedName": "LK" + }, + { + "anchor": { + "x": 1.7649715909090908, + "y": 7.220894886363636 + }, + "prevControl": { + "x": 2.030789042192635, + "y": 6.818654369259048 + }, + "nextControl": { + "x": 1.6271387237970898, + "y": 7.429466458596492 + }, + "isLocked": false, + "linkedName": "Top Coral Station" + }, + { + "anchor": { + "x": 2.9615625, + "y": 3.9801278409090908 + }, + "prevControl": { + "x": 2.5867889091633094, + "y": 4.994866966222094 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.95, + "maxWaypointRelativePos": 1.1, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to Barge.path b/src/main/deploy/pathplanner/paths/LK to Barge.path index 9fbef1d..72c14cb 100644 --- a/src/main/deploy/pathplanner/paths/LK to Barge.path +++ b/src/main/deploy/pathplanner/paths/LK to Barge.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": null, "nextControl": { - "x": 4.224264068711928, - "y": 5.724264068711928 + "x": 4.093809523257383, + "y": 5.630897591439201 }, "isLocked": false, - "linkedName": null + "linkedName": "LK" }, { "anchor": { @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": -59.99999999999999 + "velocity": 0.0, + "rotation": 119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to JI.path b/src/main/deploy/pathplanner/paths/LK to JI.path index 9471adb..2aad4cb 100644 --- a/src/main/deploy/pathplanner/paths/LK to JI.path +++ b/src/main/deploy/pathplanner/paths/LK to JI.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": null, "nextControl": { - "x": 2.9060695947806816, - "y": 5.981127203067267 + "x": 2.7756150493261362, + "y": 5.887760725794539 }, "isLocked": false, - "linkedName": null + "linkedName": "LK" }, { "anchor": { @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 5.176, - "y": 5.3 + "x": 5.344772727272727, + "y": 5.206633522727272 }, "prevControl": { - "x": 4.741004409549383, - "y": 5.774787496649876 + "x": 4.90977713682211, + "y": 5.681421019377148 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Starting IJ" } ], "rotationTargets": [], @@ -63,22 +63,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle to HG.path b/src/main/deploy/pathplanner/paths/Middle to HG.path index 424993e..9434a33 100644 --- a/src/main/deploy/pathplanner/paths/Middle to HG.path +++ b/src/main/deploy/pathplanner/paths/Middle to HG.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 7.978638698630138, + "x": 7.249544701986755, "y": 4.0 }, "prevControl": null, "nextControl": { - "x": 7.2403670394959025, + "x": 6.51127304285252, "y": 3.9896688626101993 }, "isLocked": false, - "linkedName": null + "linkedName": "Centre Starting Line" }, { "anchor": { - "x": 5.9651969178082185, + "x": 5.803994205298014, "y": 4.0 }, "prevControl": { - "x": 6.570306276096605, + "x": 6.4091035635864, "y": 3.9986716795875528 }, "nextControl": null, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Return Home.path b/src/main/deploy/pathplanner/paths/Return Home.path index bff9875..ea6cd70 100644 --- a/src/main/deploy/pathplanner/paths/Return Home.path +++ b/src/main/deploy/pathplanner/paths/Return Home.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path index 8f8b2ca..e52ff8a 100644 --- a/src/main/deploy/pathplanner/paths/Straight And Rotate.path +++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path index fe55b85..aff49ff 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Top to IJ.path b/src/main/deploy/pathplanner/paths/Top to IJ.path new file mode 100644 index 0000000..b446949 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top to IJ.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.082471026490066, + "y": 5.206633522727272 + }, + "prevControl": null, + "nextControl": { + "x": 5.956557919698722, + "y": 5.192843493784709 + }, + "isLocked": false, + "linkedName": "Top Barge Starting Line" + }, + { + "anchor": { + "x": 5.344772727272727, + "y": 5.206633522727272 + }, + "prevControl": { + "x": 6.466808432660481, + "y": 5.220637271660809 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Starting IJ" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Starting", + "idealStartingState": { + "velocity": 0.0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top to LK.path b/src/main/deploy/pathplanner/paths/Top to LK.path index a8556d0..18a4562 100644 --- a/src/main/deploy/pathplanner/paths/Top to LK.path +++ b/src/main/deploy/pathplanner/paths/Top to LK.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.5, - "y": 5.7 + "x": 7.082471026490066, + "y": 5.206633522727272 }, "prevControl": null, "nextControl": { - "x": 4.1, - "y": 5.7 + "x": 4.5109892384105965, + "y": 6.051676324503311 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8, - "y": 5.3 + "x": 3.6695454545454544, + "y": 5.206633522727272 }, "prevControl": { - "x": 4.12139380484327, - "y": 5.683022221559489 + "x": 4.249482615894039, + "y": 5.6957367549668865 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "LK" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0.0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path index faf267e..a89fede 100644 --- a/src/main/deploy/pathplanner/paths/Triangle.path +++ b/src/main/deploy/pathplanner/paths/Triangle.path @@ -70,10 +70,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.6, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6644d20..34c3c10 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,21 +8,25 @@ "Starting", "Test" ], - "autoFolders": [], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 5.5, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "autoFolders": [ + "Algae Intake", + "Scuff", + "Testing" + ], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 4.6, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 360.0, "defaultNominalVoltage": 12.0, - "robotMass": 47.0, - "robotMOI": 2.12, + "robotMass": 52.0, + "robotMOI": 5.048, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.0508, + "driveWheelRadius": 0.05715, "driveGearing": 6.12, "maxDriveSpeed": 5.4, "driveMotorType": "krakenX60", - "driveCurrentLimit": 59.0, - "wheelCOF": 1.2, + "driveCurrentLimit": 60.0, + "wheelCOF": 1.19, "flModuleX": 0.27305, "flModuleY": 0.24765, "frModuleX": 0.27305, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0312bf..9435125 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,14 +4,10 @@ package frc.robot; -import java.util.function.Consumer; - import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.Pair; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -19,7 +15,6 @@ import frc.robot.commands.DefaultDrive; import frc.robot.commands.Intake; import frc.robot.commands.RotateChassis; -import frc.robot.commands.ScoreAlgae; import frc.robot.commands.ScoreReef; import frc.robot.swerve.Swerve; import frc.robot.utility.AutomatedController; @@ -85,11 +80,10 @@ public void configureAuton(){ NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, true, 3)); NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, true, 2)); NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); - NamedCommands.registerCommand("Score Barge", new ScoreAlgae(io, true)); // TODO: SEE IF WE CAN GET AWAY WITH THESE LEVELS - NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, false, 2)); - NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, false, 3)); + NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, 2)); // TODO: MAKE ACTUAL COMMAND + NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, 3)); // TODO: MAKE ACTUAL COMMAND } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index 0978b25..375847e 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -24,7 +24,7 @@ public DefaultDrive(IO io, ChassisSpeeds chassisSpeeds) { public DefaultDrive(IO io, CommandXboxController controller) { this(io, () -> modifyAxis(controller.getLeftY()) * Swerve.Constants.MAX_VELOCITY, () -> modifyAxis(controller.getLeftX()) * Swerve.Constants.MAX_VELOCITY, - () -> modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_VELOCITY); + () -> modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_ANGULAR_VELOCITY); this.controller = controller; } @@ -47,7 +47,7 @@ public void execute() { double up_scale = (Swerve.Constants.MAX_VELOCITY * .2) * modifyAxis(controller.getRightTriggerAxis()); double scale = Constants.transFactor * down_scale + up_scale; - double rot_scale = Constants.rotFactor * down_scale + up_scale; + double rot_scale = Constants.rotFactor * down_scale; double xSpeed = x_supplier.getAsDouble() * scale; double ySpeed = y_supplier.getAsDouble() * scale; diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index cb89cfa..046453b 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -27,9 +27,9 @@ public class Intake extends Command { // public static int SCORE_ALGAE = 2; // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP - public Intake(IO io, boolean coral, boolean release, GenericHID controller) { + public Intake(IO io, boolean release, int level, GenericHID controller) { rumble = (controller != null) ? () -> controller.setRumble(RumbleType.kBothRumble, .25) : () -> {}; // TODO: Check if it's fine - holding = () -> (coral) ? ((release) ? io.claw.hasCoral() : !io.claw.hasCoral()) : ((release) ? io.claw.hasAlgae() : !io.claw.hasAlgae()); + holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); intake = () -> { io.claw.speed((release) ? 1 : .4); @@ -40,22 +40,15 @@ public Intake(IO io, boolean coral, boolean release, GenericHID controller) { io.claw.stop(); controller.setRumble(RumbleType.kBothRumble, 0.0); }; - } - - public Intake(IO io, boolean coral, boolean release){ - this(io, coral, release, null); - } - public Intake(IO io, boolean coral, boolean release, int level, GenericHID controller) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio - this(io, coral, release,controller); io.elevator.move(level); } - public Intake(IO io, boolean coral, boolean release, int level) { // negative is release, 1 is coral, 2 is algae, 3 is algae in ground positio - this(io, coral, release); - io.elevator.move(level); + public Intake(IO io, boolean release, int level){ + this(io, release, level, null); } + // Called when the command is initially scheduled. @Override public void initialize() { diff --git a/src/main/java/frc/robot/commands/ScoreAlgae.java b/src/main/java/frc/robot/commands/ScoreAlgae.java deleted file mode 100644 index 5749567..0000000 --- a/src/main/java/frc/robot/commands/ScoreAlgae.java +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.Consumer; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.utility.IO; - -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class ScoreAlgae extends ParallelCommandGroup { - - public ScoreAlgae(IO io, boolean barge, GenericHID controller) { - addCommands( - //TODO: Check if we have Algae - io.elevator.moveCommand((barge) ? 5 : 0), - // new AutoAlign(1, io), - new WaitUntilCommand(io.elevator::atPosition), - new Intake(io, false, true, controller), - io.elevator.moveCommand(0) - ); - } - - public ScoreAlgae(IO io, boolean barge) { - addCommands( - //TODO: Check if we have Algae - io.elevator.moveCommand((barge) ? 5 : 0), - // new AutoAlign(1, io), - new WaitUntilCommand(io.elevator::atPosition), - new Intake(io, false, true), - io.elevator.moveCommand(0) - ); - } -} diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 3caef7f..4c360fe 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -12,24 +12,21 @@ public class ScoreReef extends SequentialCommandGroup { - public ScoreReef(IO io, int Level, GenericHID controller) { + public ScoreReef(IO io, int level, GenericHID controller) { addCommands( - // TODO: Check if we have coral & if we don't have - io.elevator.moveCommand(Level), - new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height - new Intake(io, true, true, controller), // TODO: Set to Reef Scoring Angle + new Intake(io, true, level, controller), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); } - public ScoreReef(IO io, boolean score_right, int Level) { + public ScoreReef(IO io, boolean score_right, int level) { addCommands( // TODO: Check if we have coral & if we don't have - io.elevator.moveCommand(Level), - new AutoAlign((score_right) ? 2 : 1, io), + io.elevator.moveCommand(level), + new AutoAlign((score_right) ? 2 : 0, io), + Util.Do(() -> io.claw.angle(level), io.claw), new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height - Util.Do(() -> io.claw.angle(Level), io.claw), - new Intake(io, true, true), // TODO: Set to Reef Scoring Angle + new Intake(io, true, level), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); } diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 93be333..62544f9 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -5,7 +5,6 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; -import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -33,19 +32,17 @@ public class Claw extends SubsystemBase { SparkMax intake = new SparkMax(13, MotorType.kBrushless); SparkFlex pivot = new SparkFlex(14, MotorType.kBrushless); - DigitalInput algaeBreak = new DigitalInput(0); - DigitalInput coralBreak = new DigitalInput(1); + public static final double INTAKE_ANGLE = 0; + public static final double REEF_ANGLE = 0; - public final static double INTAKE_ANGLE = 0; // TODO: FIND INTAKE ANGLE - public final static double BARGE_ANGLE = 0; // TODO: FIND BARGE ANGLE - public final static double REEF_ANGLE = 0; // TODO: FIND REEF ANGLE + DigitalInput coralBreak = new DigitalInput(0); TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); double target = 0.0; boolean stopped = true; - public boolean algaeIntaking = false; + public boolean intaking = false; public Claw() { SparkMaxConfig config = new SparkMaxConfig(); @@ -58,10 +55,10 @@ public Claw() { pivotConfig.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); pivotConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - // pivotConfig.softLimit.forwardSoftLimitEnabled(true); - // pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit - // pivotConfig.softLimit.reverseSoftLimitEnabled(true); - // pivotConfig.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit + pivotConfig.softLimit.forwardSoftLimitEnabled(false); + pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit + pivotConfig.softLimit.reverseSoftLimitEnabled(false); + pivotConfig.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @@ -78,10 +75,6 @@ public void stop() { intake.stopMotor(); } - public boolean hasAlgae() { - return algaeBreak.get(); - } - public boolean hasCoral() { return coralBreak.get(); } @@ -100,16 +93,6 @@ public void angle(double target_angle) { time.restart(); } - public void angle(int level) { - if (level == 0) { // Rest - angle(INTAKE_ANGLE); - } else if (level < 3 || level > 5) { - angle(REEF_ANGLE); - } else { - angle(BARGE_ANGLE); - } - } - public void pivotVolts(double volts) { pivot.setVoltage(volts); } @@ -134,8 +117,7 @@ public void periodic() { // DogLog.log("Claw/Coral Full", hasCoral()); // DogLog.log("Claw/Pivot Angle", angle()); - SmartDashboard.putBoolean("Have Algae", hasAlgae()); - SmartDashboard.putBoolean("Have Algae", hasAlgae()); + SmartDashboard.putBoolean("Coral", hasCoral()); SmartDashboard.putNumber("Claw Pivot", angle()); double cTime = time.get(); diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 38b442e..7fd2ef7 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -1,9 +1,7 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.config.SoftLimitConfig; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index be613fb..b0aa046 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -21,8 +21,6 @@ public class Limelight extends SubsystemBase { double latency; public int tag; - // EXAMPLE: TagPose[1][RIGHT][X] - int[] tagLocation; final NetworkTable table = NetworkTableInstance.getDefault().getTable("Limelight"); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d6fd2e6..bad31f1 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -64,8 +63,6 @@ public class Swerve extends SubsystemBase { public boolean active = true; - private final PIDController rotationPID = new PIDController(0.0, 0.0, 0.0); // TODO tune these if Characterization fails for Theta - public Swerve() { kinematics = new SwerveDriveKinematics( createTranslation(constants.TRACKWIDTH / 2.0, constants.WHEELBASE / 2.0), @@ -165,11 +162,12 @@ public Pose2d pose() { } public void resetOdometry() { - resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(180))); + resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(0))); } public void resetOdometry(Pose2d pose) { - resetAngle(); + // resetAngle(); + pigeon2.setYaw(pose.getRotation().getDegrees()); resetPosition(); odometry.resetPosition(rotation(), modulePositions(), pose); @@ -221,11 +219,11 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( null, - Volts.of(1), + null, null, null), new SysIdRoutine.Mechanism(voltage -> { - speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_VELOCITY); + speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_ANGULAR_VELOCITY); }, log -> { for (int i = 0; i < 4; i++) { log.motor(Constants.LAYOUT_TITLE[i] + " [Steer]") diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 7957373..4ffecf5 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -35,13 +35,14 @@ public static class Constants { // AUTON CONSTANTS public double XControllerP = 5.1555; public double XControllerD = 0.61072; - public double ThetaControllerP = 0; + public double ThetaControllerP = .001; public double ThetaControllerD = 0; public RobotConfig autoConfig; public double MASS = 47; // BASE CHASSIS CONFIGURATION public static final double MAX_VELOCITY = 5.4; + public static final double MAX_ANGULAR_VELOCITY = Math.PI/6; public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR @@ -101,7 +102,7 @@ public static void SwitchDriver(int driver){ default: // Shaan transFactor = .65; - rotFactor = .48; + rotFactor = .5; break; } } @@ -112,6 +113,7 @@ public interface Encoder { public boolean connected(); public double angle(); public AngularVelocity velocity(); + } public static class Cancoder implements Encoder { diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index e8df264..6697c0b 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -4,15 +4,12 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; -import frc.robot.commands.Intake; import frc.robot.commands.LimelightAlign; -import frc.robot.commands.ScoreAlgae; import frc.robot.commands.ScoreReef; public class AutomatedController { @@ -104,37 +101,33 @@ void configureAutomated(){ controller.leftBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 1, false))); controller.rightBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 2, false))); - controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(Util.Do(() -> new Intake(io, true, false, rumble))); + // controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(Util.Do(() -> new Intake(io, true, false, rumble))); controller.a().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 1, rumble))); controller.b().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 3, rumble))); controller.x().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 2, rumble))); controller.y().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 4, rumble))); - controller.x().and(automated()).and(() -> io.claw.hasAlgae()).onTrue(io.elevator.moveCommand(7)); - controller.a().and(automated()).and(() -> io.claw.hasAlgae()).onTrue(io.elevator.moveCommand(6)); - - controller.povLeft().and(automated()).onTrue(Util.Do(() -> new Intake(io, false, true, rumble))); - controller.povUp().and(automated()).onTrue(new ConditionalCommand(new ScoreAlgae(io, true, rumble), new Intake(io, false, false, rumble), io.claw::hasAlgae)); + // controller.povLeft().and(automated()).onTrue(Util.Do(() -> new Intake(io, false, true, rumble))); controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0))); controller.back().and(automated()).onTrue(Util.Do(() -> io.chassis.resetOdometry())); } void configureDebug(){ - controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); - controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); - controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); + // controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); + // controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); + // controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + // controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + // controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + // controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + // controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); - controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); - controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); - controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); - controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + // controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); + // controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); + // controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); + // controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); @@ -152,8 +145,8 @@ void configureDebug(){ controller.a().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); controller.povUp().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); - controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); - controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); + // controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); + // controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); } } From d6bd9f4b3f7e14ca7e81fff69f258e1334856dbd Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Thu, 13 Mar 2025 23:38:00 -0400 Subject: [PATCH 71/79] Minor changes --- src/main/java/frc/robot/Robot.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 5 +- src/main/java/frc/robot/commands/Intake.java | 9 +- .../java/frc/robot/commands/ScoreReef.java | 8 +- .../java/frc/robot/subsystems/Swerve.java | 30 +++-- src/main/java/frc/robot/swerve/Swerve.java | 29 ----- .../robot/utility/AutomatedController.java | 108 ++++++++---------- src/main/java/frc/robot/utility/IO.java | 5 - 8 files changed, 80 insertions(+), 124 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3b57bf9..3d00c4e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,24 +35,20 @@ public void disabledExit() {} public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - if (m_autonomousCommand != null) { + if (m_autonomousCommand != null) m_autonomousCommand.schedule(); - } } @Override public void autonomousPeriodic() {} @Override - public void autonomousExit() { - // m_robotContainer.io.chassis.adjustRotation(); - } + public void autonomousExit() {} @Override public void teleopInit() { - if (m_autonomousCommand != null) { + if (m_autonomousCommand != null) m_autonomousCommand.cancel(); - } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9435125..30b0c06 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; import frc.robot.commands.DefaultDrive; -import frc.robot.commands.Intake; import frc.robot.commands.RotateChassis; import frc.robot.commands.ScoreReef; import frc.robot.swerve.Swerve; @@ -82,8 +81,8 @@ public void configureAuton(){ NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); // TODO: SEE IF WE CAN GET AWAY WITH THESE LEVELS - NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false, 2)); // TODO: MAKE ACTUAL COMMAND - NamedCommands.registerCommand("Clear High Algae", new Intake(io, false, 3)); // TODO: MAKE ACTUAL COMMAND + // NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND + // NamedCommands.registerCommand("Clear High Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index 046453b..d76c273 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -20,6 +20,7 @@ public class Intake extends Command { Runnable rumble; BooleanSupplier holding; double angle; + boolean release; // public static int INTAKE_CORAL = -1; // public static int INTAKE_ALGAE = -2; @@ -27,7 +28,7 @@ public class Intake extends Command { // public static int SCORE_ALGAE = 2; // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP - public Intake(IO io, boolean release, int level, GenericHID controller) { + public Intake(IO io, boolean release, GenericHID controller) { rumble = (controller != null) ? () -> controller.setRumble(RumbleType.kBothRumble, .25) : () -> {}; // TODO: Check if it's fine holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); @@ -41,11 +42,11 @@ public Intake(IO io, boolean release, int level, GenericHID controller) { controller.setRumble(RumbleType.kBothRumble, 0.0); }; - io.elevator.move(level); + this.release = release; } - public Intake(IO io, boolean release, int level){ - this(io, release, level, null); + public Intake(IO io, boolean release){ + this(io, release, null); } diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index 4c360fe..ab6601e 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -14,7 +14,9 @@ public class ScoreReef extends SequentialCommandGroup { public ScoreReef(IO io, int level, GenericHID controller) { addCommands( - new Intake(io, true, level, controller), // TODO: Set to Reef Scoring Angle + io.elevator.moveCommand(level), + new WaitUntilCommand(io.elevator::atPosition), + new Intake(io, true, controller), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); } @@ -23,10 +25,10 @@ public ScoreReef(IO io, boolean score_right, int level) { addCommands( // TODO: Check if we have coral & if we don't have io.elevator.moveCommand(level), + new WaitUntilCommand(io.elevator::atPosition), new AutoAlign((score_right) ? 2 : 0, io), Util.Do(() -> io.claw.angle(level), io.claw), - new WaitUntilCommand(io.elevator::atPosition), // TODO: Wait until we're at the height - new Intake(io, true, level), // TODO: Set to Reef Scoring Angle + new Intake(io, true), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index bad31f1..5c0e334 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -158,7 +158,6 @@ public SwerveModuleState[] moduleStates(Module[] modules) { public Pose2d pose() { return odometry.getPoseMeters(); - // return estimatePose(); } public void resetOdometry() { @@ -166,14 +165,13 @@ public void resetOdometry() { } public void resetOdometry(Pose2d pose) { - // resetAngle(); pigeon2.setYaw(pose.getRotation().getDegrees()); - resetPosition(); + resetModulePositions(); odometry.resetPosition(rotation(), modulePositions(), pose); } - public void resetPosition() { + public void resetModulePositions() { for (Module mod : modules) mod.resetDrivePosition(); } @@ -202,7 +200,8 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine driveRoutine = new SysIdRoutine(new Config( null, - Volts.of(2), + // Volts.of(2), + null, null, null), new SysIdRoutine.Mechanism(voltage -> { @@ -219,18 +218,25 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( null, - null, + Volts.of(2), null, null), new SysIdRoutine.Mechanism(voltage -> { speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_ANGULAR_VELOCITY); }, log -> { - for (int i = 0; i < 4; i++) { - log.motor(Constants.LAYOUT_TITLE[i] + " [Steer]") - .voltage(modules[i].steerVoltage()) - .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) - .angularVelocity(modules[i].steerVelocity()); - } + // TODO: Test to see if this gives us accurate PID values + log.motor("Chassis") + .voltage(modules[0].steerVoltage()) + .angularPosition(pigeon2.getYaw().getValue()) + .angularVelocity(pigeon2.getAngularVelocityYWorld().getValue()); + // .angularAcceleration(pigeon2.getAccelerationY().getValue()); // TODO: Test to see if converting this to angular acceleration would be valid + + // for (int i = 0; i < 4; i++) { + // log.motor(Constants.LAYOUT_TITLE[i] + " [Steer]") + // .voltage(modules[i].steerVoltage()) + // .angularPosition(angle[i].mut_replace(modules[i].angle(), Radians)) + // .angularVelocity(modules[i].steerVelocity()); + // } }, this)); public double getRoll() { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 4ffecf5..1e4470e 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -38,7 +38,6 @@ public static class Constants { public double ThetaControllerP = .001; public double ThetaControllerD = 0; public RobotConfig autoConfig; - public double MASS = 47; // BASE CHASSIS CONFIGURATION public static final double MAX_VELOCITY = 5.4; @@ -50,34 +49,6 @@ public static class Constants { public static final int PIGEON_ID = 6; public Constants(){ - // compChassis = (System.getenv("BLUEBOT") == null); // Determine if we're comp based on an enviornmental variable like in 2023 - if (comp) { - MASS = 60.0; - } else { - MASS = 47.0; - } - - MASS = (comp) ? 60.0 : 47.0; - - // double trackwidthMeters = Units.inchesToMeters(TRACKWIDTH); - // double wheelbaseMeters = Units.inchesToMeters(WHEELBASE); - // Translation2d[] swerve_offsets = - // new Translation2d[] { - // new Translation2d(trackwidthMeters / 2, wheelbaseMeters / 2), - // new Translation2d(trackwidthMeters / 2, -wheelbaseMeters / 2), - // new Translation2d(-trackwidthMeters / 2, wheelbaseMeters / 2), - // new Translation2d(-trackwidthMeters / 2, -wheelbaseMeters / 2) - // }; - - // double moi = 1/12 * MASS * (trackwidthMeters * trackwidthMeters + wheelbaseMeters * wheelbaseMeters); // estimate of moi - // ModuleConfig moduleConfigs = new ModuleConfig(WHEEL_RADIUS, 5.4, 1.2, null, GEAR_RATIO, 59, 4); - // autoConfig = new RobotConfig( - // MASS, - // moi, - // moduleConfigs, - // swerve_offsets - // ); - try { autoConfig = RobotConfig.fromGUISettings(); } catch (IOException | org.json.simple.parser.ParseException e) { diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 6697c0b..6a6b7c6 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; import frc.robot.commands.LimelightAlign; import frc.robot.commands.ScoreReef; @@ -26,8 +25,8 @@ public AutomatedController(int port, IO io){ selector.setDefaultOption("Automated", () -> {mode = 0;}); selector.addOption("Manual", () -> {mode = 1;}); - selector.addOption("Debug", () -> {mode = 2;}); - selector.addOption("Debug Setting", () -> {mode = 3;}); + selector.addOption("Characterise", () -> {mode = 2;}); + selector.addOption("Debug", () -> {mode = 3;}); selector.onChange((x) -> {x.run();}); @@ -38,7 +37,7 @@ public AutomatedController(int port, IO io){ controller.leftStick().debounce(2).onTrue(new InstantCommand(io.chassis::resetAngle)); // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); - configure(); + configureManual(); configureDebug(); } @@ -54,11 +53,11 @@ public BooleanSupplier manual(){ return mode(1); } - public BooleanSupplier debug(){ + public BooleanSupplier characterise(){ return mode(2); } - public BooleanSupplier debug_setting(){ + public BooleanSupplier debug(){ return mode(3); } @@ -66,37 +65,6 @@ public void switchMode(){ mode = (mode + 1) % 3; } - public void configure(){ - - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); - controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); - - // controller.leftBumper().onTrue(new SimpleAlign(io, false)); - // controller.rightBumper().onTrue(new SimpleAlign(io, true)); - - // controller.leftBumper().onTrue(new RotateChassis(io, 45)); - // controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); - // controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); - - // AUTOMATED - - // Based on the nearest element and our field orientation - // LB align Left and Score Coral & Score Barge - // RB align Right and Score Coral & Score Processor - - controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); - controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); - - // MANUAL - // RB align Right and Score Coral & Score Processor - // controller.y().and( automated() ).onTrue(Util.D - controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); - controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect - - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect - } - void configureAutomated(){ controller.leftBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 1, false))); controller.rightBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 2, false))); @@ -114,36 +82,54 @@ void configureAutomated(){ controller.back().and(automated()).onTrue(Util.Do(() -> io.chassis.resetOdometry())); } + public void configureManual(){ + + controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); + controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + + controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); + controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); + + controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); + controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); + controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect + + // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect + } + + void configureCharacterisaton(){ + + controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + + // controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); + // controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); + // controller.leftTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); + // controller.rightTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + + controller.povUp().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); + controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); + controller.povRight().and(characterise()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); + controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + } + void configureDebug(){ // controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); // controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); // controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - - // controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - // controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - // controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - // controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); - - // controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); - // controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); - // controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); - // controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); - - controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.x().and(debug_setting()).onTrue(Util.Do(io.elevator::zero, io.elevator)); - // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); - controller.povUp().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); - controller.povDown().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); - controller.povRight().and(debug()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); - controller.povLeft().and(debug()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + controller.leftBumper().and(debug()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.rightBumper().and(debug()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.x().and(debug()).onTrue(Util.Do(io.elevator::zero, io.elevator)); + // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); - controller.y().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); - controller.b().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); - controller.x().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); - controller.a().and(debug_setting()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); - controller.povUp().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); + controller.y().and(debug()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); + controller.b().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); + controller.x().and(debug()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); + controller.a().and(debug()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); + controller.povUp().and(debug()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); // controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); // controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index a0115b7..a2aa5b5 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -6,17 +6,12 @@ public class IO extends SubsystemBase { public final Swerve chassis = new Swerve(); - // public final AlgaeIntake intake = new AlgaeIntake(); public final Elevator elevator = new Elevator(); - // public final Hang hang = new Hang(); public final Limelight limelight = new Limelight(); public final Claw claw = null; - // public final Elevator elevator = null; public final Hang hang = null; - public CommandScheduler scheduler = CommandScheduler.getInstance(); - public IO() { DriverStation.silenceJoystickConnectionWarning(true); } From 3d73a5eedbce40f8cd320add95e1b6bf4c0571b4 Mon Sep 17 00:00:00 2001 From: Ayidana Date: Sat, 15 Mar 2025 21:50:44 -0400 Subject: [PATCH 72/79] Auton Testing --- .vscode/settings.json | 2 +- .../pathplanner/autos/Straight Test.auto | 12 +++++++ .../autos/Top 6 Coral Cleanout.auto | 36 +++++++++++++++++++ .../deploy/pathplanner/paths/AB to Barge.path | 4 +-- .../deploy/pathplanner/paths/Barge to AB.path | 4 +-- .../deploy/pathplanner/paths/Barge to HG.path | 4 +-- .../deploy/pathplanner/paths/Barge to IJ.path | 4 +-- .../paths/Barge to Station to CD.path | 6 ++-- .../paths/Barge to Station to LK.path | 6 ++-- .../pathplanner/paths/Bottom to CD.path | 6 ++-- .../deploy/pathplanner/paths/CD to Barge.path | 6 ++-- .../deploy/pathplanner/paths/Cycle AB.path | 9 ++--- .../deploy/pathplanner/paths/Cycle CD.path | 8 ++--- .../deploy/pathplanner/paths/Cycle JI.path | 14 ++++---- .../deploy/pathplanner/paths/Cycle LK.path | 12 +++---- .../pathplanner/paths/Forward and Back.path | 4 +-- .../deploy/pathplanner/paths/HG to Barge.path | 4 +-- .../deploy/pathplanner/paths/IJ to Barge.path | 4 +-- .../deploy/pathplanner/paths/IJ to LK.path | 8 ++--- .../deploy/pathplanner/paths/LK to AB.path | 8 ++--- .../deploy/pathplanner/paths/LK to Barge.path | 6 ++-- .../deploy/pathplanner/paths/LK to JI.path | 8 ++--- .../pathplanner/paths/Middle to HG.path | 8 ++--- .../deploy/pathplanner/paths/Return Home.path | 4 +-- .../paths/Straight And Rotate.path | 4 +-- .../paths/Straight Line Rotate 90.path | 4 +-- .../pathplanner/paths/Straight Test.path | 10 +++--- .../deploy/pathplanner/paths/Top to IJ.path | 8 ++--- .../deploy/pathplanner/paths/Top to LK.path | 10 +++--- .../deploy/pathplanner/paths/Triangle.path | 4 +-- src/main/deploy/pathplanner/settings.json | 4 +-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/DefaultDrive.java | 6 ++-- .../java/frc/robot/subsystems/Swerve.java | 15 ++++---- src/main/java/frc/robot/swerve/Module.java | 2 +- src/main/java/frc/robot/swerve/Swerve.java | 6 ++-- .../robot/utility/AutomatedController.java | 20 ++++++----- 37 files changed, 168 insertions(+), 114 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index d2f7034..0df8b28 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "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" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/deploy/pathplanner/autos/Straight Test.auto b/src/main/deploy/pathplanner/autos/Straight Test.auto index 2c0acd5..36b7ff2 100644 --- a/src/main/deploy/pathplanner/autos/Straight Test.auto +++ b/src/main/deploy/pathplanner/autos/Straight Test.auto @@ -9,6 +9,18 @@ "data": { "pathName": "Straight Test" } + }, + { + "type": "named", + "data": { + "name": "rotatelk" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto index 3030b34..5c9464b 100644 --- a/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto +++ b/src/main/deploy/pathplanner/autos/Top 6 Coral Cleanout.auto @@ -16,6 +16,12 @@ "name": "Score L-L4" } }, + { + "type": "named", + "data": { + "name": "Rotate LK" + } + }, { "type": "path", "data": { @@ -28,6 +34,12 @@ "name": "Score R-L4" } }, + { + "type": "named", + "data": { + "name": "Rotate LK" + } + }, { "type": "path", "data": { @@ -40,6 +52,12 @@ "name": "Score L-L3" } }, + { + "type": "named", + "data": { + "name": "rotate LK" + } + }, { "type": "path", "data": { @@ -58,6 +76,12 @@ "name": "Clear Low Alage" } }, + { + "type": "named", + "data": { + "name": "rotate LK" + } + }, { "type": "path", "data": { @@ -70,6 +94,12 @@ "name": "Score L-L2" } }, + { + "type": "named", + "data": { + "name": "rotate LK" + } + }, { "type": "path", "data": { @@ -81,6 +111,12 @@ "data": { "name": "score R-L2" } + }, + { + "type": "named", + "data": { + "name": "rotate LK" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/AB to Barge.path b/src/main/deploy/pathplanner/paths/AB to Barge.path index 0a9c107..342b3da 100644 --- a/src/main/deploy/pathplanner/paths/AB to Barge.path +++ b/src/main/deploy/pathplanner/paths/AB to Barge.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Barge to AB.path b/src/main/deploy/pathplanner/paths/Barge to AB.path index 68fe398..4f3b45e 100644 --- a/src/main/deploy/pathplanner/paths/Barge to AB.path +++ b/src/main/deploy/pathplanner/paths/Barge to AB.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Barge to HG.path b/src/main/deploy/pathplanner/paths/Barge to HG.path index 07b75eb..ab53dd2 100644 --- a/src/main/deploy/pathplanner/paths/Barge to HG.path +++ b/src/main/deploy/pathplanner/paths/Barge to HG.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Barge to IJ.path b/src/main/deploy/pathplanner/paths/Barge to IJ.path index ffcc170..e85d214 100644 --- a/src/main/deploy/pathplanner/paths/Barge to IJ.path +++ b/src/main/deploy/pathplanner/paths/Barge to IJ.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path index 9c63ca4..d00f1a1 100644 --- a/src/main/deploy/pathplanner/paths/Barge to Station to CD.path +++ b/src/main/deploy/pathplanner/paths/Barge to Station to CD.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path index c6ebb3c..d75f0aa 100644 --- a/src/main/deploy/pathplanner/paths/Barge to Station to LK.path +++ b/src/main/deploy/pathplanner/paths/Barge to Station to LK.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -58,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Bottom to CD.path b/src/main/deploy/pathplanner/paths/Bottom to CD.path index 05f1fbb..cd73fd1 100644 --- a/src/main/deploy/pathplanner/paths/Bottom to CD.path +++ b/src/main/deploy/pathplanner/paths/Bottom to CD.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Starting", diff --git a/src/main/deploy/pathplanner/paths/CD to Barge.path b/src/main/deploy/pathplanner/paths/CD to Barge.path index 3043ec9..e70bba4 100644 --- a/src/main/deploy/pathplanner/paths/CD to Barge.path +++ b/src/main/deploy/pathplanner/paths/CD to Barge.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle AB.path b/src/main/deploy/pathplanner/paths/Cycle AB.path index 17e7c0a..d5b3ce6 100644 --- a/src/main/deploy/pathplanner/paths/Cycle AB.path +++ b/src/main/deploy/pathplanner/paths/Cycle AB.path @@ -68,8 +68,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -77,13 +77,14 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true +} } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle CD.path b/src/main/deploy/pathplanner/paths/Cycle CD.path index 8db5514..99e6634 100644 --- a/src/main/deploy/pathplanner/paths/Cycle CD.path +++ b/src/main/deploy/pathplanner/paths/Cycle CD.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle JI.path b/src/main/deploy/pathplanner/paths/Cycle JI.path index 1a80066..3c60330 100644 --- a/src/main/deploy/pathplanner/paths/Cycle JI.path +++ b/src/main/deploy/pathplanner/paths/Cycle JI.path @@ -24,7 +24,7 @@ "y": 7.342700736229868 }, "nextControl": { - "x": 1.9845375267660235, + "x": 1.9845375267660232, "y": 7.101350753043471 }, "isLocked": false, @@ -47,7 +47,7 @@ "rotationTargets": [ { "waypointRelativePos": 1.0, - "rotationDegrees": 119.99999999999999 + "rotationDegrees": -57.0665571551223 } ], "constraintZones": [ @@ -68,22 +68,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 59.99999999999999 + "velocity": 0.0, + "rotation": -124.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -124.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Cycle LK.path b/src/main/deploy/pathplanner/paths/Cycle LK.path index 57ba928..77d43b6 100644 --- a/src/main/deploy/pathplanner/paths/Cycle LK.path +++ b/src/main/deploy/pathplanner/paths/Cycle LK.path @@ -48,8 +48,8 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.9612403100775195, - "maxWaypointRelativePos": 1.1369509043927652, + "minWaypointRelativePos": 0.8997632202052086, + "maxWaypointRelativePos": 1.2186266771902134, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.5, @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -72,13 +72,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Forward and Back.path b/src/main/deploy/pathplanner/paths/Forward and Back.path index 8db3403..0680587 100644 --- a/src/main/deploy/pathplanner/paths/Forward and Back.path +++ b/src/main/deploy/pathplanner/paths/Forward and Back.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HG to Barge.path b/src/main/deploy/pathplanner/paths/HG to Barge.path index 6810a81..281906d 100644 --- a/src/main/deploy/pathplanner/paths/HG to Barge.path +++ b/src/main/deploy/pathplanner/paths/HG to Barge.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/IJ to Barge.path b/src/main/deploy/pathplanner/paths/IJ to Barge.path index 8536217..e04cbc4 100644 --- a/src/main/deploy/pathplanner/paths/IJ to Barge.path +++ b/src/main/deploy/pathplanner/paths/IJ to Barge.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/IJ to LK.path b/src/main/deploy/pathplanner/paths/IJ to LK.path index 8e07a5f..f506f94 100644 --- a/src/main/deploy/pathplanner/paths/IJ to LK.path +++ b/src/main/deploy/pathplanner/paths/IJ to LK.path @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -72,13 +72,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -124.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to AB.path b/src/main/deploy/pathplanner/paths/LK to AB.path index 7df363c..e0040d2 100644 --- a/src/main/deploy/pathplanner/paths/LK to AB.path +++ b/src/main/deploy/pathplanner/paths/LK to AB.path @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -72,13 +72,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": -1.9549875123408296 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to Barge.path b/src/main/deploy/pathplanner/paths/LK to Barge.path index 72c14cb..d8627f2 100644 --- a/src/main/deploy/pathplanner/paths/LK to Barge.path +++ b/src/main/deploy/pathplanner/paths/LK to Barge.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LK to JI.path b/src/main/deploy/pathplanner/paths/LK to JI.path index 2aad4cb..04a5bf4 100644 --- a/src/main/deploy/pathplanner/paths/LK to JI.path +++ b/src/main/deploy/pathplanner/paths/LK to JI.path @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -72,13 +72,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -124.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle to HG.path b/src/main/deploy/pathplanner/paths/Middle to HG.path index 9434a33..a97b03d 100644 --- a/src/main/deploy/pathplanner/paths/Middle to HG.path +++ b/src/main/deploy/pathplanner/paths/Middle to HG.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Return Home.path b/src/main/deploy/pathplanner/paths/Return Home.path index ea6cd70..a8342c6 100644 --- a/src/main/deploy/pathplanner/paths/Return Home.path +++ b/src/main/deploy/pathplanner/paths/Return Home.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Straight And Rotate.path b/src/main/deploy/pathplanner/paths/Straight And Rotate.path index e52ff8a..3a0af5e 100644 --- a/src/main/deploy/pathplanner/paths/Straight And Rotate.path +++ b/src/main/deploy/pathplanner/paths/Straight And Rotate.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path index aff49ff..7f7c1fd 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Rotate 90.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Straight Test.path b/src/main/deploy/pathplanner/paths/Straight Test.path index 6b7c7ec..8b37590 100644 --- a/src/main/deploy/pathplanner/paths/Straight Test.path +++ b/src/main/deploy/pathplanner/paths/Straight Test.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.75, - "maxAcceleration": 5.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top to IJ.path b/src/main/deploy/pathplanner/paths/Top to IJ.path index b446949..1b4ecb5 100644 --- a/src/main/deploy/pathplanner/paths/Top to IJ.path +++ b/src/main/deploy/pathplanner/paths/Top to IJ.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 59.99999999999999 + "rotation": -124.0 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0.0, - "rotation": 59.99999999999999 + "rotation": -124.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top to LK.path b/src/main/deploy/pathplanner/paths/Top to LK.path index 18a4562..c57edb9 100644 --- a/src/main/deploy/pathplanner/paths/Top to LK.path +++ b/src/main/deploy/pathplanner/paths/Top to LK.path @@ -20,7 +20,7 @@ "y": 5.206633522727272 }, "prevControl": { - "x": 4.249482615894039, + "x": 4.249482615894038, "y": 5.6957367549668865 }, "nextControl": null, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Starting", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Triangle.path b/src/main/deploy/pathplanner/paths/Triangle.path index a89fede..d021046 100644 --- a/src/main/deploy/pathplanner/paths/Triangle.path +++ b/src/main/deploy/pathplanner/paths/Triangle.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.6, + "maxVelocity": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 34c3c10..9950981 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,8 +13,8 @@ "Scuff", "Testing" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 4.6, + "defaultMaxVel": 4.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 360.0, "defaultMaxAngAccel": 360.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9435125..fb6a7e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,7 +66,7 @@ public RobotContainer() { public void configureAuton(){ NamedCommands.registerCommand("Face Barge", new RotateChassis(io, 0)); NamedCommands.registerCommand("Rotate IJ", new RotateChassis(io, -120)); - NamedCommands.registerCommand("Rotate LK", new RotateChassis(io, -60)); + NamedCommands.registerCommand("rotatelk", new RotateChassis(io, 300)); NamedCommands.registerCommand("Rotate HG", new RotateChassis(io, 180)); NamedCommands.registerCommand("Rotate CD", new RotateChassis(io, 60)); NamedCommands.registerCommand("Rotate EF", new RotateChassis(io, 120)); diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index 375847e..432d143 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -22,9 +22,9 @@ public DefaultDrive(IO io, ChassisSpeeds chassisSpeeds) { } public DefaultDrive(IO io, CommandXboxController controller) { - this(io, () -> modifyAxis(controller.getLeftY()) * Swerve.Constants.MAX_VELOCITY, - () -> modifyAxis(controller.getLeftX()) * Swerve.Constants.MAX_VELOCITY, - () -> modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_ANGULAR_VELOCITY); + this(io, () -> -modifyAxis(controller.getLeftY()) * Swerve.Constants.MAX_VELOCITY, + () -> -modifyAxis(controller.getLeftX()) * Swerve.Constants.MAX_VELOCITY, + () -> -modifyAxis(controller.getRightX()) * Swerve.Constants.MAX_ANGULAR_VELOCITY); this.controller = controller; } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index bad31f1..7666541 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Seconds; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; @@ -24,6 +25,7 @@ import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -202,19 +204,18 @@ public void setModuleStates(SwerveModuleState[] states) { public final SysIdRoutine driveRoutine = new SysIdRoutine(new Config( null, - Volts.of(2), + Volts.of(3), + // Seconds.of(5), null, null), new SysIdRoutine.Mechanism(voltage -> { for (Module mod : modules) mod.set(voltage.magnitude(), 0); }, log -> { - for (int i = 0; i < 4; i++) { - log.motor(Constants.LAYOUT_TITLE[i] + " [Drive]") - .voltage(modules[i].voltage()) - .linearPosition(distance[i].mut_replace(modules[i].drivePosition(), Meters)) - .linearVelocity(modules[i].velocity()); - } + log.motor(Constants.LAYOUT_TITLE[0] + " [Drive]") + .voltage(modules[0].voltage()) + .linearPosition(distance[0].mut_replace(modules[0].drivePosition(), Meters)) + .linearVelocity(modules[0].velocity()); }, this)); public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( diff --git a/src/main/java/frc/robot/swerve/Module.java b/src/main/java/frc/robot/swerve/Module.java index 15ac8d4..f3ddf8a 100644 --- a/src/main/java/frc/robot/swerve/Module.java +++ b/src/main/java/frc/robot/swerve/Module.java @@ -67,7 +67,7 @@ public Module(ShuffleboardLayout tab, int driveID, int steerID, int encoderID, b config.CurrentLimits.SupplyCurrentLimit = 20; config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; steer.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 4ffecf5..e7a7bdc 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -33,9 +33,9 @@ public static class Constants { public static double rotFactor = .30; // .6 for tristan // AUTON CONSTANTS - public double XControllerP = 5.1555; - public double XControllerD = 0.61072; - public double ThetaControllerP = .001; + public double XControllerP = 1.6878; + public double XControllerD = 0; + public double ThetaControllerP = 0; public double ThetaControllerD = 0; public RobotConfig autoConfig; public double MASS = 47; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 6697c0b..b74017e 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -10,6 +10,7 @@ import frc.robot.commands.Aimbot; import frc.robot.commands.AutoAlign; import frc.robot.commands.LimelightAlign; +import frc.robot.commands.RotateChassis; import frc.robot.commands.ScoreReef; public class AutomatedController { @@ -87,6 +88,8 @@ public void configure(){ controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); + controller.x().and(manual()).toggleOnTrue(new RotateChassis(io, 300)); + // MANUAL // RB align Right and Score Coral & Score Processor // controller.y().and( automated() ).onTrue(Util.D @@ -119,20 +122,21 @@ void configureDebug(){ // controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); // controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - // controller.povUp().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - // controller.povDown().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - // controller.povRight().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - // controller.povLeft().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + controller.x().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + controller.a().and(debug()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + controller.y().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + controller.b().and(debug()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); // controller.x().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); // controller.a().and(debug()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); // controller.y().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); - // controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + // controller.b().and(debug()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kRevezrse)); - controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(3), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.leftBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts( + (double) Util.get("Test Elevator Volts", 1.0)), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.rightBumper().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-(double) Util.get("Test Elevator Volts", 1.0)), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.x().and(debug_setting()).onTrue(Util.Do(io.elevator::zero, io.elevator)); - // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); + // controller.x().and(debug_setting()).toggleOnTrue(new LimelighPtAlign(io, 1, false)); controller.povUp().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); controller.povDown().and(debug()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); From 49e8819a0c43606267e934d8bf4d5550de20976e Mon Sep 17 00:00:00 2001 From: Ayidana Date: Tue, 18 Mar 2025 23:41:46 -0400 Subject: [PATCH 73/79] Elevator PID and Elevator working --- .../java/frc/robot/subsystems/Elevator.java | 26 ++-- src/main/java/frc/robot/subsystems/LiDAR.java | 129 ++++++++++++++++++ .../java/frc/robot/subsystems/LiDAR_PWM.java | 40 ++++++ .../java/frc/robot/subsystems/Swerve.java | 2 - .../robot/utility/AutomatedController.java | 54 ++++---- 5 files changed, 218 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LiDAR.java create mode 100644 src/main/java/frc/robot/subsystems/LiDAR_PWM.java diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 284f361..e93dc41 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; @@ -38,6 +39,7 @@ public class Elevator extends SubsystemBase { TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); double target = 0; + public boolean stopped = true; PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); @@ -50,31 +52,38 @@ public class Elevator extends SubsystemBase { public final double L1 = 25; public final double L2 = 43.5; public final double L3 = 76; - public final double L4 = 0; + public final double L4 = 110; public final double Barge = 0; // TODO: FIND BARGE public final double Low_Algae = 0; // TODO: FIND public final double High_Algae = 0; // TODO: FIND public final double MAX_HEIGHT = 0; // TODO: FIND + public boolean softLimits = true; + public Elevator() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.01; + config.Slot0.kP = 0.3; config.Slot0.kI = 0.0; - config.Slot0.kD = 0.0; + config.Slot0.kD = 0.1; config.Slot0.kG = 0.0; - config.MotorOutput.withInverted(InvertedValue.Clockwise_Positive); + config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.SoftwareLimitSwitch - .withForwardSoftLimitEnable(false) - .withForwardSoftLimitThreshold(130.0) - .withReverseSoftLimitEnable(false) - .withReverseSoftLimitThreshold(0.0); + .withForwardSoftLimitEnable(softLimits) + .withForwardSoftLimitThreshold(0.0) + .withReverseSoftLimitEnable(softLimits) + .withReverseSoftLimitThreshold(160.0); lead.getConfigurator().apply(config); follow.setControl(new Follower(lead.getDeviceID(), true)); // TODO: Check if we need to invert } + public void toggleSoftLimits() { + softLimits = !softLimits; + lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(softLimits).withReverseSoftLimitEnable(softLimits)); + } + public void speed(double speed) { lead.set(speed); } @@ -180,6 +189,7 @@ public LinearVelocity velocity() { @Override public void periodic() { SmartDashboard.putNumber("Elevator Height", position()); + SmartDashboard.putBoolean("Elevator Soft Limits Active", softLimits); if (stopped) return; diff --git a/src/main/java/frc/robot/subsystems/LiDAR.java b/src/main/java/frc/robot/subsystems/LiDAR.java new file mode 100644 index 0000000..5eb24bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LiDAR.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.I2C.Port; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LiDAR extends SubsystemBase { + public enum LidarConfiguration { + DEFAULT, // Default mode, balanced performance + SHORT_RANGE, // Short range, high speed + DEFAULT_HIGH_SPEED, // Default range, higher speed short range + MAXIMUM_RANGE, // Maximum range + HIGH_SENSITIVE, // High sensitivity detection, high erroneous measurements + LOW_SENSITIVE // Low sensitivity detection, low erroneous measurements + } + + I2C _lidar; + int config = 0; + String[] display = {"Default", "Short Range", "Default High Speed", "Maximum Range", "High Sensetivity", "Low Sensetivity"}; + byte[] distanceBuffer = new byte[2]; + + public LiDAR() { + this(LidarConfiguration.DEFAULT, (byte) 0x62); + } + + public LiDAR(LidarConfiguration configuration, int address) { + _lidar = new I2C(Port.kMXP, 0x62); // default i2c port + // if the address isn't 0x62 (default address) change the address + if (address != 0x62) { + setI2cAddress((byte) address); + } + + changeMode(configuration); + } + + private void setI2cAddress(byte newAddress) { + // read and write back serial number + // to ensure that we have the correct sensor + byte[] dataBytes = new byte[2]; + _lidar.read(0x8F, 2, dataBytes); + _lidar.write(0x18, dataBytes[0]); + _lidar.write(0x19, dataBytes[1]); + + // Write the new I2C device address to registers + dataBytes[0] = newAddress; + _lidar.write(0x1a, dataBytes[0]); + + // Enable the new I2C device address using the default I2C device address + dataBytes[0] = 0; + _lidar.write(0x1e, dataBytes[0]); + + // delete old object, create new one at new address + _lidar = null; + _lidar = new I2C(Port.kMXP, newAddress); + + // disable default I2C device address (using the new I2C device address) + dataBytes[0] = (1 << 3); // set bit to disable default address + _lidar.write(0x1e, dataBytes[0]); + } + + public void changeMode(LidarConfiguration configuration) { + switch (configuration) { + case DEFAULT: // Default mode, balanced performance + _lidar.write(0x02, 0x80); // Default + _lidar.write(0x04, 0x08); // Default + _lidar.write(0x1c, 0x00); // Default + break; + + case SHORT_RANGE: // Short range, high speed + _lidar.write(0x02, 0x1d); + _lidar.write(0x04, 0x08); // Default + _lidar.write(0x1c, 0x00); // Default + break; + + case DEFAULT_HIGH_SPEED: // Default range, higher speed short range + _lidar.write(0x02, 0x80); // Default + _lidar.write(0x04, 0x00); + _lidar.write(0x1c, 0x00); // Default + break; + + case MAXIMUM_RANGE: // Maximum range + _lidar.write(0x02, 0xff); + _lidar.write(0x04, 0x08); // Default + _lidar.write(0x1c, 0x00); // Default + break; + + case HIGH_SENSITIVE: // High sensitivity detection, high erroneous measurements + _lidar.write(0x02, 0x80); // Default + _lidar.write(0x04, 0x08); // Default + _lidar.write(0x1c, 0x80); + break; + + case LOW_SENSITIVE: // Low sensitivity detection, low erroneous measurements + _lidar.write(0x02, 0x80); // Default + _lidar.write(0x04, 0x08); // Default + _lidar.write(0x1c, 0xb0); + break; + } + config = configuration.ordinal(); + } + + public double distance(boolean biasCorrection) { + _lidar.write(0x00, (biasCorrection) ? 0x04 : 0x03); + + _lidar.read(0x8F, 2, distanceBuffer); + + return ((distanceBuffer[0] << 8) + distanceBuffer[1]) * 10; // + } + + public void reset() { + _lidar.write(0x00, 0x00); // Centimeters to meters + } + + public LidarConfiguration getCurrentConfiguration() { + return LidarConfiguration.values()[config]; + } + + @Override + public void periodic() { + double distance = 10 * distance(true); + SmartDashboard.putNumber("Lidar Sensor Distance", distance); + SmartDashboard.putString("Lidar Config", display[config]); + } +} diff --git a/src/main/java/frc/robot/subsystems/LiDAR_PWM.java b/src/main/java/frc/robot/subsystems/LiDAR_PWM.java new file mode 100644 index 0000000..63744a1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LiDAR_PWM.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LiDAR_PWM extends SubsystemBase { + + Counter counter; + + public LiDAR_PWM(int address) { + counter = new Counter(new DigitalInput(address)); + counter.setMaxPeriod(1.0); + counter.setSemiPeriodMode(true); + reset(); + } + + public double distance() { + return (counter.getPeriod() * 100000000.0) - 18; // TODO: Check if this is in Metres + } + + public boolean connected() { + return (counter.get() != 0); + } + + public void reset() { + counter.reset(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Lidar Distance", distance()); + } +} + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 214f829..50a1dee 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Seconds; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; @@ -25,7 +24,6 @@ import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.measure.MutDistance; -import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 42e9ea1..6ffeb43 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.AutoAlign; import frc.robot.commands.LimelightAlign; -import frc.robot.commands.RotateChassis; import frc.robot.commands.ScoreReef; public class AutomatedController { @@ -131,20 +130,20 @@ public void configureManual(){ void configureCharacterisaton(){ - // controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - // controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - // controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - // controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); - - // controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); - // controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); - // controller.leftTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); - // controller.rightTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); - - controller.povUp().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); - controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); - controller.povRight().and(characterise()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); - controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + + controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); + controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); + controller.leftTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); + controller.rightTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + + // controller.povUp().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); + // controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); + // controller.povRight().and(characterise()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); + // controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); } void configureDebug(){ @@ -152,17 +151,26 @@ void configureDebug(){ // controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); // controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); - controller.leftBumper().and(debug()).onTrue(Util.Do(() -> io.elevator.volts(-1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.rightBumper().and(debug()).onTrue(Util.Do(() -> io.elevator.volts(1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.a().and(debug()).onTrue(Util.Do(() -> io.elevator.followVolts(-1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); - controller.b().and(debug()).onTrue(Util.Do(() -> io.elevator.followVolts(1), io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + // double volts = (double) Util.get("Test Elevator Volts", 1); + double volts = 8; + + controller.leftBumper().and(debug()).onTrue(Util.Do(() -> { + io.elevator.volts(-volts / 2); + io.elevator.stopped = true; + }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + controller.rightBumper().and(debug()).onTrue(Util.Do(() -> { + io.elevator.volts(volts); + io.elevator.stopped = true; + }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.x().and(debug()).onTrue(Util.Do(io.elevator::zero, io.elevator)); + controller.a().and(debug()).onTrue(Util.Do(io.elevator::toggleSoftLimits, io.elevator)); + // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); - // controller.y().and(debug()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); - // controller.b().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); - // controller.x().and(debug()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); - // controller.a().and(debug()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); + controller.povUp().and(debug()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); + controller.povLeft().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); + controller.povRight().and(debug()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); + controller.povDown().and(debug()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); // controller.povUp().and(debug()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); // controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); From 59b0b686d0b949dbcada2a2265910e322baf44d7 Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Thu, 20 Mar 2025 23:48:17 -0400 Subject: [PATCH 74/79] HANG TIMEEE! --- src/main/java/frc/robot/subsystems/Hang.java | 37 +++++++++++++++----- 1 file changed, 29 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 7fd2ef7..85d832a 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -1,18 +1,32 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { - public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet - + public SparkMax hang = new SparkMax(16, MotorType.kBrushed); + TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); + Timer time = new Timer(); + double target = 0.0; public Hang() { - // TODO: Configure a soft limit switch - hang.setNeutralMode(NeutralModeValue.Brake); + SparkMaxConfig config = new SparkMaxConfig(); + + config.idleMode(SparkMaxConfig.IdleMode.kBrake); + config.closedLoop.pid(0.3, 0.0, 0.0, ClosedLoopSlot.kSlot0); + hang.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void hangSpeed(double speed) { @@ -27,8 +41,15 @@ public void stopHang() { hang.stopMotor(); } + public double angle() { + return hang.getAbsoluteEncoder().getPosition(); + } + @Override public void periodic() { - SmartDashboard.putNumber("Hang Position", hang.getPosition().getValueAsDouble()); + double cTime = time.get(); + State out = profile.calculate(cTime, new State(angle(), 0), new State(target, 0)); + hang.getClosedLoopController().setReference(out.position, ControlType.kPosition); + SmartDashboard.putNumber("Hang Pos", angle()); } } From c4c2ead8f566e6ec0d7a7cf2fc46a558874f1059 Mon Sep 17 00:00:00 2001 From: Ayidana Date: Sat, 22 Mar 2025 17:30:57 -0400 Subject: [PATCH 75/79] lets do this --- src/main/java/frc/robot/RobotContainer.java | 6 - .../java/frc/robot/commands/DefaultDrive.java | 2 +- src/main/java/frc/robot/commands/Intake.java | 5 - .../java/frc/robot/commands/ScoreReef.java | 15 +- .../java/frc/robot/subsystems/Elevator.java | 81 ++---- .../{LiDAR_PWM.java => LiDARDio.java} | 13 +- .../subsystems/{Claw.java => Shooter.java} | 48 ++-- .../java/frc/robot/subsystems/Swerve.java | 4 +- src/main/java/frc/robot/swerve/Swerve.java | 250 +++++++++--------- .../robot/utility/AutomatedController.java | 126 ++++----- src/main/java/frc/robot/utility/IO.java | 2 +- 11 files changed, 238 insertions(+), 314 deletions(-) rename src/main/java/frc/robot/subsystems/{LiDAR_PWM.java => LiDARDio.java} (65%) rename src/main/java/frc/robot/subsystems/{Claw.java => Shooter.java} (77%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1c4cd4..4d70eef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,12 +53,6 @@ public RobotContainer() { driver_selector.addOption("Debug", 4); driver_selector.onChange( (driver) -> Swerve.Constants.SwitchDriver(driver)); - // DogLog.setOptions(new DogLogOptions() - // .withCaptureDs(true) - // .withCaptureConsole(true) - // .withLogExtras(true)); - - // DogLog.setEnabled( false); // TODO: Turn back on when we're testing proper SignalLogger.setPath("/media/sda1/ctre-logs/"); } diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java index 432d143..f046aa7 100644 --- a/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/src/main/java/frc/robot/commands/DefaultDrive.java @@ -54,7 +54,7 @@ public void execute() { double rotationSpeed = rotation_supplier.getAsDouble() * down_scale * rot_scale; ChassisSpeeds output = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - + if (io.chassis.field_oritented) output = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed, io.chassis.rotation()); diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index d76c273..afeb9f6 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Claw; import frc.robot.utility.IO; /* 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 */ @@ -17,7 +16,6 @@ public class Intake extends Command { Runnable intake; Runnable stop; - Runnable rumble; BooleanSupplier holding; double angle; boolean release; @@ -29,12 +27,10 @@ public class Intake extends Command { // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP public Intake(IO io, boolean release, GenericHID controller) { - rumble = (controller != null) ? () -> controller.setRumble(RumbleType.kBothRumble, .25) : () -> {}; // TODO: Check if it's fine holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); intake = () -> { io.claw.speed((release) ? 1 : .4); - io.claw.angle((release) ? Claw.REEF_ANGLE : Claw.INTAKE_ANGLE); // TODO: See if we need to add an angle for scoring on L4 & Barge }; stop = () -> { @@ -54,7 +50,6 @@ public Intake(IO io, boolean release){ @Override public void initialize() { intake.run(); - rumble.run(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index ab6601e..e73f243 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -4,7 +4,6 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.utility.IO; @@ -12,21 +11,11 @@ public class ScoreReef extends SequentialCommandGroup { - public ScoreReef(IO io, int level, GenericHID controller) { + public ScoreReef(IO io, int reefPosition, int level) { addCommands( + new LimelightAlign(io, reefPosition, false), io.elevator.moveCommand(level), new WaitUntilCommand(io.elevator::atPosition), - new Intake(io, true, controller), // TODO: Set to Reef Scoring Angle - io.elevator.moveCommand(0) - ); - } - - public ScoreReef(IO io, boolean score_right, int level) { - addCommands( - // TODO: Check if we have coral & if we don't have - io.elevator.moveCommand(level), - new WaitUntilCommand(io.elevator::atPosition), - new AutoAlign((score_right) ? 2 : 0, io), Util.Do(() -> io.claw.angle(level), io.claw), new Intake(io, true), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index e93dc41..8dd8a2e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -34,31 +34,27 @@ public class Elevator extends SubsystemBase { TalonFX lead = new TalonFX(11); TalonFX follow = new TalonFX(12); + // LiDARDio LIDAR = new LiDARDio(4); + SparkMaxConfig config = new SparkMaxConfig(); TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); + public boolean stopped = true; Timer time = new Timer(); double target = 0; - public - boolean stopped = true; PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0); - final String[] level_layout = { "Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae" }; + final String[] levelLayout = { "Rest", "L1", "L2", "L3", "L4", "Barge", "Low Algae", "High Algae" }; - // Target Heights + public final double[] Level = { 0, 25, 43.5, 76, 110 }; public final double Rest = 0; - public final double L0 = 0; public final double L1 = 25; public final double L2 = 43.5; public final double L3 = 76; public final double L4 = 110; - public final double Barge = 0; // TODO: FIND BARGE - public final double Low_Algae = 0; // TODO: FIND - public final double High_Algae = 0; // TODO: FIND - public final double MAX_HEIGHT = 0; // TODO: FIND - public boolean softLimits = true; + public boolean softLimits = false; public Elevator() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -67,21 +63,22 @@ public Elevator() { config.Slot0.kD = 0.1; config.Slot0.kG = 0.0; - config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive); + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.SoftwareLimitSwitch .withForwardSoftLimitEnable(softLimits) - .withForwardSoftLimitThreshold(0.0) + .withForwardSoftLimitThreshold(160.0) .withReverseSoftLimitEnable(softLimits) - .withReverseSoftLimitThreshold(160.0); + .withReverseSoftLimitThreshold(0.0); lead.getConfigurator().apply(config); - follow.setControl(new Follower(lead.getDeviceID(), true)); // TODO: Check if we need to invert + follow.setControl(new Follower(lead.getDeviceID(), true)); } public void toggleSoftLimits() { softLimits = !softLimits; - lead.getConfigurator().apply(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(softLimits).withReverseSoftLimitEnable(softLimits)); + lead.getConfigurator().apply( + new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(softLimits).withReverseSoftLimitEnable(softLimits)); } public void speed(double speed) { @@ -92,10 +89,6 @@ public void volts(double volts) { lead.setVoltage(volts); } - public void followVolts(double volts){ - follow.setVoltage(volts); - } - public void stop() { stopped = true; lead.stopMotor(); @@ -110,36 +103,8 @@ public void move(double height) { } public void move(int level) { - switch (level) { - case 0: - move(L0); - break; - case 1: - move(L1); - break; - case 2: - move(L2); - break; - case 3: - move(L3); - break; - case 4: - move(L4); - break; - case 5: - move(Barge); - break; - case 6: - move(Low_Algae); - break; - case 7: - move(High_Algae); - break; - default: - move(Rest); // LEVEL 1 // TODO: See if we need to change the height we go to - break; - } - SmartDashboard.putString("Target Level", level_layout[level]); + move(Level[Math.min(0, Math.max(4, level))]); + SmartDashboard.putString("Target Elevator Level", levelLayout[level]); } public InstantCommand moveCommand(int level) { @@ -160,7 +125,7 @@ public Voltage voltage() { public double position() { // return lead.getPosition().getValueAsDouble() * conversion; - return lead.getPosition().getValueAsDouble(); + return lead.getPosition().getValueAsDouble(); // TODO: Switch to LiDAR } public LinearVelocity velocity() { @@ -189,27 +154,25 @@ public LinearVelocity velocity() { @Override public void periodic() { SmartDashboard.putNumber("Elevator Height", position()); + // SmartDashboard.putNumber("Elevator Height LiDAR", LIDAR.distance()); SmartDashboard.putBoolean("Elevator Soft Limits Active", softLimits); + double cTime = time.get(); + if (stopped) return; - State out = profile.calculate(time.get(), new State(L2, Barge), new State(target, 0)); - lead.setControl(positionRequest.withPosition(out.position)); + State out = profile.calculate(cTime, new State(L2, lead.getVelocity().getValueAsDouble()), new State(target, 0)); + lead.setControl(positionRequest.withPosition(out.position).withEnableFOC(true)); + + stopped = profile.isFinished(cTime); SmartDashboard.putNumber("Elevator Motor Voltage", voltage().magnitude()); - // SmartDashboard.putNumber("Elevator Height", position()); SmartDashboard.putNumber("Elevator Target Height", target); SmartDashboard.putNumber("Elevator cTarget Height", out.position); SmartDashboard.putNumber("Elevator Velocity", velocity().magnitude()); SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); - - // DogLog.log("Elevator/Height", motor.getEncoder().getPosition()); - // DogLog.log("Elevator/Target Height", target); - // DogLog.log("Elevator/cTarget Height", out.position); - // DogLog.log("Elevator/Speed", motor.getEncoder().getVelocity()); - // DogLog.log("Elevator/cTarget Velocity", out.velocity); } } diff --git a/src/main/java/frc/robot/subsystems/LiDAR_PWM.java b/src/main/java/frc/robot/subsystems/LiDARDio.java similarity index 65% rename from src/main/java/frc/robot/subsystems/LiDAR_PWM.java rename to src/main/java/frc/robot/subsystems/LiDARDio.java index 63744a1..369ebff 100644 --- a/src/main/java/frc/robot/subsystems/LiDAR_PWM.java +++ b/src/main/java/frc/robot/subsystems/LiDARDio.java @@ -6,15 +6,13 @@ import edu.wpi.first.wpilibj.Counter; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class LiDAR_PWM extends SubsystemBase { +public class LiDARDio { Counter counter; - public LiDAR_PWM(int address) { - counter = new Counter(new DigitalInput(address)); + public LiDARDio(int port) { + counter = new Counter(new DigitalInput(port)); counter.setMaxPeriod(1.0); counter.setSemiPeriodMode(true); reset(); @@ -31,10 +29,5 @@ public boolean connected() { public void reset() { counter.reset(); } - - @Override - public void periodic() { - SmartDashboard.putNumber("Lidar Distance", distance()); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Shooter.java similarity index 77% rename from src/main/java/frc/robot/subsystems/Claw.java rename to src/main/java/frc/robot/subsystems/Shooter.java index 62544f9..8fcdf54 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -10,17 +10,16 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -28,14 +27,16 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -public class Claw extends SubsystemBase { +public class Shooter extends SubsystemBase { SparkMax intake = new SparkMax(13, MotorType.kBrushless); - SparkFlex pivot = new SparkFlex(14, MotorType.kBrushless); + SparkMax pivot = new SparkMax(14, MotorType.kBrushless); + DutyCycleEncoder encoder = new DutyCycleEncoder(new DigitalInput(3)); // or 4 + DigitalInput beambreak = new DigitalInput(1); + Servo hood = new Servo(0); - public static final double INTAKE_ANGLE = 0; - public static final double REEF_ANGLE = 0; - DigitalInput coralBreak = new DigitalInput(0); + double[] pivotAngle = {0,0,0,0,0}; + double[] hoodAngle = {0,0,0,0,0}; TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); @@ -44,16 +45,15 @@ public class Claw extends SubsystemBase { public boolean intaking = false; - public Claw() { + public Shooter() { SparkMaxConfig config = new SparkMaxConfig(); config.idleMode(SparkMaxConfig.IdleMode.kBrake); intake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - SparkFlexConfig pivotConfig = new SparkFlexConfig(); + SparkMaxConfig pivotConfig = new SparkMaxConfig(); pivotConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); pivotConfig.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); - pivotConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); pivotConfig.softLimit.forwardSoftLimitEnabled(false); pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit @@ -63,8 +63,12 @@ public Claw() { pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } + public void setHood(double v){ + hood.setPosition(v); + } + public void volts(double volts) { - intake.setVoltage(volts); + pivot.setVoltage(volts); } public void speed(double speed) { @@ -76,7 +80,7 @@ public void stop() { } public boolean hasCoral() { - return coralBreak.get(); + return beambreak.get(); } public Voltage voltage() { @@ -84,7 +88,10 @@ public Voltage voltage() { } public double angle() { - return pivot.getAbsoluteEncoder().getPosition(); + // return encoder.get(); + return pivot.getEncoder().getPosition(); + // return (encoder.get() * 360.0 ); + } public void angle(double target_angle) { @@ -93,6 +100,11 @@ public void angle(double target_angle) { time.restart(); } + public void angle(int level){ + angle(pivotAngle[level]); + hood.setAngle(hoodAngle[level]); + } + public void pivotVolts(double volts) { pivot.setVoltage(volts); } @@ -113,12 +125,10 @@ public void pivotVolts(double volts) { @Override public void periodic() { - // DogLog.log("Claw/Algae Full", hasAlgae()); - // DogLog.log("Claw/Coral Full", hasCoral()); - // DogLog.log("Claw/Pivot Angle", angle()); - SmartDashboard.putBoolean("Coral", hasCoral()); SmartDashboard.putNumber("Claw Pivot", angle()); + SmartDashboard.putNumber("Claw Absolute Pivot", encoder.get() * 360); + SmartDashboard.putNumber("Hood Angle", hood.get() * 180); double cTime = time.get(); if (stopped) @@ -126,6 +136,6 @@ public void periodic() { State out = profile.calculate(cTime, new State(angle(), 0), new State(target, 0)); pivot.getClosedLoopController().setReference(out.position, ControlType.kPosition); - stopped = profile.isFinished(cTime); // TODO: check if we haven't introduced any weirdness with this + stopped = profile.isFinished(cTime); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 50a1dee..ab4fabb 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -211,7 +211,8 @@ public void setModuleStates(SwerveModuleState[] states) { log.motor(Constants.LAYOUT_TITLE[0] + " [Drive]") .voltage(modules[0].voltage()) .linearPosition(distance[0].mut_replace(modules[0].drivePosition(), Meters)) - .linearVelocity(modules[0].velocity()); + .linearVelocity(modules[0].velocity()) + .linearAcceleration(pigeon2.getAccelerationX().getValue()); // NOTE: This is due to our gyro being mounted 90 degrees off centre }, this)); public final SysIdRoutine steerRoutine = new SysIdRoutine(new Config( @@ -222,7 +223,6 @@ public void setModuleStates(SwerveModuleState[] states) { new SysIdRoutine.Mechanism(voltage -> { speeds = new ChassisSpeeds(0, 0, (voltage.magnitude()/16.0) * Constants.MAX_ANGULAR_VELOCITY); }, log -> { - // TODO: Test to see if this gives us accurate PID values log.motor("Chassis") .voltage(modules[0].steerVoltage()) .angularPosition(pigeon2.getYaw().getValue()) diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 622ae29..1b97854 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -1,148 +1,148 @@ -package frc.robot.swerve; - -import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.pathplanner.lib.config.RobotConfig; -import com.reduxrobotics.sensors.canandmag.Canandmag; -import com.reduxrobotics.sensors.canandmag.CanandmagSettings; - -import edu.wpi.first.units.measure.AngularVelocity; - -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; - -import java.io.IOException; - -public class Swerve { - - public static final double PI2 = 2.0 * Math.PI; - - public static class Constants { - // BOT SWITCHING - public boolean comp = true; - - public double TRACKWIDTH = 30; // 30.0 for MKi - public double WHEELBASE = 30; // 30.0 for MKi - public double GEAR_RATIO = 8.14; - public double WHEEL_RADIUS = .1143; - - // DRIVER SETTINGS - public static int driver = 0; - public static double transFactor = .65; // factor = x/125, with x being the percentage of our max speed, same for the thing below - public static double rotFactor = .30; // .6 for tristan - - // AUTON CONSTANTS - public double XControllerP = 1.6878; - public double XControllerD = 0; - public double ThetaControllerP = 0; - public double ThetaControllerD = 0; - public RobotConfig autoConfig; - - // BASE CHASSIS CONFIGURATION - public static final double MAX_VELOCITY = 5.4; - public static final double MAX_ANGULAR_VELOCITY = Math.PI/6; - public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; - public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR - public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR - public static double[] ENCODER_OFFSETS = {-0.87890625, -0.996337890625, -0.638427734375, -0.892822265625}; - public static final int PIGEON_ID = 6; - - public Constants(){ - try { - autoConfig = RobotConfig.fromGUISettings(); - } catch (IOException | org.json.simple.parser.ParseException e) { - e.printStackTrace(); + package frc.robot.swerve; + + import com.ctre.phoenix6.configs.MagnetSensorConfigs; + import com.ctre.phoenix6.hardware.CANcoder; + import com.ctre.phoenix6.signals.SensorDirectionValue; + import com.pathplanner.lib.config.RobotConfig; + import com.reduxrobotics.sensors.canandmag.Canandmag; + import com.reduxrobotics.sensors.canandmag.CanandmagSettings; + + import edu.wpi.first.units.measure.AngularVelocity; + + import static edu.wpi.first.units.Units.Radians; + import static edu.wpi.first.units.Units.RadiansPerSecond; + + import java.io.IOException; + + public class Swerve { + + public static final double PI2 = 2.0 * Math.PI; + + public static class Constants { + // BOT SWITCHING + public boolean comp = true; + + public double TRACKWIDTH = 30; // 30.0 for MKi + public double WHEELBASE = 30; // 30.0 for MKi + public double GEAR_RATIO = 8.14; + public double WHEEL_RADIUS = .1143; + + // DRIVER SETTINGS + public static int driver = 0; + public static double transFactor = 1.0; // factor = x/125, with x being the percentage of our max speed, same for the thing below + public static double rotFactor = .30; // .6 for tristan + + // AUTON CONSTANTS + public double XControllerP = 1.6878; + public double XControllerD = 0; + public double ThetaControllerP = 0; + public double ThetaControllerD = 0; + public RobotConfig autoConfig; + + // BASE CHASSIS CONFIGURATION + public static final double MAX_VELOCITY = 5.4; + public static final double MAX_ANGULAR_VELOCITY = Math.PI/6; + public static final String[] LAYOUT_TITLE = { "Front Left", "Front Right", "Back Left", "Back Right" }; + public static final int[] CHASSIS_ID = { 2, 3, 4, 5 }; // FL, FR, BL, BR + public static final int[] ENCODER_ID = { 7, 8, 9, 10 }; // FL, FR, BL, BR + public static double[] ENCODER_OFFSETS = {-0.87890625, -0.996337890625, -0.638427734375, -0.892822265625}; + public static final int PIGEON_ID = 6; + + public Constants(){ + try { + autoConfig = RobotConfig.fromGUISettings(); + } catch (IOException | org.json.simple.parser.ParseException e) { + e.printStackTrace(); + } + SwitchDriver(driver); } - SwitchDriver(driver); - } - public static void SwitchDriver(int driver){ - switch (driver) { - case 4: // DEBUG - break; - - case 3: // Uriel - break; - - case 2: // Jason - break; - - case 1: // Norah - break; - - default: // Shaan - transFactor = .65; - rotFactor = .5; + public static void SwitchDriver(int driver){ + switch (driver) { + case 4: // DEBUG + break; + + case 3: // Uriel break; + + case 2: // Jason + break; + + case 1: // Norah + break; + + default: // Shaan is a king + transFactor = 1.0; + rotFactor = .5; + break; + } } } - } - public interface Encoder { - public void zero(); - public boolean connected(); - public double angle(); - public AngularVelocity velocity(); + public interface Encoder { + public void zero(); + public boolean connected(); + public double angle(); + public AngularVelocity velocity(); - } + } - public static class Cancoder implements Encoder { + public static class Cancoder implements Encoder { - CANcoder encoder; - MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs(); + CANcoder encoder; + MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs(); - public Cancoder(int id) { - encoder = new CANcoder(id); - magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - magnetConfig.AbsoluteSensorDiscontinuityPoint = 1.0; - magnetConfig.withMagnetOffset(Swerve.Constants.ENCODER_OFFSETS[id-7]); - encoder.getConfigurator().apply(magnetConfig); - } + public Cancoder(int id) { + encoder = new CANcoder(id); + magnetConfig.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + magnetConfig.AbsoluteSensorDiscontinuityPoint = 1.0; + magnetConfig.withMagnetOffset(Swerve.Constants.ENCODER_OFFSETS[id-7]); + encoder.getConfigurator().apply(magnetConfig); + } - public void zero() { - magnetConfig.MagnetOffset = -encoder.getAbsolutePosition().getValueAsDouble(); - encoder.getConfigurator().apply(magnetConfig); - } + public void zero() { + magnetConfig.MagnetOffset = -encoder.getAbsolutePosition().getValueAsDouble(); + encoder.getConfigurator().apply(magnetConfig); + } - public boolean connected(){ - return encoder.isConnected(); - } + public boolean connected(){ + return encoder.isConnected(); + } - public double angle() { - return ((encoder.getAbsolutePosition().getValue().in(Radians) + PI2) % PI2); - } + public double angle() { + return ((encoder.getAbsolutePosition().getValue().in(Radians) + PI2) % PI2); + } - public AngularVelocity velocity(){ - return encoder.getVelocity().getValue(); + public AngularVelocity velocity(){ + return encoder.getVelocity().getValue(); + } } - } - public static class Canand implements Encoder { - Canandmag encoder; + public static class Canand implements Encoder { + Canandmag encoder; - public Canand(int id) { - encoder = new Canandmag(id); - CanandmagSettings settings = new CanandmagSettings(); - settings.setInvertDirection(true); - encoder.clearStickyFaults(); - encoder.setSettings(settings); - } + public Canand(int id) { + encoder = new Canandmag(id); + CanandmagSettings settings = new CanandmagSettings(); + settings.setInvertDirection(true); + encoder.clearStickyFaults(); + encoder.setSettings(settings); + } - public void zero() { - encoder.setAbsPosition(0, 250); - } + public void zero() { + encoder.setAbsPosition(0, 250); + } - public boolean connected(){ - return encoder.isConnected(); - } + public boolean connected(){ + return encoder.isConnected(); + } - public double angle() { - return (encoder.getAbsPosition() * PI2) % PI2; - } + public double angle() { + return (encoder.getAbsPosition() * PI2) % PI2; + } - public AngularVelocity velocity(){ - return RadiansPerSecond.of(encoder.getVelocity()); + public AngularVelocity velocity(){ + return RadiansPerSecond.of(encoder.getVelocity()); + } } } -} diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 6ffeb43..28d91f3 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -1,14 +1,14 @@ package frc.robot.utility; import java.util.function.BooleanSupplier; +import java.util.function.IntSupplier; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.commands.AutoAlign; -import frc.robot.commands.LimelightAlign; +import frc.robot.commands.Intake; import frc.robot.commands.ScoreReef; public class AutomatedController { @@ -35,10 +35,10 @@ public AutomatedController(int port, IO io){ controller.rightStick().onTrue(new InstantCommand(() -> io.chassis.field_oritented = !io.chassis.field_oritented)); controller.leftStick().debounce(2).onTrue(new InstantCommand(io.chassis::resetAngle)); + configure(); // controller.back().onTrue(Util.Do(io.elevator::rest)); // controller.start().onTrue(Util.Do(io.elevator::zero)); - configureManual(); - configureDebug(); + } public BooleanSupplier mode(int targetMode){ @@ -66,93 +66,75 @@ public void switchMode(){ } public void configure(){ - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); - controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); - - // controller.leftBumper().onTrue(new SimpleAlign(io, false)); - // controller.rightBumper().onTrue(new SimpleAlign(io, true)); - - // controller.leftBumper().onTrue(new RotateChassis(io, 45)); - // controller.leftBumper().toggleOnTrue(new LimelightAlign(io, 0)); - // controller.rightBumper().toggleOnTrue(new LimelightAlign(io, 2)); - - // AUTOMATED - - // Based on the nearest element and our field orientation - // LB align Left and Score Coral & Score Barge - // RB align Right and Score Coral & Score Processor - - controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); - controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); - - // MANUAL - // RB align Right and Score Coral & Score Processor - // controller.y().and( automated() ).onTrue(Util.D - controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); - controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect - - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect + configureAutomated(); + configureManual(); + configureCharacterisaton(); + configureDebug(); } void configureAutomated(){ - controller.leftBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 1, false))); - controller.rightBumper().and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 2, false))); - // controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(Util.Do(() -> new Intake(io, true, false, rumble))); + IntSupplier pos = () -> { return ((controller.getHID().getLeftBumperButtonPressed()) ? -1 : 0) + ((controller.getHID().getLeftBumperButtonPressed()) ? 1 : 0); }; + // controller.leftBumper().and(controller.getHID()::getRightBumperButtonPressed).and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 0, false))); - controller.a().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 1, rumble))); - controller.b().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 3, rumble))); - controller.x().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 2, rumble))); - controller.y().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, 4, rumble))); - - // controller.povLeft().and(automated()).onTrue(Util.Do(() -> new Intake(io, false, true, rumble))); + controller.y().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4))); + controller.x().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3))); + controller.b().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2))); + controller.a().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1))); + controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(new Intake(io, false)); controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0))); - controller.back().and(automated()).onTrue(Util.Do(() -> io.chassis.resetOdometry())); + controller.back().onTrue(Util.Do(io.chassis::resetAngle, io.chassis)); } - public void configureManual(){ - - controller.start().and(controller.getHID()::getBackButtonPressed).onTrue(Util.Do(this::switchMode)); - controller.back().onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + double direction = 1; - // controller.leftBumper().and(automated()).toggleOnTrue(new AutoAlign(0, io)); - // controller.rightBumper().and(automated()).toggleOnTrue(new AutoAlign(2, io)); + public void configureManual(){ + controller.back().and(manual()).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); - controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); - controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); - controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect + // controller.y().and(manual()).onTrue(Util.Do(() -> io.elevator.move(4))); + controller.x().and(manual()).onTrue(Util.Do(() -> io.elevator.move(3))); + controller.b().and(manual()).onTrue(Util.Do(() -> io.elevator.move(2))); + controller.a().and(manual()).onTrue(Util.Do(() -> io.elevator.move(1))); - // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new Rumble(0, .5, controller.getHID(), io.chassis::zeroAbsolute)); // Add the Rumble effect + controller.leftBumper().onTrue(Util.Do(() -> io.claw.speed(direction * .4), io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); + controller.rightBumper().onTrue(Util.Do(() -> io.claw.speed(direction * 1), io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); + controller.y().and(manual()).onTrue(Util.Do(() -> { direction = -direction;})); + + // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); + // controller.povLeft().and( manual() ).onTrue(Util.Do(io.chassis::syncEncoders)); + // controller.povRight().and( manual() ).and(() -> {return !io.chassis.active;}).onTrue(new InstantCommand(io.chassis::zeroAbsolute)); // Add the Rumble effect } void configureCharacterisaton(){ - controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); - controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); - controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); - controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + // controller.x().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + // controller.a().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + // controller.y().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + // controller.b().and(characterise()).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + + // controller.povUp().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); + // controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); + // controller.povRight().and(characterise()).toggleOnTrue(io.steerRoutine.dynamic(Direction.kForward)); + // controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); + + controller.x().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.quasistatic(Direction.kForward)); + controller.a().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.quasistatic(Direction.kReverse)); + controller.y().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.dynamic(Direction.kForward)); + controller.b().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.dynamic(Direction.kReverse)); controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); controller.leftTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); controller.rightTrigger().and(characterise()).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); - // controller.povUp().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kForward)); - // controller.povDown().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.quasistatic(Direction.kReverse)); - // controller.povRight().and(characterise()).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); - // controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); } void configureDebug(){ - // controller.rightTrigger().and(debug()).toggleOnTrue(new Aimbot(io)); - // controller.leftBumper().and(debug()).toggleOnTrue(new AutoAlign(0, io)); - // controller.rightBumper().and(debug()).toggleOnTrue(new AutoAlign(2, io)); + controller.back().and(debug()).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); - // double volts = (double) Util.get("Test Elevator Volts", 1); - double volts = 8; + double volts = 3; controller.leftBumper().and(debug()).onTrue(Util.Do(() -> { io.elevator.volts(-volts / 2); @@ -162,19 +144,17 @@ void configureDebug(){ io.elevator.volts(volts); io.elevator.stopped = true; }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); + + controller.y().and(debug()).onTrue(Util.Do(() -> { + io.claw.volts(volts); + }, io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); + + controller.x().and(debug()).onTrue(Util.Do(io.elevator::zero, io.elevator)); controller.a().and(debug()).onTrue(Util.Do(io.elevator::toggleSoftLimits, io.elevator)); - - // controller.x().and(debug_setting()).toggleOnTrue(new LimelightAlign(io, 1, false)); - - controller.povUp().and(debug()).onTrue(Util.Do( () -> io.elevator.move(4),io.elevator)); - controller.povLeft().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); + controller.povLeft().and(debug()).onTrue(Util.Do( () -> io.elevator.move(3),io.elevator)); controller.povRight().and(debug()).onTrue(Util.Do( () -> io.elevator.move(2),io.elevator)); controller.povDown().and(debug()).onTrue(Util.Do( () -> io.elevator.move(1),io.elevator)); - // controller.povUp().and(debug()).onTrue(Util.Do(() -> io.elevator.move(5),io.elevator)); - - // controller.povLeft().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(4), io.elevator)); - // controller.povDown().and(debug_setting()).onTrue(Util.Do(() -> io.elevator.volts(-4), io.elevator)); } } diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index a2aa5b5..76a9797 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -9,7 +9,7 @@ public class IO extends SubsystemBase { public final Elevator elevator = new Elevator(); public final Limelight limelight = new Limelight(); - public final Claw claw = null; + public final Shooter claw = new Shooter(); public final Hang hang = null; public IO() { From bc9879461461dcbc36d9fa0007f6bc30d2567a8e Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Wed, 26 Mar 2025 23:43:26 -0400 Subject: [PATCH 76/79] Button Bindings for new Controller almost fully setup --- src/main/java/frc/robot/RobotContainer.java | 17 +- .../java/frc/robot/commands/ClearAlgae.java | 30 +++ src/main/java/frc/robot/commands/Intake.java | 6 +- .../java/frc/robot/commands/ScoreReef.java | 2 +- src/main/java/frc/robot/subsystems/Hang.java | 37 ++-- .../java/frc/robot/subsystems/Shooter.java | 32 ++- .../robot/utility/AutomatedController.java | 30 ++- src/main/java/frc/robot/utility/IO.java | 2 +- .../frc/robot/utility/ModeController.java | 190 ++++++++++++++++++ 9 files changed, 297 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ClearAlgae.java create mode 100644 src/main/java/frc/robot/utility/ModeController.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4d70eef..df1435b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import frc.robot.commands.DefaultDrive; import frc.robot.commands.RotateChassis; +import frc.robot.commands.ScoreReef; import frc.robot.swerve.Swerve; import frc.robot.utility.AutomatedController; import frc.robot.utility.IO; @@ -66,15 +67,15 @@ public void configureAuton(){ NamedCommands.registerCommand("Rotate CD", new RotateChassis(io, 60)); NamedCommands.registerCommand("Rotate EF", new RotateChassis(io, 120)); - // NamedCommands.registerCommand("Score L-L4", new ScoreReef(io, false, 4)); - // NamedCommands.registerCommand("Score L-L3", new ScoreReef(io, false, 3)); - // NamedCommands.registerCommand("Score L-L2", new ScoreReef(io, false, 2)); - // NamedCommands.registerCommand("Score L-L1", new ScoreReef(io, false, 1)); + NamedCommands.registerCommand("Score L-L4", new ScoreReef(io, 0, 4)); + NamedCommands.registerCommand("Score L-L3", new ScoreReef(io, 0, 3)); + NamedCommands.registerCommand("Score L-L2", new ScoreReef(io, 0, 2)); + NamedCommands.registerCommand("Score L-L1", new ScoreReef(io, 0, 1)); - // NamedCommands.registerCommand("Score R-L4", new ScoreReef(io, true, 4)); - // NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, true, 3)); - // NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, true, 2)); - // NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, true, 1)); + NamedCommands.registerCommand("Score R-L4", new ScoreReef(io, 2, 4)); + NamedCommands.registerCommand("Score R-L3", new ScoreReef(io, 2, 3)); + NamedCommands.registerCommand("Score R-L2", new ScoreReef(io, 2, 2)); + NamedCommands.registerCommand("Score R-L1", new ScoreReef(io, 2, 1)); // NamedCommands.registerCommand("Clear Low Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND // NamedCommands.registerCommand("Clear High Algae", new Intake(io, false)); // TODO: MAKE ACTUAL COMMAND diff --git a/src/main/java/frc/robot/commands/ClearAlgae.java b/src/main/java/frc/robot/commands/ClearAlgae.java new file mode 100644 index 0000000..e896c9c --- /dev/null +++ b/src/main/java/frc/robot/commands/ClearAlgae.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.utility.IO; +import frc.robot.utility.Util; + +public class ClearAlgae extends SequentialCommandGroup { + + public ClearAlgae(IO io) { + addCommands( + io.elevator.moveCommand(0), + new WaitUntilCommand(.2), // Wait to be at the bottom + Util.Do(() -> { + io.shooter.angle(0); + io.shooter.speed(-1); + }, io.shooter), // TODO: See if we need a seperate angle for algae clearing + io.elevator.moveCommand(4), // TODO: See if we can get away with L3 height + new WaitUntilCommand(.5), // Wait to be at the Top + Util.Do(() -> { + io.shooter.speed(0); + }, io.shooter), + io.elevator.moveCommand(0) // TODO: See if we can get away with L3 height + ); + } +} diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index afeb9f6..652a5d9 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -27,14 +27,14 @@ public class Intake extends Command { // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP public Intake(IO io, boolean release, GenericHID controller) { - holding = () -> (release) ? io.claw.hasCoral() : !io.claw.hasCoral(); + holding = () -> (release) ? io.shooter.hasCoral() : !io.shooter.hasCoral(); intake = () -> { - io.claw.speed((release) ? 1 : .4); + io.shooter.speed((release) ? 1 : .4); }; stop = () -> { - io.claw.stop(); + io.shooter.stop(); controller.setRumble(RumbleType.kBothRumble, 0.0); }; diff --git a/src/main/java/frc/robot/commands/ScoreReef.java b/src/main/java/frc/robot/commands/ScoreReef.java index e73f243..2e8209e 100644 --- a/src/main/java/frc/robot/commands/ScoreReef.java +++ b/src/main/java/frc/robot/commands/ScoreReef.java @@ -16,7 +16,7 @@ public ScoreReef(IO io, int reefPosition, int level) { new LimelightAlign(io, reefPosition, false), io.elevator.moveCommand(level), new WaitUntilCommand(io.elevator::atPosition), - Util.Do(() -> io.claw.angle(level), io.claw), + Util.Do(() -> io.shooter.angle(level), io.shooter), new Intake(io, true), // TODO: Set to Reef Scoring Angle io.elevator.moveCommand(0) ); diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index 7fd2ef7..29ca9bb 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -1,34 +1,47 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hang extends SubsystemBase { - public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet + // public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet + SparkMax motor = new SparkMax(16, MotorType.kBrushed); // CIM + public boolean up = false; + double upPosition = 0; // TODO: FIND UP POSITION + double downPosition = 0; public Hang() { - // TODO: Configure a soft limit switch - hang.setNeutralMode(NeutralModeValue.Brake); + motor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } - public void hangSpeed(double speed) { - hang.set(speed); + public void speed(double speed) { + motor.set(speed); } - public void hangVoltage(double volts) { - hang.setVoltage(volts); + public void volts(double volts) { + motor.setVoltage(volts); } - public void stopHang() { - hang.stopMotor(); + public void stop() { + motor.stopMotor(); + } + + public void toggleHang(){ + up = !up; + motor.getClosedLoopController().setReference(((up) ? upPosition : downPosition), ControlType.kPosition); } @Override public void periodic() { - SmartDashboard.putNumber("Hang Position", hang.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Hang Position", motor.getEncoder().getPosition()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8fcdf54..8b1e9a1 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -35,8 +35,8 @@ public class Shooter extends SubsystemBase { Servo hood = new Servo(0); - double[] pivotAngle = {0,0,0,0,0}; - double[] hoodAngle = {0,0,0,0,0}; + double[] pivotAngle = {0,0,0,0,0, 0}; // last one FOR BARGE + double[] hoodAngle = {0,0,0,0,0, 0}; TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); @@ -63,26 +63,46 @@ public Shooter() { pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - public void setHood(double v){ + public void hood(double v){ hood.setPosition(v); } + public void hoodSpeed(double v){ + hood.setSpeed(v); + } + public void volts(double volts) { - pivot.setVoltage(volts); + intake.setVoltage(volts); } public void speed(double speed) { intake.setVoltage(speed); } + public void pivotVolts(double volts) { + pivot.setVoltage(volts); + } + + public void pivotSpeed(double speed) { + pivot.setVoltage(speed); + } + public void stop() { intake.stopMotor(); } + public void stopPivot(){ + pivot.stopMotor(); + } + public boolean hasCoral() { return beambreak.get(); } + public boolean hasAlgae() { + return beambreak.get(); + } + public Voltage voltage() { return Volts.of(pivot.getBusVoltage()); } @@ -105,10 +125,6 @@ public void angle(int level){ hood.setAngle(hoodAngle[level]); } - public void pivotVolts(double volts) { - pivot.setVoltage(volts); - } - public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config( null, Volts.of(4), diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 28d91f3..7f86853 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -22,7 +22,6 @@ public class AutomatedController { public AutomatedController(int port, IO io){ this.io = io; - selector.setDefaultOption("Automated", () -> {mode = 0;}); selector.addOption("Manual", () -> {mode = 1;}); selector.addOption("Characterise", () -> {mode = 2;}); @@ -75,14 +74,13 @@ public void configure(){ void configureAutomated(){ - IntSupplier pos = () -> { return ((controller.getHID().getLeftBumperButtonPressed()) ? -1 : 0) + ((controller.getHID().getLeftBumperButtonPressed()) ? 1 : 0); }; - // controller.leftBumper().and(controller.getHID()::getRightBumperButtonPressed).and(automated()).onTrue(Util.Do(() -> new LimelightAlign(io, 0, false))); + IntSupplier pos = () -> { return ((controller.getHID().getLeftBumperButtonPressed()) ? -1 : 0) + ((controller.getHID().getLeftBumperButtonPressed()) ? 1 : 0) + 1; }; - controller.y().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4))); - controller.x().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3))); - controller.b().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2))); - controller.a().and(automated()).and(() -> io.claw.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1))); - controller.a().and(automated()).and(() -> !io.claw.hasCoral()).onTrue(new Intake(io, false)); + controller.y().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4))); + controller.x().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3))); + controller.b().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2))); + controller.a().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1))); + controller.a().and(automated()).and(() -> !io.shooter.hasCoral()).onTrue(new Intake(io, false)); controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0))); controller.back().onTrue(Util.Do(io.chassis::resetAngle, io.chassis)); @@ -98,8 +96,8 @@ public void configureManual(){ controller.b().and(manual()).onTrue(Util.Do(() -> io.elevator.move(2))); controller.a().and(manual()).onTrue(Util.Do(() -> io.elevator.move(1))); - controller.leftBumper().onTrue(Util.Do(() -> io.claw.speed(direction * .4), io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); - controller.rightBumper().onTrue(Util.Do(() -> io.claw.speed(direction * 1), io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); + controller.leftBumper().onTrue(Util.Do(() -> io.shooter.speed(direction * .4), io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter)); + controller.rightBumper().onTrue(Util.Do(() -> io.shooter.speed(direction * 1), io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter)); controller.y().and(manual()).onTrue(Util.Do(() -> { direction = -direction;})); // controller.povDown().and( manual() ).onTrue(Util.Do(io.chassis::toggle)); @@ -119,10 +117,10 @@ void configureCharacterisaton(){ // controller.povRight().and(characterise()).toggleOnTrue(io.steerRoutine.dynamic(Direction.kForward)); // controller.povLeft().and(characterise()).toggleOnTrue(io.chassis. steerRoutine.dynamic(Direction.kReverse)); - controller.x().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.quasistatic(Direction.kForward)); - controller.a().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.quasistatic(Direction.kReverse)); - controller.y().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.dynamic(Direction.kForward)); - controller.b().and(characterise()).toggleOnTrue(io.claw.pivotRoutine.dynamic(Direction.kReverse)); + controller.x().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kForward)); + controller.a().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kReverse)); + controller.y().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kForward)); + controller.b().and(characterise()).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kReverse)); controller.leftBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); controller.rightBumper().and(characterise()).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); @@ -146,8 +144,8 @@ void configureDebug(){ }, io.elevator)).onFalse(Util.Do(() -> io.elevator.volts(0), io.elevator)); controller.y().and(debug()).onTrue(Util.Do(() -> { - io.claw.volts(volts); - }, io.claw)).onFalse(Util.Do(() -> io.claw.volts(0), io.claw)); + io.shooter.volts(volts); + }, io.shooter)).onFalse(Util.Do(() -> io.shooter.volts(0), io.shooter)); controller.x().and(debug()).onTrue(Util.Do(io.elevator::zero, io.elevator)); diff --git a/src/main/java/frc/robot/utility/IO.java b/src/main/java/frc/robot/utility/IO.java index 76a9797..5180860 100644 --- a/src/main/java/frc/robot/utility/IO.java +++ b/src/main/java/frc/robot/utility/IO.java @@ -9,7 +9,7 @@ public class IO extends SubsystemBase { public final Elevator elevator = new Elevator(); public final Limelight limelight = new Limelight(); - public final Shooter claw = new Shooter(); + public final Shooter shooter = new Shooter(); public final Hang hang = null; public IO() { diff --git a/src/main/java/frc/robot/utility/ModeController.java b/src/main/java/frc/robot/utility/ModeController.java new file mode 100644 index 0000000..7d002ad --- /dev/null +++ b/src/main/java/frc/robot/utility/ModeController.java @@ -0,0 +1,190 @@ +package frc.robot.utility; + +import java.util.function.BooleanSupplier; +import java.util.function.IntSupplier; + +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.commands.ClearAlgae; +import frc.robot.commands.Intake; +import frc.robot.commands.ScoreReef; + +public class ModeController { + IO io; + public final CommandGenericHID controller; + public final SendableChooser selector = new SendableChooser(); + + int mode = 0; + + final int AUTOMATED = 0; + final int MANUAL = 1; + final int DEMO = 2; + + final int CHARACTERISATION = 3; + final int DEBUG = 4; + + enum povDirection { + Up, + Down, + + Left, + LeftUp, + LeftDown, + + Right, + RightUp, + RightDown, + + Centre, + } + + final int[] directions = {0, 180, 270, 315, 225, 90, 45, 135, -1}; + + + public ModeController(IO io, int port){ + this.io = io; + controller = new CommandGenericHID(port); + selector.setDefaultOption("Automated", () -> {mode = 0;}); + selector.addOption("Manual", () -> {mode = 1;}); + selector.addOption("Characterise", () -> {mode = 2;}); + selector.addOption("Debug", () -> {mode = 3;}); + selector.onChange((x) -> {x.run();}); + + configureGeneral(); + configureAutomated(); + configureManual(); + configureCharacterisaton(); + configureDebug(); + } + + public void toggleMode(){ + mode = (mode + 1 % 3); + } + + public Trigger binding(Button button, int targetMode, BooleanSupplier... conditions){ + var binding = controller.button(button.value).and(() -> (mode == targetMode)); + for (int i = 0; i < conditions.length; i++) + binding.and(conditions[i]); + return binding; + } + + public Trigger binding(Axis axis, int targetMode, BooleanSupplier... conditions){ + var binding = controller.axisMagnitudeGreaterThan(axis.value, 0.2).and(() -> (mode == targetMode)); + for (int i = 0; i < conditions.length; i++) + binding.and(conditions[i]); + return binding; + } + + public Trigger binding(povDirection direction, int targetMode, BooleanSupplier... conditions){ + var binding = controller.pov(directions[direction.ordinal()]).and(() -> (mode == targetMode)); + for (int i = 0; i < conditions.length; i++) + binding.and(conditions[i]); + return binding; + } + + public void configureGeneral(){ + controller.button(Button.kBack.value).onTrue(Util.Do(io.chassis::resetAngle, io.chassis)); + } + + public void configureAutomated(){ + IntSupplier pos = () -> { return ((controller.getHID().getRawButtonPressed(Button.kLeftBumper.value)) ? -1 : 0) + ((controller.getHID().getRawButtonPressed(Button.kRightBumper.value)) ? 1 : 0) + 1; }; + binding(Button.kY, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 4)); + binding(Button.kX, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 3)); + binding(Button.kB, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 2)); + binding(Button.kA, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 1)); + binding(Button.kA, AUTOMATED, () -> !io.shooter.hasCoral()).onTrue(new Intake(io, false)); + binding(Button.kStart, AUTOMATED).onTrue(Util.Do(() -> io.elevator.move(0))); + + binding(povDirection.Up, AUTOMATED, () -> !io.shooter.hasAlgae()).onTrue(new ClearAlgae(io)); + binding(povDirection.Up, AUTOMATED, () -> io.shooter.hasAlgae()).onTrue(Util.Do(() ->io.shooter.speed(1), io.shooter)).onFalse(Util.Do(() -> { + io.shooter.stop(); + io.shooter.angle(0); + io.elevator.move(0); + }, io.shooter)); + + // TODO: Check if this works as expected + binding(povDirection.Left, AUTOMATED, () -> !io.shooter.hasAlgae()).toggleOnTrue(Util.Do(() -> { + io.shooter.speed(-1); + io.shooter.angle(0); + }, io.shooter)).toggleOnFalse(Util.Do(() -> { + io.shooter.stop(); + io.shooter.angle(0); + }, io.shooter)); + binding(povDirection.Left, AUTOMATED, () -> io.shooter.hasAlgae()).onTrue(io.elevator.moveCommand(5)); + + binding(povDirection.Down,AUTOMATED).onTrue(Util.Do(io.hang::toggleHang, io.hang)); + } + + public void configureManual(){ + binding(Button.kBack, MANUAL).onTrue(Util.Do(io.chassis::resetOdometry, io.chassis)); + binding(Button.kLeftBumper, MANUAL).onTrue(Util.Do(() ->io.elevator.volts(-4), io.elevator)).onFalse(Util.Do(io.elevator::stop, io.elevator)); + binding(Button.kRightBumper, MANUAL).onTrue(Util.Do(() ->io.elevator.volts(4), io.elevator)).onFalse(Util.Do(io.elevator::stop, io.elevator)); + + // TODO: Check if this is safe + binding(Axis.kLeftTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.speed(controller.getRawAxis(Axis.kLeftTrigger.value)))); + binding(Axis.kRightTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.pivotSpeed(controller.getRawAxis(Axis.kRightTrigger.value)))); + + binding(Button.kY, MANUAL).onTrue(Util.Do(() -> io.shooter.speed(-1), io.shooter)).onFalse(Util.Do(io.shooter::stop, io.shooter)); + binding(Button.kX, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter)); + binding(Button.kA, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(-.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter)); + + binding(povDirection.Down, MANUAL).onTrue(Util.Do(io.chassis::toggle)); + binding(povDirection.Left, MANUAL).onTrue(Util.Do(io.chassis::syncEncoders)); + binding(povDirection.Right, MANUAL, () -> !io.chassis.active).onTrue(Util.Do(io.chassis::zeroAbsolute)); // TODO: Add rumble effect + } + + boolean mechanismMode = false; + + public void configureCharacterisaton(){ + binding(Button.kStart, CHARACTERISATION).onTrue(Util.Do(() -> mechanismMode = !mechanismMode)); + + binding(Button.kX, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kForward)); + binding(Button.kA, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.quasistatic(Direction.kReverse)); + binding(Button.kY, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kForward)); + binding(Button.kB, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.driveRoutine.dynamic(Direction.kReverse)); + + binding(povDirection.Left, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.quasistatic(Direction.kForward)); + binding(povDirection.Down, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.quasistatic(Direction.kReverse)); + binding(povDirection.Right, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kReverse)); + binding(povDirection.Up, CHARACTERISATION, () -> !mechanismMode).toggleOnTrue(io.chassis.steerRoutine.dynamic(Direction.kForward)); + + binding(Button.kX, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kForward)); + binding(Button.kA, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.quasistatic(Direction.kReverse)); + binding(Button.kY, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kForward)); + binding(Button.kB, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.shooter.pivotRoutine.dynamic(Direction.kReverse)); + + binding(povDirection.Left, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kForward)); + binding(povDirection.Down, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.quasistatic(Direction.kReverse)); + binding(povDirection.Up, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.dynamic(Direction.kForward)); + binding(povDirection.Right, CHARACTERISATION, () -> mechanismMode).toggleOnTrue(io.elevator.routine.dynamic(Direction.kReverse)); + } + + public void configureDebug(){ + // TODO: Setup Smart Dashboard buttons to pass in the Level for the elevator + // TODO: Setup a shuffleboard thing to set the claw Angle based on Level (use the adjustable for the angles) + // TODO: Setup a Shuffleboard thing to move the bot to particular States (Ground intake, no Algae, etc) + // TODO: Setup Manual Movement of key mechanisms + // TODO: Setup a toggle for Softlimits on Elevator Height, Shooter Angle, and Maybe Hang + + // TODO: Setup a toggle for Shooter Pivot Redundancy + // TODO: Setup a toggle for Coral Detection Redundancy + // TODO: Setup a toggle for Algae Detection Redundancy + // TODO: Setup a toggle for Elevator Redundancy + // TODO: Setup a toggle for Module Encoder Redundancy + // TODO: Setup a toggle for Module Redundancy / Redundant Kinemtics + + // TODO: Setup Adjustable Height Locations for each level + // TODO: Setup Adjustable Shooter Angles for each Level + // TODO: Setup adjustable Hood angles for each Level + // TODO: Setup adjustable Hang Positions + + // TODO: Setup Adjustable PID for Elevator + // TODO: Setup Adjustable PID for Shooter Pivot + // TODO: Maybe add a PID for target RPM + // TODO: Setup Adjustable PID for Hang + } +} From e974d7996024d92ea647a81f003ea5c6945e3b81 Mon Sep 17 00:00:00 2001 From: RestoneAGX Date: Tue, 1 Apr 2025 23:12:49 -0400 Subject: [PATCH 77/79] Beginnings started for Shooter Redundancy | Pivot redundancy and Coral Sensing Currently Drafted out --- src/main/java/frc/robot/commands/Intake.java | 4 +- .../java/frc/robot/subsystems/Shooter.java | 72 +++++++++++++------ .../robot/utility/AutomatedController.java | 10 +-- .../frc/robot/utility/ModeController.java | 44 ++++++------ 4 files changed, 78 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/commands/Intake.java b/src/main/java/frc/robot/commands/Intake.java index 652a5d9..bf46867 100644 --- a/src/main/java/frc/robot/commands/Intake.java +++ b/src/main/java/frc/robot/commands/Intake.java @@ -27,14 +27,14 @@ public class Intake extends Command { // public static int INTAKE_ALGAE_GROUND = -3; // THIS WILL MATTER ONLY IF WE DO GROUND PICKUP public Intake(IO io, boolean release, GenericHID controller) { - holding = () -> (release) ? io.shooter.hasCoral() : !io.shooter.hasCoral(); + holding = () -> (release) ? io.shooter.coral() : !io.shooter.coral(); intake = () -> { io.shooter.speed((release) ? 1 : .4); }; stop = () -> { - io.shooter.stop(); + io.shooter.stopIntake(); controller.setRumble(RumbleType.kBothRumble, 0.0); }; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8b1e9a1..913ba67 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -5,6 +5,9 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -18,7 +21,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -28,10 +30,18 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; public class Shooter extends SubsystemBase { + SparkMax intake = new SparkMax(13, MotorType.kBrushless); SparkMax pivot = new SparkMax(14, MotorType.kBrushless); - DutyCycleEncoder encoder = new DutyCycleEncoder(new DigitalInput(3)); // or 4 - DigitalInput beambreak = new DigitalInput(1); + + // DutyCycleEncoder encoder = new DutyCycleEncoder(new DigitalInput(3)); // or 4 + DoubleSupplier[] position; + + DigitalInput beam = new DigitalInput(1); + int currentCoralSensor = 0; + BooleanSupplier[] coral; + String[] coralSensingDisplay = {"Beam Break", "RPM"}; + Servo hood = new Servo(0); @@ -61,6 +71,9 @@ public Shooter() { pivotConfig.softLimit.reverseSoftLimit(0); // TODO: Find the Reverse soft limit pivot.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + position = new DoubleSupplier[]{pivot.getEncoder()::getPosition, pivot.getAbsoluteEncoder()::getPosition}; + coral = new BooleanSupplier[]{beam::get}; //TODO: Have the second option be RPM sensing } public void hood(double v){ @@ -71,23 +84,23 @@ public void hoodSpeed(double v){ hood.setSpeed(v); } - public void volts(double volts) { - intake.setVoltage(volts); + public void volts(double v) { + intake.setVoltage(v); } - public void speed(double speed) { - intake.setVoltage(speed); + public void speed(double s) { + intake.set(s); } - public void pivotVolts(double volts) { - pivot.setVoltage(volts); + public void pivotVolts(double v) { + pivot.setVoltage(v); } - public void pivotSpeed(double speed) { - pivot.setVoltage(speed); + public void pivotSpeed(double s) { + pivot.set(s); } - public void stop() { + public void stopIntake() { intake.stopMotor(); } @@ -95,23 +108,25 @@ public void stopPivot(){ pivot.stopMotor(); } - public boolean hasCoral() { - return beambreak.get(); + public void stop(){ + intake.stopMotor(); + pivot.stopMotor(); } - public boolean hasAlgae() { - return beambreak.get(); + public boolean coral() { + return coral[currentCoralSensor].getAsBoolean(); } + // public boolean algae() { + // return beam.get(); + // } + public Voltage voltage() { return Volts.of(pivot.getBusVoltage()); } public double angle() { - // return encoder.get(); - return pivot.getEncoder().getPosition(); - // return (encoder.get() * 360.0 ); - + return position[absoluteConnected()].getAsDouble(); } public void angle(double target_angle) { @@ -125,6 +140,10 @@ public void angle(int level){ hood.setAngle(hoodAngle[level]); } + public int absoluteConnected(){ + return (pivot.getAbsoluteEncoder().getPosition() == -2000) ? 1 : 0; // TODO: 2000 is just a dummy value, replace with actual value present when the cable is not connected + } + public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config( null, Volts.of(4), @@ -141,11 +160,18 @@ public void angle(int level){ @Override public void periodic() { - SmartDashboard.putBoolean("Coral", hasCoral()); - SmartDashboard.putNumber("Claw Pivot", angle()); - SmartDashboard.putNumber("Claw Absolute Pivot", encoder.get() * 360); + SmartDashboard.putBoolean("Coral", coral()); + SmartDashboard.putString("Current Coral Sensing", coralSensingDisplay[currentCoralSensor]); + + SmartDashboard.putNumber("Pivot Angle", angle()); SmartDashboard.putNumber("Hood Angle", hood.get() * 180); + SmartDashboard.putNumber("Pivot Relative Angle", pivot.getEncoder().getPosition()); + SmartDashboard.putNumber("Pivot Absolute Angle", pivot.getAbsoluteEncoder().getPosition()); + + SmartDashboard.putBoolean("Claw Absolute Encoder Connected", absoluteConnected() == 0); + // TODO: Find out how to indicate if the Hood Servo is connected or not + double cTime = time.get(); if (stopped) return; diff --git a/src/main/java/frc/robot/utility/AutomatedController.java b/src/main/java/frc/robot/utility/AutomatedController.java index 7f86853..10d080c 100644 --- a/src/main/java/frc/robot/utility/AutomatedController.java +++ b/src/main/java/frc/robot/utility/AutomatedController.java @@ -76,11 +76,11 @@ void configureAutomated(){ IntSupplier pos = () -> { return ((controller.getHID().getLeftBumperButtonPressed()) ? -1 : 0) + ((controller.getHID().getLeftBumperButtonPressed()) ? 1 : 0) + 1; }; - controller.y().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4))); - controller.x().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3))); - controller.b().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2))); - controller.a().and(automated()).and(() -> io.shooter.hasCoral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1))); - controller.a().and(automated()).and(() -> !io.shooter.hasCoral()).onTrue(new Intake(io, false)); + controller.y().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,4))); + controller.x().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,3))); + controller.b().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,2))); + controller.a().and(automated()).and(() -> io.shooter.coral()).onTrue(Util.Do(() -> new ScoreReef(io, pos.getAsInt() ,1))); + controller.a().and(automated()).and(() -> !io.shooter.coral()).onTrue(new Intake(io, false)); controller.start().and(automated()).onTrue(Util.Do(() -> io.elevator.move(0))); controller.back().onTrue(Util.Do(io.chassis::resetAngle, io.chassis)); diff --git a/src/main/java/frc/robot/utility/ModeController.java b/src/main/java/frc/robot/utility/ModeController.java index 7d002ad..a669603 100644 --- a/src/main/java/frc/robot/utility/ModeController.java +++ b/src/main/java/frc/robot/utility/ModeController.java @@ -92,29 +92,29 @@ public void configureGeneral(){ public void configureAutomated(){ IntSupplier pos = () -> { return ((controller.getHID().getRawButtonPressed(Button.kLeftBumper.value)) ? -1 : 0) + ((controller.getHID().getRawButtonPressed(Button.kRightBumper.value)) ? 1 : 0) + 1; }; - binding(Button.kY, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 4)); - binding(Button.kX, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 3)); - binding(Button.kB, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 2)); - binding(Button.kA, AUTOMATED, () -> io.shooter.hasCoral()).onTrue(new ScoreReef(io, pos.getAsInt(), 1)); - binding(Button.kA, AUTOMATED, () -> !io.shooter.hasCoral()).onTrue(new Intake(io, false)); + binding(Button.kY, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 4)); + binding(Button.kX, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 3)); + binding(Button.kB, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 2)); + binding(Button.kA, AUTOMATED, () -> io.shooter.coral()).onTrue(new ScoreReef(io, pos.getAsInt(), 1)); + binding(Button.kA, AUTOMATED, () -> !io.shooter.coral()).onTrue(new Intake(io, false)); binding(Button.kStart, AUTOMATED).onTrue(Util.Do(() -> io.elevator.move(0))); - binding(povDirection.Up, AUTOMATED, () -> !io.shooter.hasAlgae()).onTrue(new ClearAlgae(io)); - binding(povDirection.Up, AUTOMATED, () -> io.shooter.hasAlgae()).onTrue(Util.Do(() ->io.shooter.speed(1), io.shooter)).onFalse(Util.Do(() -> { - io.shooter.stop(); - io.shooter.angle(0); - io.elevator.move(0); - }, io.shooter)); - - // TODO: Check if this works as expected - binding(povDirection.Left, AUTOMATED, () -> !io.shooter.hasAlgae()).toggleOnTrue(Util.Do(() -> { - io.shooter.speed(-1); - io.shooter.angle(0); - }, io.shooter)).toggleOnFalse(Util.Do(() -> { - io.shooter.stop(); - io.shooter.angle(0); - }, io.shooter)); - binding(povDirection.Left, AUTOMATED, () -> io.shooter.hasAlgae()).onTrue(io.elevator.moveCommand(5)); + // binding(povDirection.Up, AUTOMATED, () -> !io.shooter.algae()).onTrue(new ClearAlgae(io)); + // binding(povDirection.Up, AUTOMATED, () -> io.shooter.algae()).onTrue(Util.Do(() ->io.shooter.speed(1), io.shooter)).onFalse(Util.Do(() -> { + // io.shooter.stopIntake(); + // io.shooter.angle(0); + // io.elevator.move(0); + // }, io.shooter)); + + // // TODO: Check if this works as expected + // binding(povDirection.Left, AUTOMATED, () -> !io.shooter.algae()).toggleOnTrue(Util.Do(() -> { + // io.shooter.speed(-1); + // io.shooter.angle(0); + // }, io.shooter)).toggleOnFalse(Util.Do(() -> { + // io.shooter.stopIntake(); + // io.shooter.angle(0); + // }, io.shooter)); + // binding(povDirection.Left, AUTOMATED, () -> io.shooter.algae()).onTrue(io.elevator.moveCommand(5)); binding(povDirection.Down,AUTOMATED).onTrue(Util.Do(io.hang::toggleHang, io.hang)); } @@ -128,7 +128,7 @@ public void configureManual(){ binding(Axis.kLeftTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.speed(controller.getRawAxis(Axis.kLeftTrigger.value)))); binding(Axis.kRightTrigger, MANUAL).whileTrue(Util.Do(() -> io.shooter.pivotSpeed(controller.getRawAxis(Axis.kRightTrigger.value)))); - binding(Button.kY, MANUAL).onTrue(Util.Do(() -> io.shooter.speed(-1), io.shooter)).onFalse(Util.Do(io.shooter::stop, io.shooter)); + binding(Button.kY, MANUAL).onTrue(Util.Do(() -> io.shooter.speed(-1), io.shooter)).onFalse(Util.Do(io.shooter::stopIntake, io.shooter)); binding(Button.kX, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter)); binding(Button.kA, MANUAL).onTrue(Util.Do(() -> io.shooter.hoodSpeed(-.1), io.shooter)).onFalse(Util.Do(() -> io.shooter.hoodSpeed(0), io.shooter)); From 68f0225f4a288fb3977e6d648f6267e338671b6e Mon Sep 17 00:00:00 2001 From: Andrew Schineller <66565336+TotallyFarhan@users.noreply.github.com> Date: Thu, 3 Apr 2025 17:53:02 -0400 Subject: [PATCH 78/79] beginings of redundancy added to Elevator code and adjustments made to shooter sensor redundancy --- .../java/frc/robot/subsystems/Elevator.java | 19 +++++++++++++------ .../java/frc/robot/subsystems/Shooter.java | 8 ++++---- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8dd8a2e..90b3284 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; @@ -34,9 +33,9 @@ public class Elevator extends SubsystemBase { TalonFX lead = new TalonFX(11); TalonFX follow = new TalonFX(12); - // LiDARDio LIDAR = new LiDARDio(4); + LiDARDio lidar = new LiDARDio(4); - SparkMaxConfig config = new SparkMaxConfig(); + boolean selectAbsolute = true; // NOTE: CAN SET TO FALSE IN CASE OF EMERGENCIES LIKE CORAL BLOCKING LIDAR TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); public boolean stopped = true; @@ -97,7 +96,6 @@ public void stop() { public void move(double height) { stopped = false; - // target = Math.max(Math.min(height, 0), MAX_HEIGHT); target = height; time.restart(); } @@ -125,7 +123,11 @@ public Voltage voltage() { public double position() { // return lead.getPosition().getValueAsDouble() * conversion; - return lead.getPosition().getValueAsDouble(); // TODO: Switch to LiDAR + return lead.getPosition().getValueAsDouble(); + } + + public boolean absoluteConnected(){ + return lidar.connected(); } public LinearVelocity velocity() { @@ -154,7 +156,8 @@ public LinearVelocity velocity() { @Override public void periodic() { SmartDashboard.putNumber("Elevator Height", position()); - // SmartDashboard.putNumber("Elevator Height LiDAR", LIDAR.distance()); + SmartDashboard.putNumber("Elevator Height LiDAR", lidar.distance()); + SmartDashboard.putBoolean("Elevator LiDAR Connected", absoluteConnected()); SmartDashboard.putBoolean("Elevator Soft Limits Active", softLimits); double cTime = time.get(); @@ -162,6 +165,10 @@ public void periodic() { if (stopped) return; + if(absoluteConnected() && selectAbsolute) { + lead.setPosition(lidar.distance()); + } + State out = profile.calculate(cTime, new State(L2, lead.getVelocity().getValueAsDouble()), new State(target, 0)); lead.setControl(positionRequest.withPosition(out.position).withEnableFOC(true)); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 913ba67..93e96c1 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -126,7 +126,7 @@ public Voltage voltage() { } public double angle() { - return position[absoluteConnected()].getAsDouble(); + return position[absoluteConnected() ? 1 : 0].getAsDouble(); } public void angle(double target_angle) { @@ -140,8 +140,8 @@ public void angle(int level){ hood.setAngle(hoodAngle[level]); } - public int absoluteConnected(){ - return (pivot.getAbsoluteEncoder().getPosition() == -2000) ? 1 : 0; // TODO: 2000 is just a dummy value, replace with actual value present when the cable is not connected + public boolean absoluteConnected(){ + return (pivot.getAbsoluteEncoder().getPosition() == -2000); // TODO: 2000 is just a dummy value, replace with actual value present when the cable is not connected } public final SysIdRoutine pivotRoutine = new SysIdRoutine(new Config( @@ -169,7 +169,7 @@ public void periodic() { SmartDashboard.putNumber("Pivot Relative Angle", pivot.getEncoder().getPosition()); SmartDashboard.putNumber("Pivot Absolute Angle", pivot.getAbsoluteEncoder().getPosition()); - SmartDashboard.putBoolean("Claw Absolute Encoder Connected", absoluteConnected() == 0); + SmartDashboard.putBoolean("Claw Absolute Encoder Connected", absoluteConnected()); // TODO: Find out how to indicate if the Hood Servo is connected or not double cTime = time.get(); From 734e0d408d0051b64df7d4b9d569efacf90d4a35 Mon Sep 17 00:00:00 2001 From: Evan Date: Wed, 30 Apr 2025 18:49:01 -0400 Subject: [PATCH 79/79] debug mode controller --- .../java/frc/robot/subsystems/Elevator.java | 29 +++++++--- src/main/java/frc/robot/subsystems/Hang.java | 22 ++++++-- .../java/frc/robot/subsystems/Shooter.java | 41 +++++++++++--- .../java/frc/robot/subsystems/Swerve.java | 10 ++++ .../frc/robot/utility/ModeController.java | 54 ++++++++++++------- 5 files changed, 121 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 90b3284..91aa9a5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.utility.Util; public class Elevator extends SubsystemBase { TalonFX lead = new TalonFX(11); @@ -48,19 +49,21 @@ public class Elevator extends SubsystemBase { public final double[] Level = { 0, 25, 43.5, 76, 110 }; public final double Rest = 0; - public final double L1 = 25; - public final double L2 = 43.5; - public final double L3 = 76; - public final double L4 = 110; + public static final double L1 = (double) Util.get("L1 Height", 25.0); + public static final double L2 = (double) Util.get("L2 Height", 43.5); + public static final double L3 = (double) Util.get("L3 Height", 76.0); + public static final double L4 = (double) Util.get("L4 Height", 110.0); + public TalonFXConfiguration config = new TalonFXConfiguration(); + + boolean redundancy = false; public boolean softLimits = false; public Elevator() { - TalonFXConfiguration config = new TalonFXConfiguration(); config.Slot0.kP = 0.3; config.Slot0.kI = 0.0; config.Slot0.kD = 0.1; - config.Slot0.kG = 0.0; + config.Slot0.kG = 0; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -74,6 +77,14 @@ public Elevator() { follow.setControl(new Follower(lead.getDeviceID(), true)); } + public void PID(double p, double i, double d, double g) { + config.Slot0.kP = p; + config.Slot0.kI = i; + config.Slot0.kD = d; + config.Slot0.kG = g; + lead.getConfigurator().apply(config); + } + public void toggleSoftLimits() { softLimits = !softLimits; lead.getConfigurator().apply( @@ -136,6 +147,10 @@ public LinearVelocity velocity() { return MetersPerSecond.of(lead.getVelocity().getValueAsDouble()); } + public void toggleRedundancy(){ + redundancy = !redundancy; + } + final double gearReduction = 1 / 17; final double conversion = Math.PI * gearReduction * (Units.inchesToMeters(2) / 60); @@ -181,5 +196,7 @@ public void periodic() { SmartDashboard.putNumber("Elevator Velocity", velocity().magnitude()); SmartDashboard.putNumber("Elevator cTarget Velocity", out.velocity); + + SmartDashboard.putBoolean("Elevator Soft Limits", softLimits); } } diff --git a/src/main/java/frc/robot/subsystems/Hang.java b/src/main/java/frc/robot/subsystems/Hang.java index a69e734..e208296 100644 --- a/src/main/java/frc/robot/subsystems/Hang.java +++ b/src/main/java/frc/robot/subsystems/Hang.java @@ -1,26 +1,40 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utility.Util; public class Hang extends SubsystemBase { // public TalonFX hang = new TalonFX(16, "rio"); // We Don't actually know the motor yet SparkMax motor = new SparkMax(16, MotorType.kBrushed); // CIM + public SparkMaxConfig config = new SparkMaxConfig(); public boolean up = false; - double upPosition = 0; // TODO: FIND UP POSITION - double downPosition = 0; + public static final double upPosition = (double) Util.get("Hang Up Position", 0); + public static final double downPosition = (double) Util.get("Hang Down Position", 0); + + public static final double kP = (double) Util.get("Hang kP", 0.0); + public static final double kI = (double) Util.get("Hang kI", 0.0); + public static final double kD = (double) Util.get("Hang kD", 0.0); public Hang() { - motor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + config.idleMode(SparkMaxConfig.IdleMode.kBrake); + config.closedLoop.pid(kP, kI, kD, ClosedLoopSlot.kSlot0); + + motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public void PID(double k, double i, double p){ + config.closedLoop.pid(k, i, p); + motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public void speed(double speed) { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 93e96c1..a547701 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -24,15 +24,19 @@ import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.utility.Util; public class Shooter extends SubsystemBase { SparkMax intake = new SparkMax(13, MotorType.kBrushless); SparkMax pivot = new SparkMax(14, MotorType.kBrushless); + public SparkMaxConfig config = new SparkMaxConfig(); + public SparkMaxConfig pivotConfig = new SparkMaxConfig(); // DutyCycleEncoder encoder = new DutyCycleEncoder(new DigitalInput(3)); // or 4 DoubleSupplier[] position; @@ -44,9 +48,12 @@ public class Shooter extends SubsystemBase { Servo hood = new Servo(0); + boolean softLimits = false; - double[] pivotAngle = {0,0,0,0,0, 0}; // last one FOR BARGE - double[] hoodAngle = {0,0,0,0,0, 0}; + boolean pivotRedundancy = false; + + public static final double[] pivotAngle = (double[]) Util.get("Pivot Angle", new double[] {0,0,0,0,0,0}); // last one FOR BARGE + public static final double[] hoodAngle = (double[]) Util.get("Hood Angle", new double[] {0,0,0,0,0,0}); // last one FOR BARGE TrapezoidProfile profile = new TrapezoidProfile(new Constraints(100, 500)); Timer time = new Timer(); @@ -56,14 +63,11 @@ public class Shooter extends SubsystemBase { public boolean intaking = false; public Shooter() { - SparkMaxConfig config = new SparkMaxConfig(); - config.idleMode(SparkMaxConfig.IdleMode.kBrake); intake.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - SparkMaxConfig pivotConfig = new SparkMaxConfig(); pivotConfig.idleMode(SparkMaxConfig.IdleMode.kBrake); - pivotConfig.closedLoop.pidf(0.0, 0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); + pivotConfig.closedLoop.pid(0.0, 0.0, 0.0, ClosedLoopSlot.kSlot0); pivotConfig.softLimit.forwardSoftLimitEnabled(false); pivotConfig.softLimit.forwardSoftLimit(0); // TODO: Find the Forward soft limit @@ -76,6 +80,18 @@ public Shooter() { coral = new BooleanSupplier[]{beam::get}; //TODO: Have the second option be RPM sensing } + public void toggleSoftLimits() { + softLimits = !softLimits; + pivotConfig.softLimit.forwardSoftLimitEnabled(softLimits); + pivotConfig.softLimit.reverseSoftLimitEnabled(softLimits); + pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void pivotPID(double p, double i, double d){ + pivotConfig.closedLoop.pid(p, i, d, ClosedLoopSlot.kSlot0); + pivot.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + public void hood(double v){ hood.setPosition(v); } @@ -140,6 +156,17 @@ public void angle(int level){ hood.setAngle(hoodAngle[level]); } + public InstantCommand angleCommand(int level){ + return new InstantCommand(() -> { + angle(pivotAngle[level]); + hood.setAngle(hoodAngle[level]); + }, this); + } + + public void togglePivotRedundancy(){ + pivotRedundancy = !pivotRedundancy; + } + public boolean absoluteConnected(){ return (pivot.getAbsoluteEncoder().getPosition() == -2000); // TODO: 2000 is just a dummy value, replace with actual value present when the cable is not connected } @@ -170,6 +197,8 @@ public void periodic() { SmartDashboard.putNumber("Pivot Absolute Angle", pivot.getAbsoluteEncoder().getPosition()); SmartDashboard.putBoolean("Claw Absolute Encoder Connected", absoluteConnected()); + + SmartDashboard.putBoolean("Shooter Pivot Soft Limits", softLimits); // TODO: Find out how to indicate if the Hood Servo is connected or not double cTime = time.get(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ab4fabb..22904c2 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -62,6 +62,8 @@ public class Swerve extends SubsystemBase { public final Constants constants = new Constants(); public boolean active = true; + boolean encoderRedundancy = false; + boolean moduleRedundancy = false; public Swerve() { kinematics = new SwerveDriveKinematics( @@ -134,6 +136,14 @@ public void stop() { speeds = new ChassisSpeeds(); } + public void toggleEncoderRedundancy() { + encoderRedundancy = !encoderRedundancy; + } + + public void toggleModuleRedundancy() { + moduleRedundancy = !moduleRedundancy; + } + public double distance(Pose2d reference_point) { return pose().getTranslation().getDistance(reference_point.getTranslation()); } diff --git a/src/main/java/frc/robot/utility/ModeController.java b/src/main/java/frc/robot/utility/ModeController.java index a669603..072fd42 100644 --- a/src/main/java/frc/robot/utility/ModeController.java +++ b/src/main/java/frc/robot/utility/ModeController.java @@ -6,10 +6,11 @@ import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.commands.ClearAlgae; import frc.robot.commands.Intake; import frc.robot.commands.ScoreReef; @@ -164,27 +165,42 @@ public void configureCharacterisaton(){ } public void configureDebug(){ - // TODO: Setup Smart Dashboard buttons to pass in the Level for the elevator - // TODO: Setup a shuffleboard thing to set the claw Angle based on Level (use the adjustable for the angles) - // TODO: Setup a Shuffleboard thing to move the bot to particular States (Ground intake, no Algae, etc) - // TODO: Setup Manual Movement of key mechanisms - // TODO: Setup a toggle for Softlimits on Elevator Height, Shooter Angle, and Maybe Hang + for (int i = 0; i < 5; i++){ + SmartDashboard.putData("Elevator Height L" + i, io.elevator.moveCommand(i)); + } + + for (int i = 0; i < 5; i++){ + SmartDashboard.putData("Shooter Angle L" + i, io.shooter.angleCommand(i)); + } + + SmartDashboard.putData("Toggle Hang", new InstantCommand(() -> io.hang.toggleHang(), io.hang)); + + SmartDashboard.putData("Toggle Elevator Soft Limits", new InstantCommand(() -> io.elevator.toggleSoftLimits(), io.elevator)); + SmartDashboard.putData("Toggle Shooter Pivot Soft Limits", new InstantCommand(() -> io.shooter.toggleSoftLimits(), io.shooter)); + + SmartDashboard.putData("Toggle Shooter Redundancy", new InstantCommand(() -> io.shooter.togglePivotRedundancy(), io.shooter)); + SmartDashboard.putData("Toggle Elevator Redundancy", new InstantCommand(() -> io.elevator.toggleRedundancy(), io.elevator)); + SmartDashboard.putData("Toggle Encoder Redundancy", new InstantCommand(() -> io.chassis.toggleEncoderRedundancy(), io.chassis)); + SmartDashboard.putData("Toggle Module Redundancy", new InstantCommand(() -> io.chassis.toggleModuleRedundancy(), io.chassis)); - // TODO: Setup a toggle for Shooter Pivot Redundancy // TODO: Setup a toggle for Coral Detection Redundancy // TODO: Setup a toggle for Algae Detection Redundancy - // TODO: Setup a toggle for Elevator Redundancy - // TODO: Setup a toggle for Module Encoder Redundancy - // TODO: Setup a toggle for Module Redundancy / Redundant Kinemtics - - // TODO: Setup Adjustable Height Locations for each level - // TODO: Setup Adjustable Shooter Angles for each Level - // TODO: Setup adjustable Hood angles for each Level - // TODO: Setup adjustable Hang Positions + + double elevatorkP = (double) Util.get("Elevator kP", 0.3); + double elevatorkI = (double) Util.get("Elevator kI", 0.0); + double elevatorkD = (double) Util.get("Elevator kD", 0.1); + double elevatorkG = (double) Util.get("Elevator kG", 0.0); + + double shooterkP = (double) Util.get("Elevator kP", 0.3); + double shooterkI = (double) Util.get("Elevator kI", 0.0); + double shooterkD = (double) Util.get("Elevator kD", 0.1); - // TODO: Setup Adjustable PID for Elevator - // TODO: Setup Adjustable PID for Shooter Pivot - // TODO: Maybe add a PID for target RPM - // TODO: Setup Adjustable PID for Hang + double hangkP = (double) Util.get("Elevator kP", 0.3); + double hangkI = (double) Util.get("Elevator kI", 0.0); + double hangkD = (double) Util.get("Elevator kD", 0.1); + + SmartDashboard.putData("Set Elevator PID", new InstantCommand(() -> io.elevator.PID(elevatorkP, elevatorkI, elevatorkD, elevatorkG), io.elevator)); + SmartDashboard.putData("Set Shooter Pivot PID", new InstantCommand(() -> io.shooter.pivotPID(shooterkD, shooterkI, shooterkP), io.shooter)); + SmartDashboard.putData("Set Hang PID", new InstantCommand(() -> io.hang.PID(hangkP, hangkI, hangkD), io.hang)); } }