From d9edd20861939a7afe4a94b0e52bae56ef895a51 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 27 Mar 2026 18:09:35 -0700 Subject: [PATCH 01/22] delete old impl of defense mode --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b5a7b1..d0dac64 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -704,8 +704,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .rightBumper() .or(Autos.autoLeftClimbReq.negate()) .onTrue(Commands.runOnce(() -> leftClimbTarget = false)); - // I HATE THIS! - operator.leftStick().whileTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret())); operator .rightStick() .onTrue( From 4e62c0943c6e31445f4759ebc45f795cde01eabf Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 27 Mar 2026 19:39:42 -0700 Subject: [PATCH 02/22] new tag map --- .../tagmaps/field_map_mar_27_18_47_20.json | 585 ++++++++++++++++++ .../swerve/constants/SwerveConstants.java | 3 +- 2 files changed, 587 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json diff --git a/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json b/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json new file mode 100644 index 0000000..ec8e9af --- /dev/null +++ b/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json @@ -0,0 +1,585 @@ +{ + "tags" : [ + { + "ID" : 1, + "pose" : { + "translation" : { + "x" : 4.822815895080566, + "y" : 7.438567638397217, + "z" : 0.8728957772254944 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0037721414119005203, + "X" : -0.006326864939182997, + "Y" : -0.00018497782002668828, + "Z" : 0.9999727606773376 + } + } + } + }, + { + "ID" : 2, + "pose" : { + "translation" : { + "x" : 11.8870210647583, + "y" : 4.671638011932373, + "z" : 1.1223087310791016 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7057818174362183, + "X" : 0.007269229739904404, + "Y" : -0.005886712111532688, + "Z" : 0.7083674073219299 + } + } + } + }, + { + "ID" : 3, + "pose" : { + "translation" : { + "x" : 4.076200485229492, + "y" : 4.363650321960449, + "z" : 1.1617555618286133 + }, + "rotation" : { + "quaternion" : { + "W" : 0.004635084420442581, + "X" : 0.0006607414106838405, + "Y" : 0.0042945570312440395, + "Z" : 0.9999797940254211 + } + } + } + }, + { + "ID" : 4, + "pose" : { + "translation" : { + "x" : 4.075459003448486, + "y" : 4.005395412445068, + "z" : 1.1609687805175781 + }, + "rotation" : { + "quaternion" : { + "W" : 0.005071163643151522, + "X" : 0.004584860987961292, + "Y" : 0.0010812842519953847, + "Z" : 0.9999760389328003 + } + } + } + }, + { + "ID" : 5, + "pose" : { + "translation" : { + "x" : 4.702306747436523, + "y" : 3.38472580909729, + "z" : 1.1592590808868408 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7048313617706299, + "X" : 0.0046210805885493755, + "Y" : -0.0013557852944359183, + "Z" : -0.7093584537506104 + } + } + } + }, + { + "ID" : 6, + "pose" : { + "translation" : { + "x" : 11.87798023223877, + "y" : 0.6444995999336243, + "z" : 0.8889999985694885 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, + { + "ID" : 7, + "pose" : { + "translation" : { + "x" : 4.7464823722839355, + "y" : 0.6324626803398132, + "z" : 0.9177830219268799 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999463558197021, + "X" : 0.004018927924335003, + "Y" : -0.008416394703090191, + "Z" : 0.00450051948428154 + } + } + } + }, + { + "ID" : 8, + "pose" : { + "translation" : { + "x" : 5.055703639984131, + "y" : 3.3834192752838135, + "z" : 1.160678505897522 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7026873826980591, + "X" : 0.001975176390260458, + "Y" : -0.004470320884138346, + "Z" : -0.7114818692207336 + } + } + } + }, + { + "ID" : 9, + "pose" : { + "translation" : { + "x" : 5.304318904876709, + "y" : 3.636298179626465, + "z" : 1.1557623147964478 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999735355377197, + "X" : 0.004210796672850847, + "Y" : -0.005924228113144636, + "Z" : 0.0002991016663145274 + } + } + } + }, + { + "ID" : 10, + "pose" : { + "translation" : { + "x" : 5.3059587478637695, + "y" : 3.9924886226654053, + "z" : 1.1551718711853027 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999508261680603, + "X" : 0.0020055281929671764, + "Y" : -0.008682952262461185, + "Z" : 0.004347731824964285 + } + } + } + }, + { + "ID" : 11, + "pose" : { + "translation" : { + "x" : 12.242338180541992, + "y" : 4.668259620666504, + "z" : 1.1203622817993164 + }, + "rotation" : { + "quaternion" : { + "W" : 0.705532968044281, + "X" : 0.0018324467819184065, + "Y" : -0.0005836429772898555, + "Z" : 0.7086744904518127 + } + } + } + }, + { + "ID" : 12, + "pose" : { + "translation" : { + "x" : 4.8416900634765625, + "y" : 7.216125965118408, + "z" : 0.9207862019538879 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999958276748657, + "X" : 0.0027650343254208565, + "Y" : 5.1801976951537654e-05, + "Z" : -0.0007708670455031097 + } + } + } + }, + { + "ID" : 13, + "pose" : { + "translation" : { + "x" : 9.405987739562988, + "y" : 7.152761936187744, + "z" : 0.5603619813919067 + }, + "rotation" : { + "quaternion" : { + "W" : -0.001173483906313777, + "X" : 0.008173578418791294, + "Y" : -0.0002410715533187613, + "Z" : 0.9999658465385437 + } + } + } + }, + { + "ID" : 14, + "pose" : { + "translation" : { + "x" : 9.405714988708496, + "y" : 6.721652507781982, + "z" : 0.561457633972168 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0004049946437589824, + "X" : 0.006965869106352329, + "Y" : 0.001270489883609116, + "Z" : 0.9999748468399048 + } + } + } + }, + { + "ID" : 15, + "pose" : { + "translation" : { + "x" : 9.392133712768555, + "y" : 4.136072635650635, + "z" : 0.5631878972053528 + }, + "rotation" : { + "quaternion" : { + "W" : 0.007338069844990969, + "X" : 0.015838444232940674, + "Y" : 0.006142545025795698, + "Z" : 0.9998286962509155 + } + } + } + }, + { + "ID" : 16, + "pose" : { + "translation" : { + "x" : 9.381674766540527, + "y" : 3.7076895236968994, + "z" : 0.5665956735610962 + }, + "rotation" : { + "quaternion" : { + "W" : 0.024910936132073402, + "X" : 0.016272999346256256, + "Y" : 0.005707239266484976, + "Z" : 0.9995409250259399 + } + } + } + }, + { + "ID" : 17, + "pose" : { + "translation" : { + "x" : -2.504611015319824, + "y" : 0.8694211840629578, + "z" : 0.9030297994613647 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999359846115112, + "X" : 0.0024782754480838776, + "Y" : -0.003686416195705533, + "Z" : -0.010405427776277065 + } + } + } + }, + { + "ID" : 18, + "pose" : { + "translation" : { + "x" : 4.714351177215576, + "y" : 3.364177703857422, + "z" : 1.1612824201583862 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7115345001220703, + "X" : -0.002445084974169731, + "Y" : -0.000266425748122856, + "Z" : -0.7026468515396118 + } + } + } + }, + { + "ID" : 19, + "pose" : { + "translation" : { + "x" : 5.305013179779053, + "y" : 3.635739803314209, + "z" : 1.1534665822982788 + }, + "rotation" : { + "quaternion" : { + "W" : 0.999992847442627, + "X" : 0.0029923561960458755, + "Y" : 0.0023054699413478374, + "Z" : -0.0002783012459985912 + } + } + } + }, + { + "ID" : 20, + "pose" : { + "translation" : { + "x" : 5.304501056671143, + "y" : 3.9921987056732178, + "z" : 1.1584936380386353 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999909400939941, + "X" : 0.0030655476730316877, + "Y" : 0.001734467106871307, + "Z" : 0.002377011813223362 + } + } + } + }, + { + "ID" : 21, + "pose" : { + "translation" : { + "x" : 4.686933994293213, + "y" : 4.584085941314697, + "z" : 1.1613006591796875 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7082415819168091, + "X" : 0.0033884502481669188, + "Y" : 0.00023085897555574775, + "Z" : 0.7059620022773743 + } + } + } + }, + { + "ID" : 22, + "pose" : { + "translation" : { + "x" : 4.734961986541748, + "y" : 7.35674524307251, + "z" : 0.9273542761802673 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999683499336243, + "X" : -0.0004531952436082065, + "Y" : 0.006745198275893927, + "Z" : 0.004174919333308935 + } + } + } + }, + { + "ID" : 23, + "pose" : { + "translation" : { + "x" : 4.639162063598633, + "y" : 7.372425556182861, + "z" : 0.9242526292800903 + }, + "rotation" : { + "quaternion" : { + "W" : -0.0031200069934129715, + "X" : 0.004842787049710751, + "Y" : 0.001103776739910245, + "Z" : 0.9999827742576599 + } + } + } + }, + { + "ID" : 24, + "pose" : { + "translation" : { + "x" : 4.3320207595825195, + "y" : 4.583142280578613, + "z" : 1.1634548902511597 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7041061520576477, + "X" : 0.0010072175646200776, + "Y" : -0.003978305030614138, + "Z" : 0.7100829482078552 + } + } + } + }, + { + "ID" : 25, + "pose" : { + "translation" : { + "x" : 4.085196018218994, + "y" : 4.3233113288879395, + "z" : 1.1617553234100342 + }, + "rotation" : { + "quaternion" : { + "W" : -0.005810739006847143, + "X" : 0.002946239896118641, + "Y" : 0.006170302629470825, + "Z" : 0.9999597072601318 + } + } + } + }, + { + "ID" : 26, + "pose" : { + "translation" : { + "x" : 4.089456081390381, + "y" : 3.9735653400421143, + "z" : 1.1618210077285767 + }, + "rotation" : { + "quaternion" : { + "W" : -5.637028516503051e-05, + "X" : 0.00033929909113794565, + "Y" : 0.0005828236462548375, + "Z" : 0.9999997019767761 + } + } + } + }, + { + "ID" : 27, + "pose" : { + "translation" : { + "x" : 4.359224796295166, + "y" : 3.3653600215911865, + "z" : 1.1619951725006104 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7068608403205872, + "X" : -0.0016970370197668672, + "Y" : 0.0007088514394126832, + "Z" : -0.7073500752449036 + } + } + } + }, + { + "ID" : 28, + "pose" : { + "translation" : { + "x" : 4.660392761230469, + "y" : 0.6959283947944641, + "z" : 0.9149965643882751 + }, + "rotation" : { + "quaternion" : { + "W" : -0.002637912053614855, + "X" : 0.0047381725162267685, + "Y" : -8.136600081343204e-05, + "Z" : 0.9999852776527405 + } + } + } + }, + { + "ID" : 29, + "pose" : { + "translation" : { + "x" : 0.1180562973022461, + "y" : 0.6825624704360962, + "z" : 0.5843039751052856 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999812245368958, + "X" : 0.0013159263180568814, + "Y" : -0.005062768701463938, + "Z" : -0.003174516139551997 + } + } + } + }, + { + "ID" : 30, + "pose" : { + "translation" : { + "x" : 0.11867809295654297, + "y" : 1.1131608486175537, + "z" : 0.5827512741088867 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999870657920837, + "X" : 0.0002074952208204195, + "Y" : -0.0050629121251404285, + "Z" : 8.447438449366018e-05 + } + } + } + }, + { + "ID" : 31, + "pose" : { + "translation" : { + "x" : 0.11109352111816406, + "y" : 3.748471736907959, + "z" : 0.5825525522232056 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9997530579566956, + "X" : -0.0011874643387272954, + "Y" : -0.021945597603917122, + "Z" : 0.003259050427004695 + } + } + } + }, + { + "ID" : 32, + "pose" : { + "translation" : { + "x" : 0.1185617446899414, + "y" : 4.175858974456787, + "z" : 0.5889672040939331 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999861717224121, + "X" : 0.0009740604436956346, + "Y" : 0.003140027867630124, + "Z" : 0.00408512307330966 + } + } + } + } + ], + "field" : { + "length" : 16.541, + "width" : 8.069 + }, + "_comment" : "generated by PractiCal 1.0.5 (3) on 3/27/2026" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index c6c9cd6..547bfd2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -31,7 +31,8 @@ public SwerveConstants() { new AprilTagFieldLayout( Filesystem.getDeployDirectory() .toPath() - .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json")); + // .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json")); + .resolve("tagmaps" + File.separator + "field_map_mar_27_18_47_20.json")); System.out.println("Successfully loaded tag map"); } catch (Exception e) { System.err.println("Failed to load custom tag map"); From 1364c168032ad59807ff257bc1134bf8183203a9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 08:41:39 -0700 Subject: [PATCH 03/22] choreo fix, turn off tuning mode, add fudge factor back --- src/main/deploy/choreo/HubtoOutpost.traj | 121 ++- .../tagmaps/field_map_mar_27_18_47_20.json | 780 +++++++++--------- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 4 - .../java/frc/robot/utils/autoaim/AutoAim.java | 14 +- 5 files changed, 453 insertions(+), 468 deletions(-) diff --git a/src/main/deploy/choreo/HubtoOutpost.traj b/src/main/deploy/choreo/HubtoOutpost.traj index 35d14c3..a0c9bbb 100644 --- a/src/main/deploy/choreo/HubtoOutpost.traj +++ b/src/main/deploy/choreo/HubtoOutpost.traj @@ -3,7 +3,7 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.674594640731811, "y":4.0000072192, "heading":3.141592653589793, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.674594640731811, "y":4.0000072192, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.6607944965362549, "y":1.6499865055084229, "heading":1.5707963267948966, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":0.4436733722686768, "y":0.443471223115921, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"StartingHub.x", "val":3.674594640731811}, "y":{"exp":"StartingHub.y", "val":4.0000072192}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"StartingHub.x", "val":3.674594640731811}, "y":{"exp":"StartingHub.y", "val":4.0000072192}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.6607944965362549 m", "val":1.6607944965362549}, "y":{"exp":"1.6499865055084229 m", "val":1.6499865055084229}, "heading":{"exp":"Outpost.heading", "val":1.5707963267948966}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"Outpost.x", "val":0.4436733722686768}, "y":{"exp":"Outpost.y", "val":0.443471223115921}, "heading":{"exp":"Outpost.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -55,71 +55,60 @@ "differentialTrackWidth":0.5427218 }, "sampleType":"Swerve", - "waypoints":[0.0,1.32654,2.14131], + "waypoints":[0.0,1.32661,2.14138], "samples":[ - {"t":0.0, "x":3.67459, "y":4.00001, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.88888, "ay":-5.71777, "alpha":-0.1115, "fx":[-81.95408,-80.92114,-79.7025,-80.7426], "fy":[-93.5674,-94.46487,-95.49302,-94.61241]}, - {"t":0.03158, "x":3.67216, "y":3.99716, "heading":3.14159, "vx":-0.15441, "vy":-0.18059, "omega":-0.00352, "ax":-4.88825, "ay":-5.71699, "alpha":-0.11482, "fx":[-81.97677,-80.91353,-79.65863,-80.72946], "fy":[-93.52581,-94.45024,-95.50841,-94.60184]}, - {"t":0.06317, "x":3.66484, "y":3.9886, "heading":3.14148, "vx":-0.3088, "vy":-0.36116, "omega":-0.00715, "ax":-4.88747, "ay":-5.71604, "alpha":-0.11886, "fx":[-82.00445,-80.90437,-79.60506,-80.71325], "fy":[-93.47501,-94.43228,-95.52716,-94.58903]}, - {"t":0.09475, "x":3.65265, "y":3.97434, "heading":3.14126, "vx":-0.46317, "vy":-0.5417, "omega":-0.0109, "ax":-4.88651, "ay":-5.71486, "alpha":-0.12392, "fx":[-82.03898,-80.89306,-79.53815,-80.69287], "fy":[-93.41153,-94.40976,-95.55055,-94.57312]}, - {"t":0.12634, "x":3.63558, "y":3.95438, "heading":3.14091, "vx":-0.61751, "vy":-0.72219, "omega":-0.01482, "ax":-4.88526, "ay":-5.71333, "alpha":-0.13041, "fx":[-82.08328,-80.87865,-79.4522,-80.66655], "fy":[-93.32996,-94.38077,-95.58051,-94.55274]}, - {"t":0.15792, "x":3.61364, "y":3.92872, "heading":3.14044, "vx":-0.7718, "vy":-0.90265, "omega":-0.01894, "ax":-4.8836, "ay":-5.71129, "alpha":-0.13906, "fx":[-82.14219,-80.85956,-79.33774,-80.63135], "fy":[-93.22128,-94.34216,-95.62031,-94.52562]}, - {"t":0.18951, "x":3.58683, "y":3.89736, "heading":3.13985, "vx":-0.92605, "vy":-1.08303, "omega":-0.02333, "ax":-4.88127, "ay":-5.70844, "alpha":-0.15116, "fx":[-82.22436,-80.83298,-79.17773,-80.58199], "fy":[-93.06925,-94.28826,-95.67574,-94.48766]}, - {"t":0.22109, "x":3.55515, "y":3.86031, "heading":3.13911, "vx":-1.08022, "vy":-1.26333, "omega":-0.0281, "ax":-4.87779, "ay":-5.70417, "alpha":-0.16927, "fx":[-82.34696,-80.79332,-78.93825,-80.50788], "fy":[-92.84151,-94.20786,-95.75826,-94.43067]}, - {"t":0.25267, "x":3.5186, "y":3.81756, "heading":3.13822, "vx":-1.23428, "vy":-1.44349, "omega":-0.03345, "ax":-4.87198, "ay":-5.69705, "alpha":-0.19935, "fx":[-82.54958,-80.7277,-78.54055,-80.38433], "fy":[-92.46279,-94.07514,-95.89417,-94.33546]}, - {"t":0.28426, "x":3.47718, "y":3.76913, "heading":3.13716, "vx":-1.38816, "vy":-1.62343, "omega":-0.03974, "ax":-4.86037, "ay":-5.68284, "alpha":-0.25914, "fx":[-82.94828,-80.59833,-77.7509,-80.13708], "fy":[-91.70884,-93.81447,-96.15989,-94.14435]}, - {"t":0.31584, "x":3.43091, "y":3.71502, "heading":3.13591, "vx":-1.54167, "vy":-1.80292, "omega":-0.04793, "ax":-4.82572, "ay":-5.64045, "alpha":-0.43517, "fx":[-84.09144,-80.22629,-75.43147,-79.39409], "fy":[-89.47925,-93.06894,-96.90875,-93.56754]}, - {"t":0.34743, "x":3.37982, "y":3.65526, "heading":3.1344, "vx":-1.69409, "vy":-1.98107, "omega":-0.06167, "ax":-1.88172, "ay":-2.24319, "alpha":-10.33102, "fx":[-92.88095,-70.14071,11.36257,27.21374], "fy":[6.88154,-73.96309,-94.33991,13.07073]}, - {"t":0.37901, "x":3.32537, "y":3.59158, "heading":3.13245, "vx":-1.75352, "vy":-2.05192, "omega":-0.38797, "ax":-0.01017, "ay":0.0075, "alpha":-11.96277, "fx":[-56.0348,-57.16631,55.79772,56.73059], "fy":[57.00263,-55.72238,-56.89454,56.11042]}, - {"t":0.4106, "x":3.26998, "y":3.52677, "heading":3.12019, "vx":-1.75384, "vy":-2.05168, "omega":-0.76581, "ax":-0.00317, "ay":0.00273, "alpha":-10.57609, "fx":[-48.85271,-51.01534,48.77541,50.88308], "fy":[50.97688,-48.75156,-50.92154,48.87658]}, - {"t":0.44218, "x":3.21459, "y":3.46197, "heading":3.09601, "vx":-1.75394, "vy":-2.05159, "omega":-1.09984, "ax":0.00003, "ay":0.00001, "alpha":-8.80275, "fx":[-39.59121,-43.37611,39.59223,43.377], "fy":[43.37675,-39.59172,-43.37635,39.59171]}, - {"t":0.47376, "x":3.15919, "y":3.39717, "heading":3.06127, "vx":-1.75394, "vy":-2.05159, "omega":-1.37787, "ax":0.00244, "ay":-0.00204, "alpha":-6.69998, "fx":[-28.93246,-33.99774,29.00624,34.08537], "fy":[34.01361,-29.00895,-34.06949,28.92977]}, - {"t":0.50535, "x":3.10379, "y":3.33237, "heading":3.01775, "vx":-1.75386, "vy":-2.05166, "omega":-1.58949, "ax":0.00477, "ay":-0.00403, "alpha":-4.59211, "fx":[-18.74532,-24.0919,18.89768,24.25519], "fy":[24.11257,-18.89373,-24.23449,18.74933]}, - {"t":0.53693, "x":3.0484, "y":3.26757, "heading":2.96755, "vx":-1.75371, "vy":-2.05178, "omega":-1.73452, "ax":0.00625, "ay":-0.00529, "alpha":-2.95295, "fx":[-11.20571,-16.02809,11.40995,16.23703], "fy":[16.04823,-11.39853,-16.21685,11.21721]}, - {"t":0.56852, "x":2.99301, "y":3.20277, "heading":2.91276, "vx":-1.75352, "vy":-2.05195, "omega":-1.82779, "ax":0.00614, "ay":-0.00521, "alpha":-1.98149, "fx":[-6.88255,-11.12267,7.08473,11.32632], "fy":[11.13978,-7.07128,-11.30919,6.89605]}, - {"t":0.6001, "x":2.93763, "y":3.13795, "heading":2.85503, "vx":-1.75332, "vy":-2.05212, "omega":-1.89037, "ax":0.00474, "ay":-0.00403, "alpha":-1.52663, "fx":[-4.79433,-8.86546,4.95088,9.02238], "fy":[8.87792,-4.93998,-9.00991,4.80527]}, - {"t":0.63168, "x":2.88226, "y":3.07314, "heading":2.79533, "vx":-1.75317, "vy":-2.05224, "omega":-1.93859, "ax":0.00257, "ay":-0.00219, "alpha":-1.35021, "fx":[-3.78735,-8.11089,3.87227,8.19583], "fy":[8.11746,-3.86629,-8.18925,3.79334]}, - {"t":0.66327, "x":2.82689, "y":3.00832, "heading":2.7341, "vx":-1.75309, "vy":-2.05231, "omega":-1.98124, "ax":-0.00023, "ay":0.00019, "alpha":-1.2695, "fx":[-3.12884,-7.87576,3.12127,7.86819], "fy":[7.87515,-3.12183,-7.8688,3.12828]}, - {"t":0.69485, "x":2.77152, "y":2.9435, "heading":2.67152, "vx":-1.7531, "vy":-2.05231, "omega":-2.02133, "ax":-0.00363, "ay":0.0031, "alpha":-1.1609, "fx":[-2.46211,-7.42321,2.34184,7.30323], "fy":[7.41412,-2.35049,-7.31232,2.45347]}, - {"t":0.72644, "x":2.71615, "y":2.87868, "heading":2.60768, "vx":-1.75321, "vy":-2.05221, "omega":-2.058, "ax":-0.00732, "ay":0.00624, "alpha":-0.9421, "fx":[-1.68514,-6.20842,1.44297,5.96682], "fy":[6.19043,-1.46051,-5.9848,1.66765]}, - {"t":0.75802, "x":2.66077, "y":2.81386, "heading":2.54268, "vx":-1.75344, "vy":-2.05201, "omega":-2.08776, "ax":-0.01044, "ay":0.00892, "alpha":-0.57737, "fx":[-0.88693,-3.95776,0.54142,3.61267], "fy":[3.93247,-0.56656,-3.63796,0.86184]}, - {"t":0.78961, "x":2.60538, "y":2.74906, "heading":2.47674, "vx":-1.75377, "vy":-2.05173, "omega":-2.10599, "ax":-0.01197, "ay":0.01023, "alpha":-0.09722, "fx":[-0.27588,-0.84179,-0.11989,0.44603], "fy":[0.81307,0.09117,-0.47474,0.24718]}, - {"t":0.82119, "x":2.54998, "y":2.68426, "heading":2.41022, "vx":-1.75415, "vy":-2.05141, "omega":-2.10906, "ax":-0.01128, "ay":0.00966, "alpha":0.40455, "fx":[-0.04086,2.50863,-0.33235,-2.88153], "fy":[-2.53553,0.30548,2.85463,0.01395]}, - {"t":0.85277, "x":2.49458, "y":2.61947, "heading":2.34361, "vx":-1.75451, "vy":-2.0511, "omega":-2.09628, "ax":-0.00865, "ay":0.00741, "alpha":0.81826, "fx":[-0.21206,5.31588,-0.07465,-5.60147], "fy":[-5.33634,0.05412,5.58103,0.19148]}, - {"t":0.88436, "x":2.43916, "y":2.55469, "heading":2.2774, "vx":-1.75478, "vy":-2.05087, "omega":-2.07044, "ax":-0.00507, "ay":0.00435, "alpha":1.07846, "fx":[-0.65054,7.08916,0.48214,-7.2563], "fy":[-7.10098,-0.49419,7.24448,0.63847]}, - {"t":0.91594, "x":2.38373, "y":2.48992, "heading":2.21201, "vx":-1.75494, "vy":-2.05073, "omega":-2.03638, "ax":-0.0016, "ay":0.00138, "alpha":1.18556, "fx":[-1.16316,7.8011,1.10985,-7.8539], "fy":[-7.80474,-1.11363,7.85026,1.15938]}, - {"t":0.94753, "x":2.3283, "y":2.42515, "heading":2.14769, "vx":-1.75499, "vy":-2.05069, "omega":-1.99893, "ax":0.00116, "ay":-0.00099, "alpha":1.18556, "fx":[-1.61802,7.75729,1.65648,-7.71923], "fy":[-7.75456,-1.6536,7.72196,1.6209]}, - {"t":0.97911, "x":2.27287, "y":2.36038, "heading":2.08456, "vx":-1.75496, "vy":-2.05072, "omega":-1.96149, "ax":0.00314, "ay":-0.00269, "alpha":1.14314, "fx":[-1.99413,7.39858,2.09845,-7.29527], "fy":[-7.39133,-2.09078,7.30251,2.00179]}, - {"t":1.0107, "x":2.21744, "y":2.29561, "heading":2.0226, "vx":-1.75486, "vy":-2.0508, "omega":-1.92538, "ax":0.00453, "ay":-0.00388, "alpha":1.13504, "fx":[-2.40434,7.22954,2.55473,-7.0806], "fy":[-7.21923,-2.54371,7.09092,2.41534]}, - {"t":1.04228, "x":2.16202, "y":2.23083, "heading":1.96179, "vx":-1.75471, "vy":-2.05093, "omega":-1.88953, "ax":0.00542, "ay":-0.00466, "alpha":1.26869, "fx":[-3.16223,7.90345,3.34261,-7.72525], "fy":[-7.89141,-3.32937,7.73731,3.17545]}, - {"t":1.07386, "x":2.1066, "y":2.16605, "heading":1.90211, "vx":-1.75454, "vy":-2.05107, "omega":-1.84946, "ax":0.00553, "ay":-0.00477, "alpha":1.71678, "fx":[-4.93157,10.3834,5.11644,-10.20253], "fy":[-10.37196,-5.10257,10.214,4.9454]}, - {"t":1.10545, "x":2.05119, "y":2.10127, "heading":1.8437, "vx":-1.75437, "vy":-2.05122, "omega":-1.79524, "ax":0.00381, "ay":-0.00331, "alpha":2.71972, "fx":[-8.83275,15.87498,8.96209,-15.75239], "fy":[-15.86893,-8.95156,15.75846,8.84325]}, - {"t":1.13703, "x":1.99578, "y":2.03648, "heading":1.787, "vx":-1.75425, "vy":-2.05133, "omega":-1.70934, "ax":-0.00234, "ay":0.00195, "alpha":4.43098, "fx":[-15.97371,24.86476,15.89125,-24.93698], "fy":[-24.86748,-15.90152,24.93427,15.96343]}, - {"t":1.16862, "x":1.94037, "y":1.97169, "heading":1.73301, "vx":-1.75432, "vy":-2.05127, "omega":-1.56939, "ax":-0.0209, "ay":0.01784, "alpha":6.6261, "fx":[-26.19507,35.60014,25.40575,-36.1931], "fy":[-35.57278,-25.5343,36.22143,26.06527]}, - {"t":1.2002, "x":1.88495, "y":1.90692, "heading":1.68344, "vx":-1.75498, "vy":-2.0507, "omega":-1.36011, "ax":-0.07595, "ay":0.06505, "alpha":8.79117, "fx":[-38.10165,44.90766,35.00104,-46.82995], "fy":[-44.58089,-35.68727,47.16789,37.40194]}, - {"t":1.23179, "x":1.82949, "y":1.84218, "heading":1.64048, "vx":-1.75738, "vy":-2.04865, "omega":-1.08245, "ax":-0.22716, "ay":0.19554, "alpha":10.57486, "fx":[-51.25365,50.65909,41.3577,-55.78593], "fy":[-48.97711,-44.09368,57.53736,48.46505]}, - {"t":1.26337, "x":1.77387, "y":1.77757, "heading":1.6063, "vx":-1.76455, "vy":-2.04247, "omega":-0.74845, "ax":-0.60145, "ay":0.52455, "alpha":11.80844, "fx":[-67.30681,51.3278,40.0132,-63.81036], "fy":[-45.36487,-48.74758,70.08512,58.71777]}, - {"t":1.29495, "x":1.71783, "y":1.71332, "heading":1.58266, "vx":-1.78355, "vy":-2.0259, "omega":-0.37549, "ax":-1.41914, "ay":1.30479, "alpha":11.88845, "fx":[-87.26047,41.06549,23.58353,-71.24171], "fy":[-26.87501,-42.72764,87.58376,68.30946]}, - {"t":1.32654, "x":1.66079, "y":1.64999, "heading":1.5708, "vx":-1.82837, "vy":-1.98469, "omega":0.0, "ax":-1.97728, "ay":1.86969, "alpha":0.0, "fx":[-32.69128,-32.69128,-32.69128,-32.69128], "fy":[30.91241,30.91241,30.91241,30.91241]}, - {"t":1.36728, "x":1.58467, "y":1.57068, "heading":1.5708, "vx":-1.90893, "vy":-1.90853, "omega":0.0, "ax":-0.25749, "ay":0.25896, "alpha":0.0, "fx":[-4.25712,-4.25712,-4.25712,-4.25712], "fy":[4.28146,4.28146,4.28146,4.28146]}, - {"t":1.40802, "x":1.50669, "y":1.49315, "heading":1.5708, "vx":-1.91942, "vy":-1.89798, "omega":0.0, "ax":-0.03005, "ay":0.03041, "alpha":0.0, "fx":[-0.4968,-0.4968,-0.4968,-0.4968], "fy":[0.50273,0.50273,0.50273,0.50273]}, - {"t":1.44875, "x":1.42847, "y":1.41585, "heading":1.5708, "vx":-1.92064, "vy":-1.89674, "omega":0.0, "ax":-0.00351, "ay":0.00355, "alpha":0.0, "fx":[-0.058,-0.058,-0.058,-0.058], "fy":[0.05874,0.05874,0.05874,0.05874]}, - {"t":1.48949, "x":1.35022, "y":1.33859, "heading":1.5708, "vx":-1.92078, "vy":-1.89659, "omega":0.0, "ax":-0.00041, "ay":0.00041, "alpha":0.0, "fx":[-0.00677,-0.00677,-0.00677,-0.00677], "fy":[0.00686,0.00686,0.00686,0.00686]}, - {"t":1.53023, "x":1.27197, "y":1.26132, "heading":1.5708, "vx":-1.9208, "vy":-1.89658, "omega":0.0, "ax":-0.00005, "ay":0.00005, "alpha":0.0, "fx":[-0.00079,-0.00079,-0.00079,-0.00079], "fy":[0.0008,0.0008,0.0008,0.0008]}, - {"t":1.57097, "x":1.19372, "y":1.18406, "heading":1.5708, "vx":-1.9208, "vy":-1.89657, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[0.00009,0.00009,0.00009,0.00009]}, - {"t":1.61171, "x":1.11547, "y":1.10679, "heading":1.5708, "vx":-1.9208, "vy":-1.89657, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.00001,0.00001,0.00001,0.00001]}, - {"t":1.65245, "x":1.03722, "y":1.02953, "heading":1.5708, "vx":-1.9208, "vy":-1.89657, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.69319, "x":0.95897, "y":0.95227, "heading":1.5708, "vx":-1.9208, "vy":-1.89657, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.73392, "x":0.88072, "y":0.875, "heading":1.5708, "vx":-1.9208, "vy":-1.89657, "omega":0.0, "ax":0.00051, "ay":0.0005, "alpha":0.0, "fx":[0.00835,0.00835,0.00835,0.00835], "fy":[0.00825,0.00825,0.00825,0.00825]}, - {"t":1.77466, "x":0.80247, "y":0.79774, "heading":1.5708, "vx":-1.92078, "vy":-1.89655, "omega":0.0, "ax":4.38714, "ay":4.3318, "alpha":0.0, "fx":[72.53456,72.53456,72.53456,72.53456], "fy":[71.61964,71.61964,71.61964,71.61964]}, - {"t":1.8154, "x":0.72786, "y":0.72407, "heading":1.5708, "vx":-1.74205, "vy":-1.72008, "omega":0.0, "ax":5.31659, "ay":5.24953, "alpha":0.0, "fx":[87.9015,87.9015,87.9015,87.9015], "fy":[86.79276,86.79276,86.79276,86.79276]}, - {"t":1.85614, "x":0.6613, "y":0.65835, "heading":1.5708, "vx":-1.52546, "vy":-1.50622, "omega":0.0, "ax":5.338, "ay":5.27067, "alpha":0.0, "fx":[88.25554,88.25554,88.25554,88.25554], "fy":[87.14233,87.14233,87.14233,87.14233]}, - {"t":1.89688, "x":0.60358, "y":0.60136, "heading":1.5708, "vx":-1.308, "vy":-1.2915, "omega":0.0, "ax":5.34535, "ay":5.27793, "alpha":0.0, "fx":[88.37708,88.37708,88.37708,88.37708], "fy":[87.26233,87.26233,87.26233,87.26233]}, - {"t":1.93762, "x":0.55473, "y":0.55313, "heading":1.5708, "vx":-1.09024, "vy":-1.07649, "omega":0.0, "ax":5.34907, "ay":5.2816, "alpha":0.0, "fx":[88.43851,88.43851,88.43851,88.43851], "fy":[87.32299,87.32299,87.32299,87.32299]}, - {"t":1.97836, "x":0.51476, "y":0.51366, "heading":1.5708, "vx":-0.87232, "vy":-0.86132, "omega":0.0, "ax":5.35131, "ay":5.28381, "alpha":0.0, "fx":[88.47559,88.47559,88.47559,88.47559], "fy":[87.3596,87.3596,87.3596,87.3596]}, - {"t":2.0191, "x":0.48366, "y":0.48295, "heading":1.5708, "vx":-0.65432, "vy":-0.64607, "omega":0.0, "ax":5.35281, "ay":5.28529, "alpha":0.0, "fx":[88.50039,88.50039,88.50039,88.50039], "fy":[87.38409,87.38409,87.38409,87.38409]}, - {"t":2.05983, "x":0.46145, "y":0.46102, "heading":1.5708, "vx":-0.43625, "vy":-0.43075, "omega":0.0, "ax":5.35389, "ay":5.28635, "alpha":0.0, "fx":[88.51815,88.51815,88.51815,88.51815], "fy":[87.40162,87.40162,87.40162,87.40162]}, - {"t":2.10057, "x":0.44812, "y":0.44786, "heading":1.5708, "vx":-0.21814, "vy":-0.21539, "omega":0.0, "ax":5.35469, "ay":5.28715, "alpha":0.0, "fx":[88.53149,88.53149,88.53149,88.53149], "fy":[87.4148,87.4148,87.4148,87.4148]}, - {"t":2.14131, "x":0.44367, "y":0.44347, "heading":1.5708, "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]}], + {"t":0.0, "x":3.67459, "y":4.00001, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.89131, "ay":-5.71932, "alpha":0.0, "fx":[-80.87018,-80.87018,-80.87018,-80.87018], "fy":[-94.56,-94.56,-94.56,-94.56]}, + {"t":0.04279, "x":3.67012, "y":3.99477, "heading":1.5708, "vx":-0.20932, "vy":-0.24475, "omega":0.0, "ax":-4.89062, "ay":-5.71851, "alpha":0.0, "fx":[-80.85883,-80.85883,-80.85883,-80.85883], "fy":[-94.54672,-94.54672,-94.54672,-94.54672]}, + {"t":0.08559, "x":3.65668, "y":3.97906, "heading":1.5708, "vx":-0.41861, "vy":-0.48947, "omega":0.0, "ax":-4.88971, "ay":-5.71744, "alpha":0.0, "fx":[-80.84369,-80.84369,-80.84369,-80.84369], "fy":[-94.52902,-94.52902,-94.52902,-94.52902]}, + {"t":0.12838, "x":3.63429, "y":3.95288, "heading":1.5708, "vx":-0.62786, "vy":-0.73414, "omega":0.0, "ax":-4.88843, "ay":-5.71595, "alpha":0.0, "fx":[-80.8225,-80.8225,-80.8225,-80.8225], "fy":[-94.50425,-94.50425,-94.50425,-94.50425]}, + {"t":0.17118, "x":3.60294, "y":3.91623, "heading":1.5708, "vx":-0.83705, "vy":-0.97875, "omega":0.0, "ax":-4.8865, "ay":-5.7137, "alpha":0.0, "fx":[-80.79073,-80.79073,-80.79073,-80.79073], "fy":[-94.4671,-94.4671,-94.4671,-94.4671]}, + {"t":0.21397, "x":3.56265, "y":3.86911, "heading":1.5708, "vx":-1.04617, "vy":-1.22326, "omega":0.0, "ax":-4.8833, "ay":-5.70996, "alpha":0.0, "fx":[-80.73782,-80.73782,-80.73782,-80.73782], "fy":[-94.40524,-94.40524,-94.40524,-94.40524]}, + {"t":0.25676, "x":3.51341, "y":3.81153, "heading":1.5708, "vx":-1.25514, "vy":-1.46762, "omega":0.0, "ax":-4.87691, "ay":-5.70249, "alpha":0.0, "fx":[-80.63218,-80.63218,-80.63218,-80.63218], "fy":[-94.28171,-94.28171,-94.28171,-94.28171]}, + {"t":0.29956, "x":3.45523, "y":3.74351, "heading":1.5708, "vx":-1.46385, "vy":-1.71165, "omega":0.0, "ax":-4.85785, "ay":-5.68019, "alpha":0.0, "fx":[-80.31695,-80.31695,-80.31695,-80.31695], "fy":[-93.91312,-93.91312,-93.91312,-93.91312]}, + {"t":0.34235, "x":3.38814, "y":3.66506, "heading":1.5708, "vx":-1.67173, "vy":-1.95473, "omega":0.0, "ax":-1.93299, "ay":-2.26021, "alpha":0.0, "fx":[-31.95901,-31.95901,-31.95901,-31.95901], "fy":[-37.36907,-37.36907,-37.36907,-37.36907]}, + {"t":0.38515, "x":3.31483, "y":3.57934, "heading":1.5708, "vx":-1.75445, "vy":-2.05145, "omega":0.0, "ax":-0.00007, "ay":-0.00008, "alpha":0.0, "fx":[-0.00118,-0.00118,-0.00118,-0.00118], "fy":[-0.00138,-0.00138,-0.00138,-0.00138]}, + {"t":0.42794, "x":3.23975, "y":3.49155, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.47073, "x":3.16467, "y":3.40376, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.51353, "x":3.08959, "y":3.31597, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.55632, "x":3.01451, "y":3.22818, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.59912, "x":2.93943, "y":3.14039, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.64191, "x":2.86435, "y":3.0526, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.6847, "x":2.78927, "y":2.96481, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.7275, "x":2.71419, "y":2.87702, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.77029, "x":2.63911, "y":2.78923, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.81309, "x":2.56403, "y":2.70144, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.85588, "x":2.48894, "y":2.61365, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.89867, "x":2.41386, "y":2.52586, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.94147, "x":2.33878, "y":2.43807, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "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.98426, "x":2.2637, "y":2.35028, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.02706, "x":2.18862, "y":2.26249, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00004,-0.00004,-0.00004,-0.00004], "fy":[0.00004,0.00004,0.00004,0.00004]}, + {"t":1.06985, "x":2.11354, "y":2.1747, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "omega":0.0, "ax":-0.00003, "ay":0.00002, "alpha":0.0, "fx":[-0.00042,-0.00042,-0.00042,-0.00042], "fy":[0.00036,0.00036,0.00036,0.00036]}, + {"t":1.11264, "x":2.03846, "y":2.08691, "heading":1.5708, "vx":-1.75446, "vy":-2.05145, "omega":0.0, "ax":-0.00024, "ay":0.00021, "alpha":0.0, "fx":[-0.00399,-0.00399,-0.00399,-0.00399], "fy":[0.00341,0.00341,0.00341,0.00341]}, + {"t":1.15544, "x":1.96338, "y":1.99912, "heading":1.5708, "vx":-1.75447, "vy":-2.05144, "omega":0.0, "ax":-0.00229, "ay":0.00196, "alpha":0.0, "fx":[-0.03791,-0.03791,-0.03791,-0.03791], "fy":[0.03242,0.03242,0.03242,0.03242]}, + {"t":1.19823, "x":1.8883, "y":1.91133, "heading":1.5708, "vx":-1.75457, "vy":-2.05136, "omega":0.0, "ax":-0.02178, "ay":0.01864, "alpha":0.0, "fx":[-0.36012,-0.36012,-0.36012,-0.36012], "fy":[0.30816,0.30816,0.30816,0.30816]}, + {"t":1.24103, "x":1.8132, "y":1.82356, "heading":1.5708, "vx":-1.7555, "vy":-2.05056, "omega":0.0, "ax":-0.2063, "ay":0.17739, "alpha":0.0, "fx":[-3.41081,-3.41081,-3.41081,-3.41081], "fy":[2.93284,2.93284,2.93284,2.93284]}, + {"t":1.28382, "x":1.73788, "y":1.73597, "heading":1.5708, "vx":-1.76433, "vy":-2.04297, "omega":0.0, "ax":-1.73046, "ay":1.57192, "alpha":0.0, "fx":[-28.61052,-28.61052,-28.61052,-28.61052], "fy":[25.98924,25.98924,25.98924,25.98924]}, + {"t":1.32661, "x":1.66079, "y":1.64999, "heading":1.5708, "vx":-1.83838, "vy":-1.9757, "omega":0.0, "ax":-1.75986, "ay":1.67718, "alpha":0.0, "fx":[-29.0965,-29.0965,-29.0965,-29.0965], "fy":[27.72954,27.72954,27.72954,27.72954]}, + {"t":1.36735, "x":1.58444, "y":1.57089, "heading":1.5708, "vx":-1.91007, "vy":-1.90738, "omega":0.0, "ax":-0.22326, "ay":0.22464, "alpha":0.0, "fx":[-3.69122,-3.69122,-3.69122,-3.69122], "fy":[3.71409,3.71409,3.71409,3.71409]}, + {"t":1.40809, "x":1.50644, "y":1.49337, "heading":1.5708, "vx":-1.91917, "vy":-1.89822, "omega":0.0, "ax":-0.02605, "ay":0.02635, "alpha":0.0, "fx":[-0.43069,-0.43069,-0.43069,-0.43069], "fy":[0.43568,0.43568,0.43568,0.43568]}, + {"t":1.44883, "x":1.42824, "y":1.41607, "heading":1.5708, "vx":-1.92023, "vy":-1.89715, "omega":0.0, "ax":-0.00304, "ay":0.00308, "alpha":0.0, "fx":[-0.05029,-0.05029,-0.05029,-0.05029], "fy":[0.0509,0.0509,0.0509,0.0509]}, + {"t":1.48957, "x":1.35001, "y":1.33878, "heading":1.5708, "vx":-1.92035, "vy":-1.89703, "omega":0.0, "ax":-0.00036, "ay":0.00036, "alpha":0.0, "fx":[-0.00587,-0.00587,-0.00587,-0.00587], "fy":[0.00595,0.00595,0.00595,0.00595]}, + {"t":1.53031, "x":1.27178, "y":1.2615, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":-0.00004, "ay":0.00004, "alpha":0.0, "fx":[-0.00069,-0.00069,-0.00069,-0.00069], "fy":[0.00069,0.00069,0.00069,0.00069]}, + {"t":1.57104, "x":1.19354, "y":1.18422, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00008,-0.00008,-0.00008,-0.00008], "fy":[0.00008,0.00008,0.00008,0.00008]}, + {"t":1.61178, "x":1.11531, "y":1.10694, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":1.65252, "x":1.03708, "y":1.02966, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69326, "x":0.95885, "y":0.95238, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.734, "x":0.88061, "y":0.8751, "heading":1.5708, "vx":-1.92037, "vy":-1.89701, "omega":0.0, "ax":0.00051, "ay":0.0005, "alpha":0.0, "fx":[0.00835,0.00835,0.00835,0.00835], "fy":[0.00825,0.00825,0.00825,0.00825]}, + {"t":1.77474, "x":0.80238, "y":0.79781, "heading":1.5708, "vx":-1.92035, "vy":-1.89699, "omega":0.0, "ax":4.38652, "ay":4.33316, "alpha":0.0, "fx":[72.52431,72.52431,72.52431,72.52431], "fy":[71.64206,71.64206,71.64206,71.64206]}, + {"t":1.81547, "x":0.72779, "y":0.72413, "heading":1.5708, "vx":-1.74165, "vy":-1.72046, "omega":0.0, "ax":5.3154, "ay":5.25074, "alpha":0.0, "fx":[87.88179,87.88179,87.88179,87.88179], "fy":[86.81272,86.81272,86.81272,86.81272]}, + {"t":1.85621, "x":0.66125, "y":0.6584, "heading":1.5708, "vx":-1.52511, "vy":-1.50656, "omega":0.0, "ax":5.3368, "ay":5.27188, "alpha":0.0, "fx":[88.23574,88.23574,88.23574,88.23574], "fy":[87.16237,87.16237,87.16237,87.16237]}, + {"t":1.89695, "x":0.60355, "y":0.6014, "heading":1.5708, "vx":-1.3077, "vy":-1.29179, "omega":0.0, "ax":5.34415, "ay":5.27914, "alpha":0.0, "fx":[88.35725,88.35725,88.35725,88.35725], "fy":[87.2824,87.2824,87.2824,87.2824]}, + {"t":1.93769, "x":0.55471, "y":0.55315, "heading":1.5708, "vx":-1.08999, "vy":-1.07673, "omega":0.0, "ax":5.34787, "ay":5.28281, "alpha":0.0, "fx":[88.41867,88.41867,88.41867,88.41867], "fy":[87.34308,87.34308,87.34308,87.34308]}, + {"t":1.97843, "x":0.51474, "y":0.51367, "heading":1.5708, "vx":-0.87212, "vy":-0.86151, "omega":0.0, "ax":5.35011, "ay":5.28503, "alpha":0.0, "fx":[88.45574,88.45574,88.45574,88.45574], "fy":[87.37969,87.37969,87.37969,87.37969]}, + {"t":2.01917, "x":0.48365, "y":0.48296, "heading":1.5708, "vx":-0.65417, "vy":-0.64621, "omega":0.0, "ax":5.35161, "ay":5.28651, "alpha":0.0, "fx":[88.48053,88.48053,88.48053,88.48053], "fy":[87.40419,87.40419,87.40419,87.40419]}, + {"t":2.0599, "x":0.46144, "y":0.46102, "heading":1.5708, "vx":-0.43615, "vy":-0.43085, "omega":0.0, "ax":5.35268, "ay":5.28757, "alpha":0.0, "fx":[88.49829,88.49829,88.49829,88.49829], "fy":[87.42173,87.42173,87.42173,87.42173]}, + {"t":2.10064, "x":0.44812, "y":0.44786, "heading":1.5708, "vx":-0.21809, "vy":-0.21544, "omega":0.0, "ax":5.35349, "ay":5.28837, "alpha":0.0, "fx":[88.51163,88.51163,88.51163,88.51163], "fy":[87.4349,87.4349,87.4349,87.4349]}, + {"t":2.14138, "x":0.44367, "y":0.44347, "heading":1.5708, "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/tagmaps/field_map_mar_27_18_47_20.json b/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json index ec8e9af..6681a1e 100644 --- a/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json +++ b/src/main/deploy/tagmaps/field_map_mar_27_18_47_20.json @@ -1,585 +1,585 @@ { - "tags" : [ + "tags": [ { - "ID" : 1, - "pose" : { - "translation" : { - "x" : 4.822815895080566, - "y" : 7.438567638397217, - "z" : 0.8728957772254944 + "ID": 1, + "pose": { + "translation": { + "x": 4.822815895080566, + "y": 7.438567638397217, + "z": 0.8728957772254944 }, - "rotation" : { - "quaternion" : { - "W" : 0.0037721414119005203, - "X" : -0.006326864939182997, - "Y" : -0.00018497782002668828, - "Z" : 0.9999727606773376 + "rotation": { + "quaternion": { + "W": 0.0037721414119005203, + "X": -0.006326864939182997, + "Y": -0.00018497782002668828, + "Z": 0.9999727606773376 } } } }, { - "ID" : 2, - "pose" : { - "translation" : { - "x" : 11.8870210647583, - "y" : 4.671638011932373, - "z" : 1.1223087310791016 + "ID": 2, + "pose": { + "translation": { + "x": 11.8870210647583, + "y": 4.671638011932373, + "z": 1.1223087310791016 }, - "rotation" : { - "quaternion" : { - "W" : 0.7057818174362183, - "X" : 0.007269229739904404, - "Y" : -0.005886712111532688, - "Z" : 0.7083674073219299 + "rotation": { + "quaternion": { + "W": 0.7057818174362183, + "X": 0.007269229739904404, + "Y": -0.005886712111532688, + "Z": 0.7083674073219299 } } } }, { - "ID" : 3, - "pose" : { - "translation" : { - "x" : 4.076200485229492, - "y" : 4.363650321960449, - "z" : 1.1617555618286133 + "ID": 3, + "pose": { + "translation": { + "x": 4.076200485229492, + "y": 4.363650321960449, + "z": 1.1617555618286133 }, - "rotation" : { - "quaternion" : { - "W" : 0.004635084420442581, - "X" : 0.0006607414106838405, - "Y" : 0.0042945570312440395, - "Z" : 0.9999797940254211 + "rotation": { + "quaternion": { + "W": 0.004635084420442581, + "X": 0.0006607414106838405, + "Y": 0.0042945570312440395, + "Z": 0.9999797940254211 } } } }, { - "ID" : 4, - "pose" : { - "translation" : { - "x" : 4.075459003448486, - "y" : 4.005395412445068, - "z" : 1.1609687805175781 + "ID": 4, + "pose": { + "translation": { + "x": 4.075459003448486, + "y": 4.005395412445068, + "z": 1.1609687805175781 }, - "rotation" : { - "quaternion" : { - "W" : 0.005071163643151522, - "X" : 0.004584860987961292, - "Y" : 0.0010812842519953847, - "Z" : 0.9999760389328003 + "rotation": { + "quaternion": { + "W": 0.005071163643151522, + "X": 0.004584860987961292, + "Y": 0.0010812842519953847, + "Z": 0.9999760389328003 } } } }, { - "ID" : 5, - "pose" : { - "translation" : { - "x" : 4.702306747436523, - "y" : 3.38472580909729, - "z" : 1.1592590808868408 + "ID": 5, + "pose": { + "translation": { + "x": 4.702306747436523, + "y": 3.38472580909729, + "z": 1.1592590808868408 }, - "rotation" : { - "quaternion" : { - "W" : 0.7048313617706299, - "X" : 0.0046210805885493755, - "Y" : -0.0013557852944359183, - "Z" : -0.7093584537506104 + "rotation": { + "quaternion": { + "W": 0.7048313617706299, + "X": 0.0046210805885493755, + "Y": -0.0013557852944359183, + "Z": -0.7093584537506104 } } } }, { - "ID" : 6, - "pose" : { - "translation" : { - "x" : 11.87798023223877, - "y" : 0.6444995999336243, - "z" : 0.8889999985694885 + "ID": 6, + "pose": { + "translation": { + "x": 11.87798023223877, + "y": 0.6444995999336243, + "z": 0.8889999985694885 }, - "rotation" : { - "quaternion" : { - "W" : 0.0, - "X" : 0.0, - "Y" : 0.0, - "Z" : 1.0 + "rotation": { + "quaternion": { + "W": 0.0, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 } } } }, { - "ID" : 7, - "pose" : { - "translation" : { - "x" : 4.7464823722839355, - "y" : 0.6324626803398132, - "z" : 0.9177830219268799 + "ID": 7, + "pose": { + "translation": { + "x": 4.7464823722839355, + "y": 0.6324626803398132, + "z": 0.9177830219268799 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999463558197021, - "X" : 0.004018927924335003, - "Y" : -0.008416394703090191, - "Z" : 0.00450051948428154 + "rotation": { + "quaternion": { + "W": 0.9999463558197021, + "X": 0.004018927924335003, + "Y": -0.008416394703090191, + "Z": 0.00450051948428154 } } } }, { - "ID" : 8, - "pose" : { - "translation" : { - "x" : 5.055703639984131, - "y" : 3.3834192752838135, - "z" : 1.160678505897522 + "ID": 8, + "pose": { + "translation": { + "x": 5.055703639984131, + "y": 3.3834192752838135, + "z": 1.160678505897522 }, - "rotation" : { - "quaternion" : { - "W" : 0.7026873826980591, - "X" : 0.001975176390260458, - "Y" : -0.004470320884138346, - "Z" : -0.7114818692207336 + "rotation": { + "quaternion": { + "W": 0.7026873826980591, + "X": 0.001975176390260458, + "Y": -0.004470320884138346, + "Z": -0.7114818692207336 } } } }, { - "ID" : 9, - "pose" : { - "translation" : { - "x" : 5.304318904876709, - "y" : 3.636298179626465, - "z" : 1.1557623147964478 + "ID": 9, + "pose": { + "translation": { + "x": 5.304318904876709, + "y": 3.636298179626465, + "z": 1.1557623147964478 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999735355377197, - "X" : 0.004210796672850847, - "Y" : -0.005924228113144636, - "Z" : 0.0002991016663145274 + "rotation": { + "quaternion": { + "W": 0.9999735355377197, + "X": 0.004210796672850847, + "Y": -0.005924228113144636, + "Z": 0.0002991016663145274 } } } }, { - "ID" : 10, - "pose" : { - "translation" : { - "x" : 5.3059587478637695, - "y" : 3.9924886226654053, - "z" : 1.1551718711853027 + "ID": 10, + "pose": { + "translation": { + "x": 5.3059587478637695, + "y": 3.9924886226654053, + "z": 1.1551718711853027 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999508261680603, - "X" : 0.0020055281929671764, - "Y" : -0.008682952262461185, - "Z" : 0.004347731824964285 + "rotation": { + "quaternion": { + "W": 0.9999508261680603, + "X": 0.0020055281929671764, + "Y": -0.008682952262461185, + "Z": 0.004347731824964285 } } } }, { - "ID" : 11, - "pose" : { - "translation" : { - "x" : 12.242338180541992, - "y" : 4.668259620666504, - "z" : 1.1203622817993164 + "ID": 11, + "pose": { + "translation": { + "x": 12.242338180541992, + "y": 4.668259620666504, + "z": 1.1203622817993164 }, - "rotation" : { - "quaternion" : { - "W" : 0.705532968044281, - "X" : 0.0018324467819184065, - "Y" : -0.0005836429772898555, - "Z" : 0.7086744904518127 + "rotation": { + "quaternion": { + "W": 0.705532968044281, + "X": 0.0018324467819184065, + "Y": -0.0005836429772898555, + "Z": 0.7086744904518127 } } } }, { - "ID" : 12, - "pose" : { - "translation" : { - "x" : 4.8416900634765625, - "y" : 7.216125965118408, - "z" : 0.9207862019538879 + "ID": 12, + "pose": { + "translation": { + "x": 4.8416900634765625, + "y": 7.216125965118408, + "z": 0.9207862019538879 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999958276748657, - "X" : 0.0027650343254208565, - "Y" : 5.1801976951537654e-05, - "Z" : -0.0007708670455031097 + "rotation": { + "quaternion": { + "W": 0.9999958276748657, + "X": 0.0027650343254208565, + "Y": 5.1801976951537654e-05, + "Z": -0.0007708670455031097 } } } }, { - "ID" : 13, - "pose" : { - "translation" : { - "x" : 9.405987739562988, - "y" : 7.152761936187744, - "z" : 0.5603619813919067 + "ID": 13, + "pose": { + "translation": { + "x": 9.405987739562988, + "y": 7.152761936187744, + "z": 0.5603619813919067 }, - "rotation" : { - "quaternion" : { - "W" : -0.001173483906313777, - "X" : 0.008173578418791294, - "Y" : -0.0002410715533187613, - "Z" : 0.9999658465385437 + "rotation": { + "quaternion": { + "W": -0.001173483906313777, + "X": 0.008173578418791294, + "Y": -0.0002410715533187613, + "Z": 0.9999658465385437 } } } }, { - "ID" : 14, - "pose" : { - "translation" : { - "x" : 9.405714988708496, - "y" : 6.721652507781982, - "z" : 0.561457633972168 + "ID": 14, + "pose": { + "translation": { + "x": 9.405714988708496, + "y": 6.721652507781982, + "z": 0.561457633972168 }, - "rotation" : { - "quaternion" : { - "W" : 0.0004049946437589824, - "X" : 0.006965869106352329, - "Y" : 0.001270489883609116, - "Z" : 0.9999748468399048 + "rotation": { + "quaternion": { + "W": 0.0004049946437589824, + "X": 0.006965869106352329, + "Y": 0.001270489883609116, + "Z": 0.9999748468399048 } } } }, { - "ID" : 15, - "pose" : { - "translation" : { - "x" : 9.392133712768555, - "y" : 4.136072635650635, - "z" : 0.5631878972053528 + "ID": 15, + "pose": { + "translation": { + "x": 9.392133712768555, + "y": 4.136072635650635, + "z": 0.5631878972053528 }, - "rotation" : { - "quaternion" : { - "W" : 0.007338069844990969, - "X" : 0.015838444232940674, - "Y" : 0.006142545025795698, - "Z" : 0.9998286962509155 + "rotation": { + "quaternion": { + "W": 0.007338069844990969, + "X": 0.015838444232940674, + "Y": 0.006142545025795698, + "Z": 0.9998286962509155 } } } }, { - "ID" : 16, - "pose" : { - "translation" : { - "x" : 9.381674766540527, - "y" : 3.7076895236968994, - "z" : 0.5665956735610962 + "ID": 16, + "pose": { + "translation": { + "x": 9.381674766540527, + "y": 3.7076895236968994, + "z": 0.5665956735610962 }, - "rotation" : { - "quaternion" : { - "W" : 0.024910936132073402, - "X" : 0.016272999346256256, - "Y" : 0.005707239266484976, - "Z" : 0.9995409250259399 + "rotation": { + "quaternion": { + "W": 0.024910936132073402, + "X": 0.016272999346256256, + "Y": 0.005707239266484976, + "Z": 0.9995409250259399 } } } }, { - "ID" : 17, - "pose" : { - "translation" : { - "x" : -2.504611015319824, - "y" : 0.8694211840629578, - "z" : 0.9030297994613647 + "ID": 17, + "pose": { + "translation": { + "x": -2.504611015319824, + "y": 0.8694211840629578, + "z": 0.9030297994613647 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999359846115112, - "X" : 0.0024782754480838776, - "Y" : -0.003686416195705533, - "Z" : -0.010405427776277065 + "rotation": { + "quaternion": { + "W": 0.9999359846115112, + "X": 0.0024782754480838776, + "Y": -0.003686416195705533, + "Z": -0.010405427776277065 } } } }, { - "ID" : 18, - "pose" : { - "translation" : { - "x" : 4.714351177215576, - "y" : 3.364177703857422, - "z" : 1.1612824201583862 + "ID": 18, + "pose": { + "translation": { + "x": 4.714351177215576, + "y": 3.364177703857422, + "z": 1.1612824201583862 }, - "rotation" : { - "quaternion" : { - "W" : 0.7115345001220703, - "X" : -0.002445084974169731, - "Y" : -0.000266425748122856, - "Z" : -0.7026468515396118 + "rotation": { + "quaternion": { + "W": 0.7115345001220703, + "X": -0.002445084974169731, + "Y": -0.000266425748122856, + "Z": -0.7026468515396118 } } } }, { - "ID" : 19, - "pose" : { - "translation" : { - "x" : 5.305013179779053, - "y" : 3.635739803314209, - "z" : 1.1534665822982788 + "ID": 19, + "pose": { + "translation": { + "x": 5.305013179779053, + "y": 3.635739803314209, + "z": 1.1534665822982788 }, - "rotation" : { - "quaternion" : { - "W" : 0.999992847442627, - "X" : 0.0029923561960458755, - "Y" : 0.0023054699413478374, - "Z" : -0.0002783012459985912 + "rotation": { + "quaternion": { + "W": 0.999992847442627, + "X": 0.0029923561960458755, + "Y": 0.0023054699413478374, + "Z": -0.0002783012459985912 } } } }, { - "ID" : 20, - "pose" : { - "translation" : { - "x" : 5.304501056671143, - "y" : 3.9921987056732178, - "z" : 1.1584936380386353 + "ID": 20, + "pose": { + "translation": { + "x": 5.304501056671143, + "y": 3.9921987056732178, + "z": 1.1584936380386353 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999909400939941, - "X" : 0.0030655476730316877, - "Y" : 0.001734467106871307, - "Z" : 0.002377011813223362 + "rotation": { + "quaternion": { + "W": 0.9999909400939941, + "X": 0.0030655476730316877, + "Y": 0.001734467106871307, + "Z": 0.002377011813223362 } } } }, { - "ID" : 21, - "pose" : { - "translation" : { - "x" : 4.686933994293213, - "y" : 4.584085941314697, - "z" : 1.1613006591796875 + "ID": 21, + "pose": { + "translation": { + "x": 4.686933994293213, + "y": 4.584085941314697, + "z": 1.1613006591796875 }, - "rotation" : { - "quaternion" : { - "W" : 0.7082415819168091, - "X" : 0.0033884502481669188, - "Y" : 0.00023085897555574775, - "Z" : 0.7059620022773743 + "rotation": { + "quaternion": { + "W": 0.7082415819168091, + "X": 0.0033884502481669188, + "Y": 0.00023085897555574775, + "Z": 0.7059620022773743 } } } }, { - "ID" : 22, - "pose" : { - "translation" : { - "x" : 4.734961986541748, - "y" : 7.35674524307251, - "z" : 0.9273542761802673 + "ID": 22, + "pose": { + "translation": { + "x": 4.734961986541748, + "y": 7.35674524307251, + "z": 0.9273542761802673 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999683499336243, - "X" : -0.0004531952436082065, - "Y" : 0.006745198275893927, - "Z" : 0.004174919333308935 + "rotation": { + "quaternion": { + "W": 0.9999683499336243, + "X": -0.0004531952436082065, + "Y": 0.006745198275893927, + "Z": 0.004174919333308935 } } } }, { - "ID" : 23, - "pose" : { - "translation" : { - "x" : 4.639162063598633, - "y" : 7.372425556182861, - "z" : 0.9242526292800903 + "ID": 23, + "pose": { + "translation": { + "x": 4.639162063598633, + "y": 7.372425556182861, + "z": 0.9242526292800903 }, - "rotation" : { - "quaternion" : { - "W" : -0.0031200069934129715, - "X" : 0.004842787049710751, - "Y" : 0.001103776739910245, - "Z" : 0.9999827742576599 + "rotation": { + "quaternion": { + "W": -0.0031200069934129715, + "X": 0.004842787049710751, + "Y": 0.001103776739910245, + "Z": 0.9999827742576599 } } } }, { - "ID" : 24, - "pose" : { - "translation" : { - "x" : 4.3320207595825195, - "y" : 4.583142280578613, - "z" : 1.1634548902511597 + "ID": 24, + "pose": { + "translation": { + "x": 4.3320207595825195, + "y": 4.583142280578613, + "z": 1.1634548902511597 }, - "rotation" : { - "quaternion" : { - "W" : 0.7041061520576477, - "X" : 0.0010072175646200776, - "Y" : -0.003978305030614138, - "Z" : 0.7100829482078552 + "rotation": { + "quaternion": { + "W": 0.7041061520576477, + "X": 0.0010072175646200776, + "Y": -0.003978305030614138, + "Z": 0.7100829482078552 } } } }, { - "ID" : 25, - "pose" : { - "translation" : { - "x" : 4.085196018218994, - "y" : 4.3233113288879395, - "z" : 1.1617553234100342 + "ID": 25, + "pose": { + "translation": { + "x": 4.085196018218994, + "y": 4.3233113288879395, + "z": 1.1617553234100342 }, - "rotation" : { - "quaternion" : { - "W" : -0.005810739006847143, - "X" : 0.002946239896118641, - "Y" : 0.006170302629470825, - "Z" : 0.9999597072601318 + "rotation": { + "quaternion": { + "W": -0.005810739006847143, + "X": 0.002946239896118641, + "Y": 0.006170302629470825, + "Z": 0.9999597072601318 } } } }, { - "ID" : 26, - "pose" : { - "translation" : { - "x" : 4.089456081390381, - "y" : 3.9735653400421143, - "z" : 1.1618210077285767 + "ID": 26, + "pose": { + "translation": { + "x": 4.089456081390381, + "y": 3.9735653400421143, + "z": 1.1618210077285767 }, - "rotation" : { - "quaternion" : { - "W" : -5.637028516503051e-05, - "X" : 0.00033929909113794565, - "Y" : 0.0005828236462548375, - "Z" : 0.9999997019767761 + "rotation": { + "quaternion": { + "W": -5.637028516503051e-05, + "X": 0.00033929909113794565, + "Y": 0.0005828236462548375, + "Z": 0.9999997019767761 } } } }, { - "ID" : 27, - "pose" : { - "translation" : { - "x" : 4.359224796295166, - "y" : 3.3653600215911865, - "z" : 1.1619951725006104 + "ID": 27, + "pose": { + "translation": { + "x": 4.359224796295166, + "y": 3.3653600215911865, + "z": 1.1619951725006104 }, - "rotation" : { - "quaternion" : { - "W" : 0.7068608403205872, - "X" : -0.0016970370197668672, - "Y" : 0.0007088514394126832, - "Z" : -0.7073500752449036 + "rotation": { + "quaternion": { + "W": 0.7068608403205872, + "X": -0.0016970370197668672, + "Y": 0.0007088514394126832, + "Z": -0.7073500752449036 } } } }, { - "ID" : 28, - "pose" : { - "translation" : { - "x" : 4.660392761230469, - "y" : 0.6959283947944641, - "z" : 0.9149965643882751 + "ID": 28, + "pose": { + "translation": { + "x": 4.660392761230469, + "y": 0.6959283947944641, + "z": 0.9149965643882751 }, - "rotation" : { - "quaternion" : { - "W" : -0.002637912053614855, - "X" : 0.0047381725162267685, - "Y" : -8.136600081343204e-05, - "Z" : 0.9999852776527405 + "rotation": { + "quaternion": { + "W": -0.002637912053614855, + "X": 0.0047381725162267685, + "Y": -8.136600081343204e-05, + "Z": 0.9999852776527405 } } } }, { - "ID" : 29, - "pose" : { - "translation" : { - "x" : 0.1180562973022461, - "y" : 0.6825624704360962, - "z" : 0.5843039751052856 + "ID": 29, + "pose": { + "translation": { + "x": 0.1180562973022461, + "y": 0.6825624704360962, + "z": 0.5843039751052856 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999812245368958, - "X" : 0.0013159263180568814, - "Y" : -0.005062768701463938, - "Z" : -0.003174516139551997 + "rotation": { + "quaternion": { + "W": 0.9999812245368958, + "X": 0.0013159263180568814, + "Y": -0.005062768701463938, + "Z": -0.003174516139551997 } } } }, { - "ID" : 30, - "pose" : { - "translation" : { - "x" : 0.11867809295654297, - "y" : 1.1131608486175537, - "z" : 0.5827512741088867 + "ID": 30, + "pose": { + "translation": { + "x": 0.11867809295654297, + "y": 1.1131608486175537, + "z": 0.5827512741088867 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999870657920837, - "X" : 0.0002074952208204195, - "Y" : -0.0050629121251404285, - "Z" : 8.447438449366018e-05 + "rotation": { + "quaternion": { + "W": 0.9999870657920837, + "X": 0.0002074952208204195, + "Y": -0.0050629121251404285, + "Z": 8.447438449366018e-05 } } } }, { - "ID" : 31, - "pose" : { - "translation" : { - "x" : 0.11109352111816406, - "y" : 3.748471736907959, - "z" : 0.5825525522232056 + "ID": 31, + "pose": { + "translation": { + "x": 0.11109352111816406, + "y": 3.748471736907959, + "z": 0.5825525522232056 }, - "rotation" : { - "quaternion" : { - "W" : 0.9997530579566956, - "X" : -0.0011874643387272954, - "Y" : -0.021945597603917122, - "Z" : 0.003259050427004695 + "rotation": { + "quaternion": { + "W": 0.9997530579566956, + "X": -0.0011874643387272954, + "Y": -0.021945597603917122, + "Z": 0.003259050427004695 } } } }, { - "ID" : 32, - "pose" : { - "translation" : { - "x" : 0.1185617446899414, - "y" : 4.175858974456787, - "z" : 0.5889672040939331 + "ID": 32, + "pose": { + "translation": { + "x": 0.1185617446899414, + "y": 4.175858974456787, + "z": 0.5889672040939331 }, - "rotation" : { - "quaternion" : { - "W" : 0.9999861717224121, - "X" : 0.0009740604436956346, - "Y" : 0.003140027867630124, - "Z" : 0.00408512307330966 + "rotation": { + "quaternion": { + "W": 0.9999861717224121, + "X": 0.0009740604436956346, + "Y": 0.003140027867630124, + "Z": 0.00408512307330966 } } } } ], - "field" : { - "length" : 16.541, - "width" : 8.069 + "field": { + "length": 16.541, + "width": 8.069 }, - "_comment" : "generated by PractiCal 1.0.5 (3) on 3/27/2026" -} \ No newline at end of file + "_comment": "generated by PractiCal 1.0.5 (3) on 3/27/2026" +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d0dac64..ba5e573 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -151,7 +151,7 @@ public enum RobotEdition { * This is for when we're testing shot and extension numbers and should be FALSE once bring up is * complete */ - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; public boolean hasZeroedSinceStartup = false; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 6eb40a6..c53070b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -22,7 +22,6 @@ 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.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import frc.robot.utils.autoaim.NewAutoAim.ShotParams; @@ -67,9 +66,6 @@ public class ShooterSubsystem extends SubsystemBase implements Shooter { (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); - private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); - private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); - private LinearFilter currentFilter = LinearFilter.movingAverage(10); @AutoLogOutput(key = "Shooter/Hood/Setpoint") diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8f63924..41d69d1 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -63,43 +63,43 @@ public class AutoAim { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO, // + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, From 4718b4250b00b578455cf02df88512540d43f58c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 09:20:41 -0700 Subject: [PATCH 04/22] go back to default atag field --- src/main/java/frc/robot/Robot.java | 5 ++ .../swerve/constants/SwerveConstants.java | 13 ++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 70 +++++++++++-------- 3 files changed, 53 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ba5e573..b7d5b8d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,6 +72,7 @@ import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; import frc.robot.utils.FuelSim; +import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.NewAutoAim; import java.io.File; import java.util.Arrays; @@ -766,6 +767,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta shooter::getTurretPosition, () -> Superstructure.getFeedTarget())); + operator.povRight().onTrue(Commands.runOnce(() -> AutoAim.incrementFudgeFactor())); + operator.povLeft().onTrue(Commands.runOnce(() -> AutoAim.decrementFudgeFactor())); + // create triggers for joystick disconnect alerts new Trigger(() -> DriverStation.isJoystickConnected(0)) .negate() @@ -882,6 +886,7 @@ public void robotPeriodic() { }); updateAlerts(); + Logger.recordOutput("Flywheel Fudge Factor", AutoAim.getFudgeFactor()); // Log climb poses Logger.recordOutput( diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index 547bfd2..c085ca6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -12,10 +12,8 @@ import edu.wpi.first.units.measure.Mass; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.Filesystem; import frc.robot.components.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.module.Module.ModuleConstants; -import java.io.File; public abstract class SwerveConstants { @@ -28,11 +26,12 @@ public abstract class SwerveConstants { public SwerveConstants() { try { fieldTags = - new AprilTagFieldLayout( - Filesystem.getDeployDirectory() - .toPath() - // .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json")); - .resolve("tagmaps" + File.separator + "field_map_mar_27_18_47_20.json")); + // new AprilTagFieldLayout( + // Filesystem.getDeployDirectory() + // .toPath() + // // .resolve("tagmaps" + File.separator + "field_map_mar_13_18_10_19.json")); + // .resolve("tagmaps" + File.separator + "field_map_mar_27_18_47_20.json")); + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); System.out.println("Successfully loaded tag map"); } catch (Exception e) { System.err.println("Failed to load custom tag map"); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 41d69d1..afee76c 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -30,6 +30,8 @@ public class AutoAim { public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-109.775391); + public static int fudgeFactor = 0; + static { // For hub shot tree ALPHA_HUB_SHOT_TREE.put( Units.inchesToMeters(24 + 17), new ShotData(Rotation2d.fromDegrees(8), 27.5, 1.46)); @@ -63,91 +65,91 @@ public class AutoAim { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, new ShotData( Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.24)); COMP_HUB_SHOT_TREE.put( 3.773999, new ShotData( Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.21)); COMP_HUB_SHOT_TREE.put( 3.899275, new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); COMP_HUB_SHOT_TREE.put( 4.138058, new ShotData( Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.13)); COMP_HUB_SHOT_TREE.put( 4.602258, new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.15)); COMP_HUB_SHOT_TREE.put( 4.893493, new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); COMP_HUB_SHOT_TREE.put( 5.225402, new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); COMP_HUB_SHOT_TREE.put( 5.584793, new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.17)); } @@ -183,80 +185,80 @@ public class AutoAim { Units.feetToMeters(18), new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.42)); // - 2, 1.42)); FEED_SHOT_TREE.put( Units.feetToMeters(20), new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.36)); // - 2, 1.36)); FEED_SHOT_TREE.put( Units.feetToMeters(22), new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.34)); // - 2, 1.34)); FEED_SHOT_TREE.put( Units.feetToMeters(24), new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.25)); // - 2, 1.25)); FEED_SHOT_TREE.put( Units.feetToMeters(26), new ShotData( Rotation2d.fromDegrees(48 - 13.16), - 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.28)); // - 2, 1.28)); FEED_SHOT_TREE.put( Units.feetToMeters(28), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.27)); // - 2, 1.27)); FEED_SHOT_TREE.put( Units.feetToMeters(30), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.32)); // - 2, 1.32)); FEED_SHOT_TREE.put( Units.feetToMeters(32), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.4)); // - 2, 1.4)); FEED_SHOT_TREE.put( Units.feetToMeters(34), new ShotData( Rotation2d.fromDegrees(52 - 13.16), - 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(36), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.33)); // - 2, 1.33)); FEED_SHOT_TREE.put( Units.feetToMeters(38), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(40), new ShotData( Rotation2d.fromDegrees(55 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); // - 2, 1.2)); FEED_SHOT_TREE.put( Units.feetToMeters(42), new ShotData( Rotation2d.fromDegrees(56 - 13.16), - 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, 1.2)); // - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight @@ -495,4 +497,16 @@ public static ShotData getRightFixedShotData() { public static ShotData getMidFixedShotData() { return new ShotData(Rotation2d.fromDegrees(32.84), 35, 0); } + + public static void incrementFudgeFactor() { + fudgeFactor++; + } + + public static void decrementFudgeFactor() { + fudgeFactor--; + } + + public static int getFudgeFactor() { + return fudgeFactor; + } } From 389f9d4061653b0ecdfe0c27c889119006da1385 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 09:21:58 -0700 Subject: [PATCH 05/22] fix fudge factor --- .../subsystems/shooter/TurretSubsystem.java | 4 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 58 +++++++++---------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 117cb1d..441b746 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -253,7 +253,7 @@ public Command feed( hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); })); } @@ -352,7 +352,7 @@ public Command score(Supplier shotParamsSupplier) { // case NONE: hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); // } })); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index afee76c..43fed1f 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -30,7 +30,7 @@ public class AutoAim { public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-109.775391); - public static int fudgeFactor = 0; + private static int fudgeFactor = 0; static { // For hub shot tree ALPHA_HUB_SHOT_TREE.put( @@ -65,91 +65,91 @@ public class AutoAim { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, new ShotData( Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.24)); COMP_HUB_SHOT_TREE.put( 3.773999, new ShotData( Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.21)); COMP_HUB_SHOT_TREE.put( 3.899275, new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); COMP_HUB_SHOT_TREE.put( 4.138058, new ShotData( Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.13)); COMP_HUB_SHOT_TREE.put( 4.602258, new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.15)); COMP_HUB_SHOT_TREE.put( 4.893493, new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); COMP_HUB_SHOT_TREE.put( 5.225402, new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); COMP_HUB_SHOT_TREE.put( 5.584793, new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.17)); } @@ -185,80 +185,80 @@ public class AutoAim { Units.feetToMeters(18), new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.42)); // - 2, 1.42)); FEED_SHOT_TREE.put( Units.feetToMeters(20), new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.36)); // - 2, 1.36)); FEED_SHOT_TREE.put( Units.feetToMeters(22), new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.34)); // - 2, 1.34)); FEED_SHOT_TREE.put( Units.feetToMeters(24), new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.25)); // - 2, 1.25)); FEED_SHOT_TREE.put( Units.feetToMeters(26), new ShotData( Rotation2d.fromDegrees(48 - 13.16), - 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.28)); // - 2, 1.28)); FEED_SHOT_TREE.put( Units.feetToMeters(28), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.27)); // - 2, 1.27)); FEED_SHOT_TREE.put( Units.feetToMeters(30), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.32)); // - 2, 1.32)); FEED_SHOT_TREE.put( Units.feetToMeters(32), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.4)); // - 2, 1.4)); FEED_SHOT_TREE.put( Units.feetToMeters(34), new ShotData( Rotation2d.fromDegrees(52 - 13.16), - 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(36), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.33)); // - 2, 1.33)); FEED_SHOT_TREE.put( Units.feetToMeters(38), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(40), new ShotData( Rotation2d.fromDegrees(55 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); // - 2, 1.2)); FEED_SHOT_TREE.put( Units.feetToMeters(42), new ShotData( Rotation2d.fromDegrees(56 - 13.16), - 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 + fudgeFactor, + 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , 1.2)); // - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight From c20c456407582e05f6b07fa0308bef0a3e7a7330 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 11:21:57 -0700 Subject: [PATCH 06/22] fix fudge factor --- .../subsystems/shooter/TurretSubsystem.java | 6 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 56 +++++++++---------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 441b746..2697691 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -253,7 +253,8 @@ public Command feed( hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); })); } @@ -352,7 +353,8 @@ public Command score(Supplier shotParamsSupplier) { // case NONE: hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); // } })); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 43fed1f..15d4c4a 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -65,91 +65,91 @@ public class AutoAim { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, new ShotData( Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.24)); COMP_HUB_SHOT_TREE.put( 3.773999, new ShotData( Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.21)); COMP_HUB_SHOT_TREE.put( 3.899275, new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 4.138058, new ShotData( Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.13)); COMP_HUB_SHOT_TREE.put( 4.602258, new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.15)); COMP_HUB_SHOT_TREE.put( 4.893493, new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 5.225402, new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); COMP_HUB_SHOT_TREE.put( 5.584793, new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.17)); } @@ -185,80 +185,80 @@ public class AutoAim { Units.feetToMeters(18), new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.42)); // - 2, 1.42)); FEED_SHOT_TREE.put( Units.feetToMeters(20), new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.36)); // - 2, 1.36)); FEED_SHOT_TREE.put( Units.feetToMeters(22), new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.34)); // - 2, 1.34)); FEED_SHOT_TREE.put( Units.feetToMeters(24), new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.25)); // - 2, 1.25)); FEED_SHOT_TREE.put( Units.feetToMeters(26), new ShotData( Rotation2d.fromDegrees(48 - 13.16), - 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.28)); // - 2, 1.28)); FEED_SHOT_TREE.put( Units.feetToMeters(28), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.27)); // - 2, 1.27)); FEED_SHOT_TREE.put( Units.feetToMeters(30), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.32)); // - 2, 1.32)); FEED_SHOT_TREE.put( Units.feetToMeters(32), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.4)); // - 2, 1.4)); FEED_SHOT_TREE.put( Units.feetToMeters(34), new ShotData( Rotation2d.fromDegrees(52 - 13.16), - 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(36), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.33)); // - 2, 1.33)); FEED_SHOT_TREE.put( Units.feetToMeters(38), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(40), new ShotData( Rotation2d.fromDegrees(55 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); // - 2, 1.2)); FEED_SHOT_TREE.put( Units.feetToMeters(42), new ShotData( Rotation2d.fromDegrees(56 - 13.16), - 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1 , + 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, 1.2)); // - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight From 3b1df6fa5c554cace10da93f49e0847f6acd78b0 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 12:14:48 -0700 Subject: [PATCH 07/22] make indexer use vel again and fix intake max pos --- src/main/java/frc/robot/Superstructure.java | 11 +++++++++-- .../indexer/SpindexerSubsystem.java | 19 +++++++++---------- .../subsystems/intake/SlapdownSubsystem.java | 2 +- 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index dee60ae..77d75a5 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -406,7 +406,11 @@ private void addTransitions() { // Transition from any state to SPIT for anti jamming antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); - bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate()); + bindTransition( + SuperState.SPIT, + SuperState.IDLE, + antiJamReq.negate(), + Commands.runOnce(() -> defense = false)); defenseReq.onTrue(changeStateTo(SuperState.DEFENSE)); @@ -422,7 +426,10 @@ private void addTransitions() { SuperState.CLIMB, climbReq); // TODO maybe add transition out of climb in case we fall bindTransition( - SuperState.PRE_CLIMB, SuperState.IDLE, preClimbReq.negate().and(climbReq.negate())); + SuperState.PRE_CLIMB, + SuperState.IDLE, + preClimbReq.negate().and(climbReq.negate()), + Commands.runOnce(() -> defense = false)); bindTransition( SuperState.SPIN_UP_SCORE_FLOW, diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index d4e567e..8310c05 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -67,16 +67,15 @@ public Command kick() { return Commands.sequence( this.run( () -> { - spinnerIO.setRollerVoltage(12); - kickerIO.setRollerVoltage(12); - }) - // .withTimeout(3), - // this.run( - // () -> { - // spinnerIO.setRollerVelocity(30); - // kickerIO.setRollerVelocity(20); - // }) - ); + // spinnerIO.setRollerVoltage(12); + // kickerIO.setRollerVoltage(12); + // }) + // .withTimeout(3), + // this.run( + // () -> { + spinnerIO.setRollerVelocity(30); + kickerIO.setRollerVelocity(20); + })); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 8004ac6..7bcb8c8 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -28,7 +28,7 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(122); // Not so sure abt this one... + Rotation2d.fromDegrees(115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE From 284d1aede6a59ea74b63f0b3ff81c6b55a072986 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 12:51:50 -0700 Subject: [PATCH 08/22] Update at 'Sat Mar 28 12:51:50 PDT 2026' --- src/main/java/frc/robot/Autos.java | 16 ++++++++-------- src/main/java/frc/robot/Robot.java | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 697d86f..cfa2bd4 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -676,14 +676,14 @@ public Command getRightNeutralOutpostScore() { setRightClimb()); } - public Command getLeftNeutralOutpostScore() { - return createAuto( - "Left Neutral Outpost Score", - new Path[] { - Path.StartingLTrenchtoLNeutral, Path.LNeutraltoLPreTrench, Path.LPreTrenchtoDepot - }, - setLeftClimb()); - } + // public Command getLeftNeutralOutpostScore() { + // return createAuto( + // "Left Neutral Outpost Score", + // new Path[] { + // Path.StartingLTrenchtoLNeutral, Path.LNeutraltoLPreTrench, Path.LPreTrenchtoDepot + // }, + // setLeftClimb()); + // } public Command getLeftNeutralScoreTwice() { return createAuto( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b7d5b8d..3d12c66 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -805,7 +805,7 @@ private void addAutos() { autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto()); autoChooser.addOption("Right Trench Double Dip Auto", autos.getDoubleDipRightTrench()); autoChooser.addOption("Left Neutral Score Twice", autos.getLeftNeutralScoreTwice()); - autoChooser.addOption("Left Neutral Outpost Score", autos.getLeftNeutralOutpostScore()); + // autoChooser.addOption("Left Neutral Outpost Score", autos.getLeftNeutralOutpostScore()); autoChooser.addOption("Hub Depot Outpost", autos.getHubDepotOutpostAuto()); autoChooser.addOption("Hub Outpost Depot", autos.getHubOutpostDepotAuto()); From 3807fd1334f822e639ea17813e30210e95459960 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:05:39 -0700 Subject: [PATCH 09/22] adjust path to shoot from somewhat in front of the outpost --- src/main/java/frc/robot/Autos.java | 38 +++++++++++++++++++++++++++--- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index cfa2bd4..34acf56 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -75,13 +75,15 @@ public enum Action { DEPOT, NOTHING, CLIMB_ONLY, - SCORE_AT_END; + SCORE_AT_END, + OUTPOST_NO_SCORE; } public enum Path { // OUTPOST RTrenchtoOutpost("RTrenchtoOutpost", Action.OUTPOST), RPreTrenchReversedtoOutpost("RPreTrenchReversedtoOutpost", Action.OUTPOST), + NoScoreRPreTrenchReversedtoOutpost("RPreTrenchReversedtoOutpost", Action.OUTPOST_NO_SCORE), PreOutposttoOutpost("PreOutposttoOutpost", Action.OUTPOST), @@ -118,7 +120,7 @@ public enum Path { OutposttoRPreTrench("OutposttoRPreTrench", Action.NOTHING), DepottoPreOutpost("DepottoPreOutpost", Action.DELAYED_SCORE), OutposttoPreDepot("OutposttoPreDepot", Action.NOTHING), - OutposttoPreOutpost("OutposttoPreOutpost", Action.SCORE), + OutposttoPreOutpost("OutposttoPreOutpost", Action.SCORE_AT_END), HubtoCenter("HubtoCenter", Action.SCORE), DepottoPreDepot("DepottoPreDepot", Action.SCORE_AT_END), @@ -210,6 +212,8 @@ public Command runPath(Path path, AutoRoutine routine) { return scoreAtEndPath(path, routine); case NOTHING: return emptyPath(path, routine); + case OUTPOST_NO_SCORE: + return outpostNoScorePath(path, routine); default: // this should never happen return Commands.none(); } @@ -360,6 +364,33 @@ public Command outpostPath(Path path, AutoRoutine routine) { swerve.stopForTime(() -> 4)); // TODO tune time } + public Command outpostNoScorePath(Path path, AutoRoutine routine) { + return Commands.sequence( + stopScoring(), + stopFeeding(), + stopFlowing(), + stopIntaking(), + // holy chopped + // this is here because i suspect the time based at end will cause it to stop early if it + // hits something and messes up the time + // though this may be fixed by having a better path?? idk + path.getTrajectory(routine) + .cmd() + .until( + path.getTrajectory(routine) + .atPose( + path.getTrajectory(routine) + .getFinalPose() + .orElse( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? BLUE_OUTPOST + : RED_OUTPOST), + 0.25, + Units.degreesToRadians(30))), + // TODO tune tolerance + swerve.stopForTime(() -> 4)); // TODO tune time + } + public Command depotPath(Path path, AutoRoutine routine) { return Commands.sequence( stopScoring(), @@ -671,7 +702,8 @@ public Command getRightNeutralOutpostScore() { new Path[] { Path.StartingRTrenchtoRNeutral, Path.RNeutraltoRPreTrenchReversed, - Path.RPreTrenchReversedtoOutpost + Path.NoScoreRPreTrenchReversedtoOutpost, + Path.OutposttoPreOutpost }, setRightClimb()); } From 7a368ba170a49f210a6b2c2c50f856fb026fe200 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:07:56 -0700 Subject: [PATCH 10/22] Update at 'Sat Mar 28 14:07:56 PDT 2026' --- .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 7bcb8c8..4268f68 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -28,7 +28,7 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(115); // Not so sure abt this one... + Rotation2d.fromDegrees(102.568359);//115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE From 8a9b77a7981ac09c9cc64e6c19888ea9f6cc7a8d Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 14:16:37 -0700 Subject: [PATCH 11/22] Update at 'Sat Mar 28 14:16:37 PDT 2026' --- .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 4268f68..fc77db9 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -28,7 +28,7 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(102.568359);//115); // Not so sure abt this one... + Rotation2d.fromDegrees(106.523438);//115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE From faa376b54cba191ede8f30e846d032ac2c4a580a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:01:41 -0700 Subject: [PATCH 12/22] Update at 'Sat Mar 28 15:01:41 PDT 2026' --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 2 +- .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 8310c05..c31d42a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -74,7 +74,7 @@ public Command kick() { // this.run( // () -> { spinnerIO.setRollerVelocity(30); - kickerIO.setRollerVelocity(20); + kickerIO.setRollerVelocity(40); })); } diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index fc77db9..981655c 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -28,7 +28,7 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(106.523438);//115); // Not so sure abt this one... + Rotation2d.fromDegrees(106.523438); // 115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE From c2d10d74cebfebd8268e6dd4452c758e717f3b5e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:06:14 -0700 Subject: [PATCH 13/22] Update at 'Sat Mar 28 15:06:14 PDT 2026' --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index c31d42a..caedaec 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -91,7 +91,7 @@ public Command spit() { public Command rest() { return this.run( () -> { - spinnerIO.setRollerVoltage(0.0); + spinnerIO.setRollerVoltage(-2); kickerIO.setRollerVoltage(0.0); }); } From c71a8749758a646f70d4279f33b11db801e6f09e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:08:07 -0700 Subject: [PATCH 14/22] Update at 'Sat Mar 28 15:08:07 PDT 2026' --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index caedaec..87097de 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -91,8 +91,8 @@ public Command spit() { public Command rest() { return this.run( () -> { - spinnerIO.setRollerVoltage(-2); - kickerIO.setRollerVoltage(0.0); + spinnerIO.setRollerVoltage(0.0); + kickerIO.setRollerVoltage(-2); }); } From 70e73b94ac662e7e366d5a453c4eca1acfb8982c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:19:14 -0700 Subject: [PATCH 15/22] Update at 'Sat Mar 28 15:19:13 PDT 2026' --- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 87097de..8361a87 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -74,7 +74,7 @@ public Command kick() { // this.run( // () -> { spinnerIO.setRollerVelocity(30); - kickerIO.setRollerVelocity(40); + kickerIO.setRollerVelocity(20); })); } From bde2422d00c5e05481788c77468f5cabed102eee Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:28:28 -0700 Subject: [PATCH 16/22] Update at 'Sat Mar 28 15:28:28 PDT 2026' --- src/main/java/frc/robot/Superstructure.java | 8 ++++---- src/main/java/frc/robot/subsystems/indexer/Indexer.java | 7 ++++++- .../frc/robot/subsystems/indexer/SpindexerSubsystem.java | 9 +++++++++ 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 77d75a5..8d2a32f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -608,7 +608,7 @@ private void addCommands() { bindCommands( SuperState.PRE_CLIMB, intake.restRetracted(), - indexer.rest(), + indexer.stop(), shooter.rest( swerve::getPose, swerve::getVelocityRobotRelative, @@ -618,7 +618,7 @@ private void addCommands() { bindCommands( SuperState.CLIMB, intake.restRetracted(), - indexer.rest(), + indexer.stop(), shooter.rest( swerve::getPose, swerve::getVelocityRobotRelative, @@ -628,7 +628,7 @@ private void addCommands() { bindCommands( SuperState.POST_CLIMB, intake.restRetracted(), - indexer.rest(), + indexer.stop(), shooter.rest( swerve::getPose, swerve::getVelocityRobotRelative, @@ -669,7 +669,7 @@ private void addCommands() { bindCommands( SuperState.DEFENSE, intake.restRetracted(), - indexer.rest(), + indexer.stop(), shooter.stopTurret(), climber.retract()); } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 4262c30..017b0c1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -5,6 +5,7 @@ package frc.robot.subsystems.indexer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; /** Add your docs here. */ @@ -19,6 +20,10 @@ public interface Indexer extends Subsystem { /** Run both indexer and kicker towards the shooter */ public Command kick(); - /** Not running (set to 0) */ + /** Not running (set spinner to 0 but idle kicker) */ public Command rest(); + + public default Command stop() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 8361a87..e5e2052 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -96,6 +96,15 @@ public Command rest() { }); } + @Override + public Command stop() { + return this.run( + () -> { + spinnerIO.setRollerVoltage(0.0); + kickerIO.setRollerVoltage(0.0); + }); + } + @Override public void periodic() { spinnerIO.updateInputs(spinnerInputs); From bae4878f2f68f3bd346d2c21be32e4a2c9c5f564 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 15:42:54 -0700 Subject: [PATCH 17/22] Update at 'Sat Mar 28 15:42:54 PDT 2026' --- .../java/frc/robot/subsystems/intake/SlapdownSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 981655c..f64e866 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -28,7 +28,7 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(106.523438); // 115); // Not so sure abt this one... + Rotation2d.fromDegrees(111.445313);//106.523438); // 115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE From 9c54cf3059b5eae6b2130b163ca7865f731547c6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 19:52:37 -0700 Subject: [PATCH 18/22] Update at 'Sat Mar 28 19:52:36 PDT 2026' --- .../subsystems/intake/SlapdownSubsystem.java | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index f64e866..f509a1f 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -117,13 +117,14 @@ public Command intake() { pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); rollerIO.setRollerVelocity(80); }) - .until(atExtensionTrigger) - .andThen( - this.run( - () -> { - pivotIO.setMotorVoltage(0); - rollerIO.setRollerVelocity(80); - })); + // .until(atExtensionTrigger) + // .andThen( + // this.run( + // () -> { + // pivotIO.setMotorVoltage(0); + // rollerIO.setRollerVelocity(80); + // })); + ; } @Override @@ -142,13 +143,14 @@ public Command restExtended() { pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); rollerIO.setRollerVoltage(0.0); }) - .until(atExtensionTrigger) - .andThen( - this.run( - () -> { - pivotIO.setMotorVoltage(0); - rollerIO.setRollerVoltage(0); - })); + // .until(atExtensionTrigger) + // .andThen( + // this.run( + // () -> { + // pivotIO.setMotorVoltage(0); + // rollerIO.setRollerVoltage(0); + // })); + ; } @Override From e618831af515f735214711d8d750a38f4f9ab8f3 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 28 Mar 2026 19:57:32 -0700 Subject: [PATCH 19/22] Update at 'Sat Mar 28 19:57:32 PDT 2026' --- .../indexer/SpindexerSubsystem.java | 2 +- .../subsystems/intake/SlapdownSubsystem.java | 52 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index e5e2052..656b8d8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -96,7 +96,7 @@ public Command rest() { }); } - @Override + @Override public Command stop() { return this.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index f509a1f..408c7b5 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -26,9 +26,9 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { public static final Rotation2d PIVOT_MIN_POSITION = - Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002); + Rotation2d.fromDegrees(-28.894531); // Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = - Rotation2d.fromDegrees(111.445313);//106.523438); // 115); // Not so sure abt this one... + Rotation2d.fromDegrees(111.445313); // 106.523438); // 115); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION; public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE @@ -113,18 +113,18 @@ public Command agitate() { @Override public Command intake() { return this.run( - () -> { - pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); - rollerIO.setRollerVelocity(80); - }) - // .until(atExtensionTrigger) - // .andThen( - // this.run( - // () -> { - // pivotIO.setMotorVoltage(0); - // rollerIO.setRollerVelocity(80); - // })); - ; + () -> { + pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); + rollerIO.setRollerVelocity(80); + }) + // .until(atExtensionTrigger) + // .andThen( + // this.run( + // () -> { + // pivotIO.setMotorVoltage(0); + // rollerIO.setRollerVelocity(80); + // })); + ; } @Override @@ -139,18 +139,18 @@ public Command outtake() { @Override public Command restExtended() { return this.run( - () -> { - pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); - rollerIO.setRollerVoltage(0.0); - }) - // .until(atExtensionTrigger) - // .andThen( - // this.run( - // () -> { - // pivotIO.setMotorVoltage(0); - // rollerIO.setRollerVoltage(0); - // })); - ; + () -> { + pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); + rollerIO.setRollerVoltage(0.0); + }) + // .until(atExtensionTrigger) + // .andThen( + // this.run( + // () -> { + // pivotIO.setMotorVoltage(0); + // rollerIO.setRollerVoltage(0); + // })); + ; } @Override From 4172f5219fcf0d7e10cea0ccffa0b7116ff4b217 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 29 Mar 2026 08:31:56 -0700 Subject: [PATCH 20/22] Update at 'Sun Mar 29 08:31:55 PDT 2026' --- src/main/java/frc/robot/components/pivot/PivotIO.java | 5 +++++ .../frc/robot/subsystems/intake/SlapdownSubsystem.java | 10 +++++----- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/components/pivot/PivotIO.java b/src/main/java/frc/robot/components/pivot/PivotIO.java index 446b7aa..81e1f39 100644 --- a/src/main/java/frc/robot/components/pivot/PivotIO.java +++ b/src/main/java/frc/robot/components/pivot/PivotIO.java @@ -85,6 +85,11 @@ public void setMotorPositionSetpoint(Rotation2d setpoint) { motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure())); } + public void setMotorPositionSetpoint(Rotation2d setpoint, double ffVolts) { + this.setpoint = setpoint; + motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(ffVolts)); + } + public Rotation2d getSetpoint() { return setpoint; } diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 408c7b5..d149dd1 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -114,7 +114,7 @@ public Command agitate() { public Command intake() { return this.run( () -> { - pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); + pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION, -0.5); rollerIO.setRollerVelocity(80); }) // .until(atExtensionTrigger) @@ -208,13 +208,13 @@ public static TalonFXConfiguration getPivotConfig() { config.Feedback.SensorToMechanismRatio = 1; config.Slot0.kS = 0.05; - config.Slot0.kV = 8.0; // Might suck\ + config.Slot0.kV = 8.0; // Might suck config.Slot0.kA = 0.0; - config.Slot0.kG = 0.55; + config.Slot0.kG = 0.4; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.GravityArmPositionOffset = 0.0; // Maybe need this?? - config.Slot0.kP = 15.0; - config.Slot0.kD = 0.3; + config.Slot0.kP = 40.0; + config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 45.0; // glup config.CurrentLimits.StatorCurrentLimitEnable = true; From a232eab297de799d9de8b9b2c0edc677de0c1557 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 29 Mar 2026 08:34:11 -0700 Subject: [PATCH 21/22] Update at 'Sun Mar 29 08:34:11 PDT 2026' --- src/main/java/frc/robot/components/pivot/PivotIO.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/components/pivot/PivotIO.java b/src/main/java/frc/robot/components/pivot/PivotIO.java index 81e1f39..d2438c7 100644 --- a/src/main/java/frc/robot/components/pivot/PivotIO.java +++ b/src/main/java/frc/robot/components/pivot/PivotIO.java @@ -82,12 +82,13 @@ public void setMotorVoltage(double voltage) { public void setMotorPositionSetpoint(Rotation2d setpoint) { this.setpoint = setpoint; - motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure())); + motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(0.0)); } public void setMotorPositionSetpoint(Rotation2d setpoint, double ffVolts) { this.setpoint = setpoint; - motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(ffVolts)); + motor.setControl( + motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(ffVolts)); } public Rotation2d getSetpoint() { From 903969157be7162dab393cde8f497574e4a93f6b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 29 Mar 2026 10:00:16 -0700 Subject: [PATCH 22/22] make r dd return score at end --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 34acf56..030a63b 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -100,7 +100,7 @@ public enum Path { EndWScoreLNeutraltoLPreTrench("LNeutraltoLPreTrench", Action.SCORE_AT_END), EndWScoreLCleanuptoLPreTrench("LCleanuptoLPreTrench", Action.SCORE_AT_END), - RNeutraltoRPreTrenchReversed("RNeutraltoRPreTrenchReversed", Action.INTAKE), + RNeutraltoRPreTrenchReversed("RNeutraltoRPreTrenchReversed", Action.SCORE_AT_END), LPreTrenchtoLNeutral("LPreTrenchtoLNeutral", Action.INTAKE), LPreTrenchtoLCleanup("LPreTrenchtoLCleanup", Action.INTAKE),