From 0d81b8c11288efca4a4c7b4f60b7921eb2887efd Mon Sep 17 00:00:00 2001 From: rishabhreng Date: Fri, 3 Jan 2025 13:02:23 -0600 Subject: [PATCH] Add new auto paths and update settings for path planning and robot dimensions functional three piece --- .../AdvantageScope Swerve Calibration.json | 10 +- FlexAdvKitBeta/New Path.traj | 27 --- .../src/main/deploy/choreo/3Piece.traj | 187 +++++++++++++++++ .../src/main/deploy/choreo/Forward.traj | 107 ++++++++++ .../src/main/deploy/choreo/New Path.traj | 27 --- .../src/main/deploy/choreo/proj.chor | 56 ++--- .../main/deploy/pathplanner/autos/3Piece.auto | 12 ++ .../deploy/pathplanner/autos/Forward.auto | 19 ++ .../pathplanner/paths/Example Path.path | 36 +++- .../deploy/pathplanner/paths/ForwardAuto.path | 54 +++++ .../src/main/deploy/pathplanner/settings.json | 10 +- .../src/main/java/frc/robot/Constants.java | 191 +++++++++++------- .../src/main/java/frc/robot/Robot.java | 2 +- .../main/java/frc/robot/RobotContainer.java | 132 +++++------- .../main/java/frc/robot/commands/Autos.java | 119 +++++++++++ .../frc/robot/commands/DriveCommands.java | 38 ++-- .../java/frc/robot/commands/NoteCommands.java | 93 +++++++++ .../frc/robot/subsystems/drive/Drive.java | 134 +++++------- .../robot/subsystems/drive/module/Module.java | 8 +- .../drive/module/ModuleIOTalonFX.java | 11 +- .../frc/robot/subsystems/index/Index.java | 48 +---- .../frc/robot/subsystems/index/IndexIO.java | 3 - .../robot/subsystems/index/IndexIOSim.java | 22 +- .../subsystems/index/IndexIOSparkFlex.java | 12 +- .../frc/robot/subsystems/intake/Intake.java | 42 +--- .../frc/robot/subsystems/intake/IntakeIO.java | 3 - .../robot/subsystems/intake/IntakeIOSim.java | 17 +- .../subsystems/intake/IntakeIOSparkFlex.java | 11 +- .../subsystems/intake/IntakeIOTalonFX.java | 7 +- .../frc/robot/subsystems/pivot/Pivot.java | 35 ++-- .../frc/robot/subsystems/pivot/PivotIO.java | 8 +- .../robot/subsystems/pivot/PivotIOSim.java | 43 +--- .../subsystems/pivot/PivotIOTalonFX.java | 55 ++--- .../frc/robot/subsystems/shooter/Shooter.java | 67 ++---- .../robot/subsystems/shooter/ShooterIO.java | 3 - .../subsystems/shooter/ShooterIOSim.java | 54 ++--- .../shooter/ShooterIOSparkFlex.java | 15 +- .../vision/VisionIOPhotonVision.java | 25 +++ .../src/main/deploy/pathplanner/settings.json | 21 ++ 39 files changed, 1071 insertions(+), 693 deletions(-) delete mode 100644 FlexAdvKitBeta/New Path.traj create mode 100644 FlexAdvKitBeta/src/main/deploy/choreo/3Piece.traj create mode 100644 FlexAdvKitBeta/src/main/deploy/choreo/Forward.traj delete mode 100644 FlexAdvKitBeta/src/main/deploy/choreo/New Path.traj create mode 100644 FlexAdvKitBeta/src/main/deploy/pathplanner/autos/3Piece.auto create mode 100644 FlexAdvKitBeta/src/main/deploy/pathplanner/autos/Forward.auto create mode 100644 FlexAdvKitBeta/src/main/deploy/pathplanner/paths/ForwardAuto.path create mode 100644 FlexAdvKitBeta/src/main/java/frc/robot/commands/Autos.java create mode 100644 FlexAdvKitBeta/src/main/java/frc/robot/commands/NoteCommands.java create mode 100644 FlexReboot/src/main/deploy/pathplanner/settings.json diff --git a/FlexAdvKitBeta/AdvantageScope Swerve Calibration.json b/FlexAdvKitBeta/AdvantageScope Swerve Calibration.json index e3c2d54..5aca93d 100644 --- a/FlexAdvKitBeta/AdvantageScope Swerve Calibration.json +++ b/FlexAdvKitBeta/AdvantageScope Swerve Calibration.json @@ -1,10 +1,10 @@ { "hubs": [ { - "x": 139, - "y": 95, - "width": 1100, - "height": 650, + "x": 138, + "y": 94, + "width": 1104, + "height": 654, "state": { "sidebar": { "width": 300, @@ -459,5 +459,5 @@ } ], "satellites": [], - "version": "4.0.0-beta-3" + "version": "4.1.0" } diff --git a/FlexAdvKitBeta/New Path.traj b/FlexAdvKitBeta/New Path.traj deleted file mode 100644 index f746de3..0000000 --- a/FlexAdvKitBeta/New Path.traj +++ /dev/null @@ -1,27 +0,0 @@ -{ - "name":"New Path", - "version":1, - "snapshot":{ - "waypoints":[], - "constraints":[], - "targetDt":0.05 - }, - "params":{ - "waypoints":[], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":null, - "waypoints":[], - "samples":[], - "splits":[] - }, - "events":[] -} diff --git a/FlexAdvKitBeta/src/main/deploy/choreo/3Piece.traj b/FlexAdvKitBeta/src/main/deploy/choreo/3Piece.traj new file mode 100644 index 0000000..f12ba9c --- /dev/null +++ b/FlexAdvKitBeta/src/main/deploy/choreo/3Piece.traj @@ -0,0 +1,187 @@ +{ + "name":"3Piece", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":0.8337379097938538, "y":6.706357002258301, "heading":1.0427220051285575, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.4693517684936523, "y":6.843776702880859, "heading":0.4434486948213219, "intervals":27, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.932753324508667, "y":6.007640361785889, "heading":0.26299462521425965, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.508333444595337, "y":5.547636032104492, "heading":0.0, "intervals":27, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8164089918136597, "y":5.001893043518066, "heading":-0.30451114220791814, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.508333444595337, "y":4.2125139236450195, "heading":-0.528074321666339, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.8337379097938538 m", "val":0.8337379097938538}, "y":{"exp":"6.706357002258301 m", "val":6.706357002258301}, "heading":{"exp":"1.0427220051285575 rad", "val":1.0427220051285575}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.4693517684936523 m", "val":2.4693517684936523}, "y":{"exp":"6.843776702880859 m", "val":6.843776702880859}, "heading":{"exp":"0.4434486948213219 rad", "val":0.4434486948213219}, "intervals":27, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.932753324508667 m", "val":1.932753324508667}, "y":{"exp":"6.007640361785889 m", "val":6.007640361785889}, "heading":{"exp":"0.26299462521425965 rad", "val":0.26299462521425965}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.508333444595337 m", "val":2.508333444595337}, "y":{"exp":"5.547636032104492 m", "val":5.547636032104492}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8164089918136597 m", "val":1.8164089918136597}, "y":{"exp":"5.001893043518066 m", "val":5.001893043518066}, "heading":{"exp":"-0.30451114220791814 rad", "val":-0.30451114220791814}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.508333444595337 m", "val":2.508333444595337}, "y":{"exp":"4.2125139236450195 m", "val":4.2125139236450195}, "heading":{"exp":"-0.528074321666339 rad", "val":-0.528074321666339}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.3553,2.24424,3.01227,3.8542,4.83831], + "samples":[ + {"t":0.0, "x":0.83374, "y":6.70636, "heading":1.04272, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.41677, "ay":0.89677, "alpha":-0.7486, "fx":[44.58313,43.99142,44.5792,45.07545], "fy":[11.70279,13.75354,11.69888,9.62334]}, + {"t":0.03663, "x":0.83603, "y":6.70696, "heading":1.04272, "vx":0.12516, "vy":0.03285, "omega":-0.02742, "ax":3.41763, "ay":0.89247, "alpha":-0.75313, "fx":[44.59415,44.00164,44.59127,45.08703], "fy":[11.64927,13.71057,11.6401,9.55431]}, + {"t":0.07326, "x":0.84291, "y":6.70876, "heading":1.04172, "vx":0.25034, "vy":0.06554, "omega":-0.05501, "ax":3.4186, "ay":0.88761, "alpha":-0.75811, "fx":[44.60708,44.01333,44.60428,45.09995], "fy":[11.58673,13.66147,11.57576,9.47651]}, + {"t":0.10989, "x":0.85437, "y":6.71176, "heading":1.0397, "vx":0.37557, "vy":0.09805, "omega":-0.08278, "ax":3.41969, "ay":0.88207, "alpha":-0.76364, "fx":[44.62216,44.02674,44.61852,45.11447], "fy":[11.51374,13.60507,11.50436,9.38817]}, + {"t":0.14652, "x":0.87042, "y":6.71594, "heading":1.03667, "vx":0.50083, "vy":0.13036, "omega":-0.11075, "ax":3.42094, "ay":0.8757, "alpha":-0.76986, "fx":[44.63974,44.04221,44.63433,45.13088], "fy":[11.42843,13.53985,11.42399,9.28701]}, + {"t":0.18315, "x":0.89106, "y":6.7213, "heading":1.03261, "vx":0.62614, "vy":0.16244, "omega":-0.13895, "ax":3.42238, "ay":0.86832, "alpha":-0.77693, "fx":[44.66023,44.06018,44.65217,45.14957], "fy":[11.32837,13.46382,11.3321,9.17005]}, + {"t":0.21978, "x":0.91629, "y":6.72784, "heading":1.02752, "vx":0.7515, "vy":0.19425, "omega":-0.16741, "ax":3.42405, "ay":0.85967, "alpha":-0.78509, "fx":[44.68417,44.08121,44.67263,45.17103], "fy":[11.21034,13.37434,11.22531,9.03332]}, + {"t":0.25641, "x":0.94612, "y":6.73553, "heading":1.02139, "vx":0.87692, "vy":0.22574, "omega":-0.19617, "ax":3.426, "ay":0.84941, "alpha":-0.79465, "fx":[44.71228,44.10606,44.69649,45.19592], "fy":[11.06993,13.26779,11.09898,8.87141]}, + {"t":0.29304, "x":0.98054, "y":6.74437, "heading":1.01421, "vx":1.00241, "vy":0.25685, "omega":-0.22527, "ax":3.42831, "ay":0.83706, "alpha":-0.80606, "fx":[44.74552,44.13576,44.72482,45.22508], "fy":[10.90102,13.13913,10.9466,8.67672]}, + {"t":0.32967, "x":1.01956, "y":6.75434, "heading":1.00595, "vx":1.12799, "vy":0.28751, "omega":-0.2548, "ax":3.43108, "ay":0.8219, "alpha":-0.81993, "fx":[44.78522,44.17178,44.75909,45.25965], "fy":[10.69476,12.98106,10.7587,8.43827]}, + {"t":0.3663, "x":1.06318, "y":6.76542, "heading":0.99662, "vx":1.25367, "vy":0.31762, "omega":-0.28483, "ax":3.43446, "ay":0.80289, "alpha":-0.83723, "fx":[44.83327,44.21622,44.80143,45.30118], "fy":[10.4379,12.78261,10.52105,8.13957]}, + {"t":0.40293, "x":1.1114, "y":6.77759, "heading":0.98619, "vx":1.37948, "vy":0.34703, "omega":-0.3155, "ax":3.43866, "ay":0.77837, "alpha":-0.85943, "fx":[44.8924,44.27226,44.85491,45.35178], "fy":[10.1097,12.52658,10.21113,7.75472]}, + {"t":0.43956, "x":1.16424, "y":6.79083, "heading":0.97463, "vx":1.50543, "vy":0.37554, "omega":-0.34698, "ax":3.444, "ay":0.74558, "alpha":-0.889, "fx":[44.96669,44.34484,44.92421,45.4143], "fy":[9.67576,12.18425,9.79116,7.24052]}, + {"t":0.47619, "x":1.22169, "y":6.80508, "heading":0.96192, "vx":1.63159, "vy":0.40285, "omega":-0.37955, "ax":3.45097, "ay":0.69955, "alpha":-0.93035, "fx":[45.06239,44.44216,45.01655,45.49219], "fy":[9.07455,11.70392,9.19262,6.51937]}, + {"t":0.51282, "x":1.28377, "y":6.82031, "heading":0.94802, "vx":1.758, "vy":0.42847, "omega":-0.41362, "ax":3.46026, "ay":0.63034, "alpha":-0.99219, "fx":[45.18889,44.57848,45.1429,45.58776], "fy":[8.18429,10.98215,8.27714,5.4368]}, + {"t":0.54945, "x":1.35049, "y":6.83642, "heading":0.93287, "vx":1.88474, "vy":0.45156, "omega":-0.44997, "ax":3.47264, "ay":0.5149, "alpha":-1.09449, "fx":[45.35754,44.77962,45.31627,45.69019], "fy":[6.72615,9.77773,6.71834,3.63674]}, + {"t":0.58608, "x":1.42186, "y":6.85331, "heading":0.91639, "vx":2.01195, "vy":0.47042, "omega":-0.49006, "ax":3.48577, "ay":0.28536, "alpha":-1.29449, "fx":[45.5509,45.08427,45.50689,45.6865], "fy":[3.89729,7.37195,3.52839,0.08782]}, + {"t":0.62271, "x":1.49789, "y":6.87073, "heading":0.89843, "vx":2.13963, "vy":0.48088, "omega":-0.53748, "ax":3.4402, "ay":-0.36713, "alpha":-1.8386, "fx":[45.19472,45.24956,44.75278,44.25443], "fy":[-3.8023,0.33907,-6.04306,-9.64442]}, + {"t":0.65934, "x":1.57857, "y":6.8881, "heading":0.87875, "vx":2.26564, "vy":0.46743, "omega":-0.60482, "ax":0.71457, "ay":-3.25159, "alpha":-3.15741, "fx":[17.35184,10.56284,0.75334,8.60595], "fy":[-40.37989,-42.01078,-43.84156,-43.38074]}, + {"t":0.69597, "x":1.66204, "y":6.90304, "heading":0.85659, "vx":2.29182, "vy":0.34832, "omega":-0.72048, "ax":-2.76784, "ay":-2.09783, "alpha":-0.55091, "fx":[-35.55597,-36.87346,-36.61305,-35.3366], "fy":[-28.03647,-26.29491,-26.72181,-28.37599]}, + {"t":0.7326, "x":1.74414, "y":6.91439, "heading":0.8302, "vx":2.19043, "vy":0.27148, "omega":-0.74066, "ax":-3.11913, "ay":-1.59645, "alpha":0.03068, "fx":[-40.69303,-40.6403,-40.65883,-40.71158], "fy":[-20.78732,-20.88931,-20.85075,-20.7485]}, + {"t":0.76923, "x":1.82228, "y":6.92327, "heading":0.80307, "vx":2.07618, "vy":0.213, "omega":-0.73954, "ax":-3.22332, "ay":-1.40156, "alpha":0.24065, "fx":[-42.1428,-41.7846,-41.92687,-42.2843], "fy":[-18.04365,-18.85256,-18.51925,-17.69444]}, + {"t":0.80586, "x":1.89617, "y":6.93013, "heading":0.77598, "vx":1.95811, "vy":0.16166, "omega":-0.73072, "ax":-3.27169, "ay":-1.29957, "alpha":0.34789, "fx":[-42.80842,-42.32882,-42.52397,-43.0006], "fy":[-16.61011,-17.79016,-17.30116,-16.08806]}, + {"t":0.84249, "x":1.9657, "y":6.93518, "heading":0.74922, "vx":1.83827, "vy":0.11406, "omega":-0.71798, "ax":-3.29936, "ay":-1.23708, "alpha":0.41292, "fx":[-43.19018,-42.64652,-42.86452,-43.40355], "fy":[-15.72261,-17.13608,-16.56588,-15.10535]}, + {"t":0.87912, "x":2.03082, "y":6.93853, "heading":0.72292, "vx":1.71741, "vy":0.06875, "omega":-0.70285, "ax":-3.31719, "ay":-1.19494, "alpha":0.45657, "fx":[-43.43885,-42.85533,-43.08129,-43.65948], "fy":[-15.11352,-16.69063,-16.08176,-14.44609]}, + {"t":0.91575, "x":2.0915, "y":6.94024, "heading":0.69717, "vx":1.5959, "vy":0.02498, "omega":-0.68613, "ax":-3.32961, "ay":-1.16465, "alpha":0.48793, "fx":[-43.61472,-43.00365,-43.22933,-43.83523], "fy":[-14.66564,-16.36579,-15.74429,-13.97583]}, + {"t":0.95238, "x":2.14773, "y":6.94038, "heading":0.67204, "vx":1.47394, "vy":-0.01769, "omega":-0.66826, "ax":-3.33875, "ay":-1.14183, "alpha":0.51155, "fx":[-43.74645,-43.11499,-43.33547,-43.9626], "fy":[-14.31964,-16.11682,-15.49945,-13.62555]}, + {"t":0.98901, "x":2.19948, "y":6.93896, "heading":0.64756, "vx":1.35164, "vy":-0.05951, "omega":-0.64952, "ax":-3.34574, "ay":-1.12405, "alpha":0.52998, "fx":[-43.84936,-43.20209,-43.41429,-44.05859], "fy":[-14.04234,-15.91863,-15.3166,-13.35621]}, + {"t":1.02564, "x":2.24674, "y":6.93603, "heading":0.62377, "vx":1.22909, "vy":-0.10068, "omega":-0.6301, "ax":-3.35126, "ay":-1.1098, "alpha":0.54477, "fx":[-43.93238,-43.27246,-43.47439,-44.13308], "fy":[-13.81374,-15.75609,-15.17702,-13.14405]}, + {"t":1.06227, "x":2.28952, "y":6.9316, "heading":0.60069, "vx":1.10633, "vy":-0.14134, "omega":-0.61015, "ax":-3.35573, "ay":-1.09815, "alpha":0.55689, "fx":[-44.00105,-43.33078,-43.52118,-44.19222], "fy":[-13.62106,-15.61955,-15.06866,-12.97376]}, + {"t":1.0989, "x":2.32779, "y":6.92568, "heading":0.57834, "vx":0.98341, "vy":-0.18156, "omega":-0.58975, "ax":-3.35941, "ay":-1.08845, "alpha":0.56701, "fx":[-44.05899,-43.38014,-43.5582,-44.24004], "fy":[-13.45576,-15.50257,-14.98343,-12.83504]}, + {"t":1.13552, "x":2.36156, "y":6.9183, "heading":0.55674, "vx":0.86036, "vy":-0.22143, "omega":-0.56898, "ax":-3.3625, "ay":-1.08025, "alpha":0.57558, "fx":[-44.10868,-43.42264,-43.58789,-44.27925], "fy":[-13.31191,-15.40072,-14.91568,-12.72068]}, + {"t":1.17215, "x":2.39082, "y":6.90947, "heading":0.53589, "vx":0.73719, "vy":-0.261, "omega":-0.5479, "ax":-3.36512, "ay":-1.07323, "alpha":0.58293, "fx":[-44.15185,-43.45975,-43.61197,-44.31181], "fy":[-13.18525,-15.31085,-14.86134,-12.62548]}, + {"t":1.20878, "x":2.41556, "y":6.89919, "heading":0.51582, "vx":0.61393, "vy":-0.30031, "omega":-0.52654, "ax":-3.36738, "ay":-1.06716, "alpha":0.5893, "fx":[-44.18976,-43.49254,-43.6317,-44.33911], "fy":[-13.07268,-15.23065,-14.81746,-12.54558]}, + {"t":1.24541, "x":2.43579, "y":6.88747, "heading":0.49654, "vx":0.49058, "vy":-0.3394, "omega":-0.50496, "ax":-3.36934, "ay":-1.06186, "alpha":0.59487, "fx":[-44.22337,-43.52181,-43.648,-44.36219], "fy":[-12.97183,-15.15842,-14.78178,-12.47807]}, + {"t":1.28204, "x":2.4515, "y":6.87432, "heading":0.47804, "vx":0.36716, "vy":-0.3783, "omega":-0.48317, "ax":-3.37106, "ay":-1.0572, "alpha":0.59978, "fx":[-44.25337,-43.54815,-43.66158,-44.38186], "fy":[-12.88089,-15.09286,-14.75261,-12.42068]}, + {"t":1.31867, "x":2.46269, "y":6.85976, "heading":0.46034, "vx":0.24368, "vy":-0.41702, "omega":-0.4612, "ax":-3.37258, "ay":-1.05308, "alpha":0.60414, "fx":[-44.28033,-43.57202,-43.67298,-44.39872], "fy":[-12.79846,-15.03299,-14.72861,-12.37163]}, + {"t":1.3553, "x":2.46935, "y":6.84378, "heading":0.44345, "vx":0.12014, "vy":-0.4556, "omega":-0.43907, "ax":-3.37085, "ay":-1.05739, "alpha":0.57292, "fx":[-44.25036,-43.57474,-43.65756,-44.35128], "fy":[-12.87938,-15.006,-14.75427,-12.51699]}, + {"t":1.38823, "x":2.47148, "y":6.8282, "heading":0.42899, "vx":0.00916, "vy":-0.49041, "omega":-0.42021, "ax":-3.36599, "ay":-1.07148, "alpha":0.57933, "fx":[-44.20104,-43.50847,-43.57862,-44.29254], "fy":[-13.03161,-15.1834,-14.97113,-12.70586]}, + {"t":1.42115, "x":2.46996, "y":6.81148, "heading":0.41516, "vx":-0.10166, "vy":-0.52569, "omega":-0.40113, "ax":-3.36021, "ay":-1.08804, "alpha":0.58684, "fx":[-44.14126,-43.42885,-43.4857,-44.22301], "fy":[-13.21414,-15.39382,-15.22191,-12.92567]}, + {"t":1.45407, "x":2.46479, "y":6.79358, "heading":0.40195, "vx":-0.21229, "vy":-0.56151, "omega":-0.38181, "ax":-3.3532, "ay":-1.10775, "alpha":0.59576, "fx":[-44.06783,-43.33175,-43.37435,-44.13928], "fy":[-13.43531,-15.6465,-15.5165,-13.18528]}, + {"t":1.487, "x":2.45598, "y":6.77449, "heading":0.38938, "vx":-0.32269, "vy":-0.59798, "omega":-0.3622, "ax":-3.34454, "ay":-1.1316, "alpha":0.60653, "fx":[-43.97606,-43.21117,-43.23802,-44.03633], "fy":[-13.70694,-15.9545,-15.86888,-13.49729]}, + {"t":1.51992, "x":2.44355, "y":6.75419, "heading":0.37746, "vx":-0.4328, "vy":-0.63524, "omega":-0.34223, "ax":-3.33359, "ay":-1.16103, "alpha":0.6198, "fx":[-43.85889,-43.058,-43.0668,-43.90654], "fy":[-14.04633,-16.33685,-16.29952,-13.88023]}, + {"t":1.55285, "x":2.42749, "y":6.73265, "heading":0.36619, "vx":-0.54256, "vy":-0.67346, "omega":-0.32182, "ax":-3.31931, "ay":-1.19825, "alpha":0.63655, "fx":[-43.70504,-42.85783,-42.84485,-43.73766], "fy":[-14.47987,-16.82241,-16.83954,-14.36245]}, + {"t":1.58577, "x":2.40783, "y":6.70983, "heading":0.35559, "vx":-0.65184, "vy":-0.71291, "omega":-0.30087, "ax":-3.29996, "ay":-1.24677, "alpha":0.65835, "fx":[-43.49548,-42.58644,-42.54539,-43.50892], "fy":[-15.04978,-17.45713,-17.53875,-14.9896]}, + {"t":1.61869, "x":2.38458, "y":6.68568, "heading":0.34569, "vx":-0.76049, "vy":-0.75396, "omega":-0.27919, "ax":-3.27237, "ay":-1.31259, "alpha":0.68789, "fx":[-43.19558,-42.19997,-42.1195,-43.18208], "fy":[-15.82811,-18.31872,-18.48179,-15.84001]}, + {"t":1.65162, "x":2.35777, "y":6.66014, "heading":0.3365, "vx":-0.86822, "vy":-0.79718, "omega":-0.25654, "ax":-3.23012, "ay":-1.40677, "alpha":0.7301, "fx":[-42.73546,-41.61072,-41.46817,-42.67901], "fy":[-16.94814,-19.54932,-19.82442,-17.05977]}, + {"t":1.68454, "x":2.32743, "y":6.63313, "heading":0.32805, "vx":-0.97457, "vy":-0.84349, "omega":-0.2325, "ax":-3.15827, "ay":-1.55208, "alpha":0.79512, "fx":[-41.95249,-40.61724,-40.3606,-41.81475], "fy":[-18.68533,-21.43729,-21.88483,-18.95382]}, + {"t":1.71746, "x":2.29363, "y":6.60452, "heading":0.32039, "vx":-1.07855, "vy":-0.89459, "omega":-0.20633, "ax":-3.01369, "ay":-1.80271, "alpha":0.9069, "fx":[-40.37733,-38.65212,-38.13232,-40.04145], "fy":[-21.70422,-24.65464,-25.4091,-22.26725]}, + {"t":1.75039, "x":2.25649, "y":6.57409, "heading":0.3136, "vx":-1.17777, "vy":-0.95394, "omega":-0.17647, "ax":-2.621, "ay":-2.31337, "alpha":1.13212, "fx":[-36.07826,-33.50559,-32.12236,-35.01308], "fy":[-27.98427,-31.04335,-32.41307,-29.23184]}, + {"t":1.78331, "x":2.21629, "y":6.54143, "heading":0.30779, "vx":-1.26407, "vy":-1.03011, "omega":-0.13919, "ax":-0.8406, "ay":-3.35294, "alpha":1.57005, "fx":[-15.16177,-11.90489,-6.9008,-9.88111], "fy":[-42.54934,-43.6758,-44.68201,-43.99248]}, + {"t":1.81624, "x":2.17422, "y":6.5057, "heading":0.30321, "vx":-1.29174, "vy":-1.1405, "omega":-0.0875, "ax":2.86309, "ay":-1.96619, "alpha":0.8216, "fx":[37.16125,36.03201,37.54158,38.6129], "fy":[-25.87117,-27.50311,-25.46302,-23.72508]}, + {"t":1.84916, "x":2.13324, "y":6.46708, "heading":0.30033, "vx":-1.19748, "vy":-1.20523, "omega":-0.06045, "ax":3.45937, "ay":-0.56194, "alpha":0.15829, "fx":[45.13279,45.04243,45.09507,45.18123], "fy":[-7.18564,-7.74799,-7.46836,-6.91065]}, + {"t":1.88208, "x":2.09569, "y":6.4271, "heading":0.29834, "vx":-1.08358, "vy":-1.22374, "omega":-0.05524, "ax":3.51616, "ay":-0.04532, "alpha":-0.07852, "fx":[45.85353,45.85648,45.85369,45.8501], "fy":[-0.69225,-0.39319,-0.48877,-0.78957]}, + {"t":1.91501, "x":2.06192, "y":6.38678, "heading":0.29652, "vx":-0.96782, "vy":-1.22523, "omega":-0.05783, "ax":3.51623, "ay":0.20361, "alpha":-0.19132, "fx":[45.87342,45.82828,45.83647,45.87935], "fy":[2.37601,3.11613,2.94023,2.18857]}, + {"t":1.94793, "x":2.03196, "y":6.34656, "heading":0.29461, "vx":-0.85205, "vy":-1.21852, "omega":-0.06412, "ax":3.5082, "ay":0.34795, "alpha":-0.25637, "fx":[45.79247,45.6908,45.70798,45.80741], "fy":[4.1396,5.13549,4.94636,3.92885]}, + {"t":1.98085, "x":2.00581, "y":6.30663, "heading":0.2925, "vx":-0.73655, "vy":-1.20707, "omega":-0.07257, "ax":3.49979, "ay":0.44173, "alpha":-0.29851, "fx":[45.70413,45.55452,45.57651,45.72477], "fy":[5.27942,6.43996,6.25646,5.06596]}, + {"t":2.01378, "x":1.98346, "y":6.26712, "heading":0.29011, "vx":-0.62132, "vy":-1.19252, "omega":-0.08239, "ax":3.49239, "ay":0.5074, "alpha":-0.32797, "fx":[45.62565,45.43726,45.46139,45.64964], "fy":[6.07472,7.34952,7.17748,5.86609]}, + {"t":2.0467, "x":1.9649, "y":6.22814, "heading":0.2874, "vx":-0.50634, "vy":-1.17582, "omega":-0.09319, "ax":3.48612, "ay":0.55592, "alpha":-0.34971, "fx":[45.559,45.33923,45.3639,45.58481], "fy":[6.66019,8.01872,7.85993,6.45949]}, + {"t":2.07962, "x":1.95011, "y":6.18973, "heading":0.28433, "vx":-0.39157, "vy":-1.15752, "omega":-0.10471, "ax":3.48084, "ay":0.59319, "alpha":-0.36641, "fx":[45.50287,45.25743,45.28163,45.52948], "fy":[7.1085,8.53099,8.3859,6.91721]}, + {"t":2.11255, "x":1.93911, "y":6.15194, "heading":0.28089, "vx":-0.27696, "vy":-1.13799, "omega":-0.11677, "ax":3.47637, "ay":0.62271, "alpha":-0.37963, "fx":[45.45545,45.18874,45.21179,45.48214], "fy":[7.46227,8.93518,8.80385,7.28118]}, + {"t":2.14547, "x":1.93187, "y":6.11481, "heading":0.27704, "vx":-0.16251, "vy":-1.11748, "omega":-0.12927, "ax":3.47255, "ay":0.64666, "alpha":-0.39036, "fx":[45.41513,45.13055,45.15199,45.44136], "fy":[7.74808,9.26178,9.14419,7.57778]}, + {"t":2.1784, "x":1.92841, "y":6.07837, "heading":0.27279, "vx":-0.04818, "vy":-1.09619, "omega":-0.14212, "ax":3.46926, "ay":0.66648, "alpha":-0.39925, "fx":[45.3806,45.08083,45.10027,45.40595], "fy":[7.9834,9.53078,9.42696,7.82437]}, + {"t":2.21132, "x":1.9287, "y":6.04264, "heading":0.26811, "vx":0.06604, "vy":-1.07425, "omega":-0.15526, "ax":3.46641, "ay":0.68314, "alpha":-0.40672, "fx":[45.35081,45.03799,45.05514,45.37492], "fy":[8.18014,9.75581,9.6659,8.03288]}, + {"t":2.24424, "x":1.93275, "y":6.00764, "heading":0.26299, "vx":0.18017, "vy":-1.05176, "omega":-0.16866, "ax":3.46179, "ay":0.70471, "alpha":-0.38482, "fx":[45.28883,44.98347,44.9965,45.30903], "fy":[8.49165,9.98309,9.91386,8.37109]}, + {"t":2.27496, "x":1.93992, "y":5.97566, "heading":0.25781, "vx":0.28652, "vy":-1.03011, "omega":-0.18048, "ax":3.45452, "ay":0.73714, "alpha":-0.39555, "fx":[45.20713,44.87917,44.88763,45.22449], "fy":[8.88503,10.41532,10.36768,8.78324]}, + {"t":2.30568, "x":1.95035, "y":5.94436, "heading":0.25227, "vx":0.39264, "vy":-1.00746, "omega":-0.19263, "ax":3.44534, "ay":0.77616, "alpha":-0.40846, "fx":[45.10402,44.74794,44.75019,45.11739], "fy":[9.35866,10.93486,10.91367,9.27993]}, + {"t":2.33641, "x":1.96404, "y":5.91378, "heading":0.24635, "vx":0.49849, "vy":-0.98362, "omega":-0.20518, "ax":3.43344, "ay":0.82402, "alpha":-0.42428, "fx":[44.9704,44.57853,44.57202,44.97799], "fy":[9.93962,11.57083,11.58283,9.88993]}, + {"t":2.36713, "x":1.98098, "y":5.88395, "heading":0.24005, "vx":0.60396, "vy":-0.95831, "omega":-0.21821, "ax":3.41751, "ay":0.88403, "alpha":-0.44411, "fx":[44.79151,44.35274,44.33336,44.79035], "fy":[10.66856,12.36674,12.42164,10.65662]}, + {"t":2.39785, "x":2.00114, "y":5.85493, "heading":0.23334, "vx":0.70895, "vy":-0.93115, "omega":-0.23185, "ax":3.39528, "ay":0.96142, "alpha":-0.46965, "fx":[44.54196,44.0395,44.0002,44.52684], "fy":[11.60919,13.3903,13.50285,11.64839]}, + {"t":2.42857, "x":2.02453, "y":5.82678, "heading":0.22622, "vx":0.81326, "vy":-0.90161, "omega":-0.24628, "ax":3.36256, "ay":1.06484, "alpha":-0.5037, "fx":[44.17475,43.58178,43.50946,44.1357], "fy":[12.86701,14.75256,14.94638,12.97924]}, + {"t":2.45929, "x":2.0511, "y":5.79958, "heading":0.21866, "vx":0.91656, "vy":-0.8689, "omega":-0.26176, "ax":3.31081, "ay":1.20951, "alpha":-0.55118, "fx":[43.59413,42.86498,42.7328,43.51011], "fy":[14.62885,16.64732,16.96288,14.85263]}, + {"t":2.49001, "x":2.08082, "y":5.77346, "heading":0.21061, "vx":1.01827, "vy":-0.83174, "omega":-0.27869, "ax":3.22032, "ay":1.42453, "alpha":-0.62131, "fx":[42.57897,41.62941,41.3747,42.39874], "fy":[17.25443,19.43886,19.95066,17.66397]}, + {"t":2.52073, "x":2.11362, "y":5.74858, "heading":0.20205, "vx":1.1172, "vy":-0.78798, "omega":-0.29778, "ax":3.03728, "ay":1.77037, "alpha":-0.73274, "fx":[40.5214,39.1845,38.63401,40.09424], "fy":[21.50697,23.8658,24.71728,22.25806]}, + {"t":2.55145, "x":2.14937, "y":5.72521, "heading":0.1929, "vx":1.21051, "vy":-0.73359, "omega":-0.32029, "ax":2.57904, "ay":2.37376, "alpha":-0.92138, "fx":[35.30867,33.28379,31.86406,34.07422], "fy":[29.09743,31.41537,32.81912,30.49057]}, + {"t":2.58217, "x":2.18778, "y":5.70379, "heading":0.18306, "vx":1.28974, "vy":-0.66067, "omega":-0.34859, "ax":1.15827, "ay":3.29004, "alpha":-1.18143, "fx":[18.16866,15.90238,12.12142,14.22632], "fy":[41.75608,42.72302,43.91792,43.22163]}, + {"t":2.61289, "x":2.22795, "y":5.68505, "heading":0.17236, "vx":1.32532, "vy":-0.5596, "omega":-0.38489, "ax":-1.60059, "ay":3.09911, "alpha":-1.002, "fx":[-19.64282,-18.71498,-21.99955,-23.13445], "fy":[41.03711,41.52636,39.89977,39.196]}, + {"t":2.64361, "x":2.26791, "y":5.66932, "heading":0.16053, "vx":1.27615, "vy":-0.46439, "omega":-0.41567, "ax":-2.95136, "ay":1.89245, "alpha":-0.52523, "fx":[-38.53201,-37.67495,-38.46649,-39.27855], "fy":[24.60745,25.92384,24.75879,23.42597]}, + {"t":2.67434, "x":2.30572, "y":5.65594, "heading":0.14776, "vx":1.18548, "vy":-0.40625, "omega":-0.43181, "ax":-3.31637, "ay":1.16954, "alpha":-0.26157, "fx":[-43.32001,-43.00739,-43.18293,-43.48189], "fy":[15.04196,15.92269,15.45685,14.58554]}, + {"t":2.70506, "x":2.34057, "y":5.64401, "heading":0.1345, "vx":1.0836, "vy":-0.37032, "omega":-0.43984, "ax":-3.43587, "ay":0.77514, "alpha":-0.12155, "fx":[-44.83728,-44.73602,-44.77683,-44.87559], "fy":[9.96712,10.41567,10.24813,9.80304]}, + {"t":2.73578, "x":2.37224, "y":5.633, "heading":0.12098, "vx":0.97805, "vy":-0.34651, "omega":-0.44358, "ax":-3.48448, "ay":0.537, "alpha":-0.03817, "fx":[-45.44809,-45.42563,-45.43258,-45.45485], "fy":[6.95016,7.09649,7.05532,6.90939]}, + {"t":2.7665, "x":2.40064, "y":5.62261, "heading":0.10736, "vx":0.871, "vy":-0.33001, "omega":-0.44475, "ax":-3.50734, "ay":0.37975, "alpha":0.01644, "fx":[-45.73581,-45.74272,-45.74102,-45.73409], "fy":[4.97729,4.91306,4.92705,4.99136]}, + {"t":2.79722, "x":2.42575, "y":5.61265, "heading":0.09369, "vx":0.76325, "vy":-0.31834, "omega":-0.44424, "ax":-3.51915, "ay":0.26879, "alpha":0.05476, "fx":[-45.88595,-45.90228,-45.89902,-45.88243], "fy":[3.59477,3.37851,3.41517,3.6324]}, + {"t":2.82794, "x":2.44753, "y":5.603, "heading":0.08005, "vx":0.65514, "vy":-0.31009, "omega":-0.44256, "ax":-3.52562, "ay":0.18653, "alpha":0.08306, "fx":[-45.96977,-45.98698,-45.98413,-45.96648], "fy":[2.57542,2.24534,2.28842,2.62082]}, + {"t":2.85866, "x":2.466, "y":5.59356, "heading":0.06645, "vx":0.54683, "vy":-0.30436, "omega":-0.44001, "ax":-3.52929, "ay":0.12321, "alpha":0.10479, "fx":[-46.01867,-46.03298,-46.03093,-46.01609], "fy":[1.79436,1.37637,1.41715,1.83891]}, + {"t":2.88938, "x":2.48113, "y":5.58427, "heading":0.05293, "vx":0.43841, "vy":-0.30057, "omega":-0.43679, "ax":-3.53139, "ay":0.073, "alpha":0.12198, "fx":[-46.04791,-46.05774,-46.05637,-46.04605], "fy":[1.1777,0.69003,0.72357,1.21641]}, + {"t":2.9201, "x":2.49293, "y":5.57507, "heading":0.03951, "vx":0.32992, "vy":-0.29833, "omega":-0.43304, "ax":-3.53256, "ay":0.03223, "alpha":0.13592, "fx":[-46.06554,-46.0703,-46.06936,-46.06422], "fy":[0.67903,0.13499,0.15839,0.7089]}, + {"t":2.95082, "x":2.5014, "y":5.56592, "heading":0.02621, "vx":0.2214, "vy":-0.29734, "omega":-0.42887, "ax":-3.53318, "ay":-0.00151, "alpha":0.14744, "fx":[-46.07603,-46.07565,-46.07488,-46.07506], "fy":[0.26784,-0.32261,-0.31111,0.28699]}, + {"t":2.98154, "x":2.50653, "y":5.55679, "heading":0.01304, "vx":0.11286, "vy":-0.29739, "omega":-0.42434, "ax":-3.53345, "ay":-0.0299, "alpha":0.15712, "fx":[-46.08201,-46.07666,-46.0758,-46.08118], "fy":[-0.07676,-0.70596,-0.70741,-0.06951]}, + {"t":3.01227, "x":2.50833, "y":5.54764, "heading":0.0, "vx":0.00431, "vy":-0.2983, "omega":-0.41951, "ax":-3.53326, "ay":-0.05401, "alpha":0.14761, "fx":[-46.08137,-46.0724,-46.07134,-46.08055], "fy":[-0.4042,-0.99546,-1.00838,-0.40945]}, + {"t":3.04345, "x":2.50675, "y":5.53831, "heading":-0.01308, "vx":-0.10587, "vy":-0.29999, "omega":-0.41491, "ax":-3.53241, "ay":-0.07981, "alpha":0.14947, "fx":[-46.07267,-46.05933,-46.05774,-46.07155], "fy":[-0.73096,-1.3292,-1.35459,-0.7485]}, + {"t":3.07463, "x":2.50173, "y":5.52891, "heading":-0.02602, "vx":-0.21602, "vy":-0.30248, "omega":-0.41025, "ax":-3.53117, "ay":-0.10998, "alpha":0.15165, "fx":[-46.05955,-46.04096,-46.03856,-46.05786], "fy":[-1.11376,-1.71984,-1.75873,-1.14462]}, + {"t":3.10581, "x":2.49328, "y":5.51943, "heading":-0.03881, "vx":-0.32613, "vy":-0.30591, "omega":-0.40552, "ax":-3.52938, "ay":-0.14572, "alpha":0.15421, "fx":[-46.03986,-46.01488,-46.01127,-46.03726], "fy":[-1.56802,-2.18302,-2.23677,-1.61354]}, + {"t":3.137, "x":2.48139, "y":5.50982, "heading":-0.05146, "vx":-0.43619, "vy":-0.31045, "omega":-0.40071, "ax":-3.52674, "ay":-0.18872, "alpha":0.15727, "fx":[-46.01019,-45.97731,-45.9719,-46.00614], "fy":[-2.11533,-2.74059,-2.81103,-2.17731]}, + {"t":3.16818, "x":2.46608, "y":5.50005, "heading":-0.06395, "vx":-0.54616, "vy":-0.31633, "omega":-0.39581, "ax":-3.52278, "ay":-0.24141, "alpha":0.16099, "fx":[-45.96488,-45.92198,-45.9139,-45.95858], "fy":[-2.78686,-3.42407,-3.51374,-2.86783]}, + {"t":3.19936, "x":2.44733, "y":5.49007, "heading":-0.07629, "vx":-0.65601, "vy":-0.32386, "omega":-0.39079, "ax":-3.51671, "ay":-0.30741, "alpha":0.16561, "fx":[-45.89412,-45.83821,-45.82605,-45.88429], "fy":[-3.62928,-4.28056,-4.39307,-3.73281]}, + {"t":3.23054, "x":2.42517, "y":5.47982, "heading":-0.08848, "vx":-0.76567, "vy":-0.33345, "omega":-0.38562, "ax":-3.50705, "ay":-0.39242, "alpha":0.17147, "fx":[-45.77989,-45.70651,-45.68793,-45.76436], "fy":[-4.71545,-5.38346,-5.52413,-4.84689]}, + {"t":3.26173, "x":2.39959, "y":5.46923, "heading":-0.1005, "vx":-0.87503, "vy":-0.34569, "omega":-0.38027, "ax":-3.49091, "ay":-0.50576, "alpha":0.17915, "fx":[-45.58671,-45.48892,-45.45973,-45.56154], "fy":[-6.16537,-6.8532,-7.03027,-6.33298]}, + {"t":3.29291, "x":2.3706, "y":5.4582, "heading":-0.11236, "vx":-0.98388, "vy":-0.36146, "omega":-0.37469, "ax":-3.46211, "ay":-0.66377, "alpha":0.18959, "fx":[-45.23808,-45.10449,-45.0564,-45.19544], "fy":[-8.1897,-8.90031,-9.12717,-8.40703]}, + {"t":3.32409, "x":2.33824, "y":5.44661, "heading":-0.12405, "vx":-1.09184, "vy":-0.38215, "omega":-0.36878, "ax":-3.40569, "ay":-0.89737, "alpha":0.20446, "fx":[-44.54819,-44.35898,-44.27379,-44.47053], "fy":[-11.18824,-11.92162,-12.22114,-11.47882]}, + {"t":3.35527, "x":2.30254, "y":5.43426, "heading":-0.13555, "vx":-1.19804, "vy":-0.41014, "omega":-0.3624, "ax":-3.27947, "ay":-1.27005, "alpha":0.22671, "fx":[-42.98858,-42.70867,-42.53994,-42.83029], "fy":[-15.98783,-16.7283,-17.14014,-16.39347]}, + {"t":3.38646, "x":2.26359, "y":5.42085, "heading":-0.14685, "vx":-1.3003, "vy":-0.44974, "omega":-0.35533, "ax":-2.93902, "ay":-1.91537, "alpha":0.26032, "fx":[-38.72625,-38.3106,-37.92231,-38.34914], "fy":[-24.36094,-25.02002,-25.59309,-24.93788]}, + {"t":3.41764, "x":2.22161, "y":5.40589, "heading":-0.15793, "vx":-1.39195, "vy":-0.50947, "omega":-0.34721, "ax":-1.84225, "ay":-2.96928, "alpha":0.29319, "fx":[-24.67559,-24.29221,-23.37918,-23.75036], "fy":[-38.3099,-38.56975,-39.1236,-38.88345]}, + {"t":3.44882, "x":2.17731, "y":5.38856, "heading":-0.16875, "vx":-1.44939, "vy":-0.60206, "omega":-0.33807, "ax":0.6311, "ay":-3.43313, "alpha":0.2252, "fx":[7.96271,7.66788,8.49129,8.79824], "fy":[-44.81325,-44.87632,-44.73195,-44.66109]}, + {"t":3.48, "x":2.13242, "y":5.36812, "heading":-0.1793, "vx":-1.42971, "vy":-0.70911, "omega":-0.33105, "ax":2.33984, "ay":-2.60872, "alpha":0.09384, "fx":[30.53669,30.31677,30.49082,30.70922], "fy":[-33.99584,-34.19511,-34.04382,-33.84397]}, + {"t":3.51119, "x":2.08898, "y":5.34474, "heading":-0.18962, "vx":-1.35675, "vy":-0.79046, "omega":-0.32812, "ax":2.94405, "ay":-1.91982, "alpha":0.01871, "fx":[38.40372,38.3659,38.38185,38.41958], "fy":[-25.01853,-25.07684,-25.05327,-24.99501]}, + {"t":3.54237, "x":2.0481, "y":5.31916, "heading":-0.19985, "vx":-1.26495, "vy":-0.85032, "omega":-0.32754, "ax":3.18016, "ay":-1.51061, "alpha":-0.02065, "fx":[41.45907,41.49349,41.48458,41.45008], "fy":[-19.72695,-19.65422,-19.67207,-19.74491]}, + {"t":3.57355, "x":2.0102, "y":5.29191, "heading":-0.21006, "vx":-1.16578, "vy":-0.89743, "omega":-0.32818, "ax":3.29294, "ay":-1.25614, "alpha":-0.04365, "fx":[42.91686,42.97843,42.96833,42.90644], "fy":[-16.44946,-16.28762,-16.31232,-16.47472]}, + {"t":3.60473, "x":1.97545, "y":5.26332, "heading":-0.2203, "vx":-1.0631, "vy":-0.9366, "omega":-0.32954, "ax":3.3555, "ay":-1.08593, "alpha":-0.05846, "fx":[43.72608,43.79792,43.79072,43.71842], "fy":[-14.26233,-14.03995,-14.05989,-14.28336]}, + {"t":3.63592, "x":1.94393, "y":5.23358, "heading":-0.23057, "vx":-0.95847, "vy":-0.97046, "omega":-0.33137, "ax":3.39404, "ay":-0.96503, "alpha":-0.06872, "fx":[44.22542,44.30073,44.29674,44.22091], "fy":[-12.71125,-12.44614,-12.4575,-12.72418]}, + {"t":3.6671, "x":1.9157, "y":5.20285, "heading":-0.24091, "vx":-0.85263, "vy":-1.00055, "omega":-0.33351, "ax":3.41967, "ay":-0.87507, "alpha":-0.07621, "fx":[44.55793,44.63376,44.63258,44.55622], "fy":[-11.55843,-11.26199,-11.26368,-11.56209]}, + {"t":3.69828, "x":1.89077, "y":5.17123, "heading":-0.25131, "vx":-0.746, "vy":-1.02784, "omega":-0.33589, "ax":3.4377, "ay":-0.80565, "alpha":-0.08192, "fx":[44.79227,44.86734,44.86849,44.79293], "fy":[-10.66985,-10.34971,-10.34161,-10.66406]}, + {"t":3.72946, "x":1.86918, "y":5.13878, "heading":-0.26178, "vx":-0.6388, "vy":-1.05296, "omega":-0.33844, "ax":3.45095, "ay":-0.75053, "alpha":-0.0864, "fx":[44.9648,45.03853,45.04162,44.96746], "fy":[-9.96497,-9.6264,-9.60877,-9.94992]}, + {"t":3.76065, "x":1.85094, "y":5.10559, "heading":-0.27233, "vx":-0.53119, "vy":-1.07636, "omega":-0.34114, "ax":3.46104, "ay":-0.70574, "alpha":-0.09001, "fx":[45.09628,45.16845,45.17317,45.10064], "fy":[-9.39268,-9.03949,-9.01268,-9.36868]}, + {"t":3.79183, "x":1.83606, "y":5.07168, "heading":-0.28297, "vx":-0.42327, "vy":-1.09837, "omega":-0.34394, "ax":3.46893, "ay":-0.66864, "alpha":-0.09298, "fx":[45.19931,45.26986,45.27599,45.20514], "fy":[-8.91911,-8.55412,-8.5185,-8.88648]}, + {"t":3.82301, "x":1.82454, "y":5.0371, "heading":-0.2937, "vx":-0.3151, "vy":-1.11922, "omega":-0.34684, "ax":3.47525, "ay":-0.63741, "alpha":-0.09546, "fx":[45.28192,45.35088,45.35822,45.28905], "fy":[-8.52097,-8.14633,-8.10223,-8.48001]}, + {"t":3.8542, "x":1.81641, "y":5.00189, "heading":-0.30451, "vx":-0.20673, "vy":-1.1391, "omega":-0.34982, "ax":3.4812, "ay":-0.60758, "alpha":-0.07903, "fx":[45.36694,45.42127,45.42811,45.37369], "fy":[-8.09905,-7.78888,-7.74651,-8.05882]}, + {"t":3.88934, "x":1.81129, "y":4.96148, "heading":-0.31681, "vx":-0.08438, "vy":-1.16045, "omega":-0.3526, "ax":3.48696, "ay":-0.57134, "alpha":-0.06796, "fx":[45.44748,45.49132,45.49777,45.45392], "fy":[-7.60477,-7.33835,-7.29591,-7.56391]}, + {"t":3.92449, "x":1.81048, "y":4.92034, "heading":-0.3292, "vx":0.03818, "vy":-1.18053, "omega":-0.35498, "ax":3.49326, "ay":-0.52854, "alpha":-0.0549, "fx":[45.53577,45.56843,45.57398,45.54136], "fy":[-7.01951,-6.80465,-6.76512,-6.98099]}, + {"t":3.95964, "x":1.81398, "y":4.87852, "heading":-0.34168, "vx":0.16096, "vy":-1.19911, "omega":-0.35691, "ax":3.50012, "ay":-0.47724, "alpha":-0.03927, "fx":[45.63173,45.65274,45.65684,45.63587], "fy":[-6.31632,-6.16303,-6.13063,-6.28443]}, + {"t":3.99478, "x":1.8218, "y":4.83608, "heading":-0.35422, "vx":0.28398, "vy":-1.21588, "omega":-0.35829, "ax":3.50745, "ay":-0.41469, "alpha":-0.02025, "fx":[45.73413,45.74349,45.74558,45.73624], "fy":[-5.4567,-5.37797,-5.35889,-5.43776]}, + {"t":4.02993, "x":1.83395, "y":4.79309, "heading":-0.36681, "vx":0.40725, "vy":-1.23046, "omega":-0.35901, "ax":3.515, "ay":-0.3368, "alpha":0.00338, "fx":[45.8391,45.83784,45.83752,45.83878], "fy":[-4.38374,-4.39681,-4.40045,-4.38738]}, + {"t":4.06508, "x":1.85043, "y":4.74964, "heading":-0.37943, "vx":0.5308, "vy":-1.24229, "omega":-0.35889, "ax":3.52211, "ay":-0.23732, "alpha":0.03349, "fx":[45.93669,45.928,45.92547,45.93425], "fy":[-3.01008,-3.13835,-3.17955,-3.0516]}, + {"t":4.10022, "x":1.87126, "y":4.70583, "heading":-0.39204, "vx":0.65459, "vy":-1.25064, "omega":-0.35771, "ax":3.52719, "ay":-0.10623, "alpha":0.07305, "fx":[46.00265,45.99437,45.9917,46.00055], "fy":[-1.19545,-1.47115,-1.57458,-1.3003]}, + {"t":4.13537, "x":1.89645, "y":4.66181, "heading":-0.40462, "vx":0.77856, "vy":-1.25437, "omega":-0.35514, "ax":3.52624, "ay":0.07347, "alpha":0.12706, "fx":[45.97685,45.98698,45.99178,45.98384], "fy":[1.2974,0.82979,0.62064,1.08456]}, + {"t":4.17052, "x":1.92599, "y":4.61776, "heading":-0.4171, "vx":0.90249, "vy":-1.25179, "omega":-0.35068, "ax":3.50873, "ay":0.33235, "alpha":0.20445, "fx":[45.70144,45.77096,45.80828,45.74566], "fy":[4.89312,4.17556,3.77861,4.48916]}, + {"t":4.20567, "x":1.95988, "y":4.57397, "heading":-0.42942, "vx":1.02582, "vy":-1.24011, "omega":-0.34349, "ax":3.44434, "ay":0.72852, "alpha":0.32191, "fx":[44.72423,44.94508,45.10004,44.89818], "fy":[10.38713,9.36631,8.61731,9.63137]}, + {"t":4.24081, "x":1.99806, "y":4.53084, "heading":-0.4415, "vx":1.14687, "vy":-1.2145, "omega":-0.33218, "ax":3.23567, "ay":1.37151, "alpha":0.5098, "fx":[41.62356,42.16394,42.7493,42.24582], "fy":[19.20834,17.96452,16.5493,17.81997]}, + {"t":4.27596, "x":2.04037, "y":4.489, "heading":-0.45317, "vx":1.2606, "vy":-1.1663, "omega":-0.31426, "ax":2.56633, "ay":2.3874, "alpha":0.79862, "fx":[32.08266,32.81393,34.88032,34.09085], "fy":[32.60938,31.83423,29.57294,30.51763]}, + {"t":4.31111, "x":2.08626, "y":4.44948, "heading":-0.46422, "vx":1.3508, "vy":-1.08239, "omega":-0.28619, "ax":0.90935, "ay":3.37852, "alpha":1.05249, "fx":[10.37149,9.37327,13.52478,14.16518], "fy":[44.50714,44.68644,43.59958,43.44098]}, + {"t":4.34625, "x":2.1343, "y":4.41352, "heading":-0.47428, "vx":1.38276, "vy":-0.96364, "omega":-0.2492, "ax":-0.95613, "ay":3.37197, "alpha":0.98891, "fx":[-12.57514,-15.15431,-12.37342,-9.7716], "fy":[44.01128,43.16708,44.01844,44.69584]}, + {"t":4.3814, "x":2.18231, "y":4.38174, "heading":-0.48304, "vx":1.34915, "vy":-0.84513, "omega":-0.21444, "ax":-1.98808, "ay":2.89725, "alpha":0.81438, "fx":[-25.41183,-27.70863,-26.5207,-24.06315], "fy":[38.18149,36.53781,37.38542,39.02508]}, + {"t":4.41655, "x":2.2285, "y":4.35382, "heading":-0.49057, "vx":1.27928, "vy":-0.7433, "omega":-0.18582, "ax":-2.47959, "ay":2.49818, "alpha":0.6791, "fx":[-31.73248,-33.51759,-33.00224,-31.09098], "fy":[33.20537,31.39735,31.92014,33.79022]}, + {"t":4.4517, "x":2.27193, "y":4.32924, "heading":-0.4971, "vx":1.19213, "vy":-0.65549, "omega":-0.16195, "ax":-2.73722, "ay":2.21911, "alpha":0.58702, "fx":[-35.12709,-36.54343,-36.30762,-34.80369], "fy":[29.65772,27.89141,28.18311,30.02327]}, + {"t":4.48684, "x":2.31214, "y":4.30757, "heading":-0.5028, "vx":1.09592, "vy":-0.5775, "omega":-0.14132, "ax":-2.88845, "ay":2.02288, "alpha":0.52323, "fx":[-37.15139,-38.3204,-38.21508,-36.98392], "fy":[27.12881,25.44964,25.59567,27.34575]}, + {"t":4.52199, "x":2.34887, "y":4.28853, "heading":-0.50776, "vx":0.9944, "vy":-0.5064, "omega":-0.12293, "ax":-2.9856, "ay":1.88001, "alpha":0.47724, "fx":[-38.46519,-39.4642,-39.4268,-38.3818], "fy":[25.26992,23.67896,23.73129,25.38722]}, + {"t":4.55714, "x":2.38198, "y":4.27189, "heading":-0.51208, "vx":0.88947, "vy":-0.44032, "omega":-0.10615, "ax":-3.05238, "ay":1.77224, "alpha":0.44279, "fx":[-39.37489,-40.25244,-40.25354,-39.3406], "fy":[23.85809,22.34562,22.3352,23.90673]}, + {"t":4.59228, "x":2.41136, "y":4.25751, "heading":-0.51581, "vx":0.78219, "vy":-0.37803, "omega":-0.09059, "ax":-3.10072, "ay":1.68841, "alpha":0.41613, "fx":[-40.0369,-40.82442,-40.8488,-40.03311], "fy":[22.7543,21.30924,21.25511,22.7541]}, + {"t":4.62743, "x":2.43693, "y":4.24526, "heading":-0.519, "vx":0.6732, "vy":-0.31869, "omega":-0.07596, "ax":-3.13715, "ay":1.62151, "alpha":0.39494, "fx":[-40.53774,-41.25638,-41.29555,-40.55385], "fy":[21.8699,20.48226,20.39669,21.83395]}, + {"t":4.66258, "x":2.45866, "y":4.23507, "heading":-0.52167, "vx":0.56294, "vy":-0.2617, "omega":-0.06208, "ax":-3.16549, "ay":1.56695, "alpha":0.37772, "fx":[-40.92858,-41.5931,-41.64199,-40.95815], "fy":[21.14652,19.80788,19.69909,21.08376]}, + {"t":4.69773, "x":2.47649, "y":4.22683, "heading":-0.52385, "vx":0.45168, "vy":-0.20662, "omega":-0.04881, "ax":-3.18811, "ay":1.52167, "alpha":0.36347, "fx":[-41.24135,-41.86238,-41.91781,-41.28027], "fy":[20.54443,19.24783,19.12158,20.46119]}, + {"t":4.73287, "x":2.49039, "y":4.22051, "heading":-0.52556, "vx":0.33963, "vy":-0.15314, "omega":-0.03603, "ax":-3.20655, "ay":1.4835, "alpha":0.35148, "fx":[-41.4969,-42.08231,-42.1422,-41.54244], "fy":[20.03579,18.77552,18.63595,19.9367]}, + {"t":4.76802, "x":2.50035, "y":4.21605, "heading":-0.52683, "vx":0.22693, "vy":-0.101, "omega":-0.02368, "ax":-3.22186, "ay":1.45091, "alpha":0.34126, "fx":[-41.70936,-42.26514,-42.32806,-41.75964], "fy":[19.60058,18.37192,18.22213,19.48912]}, + {"t":4.80317, "x":2.50634, "y":4.21339, "heading":-0.52766, "vx":0.11369, "vy":-0.05001, "omega":-0.01168, "ax":-3.23475, "ay":1.42276, "alpha":0.33245, "fx":[-41.88864,-42.41942,-42.48436,-41.9423], "fy":[19.22406,18.02309,17.86546,19.10292]}, + {"t":4.83831, "x":2.50833, "y":4.21251, "heading":-0.52807, "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,37,89] + }, + "events":[] +} diff --git a/FlexAdvKitBeta/src/main/deploy/choreo/Forward.traj b/FlexAdvKitBeta/src/main/deploy/choreo/Forward.traj new file mode 100644 index 0000000..8dfdfa3 --- /dev/null +++ b/FlexAdvKitBeta/src/main/deploy/choreo/Forward.traj @@ -0,0 +1,107 @@ +{ + "name":"Forward", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":0.9149709343910216, "y":6.988541603088379, "heading":0.0, "intervals":68, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.170864105224609, "y":6.938450813293457, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":10.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.9149709343910217 m", "val":0.9149709343910216}, "y":{"exp":"6.988541603088379 m", "val":6.988541603088379}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":68, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.170864105224609 m", "val":4.170864105224609}, "y":{"exp":"6.938450813293457 m", "val":6.938450813293457}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"10 m / s ^ 2", "val":10.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,3.35757], + "samples":[ + {"t":0.0, "x":0.91497, "y":6.98854, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.99442, "ay":-0.16673, "alpha":-0.25573, "fx":[130.45343,130.45789,130.58861,129.84015], "fy":[-3.49192,-1.77872,-1.77842,-1.64788]}, + {"t":0.04938, "x":0.92715, "y":6.98834, "heading":0.0, "vx":0.49348, "vy":-0.00823, "omega":-0.01263, "ax":9.99436, "ay":-0.16777, "alpha":0.12844, "fx":[130.52656,129.67616,130.56723,130.56712], "fy":[-2.16137,-2.21605,-2.21249,-2.1615]}, + {"t":0.09875, "x":0.9637, "y":6.98773, "heading":-0.00062, "vx":0.98697, "vy":-0.01652, "omega":-0.00628, "ax":0.25711, "ay":0.00332, "alpha":0.00472, "fx":[3.26876,3.47444,3.33417,3.3341], "fy":[0.07151,0.01495,0.01499,0.07158]}, + {"t":0.14813, "x":1.01275, "y":6.98692, "heading":-0.00093, "vx":0.99966, "vy":-0.01635, "omega":-0.00605, "ax":-0.00005, "ay":-0.00289, "alpha":0.0277, "fx":[-0.01323,-0.05422,0.03258,0.03239], "fy":[-0.01489,-0.06051,-0.06044,-0.01481]}, + {"t":0.1975, "x":1.06211, "y":6.9861, "heading":-0.00123, "vx":0.99966, "vy":-0.01649, "omega":-0.00468, "ax":0.0, "ay":0.00013, "alpha":0.0226, "fx":[-0.02244,-0.02223,0.0225,0.02228], "fy":[0.02491,-0.0216,-0.02151,0.025]}, + {"t":0.24688, "x":1.11147, "y":6.98529, "heading":-0.00146, "vx":0.99966, "vy":-0.01649, "omega":-0.00357, "ax":0.00002, "ay":0.00115, "alpha":0.02096, "fx":[-0.02084,-0.01954,0.02079,0.02057], "fy":[0.03683,-0.00694,-0.00684,0.03692]}, + {"t":0.29626, "x":1.16083, "y":6.98448, "heading":-0.00164, "vx":0.99966, "vy":-0.01643, "omega":-0.00253, "ax":0.00002, "ay":0.00103, "alpha":0.01821, "fx":[-0.0181,-0.01645,0.01781,0.01761], "fy":[0.03262,-0.00593,-0.00584,0.03271]}, + {"t":0.34563, "x":1.21019, "y":6.98367, "heading":-0.00177, "vx":0.99966, "vy":-0.01638, "omega":-0.00163, "ax":0.00001, "ay":0.00082, "alpha":0.01538, "fx":[-0.01507,-0.01367,0.01481,0.01462], "fy":[0.02712,-0.00592,-0.00583,0.0272]}, + {"t":0.39501, "x":1.25955, "y":6.98286, "heading":-0.00185, "vx":0.99966, "vy":-0.01634, "omega":-0.00087, "ax":0.00001, "ay":0.00073, "alpha":0.01284, "fx":[-0.0123,-0.01116,0.01212,0.01195], "fy":[0.02353,-0.00454,-0.00447,0.02361]}, + {"t":0.44438, "x":1.3089, "y":6.98205, "heading":-0.00189, "vx":0.99966, "vy":-0.0163, "omega":-0.00024, "ax":0.00001, "ay":0.00072, "alpha":0.01066, "fx":[-0.00991,-0.009,0.00983,0.00968], "fy":[0.02133,-0.0025,-0.00244,0.0214]}, + {"t":0.49376, "x":1.35826, "y":6.98125, "heading":-0.0019, "vx":0.99966, "vy":-0.01627, "omega":0.00029, "ax":0.00001, "ay":0.00074, "alpha":0.00882, "fx":[-0.0079,-0.00717,0.0079,0.00778], "fy":[0.01973,-0.0005,-0.00044,0.01979]}, + {"t":0.54314, "x":1.40762, "y":6.98045, "heading":-0.00189, "vx":0.99966, "vy":-0.01623, "omega":0.00072, "ax":0.00001, "ay":0.00075, "alpha":0.00725, "fx":[-0.0062,-0.00561,0.00627,0.00617], "fy":[0.01836,0.00123,0.00127,0.0184]}, + {"t":0.59251, "x":1.45698, "y":6.97965, "heading":-0.00185, "vx":0.99966, "vy":-0.0162, "omega":0.00108, "ax":0.00001, "ay":0.00076, "alpha":0.00589, "fx":[-0.00476,-0.00428,0.00488,0.0048], "fy":[0.01708,0.00267,0.00271,0.01712]}, + {"t":0.64189, "x":1.50634, "y":6.97885, "heading":-0.0018, "vx":0.99966, "vy":-0.01616, "omega":0.00137, "ax":0.00001, "ay":0.00076, "alpha":0.0047, "fx":[-0.00354,-0.00312,0.00368,0.00361], "fy":[0.01591,0.0039,0.00393,0.01594]}, + {"t":0.69126, "x":1.5557, "y":6.97805, "heading":-0.00173, "vx":0.99966, "vy":-0.01612, "omega":0.0016, "ax":0.00001, "ay":0.00076, "alpha":0.00366, "fx":[-0.00249,-0.00211,0.00264,0.00259], "fy":[0.01484,0.00497,0.00499,0.01486]}, + {"t":0.74064, "x":1.60506, "y":6.97726, "heading":-0.00165, "vx":0.99967, "vy":-0.01608, "omega":0.00178, "ax":0.00001, "ay":0.00076, "alpha":0.00275, "fx":[-0.00159,-0.00124,0.00175,0.0017], "fy":[0.01387,0.00591,0.00593,0.01389]}, + {"t":0.79002, "x":1.65442, "y":6.97646, "heading":-0.00156, "vx":0.99967, "vy":-0.01605, "omega":0.00192, "ax":0.00001, "ay":0.00076, "alpha":0.00195, "fx":[-0.00082,-0.00048,0.00098,0.00095], "fy":[0.01302,0.00674,0.00676,0.01303]}, + {"t":0.83939, "x":1.70378, "y":6.97567, "heading":-0.00147, "vx":0.99967, "vy":-0.01601, "omega":0.00201, "ax":0.00001, "ay":0.00076, "alpha":0.00126, "fx":[-0.00018,0.00018,0.00033,0.0003], "fy":[0.01226,0.00748,0.0075,0.01227]}, + {"t":0.88877, "x":1.75314, "y":6.97488, "heading":-0.00137, "vx":0.99967, "vy":-0.01597, "omega":0.00208, "ax":0.00001, "ay":0.00076, "alpha":0.00066, "fx":[0.00036,0.00074,-0.00023,-0.00025], "fy":[0.0116,0.00813,0.00814,0.01161]}, + {"t":0.93814, "x":1.8025, "y":6.97409, "heading":-0.00127, "vx":0.99967, "vy":-0.01593, "omega":0.00211, "ax":0.00001, "ay":0.00076, "alpha":0.00014, "fx":[0.00081,0.00123,-0.0007,-0.00072], "fy":[0.01102,0.00871,0.00871,0.01103]}, + {"t":0.98752, "x":1.85186, "y":6.97331, "heading":-0.00116, "vx":0.99967, "vy":-0.0159, "omega":0.00212, "ax":0.00001, "ay":0.00076, "alpha":-0.0003, "fx":[0.00117,0.00165,-0.0011,-0.00111], "fy":[0.01051,0.0092,0.00921,0.01052]}, + {"t":1.0369, "x":1.90122, "y":6.97253, "heading":-0.00106, "vx":0.99967, "vy":-0.01586, "omega":0.0021, "ax":0.00001, "ay":0.00076, "alpha":-0.00068, "fx":[0.00147,0.00201,-0.00142,-0.00143], "fy":[0.01007,0.00963,0.00964,0.01008]}, + {"t":1.08627, "x":1.95058, "y":6.97174, "heading":-0.00095, "vx":0.99967, "vy":-0.01582, "omega":0.00207, "ax":0.00001, "ay":0.00076, "alpha":-0.001, "fx":[0.0017,0.00231,-0.0017,-0.0017], "fy":[0.00969,0.01001,0.01001,0.00969]}, + {"t":1.13565, "x":1.99994, "y":6.97096, "heading":-0.00085, "vx":0.99967, "vy":-0.01578, "omega":0.00202, "ax":0.00001, "ay":0.00075, "alpha":-0.00127, "fx":[0.00188,0.00258,-0.00192,-0.00192], "fy":[0.00936,0.01033,0.01033,0.00936]}, + {"t":1.18502, "x":2.0493, "y":6.97018, "heading":-0.00075, "vx":0.99967, "vy":-0.01575, "omega":0.00196, "ax":0.00001, "ay":0.00075, "alpha":-0.0015, "fx":[0.00201,0.00281,-0.0021,-0.0021], "fy":[0.00907,0.01061,0.01061,0.00907]}, + {"t":1.2344, "x":2.09866, "y":6.96941, "heading":-0.00066, "vx":0.99967, "vy":-0.01571, "omega":0.00188, "ax":0.00001, "ay":0.00075, "alpha":-0.0017, "fx":[0.00211,0.00302,-0.00226,-0.00226], "fy":[0.00881,0.01085,0.01085,0.00881]}, + {"t":1.28378, "x":2.14802, "y":6.96863, "heading":-0.00056, "vx":0.99967, "vy":-0.01567, "omega":0.0018, "ax":0.00001, "ay":0.00075, "alpha":-0.00187, "fx":[0.00218,0.0032,-0.00239,-0.00238], "fy":[0.00859,0.01107,0.01107,0.00859]}, + {"t":1.33315, "x":2.19738, "y":6.96786, "heading":-0.00047, "vx":0.99967, "vy":-0.01563, "omega":0.00171, "ax":0.00001, "ay":0.00075, "alpha":-0.00202, "fx":[0.00222,0.00337,-0.00249,-0.00249], "fy":[0.00839,0.01126,0.01125,0.00839]}, + {"t":1.38253, "x":2.24674, "y":6.96709, "heading":-0.00039, "vx":0.99967, "vy":-0.0156, "omega":0.00161, "ax":0.00001, "ay":0.00075, "alpha":-0.00215, "fx":[0.00224,0.00353,-0.00259,-0.00258], "fy":[0.00821,0.01142,0.01142,0.00821]}, + {"t":1.4319, "x":2.2961, "y":6.96632, "heading":-0.00031, "vx":0.99967, "vy":-0.01556, "omega":0.0015, "ax":0.00001, "ay":0.00075, "alpha":-0.00227, "fx":[0.00225,0.00368,-0.00267,-0.00266], "fy":[0.00804,0.01157,0.01157,0.00804]}, + {"t":1.48128, "x":2.34546, "y":6.96555, "heading":-0.00024, "vx":0.99967, "vy":-0.01552, "omega":0.00139, "ax":0.00001, "ay":0.00075, "alpha":-0.00238, "fx":[0.00225,0.00383,-0.00274,-0.00273], "fy":[0.00789,0.01171,0.01171,0.00789]}, + {"t":1.53066, "x":2.39482, "y":6.96479, "heading":-0.00017, "vx":0.99967, "vy":-0.01549, "omega":0.00127, "ax":0.00001, "ay":0.00075, "alpha":-0.00248, "fx":[0.00223,0.00398,-0.00281,-0.0028], "fy":[0.00775,0.01185,0.01184,0.00774]}, + {"t":1.58003, "x":2.44418, "y":6.96402, "heading":-0.00011, "vx":0.99968, "vy":-0.01545, "omega":0.00115, "ax":0.00001, "ay":0.00075, "alpha":-0.00258, "fx":[0.00221,0.00414,-0.00288,-0.00287], "fy":[0.00761,0.01198,0.01197,0.0076]}, + {"t":1.62941, "x":2.49354, "y":6.96326, "heading":-0.00005, "vx":0.99968, "vy":-0.01541, "omega":0.00102, "ax":0.00001, "ay":0.00075, "alpha":-0.00268, "fx":[0.00219,0.00429,-0.00295,-0.00294], "fy":[0.00747,0.0121,0.0121,0.00746]}, + {"t":1.67878, "x":2.5429, "y":6.9625, "heading":0.0, "vx":0.99968, "vy":-0.01537, "omega":0.00089, "ax":0.00001, "ay":0.00075, "alpha":-0.00278, "fx":[0.00217,0.00446,-0.00302,-0.00301], "fy":[0.00732,0.01223,0.01222,0.00732]}, + {"t":1.72816, "x":2.59226, "y":6.96174, "heading":0.00005, "vx":0.99968, "vy":-0.01534, "omega":0.00075, "ax":0.00001, "ay":0.00075, "alpha":-0.00289, "fx":[0.00214,0.00464,-0.00309,-0.00309], "fy":[0.00718,0.01236,0.01235,0.00718]}, + {"t":1.77754, "x":2.64162, "y":6.96099, "heading":0.00008, "vx":0.99968, "vy":-0.0153, "omega":0.00061, "ax":0.00001, "ay":0.00075, "alpha":-0.00299, "fx":[0.00211,0.00482,-0.00317,-0.00317], "fy":[0.00704,0.01249,0.01248,0.00703]}, + {"t":1.82691, "x":2.69098, "y":6.96023, "heading":0.00011, "vx":0.99968, "vy":-0.01526, "omega":0.00046, "ax":0.00001, "ay":0.00075, "alpha":-0.0031, "fx":[0.00208,0.005,-0.00325,-0.00325], "fy":[0.00689,0.01263,0.01262,0.00688]}, + {"t":1.87629, "x":2.74034, "y":6.95948, "heading":0.00014, "vx":0.99968, "vy":-0.01523, "omega":0.00031, "ax":0.00001, "ay":0.00075, "alpha":-0.00321, "fx":[0.00205,0.0052,-0.00333,-0.00333], "fy":[0.00674,0.01276,0.01276,0.00673]}, + {"t":1.92566, "x":2.7897, "y":6.95873, "heading":0.00015, "vx":0.99968, "vy":-0.01519, "omega":0.00015, "ax":0.00001, "ay":0.00075, "alpha":-0.00333, "fx":[0.00201,0.0054,-0.00341,-0.00341], "fy":[0.00659,0.0129,0.01289,0.00658]}, + {"t":1.97504, "x":2.83906, "y":6.95798, "heading":0.00016, "vx":0.99968, "vy":-0.01515, "omega":-0.00002, "ax":0.00001, "ay":0.00075, "alpha":-0.00343, "fx":[0.00196,0.0056,-0.00349,-0.00348], "fy":[0.00644,0.01304,0.01303,0.00643]}, + {"t":2.02442, "x":2.88842, "y":6.95723, "heading":0.00016, "vx":0.99968, "vy":-0.01512, "omega":-0.00019, "ax":0.00001, "ay":0.00075, "alpha":-0.00354, "fx":[0.0019,0.00579,-0.00356,-0.00355], "fy":[0.00629,0.01317,0.01316,0.00629]}, + {"t":2.07379, "x":2.93778, "y":6.95649, "heading":0.00015, "vx":0.99968, "vy":-0.01508, "omega":-0.00036, "ax":0.00001, "ay":0.00075, "alpha":-0.00362, "fx":[0.00182,0.00598,-0.00361,-0.0036], "fy":[0.00616,0.01329,0.01328,0.00615]}, + {"t":2.12317, "x":2.98714, "y":6.95574, "heading":0.00013, "vx":0.99968, "vy":-0.01504, "omega":-0.00054, "ax":0.00001, "ay":0.00074, "alpha":-0.00369, "fx":[0.00171,0.00615,-0.00364,-0.00364], "fy":[0.00604,0.01339,0.01338,0.00604]}, + {"t":2.17254, "x":3.0365, "y":6.955, "heading":0.0001, "vx":0.99968, "vy":-0.01501, "omega":-0.00072, "ax":0.00001, "ay":0.00074, "alpha":-0.00374, "fx":[0.00157,0.0063,-0.00365,-0.00364], "fy":[0.00595,0.01347,0.01346,0.00594]}, + {"t":2.22192, "x":3.08586, "y":6.95426, "heading":0.00007, "vx":0.99968, "vy":-0.01497, "omega":-0.00091, "ax":0.00001, "ay":0.00074, "alpha":-0.00375, "fx":[0.00139,0.00641,-0.00361,-0.00361], "fy":[0.00589,0.01351,0.01351,0.00589]}, + {"t":2.2713, "x":3.13522, "y":6.95352, "heading":0.00002, "vx":0.99968, "vy":-0.01493, "omega":-0.00109, "ax":0.00001, "ay":0.00074, "alpha":-0.00371, "fx":[0.00114,0.00648,-0.00353,-0.00352], "fy":[0.00587,0.01351,0.01351,0.00587]}, + {"t":2.32067, "x":3.18458, "y":6.95279, "heading":-0.00003, "vx":0.99968, "vy":-0.0149, "omega":-0.00127, "ax":0.00001, "ay":0.00074, "alpha":-0.00361, "fx":[0.00083,0.00649,-0.00337,-0.00337], "fy":[0.00592,0.01345,0.01345,0.00592]}, + {"t":2.37005, "x":3.23394, "y":6.95205, "heading":-0.00009, "vx":0.99968, "vy":-0.01486, "omega":-0.00145, "ax":0.00001, "ay":0.00074, "alpha":-0.00343, "fx":[0.00043,0.00643,-0.00314,-0.00314], "fy":[0.00604,0.01331,0.01332,0.00604]}, + {"t":2.41943, "x":3.2833, "y":6.95132, "heading":-0.00017, "vx":0.99968, "vy":-0.01482, "omega":-0.00162, "ax":0.00001, "ay":0.00074, "alpha":-0.00315, "fx":[-0.00008,0.00627,-0.00281,-0.00281], "fy":[0.00625,0.01308,0.01309,0.00626]}, + {"t":2.4688, "x":3.33266, "y":6.95059, "heading":-0.00025, "vx":0.99969, "vy":-0.01479, "omega":-0.00178, "ax":0.00001, "ay":0.00074, "alpha":-0.00276, "fx":[-0.00072,0.00599,-0.00235,-0.00235], "fy":[0.00658,0.01273,0.01274,0.0066]}, + {"t":2.51818, "x":3.38202, "y":6.94986, "heading":-0.00033, "vx":0.99969, "vy":-0.01475, "omega":-0.00191, "ax":0.00001, "ay":0.00074, "alpha":-0.00222, "fx":[-0.00152,0.00558,-0.00174,-0.00175], "fy":[0.00705,0.01223,0.01226,0.00708]}, + {"t":2.56755, "x":3.43138, "y":6.94913, "heading":-0.00043, "vx":0.99969, "vy":-0.01471, "omega":-0.00202, "ax":0.00001, "ay":0.00074, "alpha":-0.00151, "fx":[-0.00249,0.005,-0.00097,-0.00098], "fy":[0.00769,0.01157,0.0116,0.00772]}, + {"t":2.61693, "x":3.48074, "y":6.94841, "heading":-0.00053, "vx":0.99969, "vy":-0.01468, "omega":-0.0021, "ax":0.00001, "ay":0.00074, "alpha":-0.00061, "fx":[-0.00367,0.00423,0.00001,-0.00001], "fy":[0.00851,0.01071,0.01076,0.00856]}, + {"t":2.66631, "x":3.5301, "y":6.94768, "heading":-0.00063, "vx":0.99969, "vy":-0.01464, "omega":-0.00213, "ax":0.00001, "ay":0.00074, "alpha":0.0005, "fx":[-0.00509,0.00325,0.00121,0.00119], "fy":[0.00955,0.00963,0.00969,0.00961]}, + {"t":2.71568, "x":3.57946, "y":6.94696, "heading":-0.00074, "vx":0.99969, "vy":-0.0146, "omega":-0.0021, "ax":0.00001, "ay":0.00074, "alpha":0.00188, "fx":[-0.00678,0.002,0.00269,0.00265], "fy":[0.01084,0.00829,0.00836,0.01091]}, + {"t":2.76506, "x":3.62882, "y":6.94624, "heading":-0.00084, "vx":0.99969, "vy":-0.01457, "omega":-0.00201, "ax":0.00001, "ay":0.00074, "alpha":0.00355, "fx":[-0.0088,0.00051,0.00444,0.0044], "fy":[0.01244,0.00668,0.00676,0.01252]}, + {"t":2.81443, "x":3.67819, "y":6.94552, "heading":-0.00094, "vx":0.99969, "vy":-0.01453, "omega":-0.00183, "ax":0.00001, "ay":0.00074, "alpha":0.0056, "fx":[-0.01113,-0.00154,0.00664,0.00659], "fy":[0.0144,0.00474,0.00483,0.0145]}, + {"t":2.86381, "x":3.72755, "y":6.94481, "heading":-0.00103, "vx":0.99969, "vy":-0.01449, "omega":-0.00156, "ax":0.00001, "ay":0.00074, "alpha":0.00797, "fx":[-0.01424,-0.00318,0.00902,0.00897], "fy":[0.01688,0.0024,0.00251,0.01699]}, + {"t":2.91319, "x":3.77691, "y":6.94409, "heading":-0.00111, "vx":0.99969, "vy":-0.01446, "omega":-0.00116, "ax":0.00001, "ay":0.00076, "alpha":0.01151, "fx":[-0.01703,-0.00856,0.01311,0.01304], "fy":[0.02011,-0.00051,-0.00039,0.02023]}, + {"t":2.96256, "x":3.82627, "y":6.94338, "heading":-0.00116, "vx":0.99969, "vy":-0.01442, "omega":-0.0006, "ax":0.00001, "ay":0.00077, "alpha":0.01425, "fx":[-0.02478,-0.00357,0.0145,0.01443], "fy":[0.02442,-0.00449,-0.00436,0.02455]}, + {"t":3.01194, "x":3.87563, "y":6.94267, "heading":-0.00119, "vx":0.99969, "vy":-0.01438, "omega":0.00011, "ax":0.00001, "ay":0.00076, "alpha":0.02507, "fx":[-0.02108,-0.03925,0.03049,0.03041], "fy":[0.0301,-0.0105,-0.01036,0.03024]}, + {"t":3.06131, "x":3.92499, "y":6.94196, "heading":-0.00119, "vx":0.99969, "vy":-0.01434, "omega":0.00135, "ax":0.00001, "ay":0.00066, "alpha":0.01586, "fx":[-0.06765,0.0622,0.003,0.00294], "fy":[0.03768,-0.02073,-0.02059,0.03783]}, + {"t":3.11069, "x":3.97435, "y":6.94125, "heading":-0.00112, "vx":0.99969, "vy":-0.01431, "omega":0.00213, "ax":0.00001, "ay":0.00042, "alpha":0.0805, "fx":[0.03525,-0.28125,0.12321,0.12311], "fy":[0.04479,-0.03393,-0.03376,0.04495]}, + {"t":3.16007, "x":4.02371, "y":6.94054, "heading":-0.00102, "vx":0.99969, "vy":-0.01429, "omega":0.0061, "ax":0.00001, "ay":0.00055, "alpha":-0.123, "fx":[-0.39258,0.97917,-0.29345,-0.29275], "fy":[0.05223,-0.03734,-0.03865,0.05258]}, + {"t":3.20944, "x":4.07307, "y":6.93984, "heading":-0.00072, "vx":0.99969, "vy":-0.01426, "omega":0.00003, "ax":-0.25487, "ay":0.00582, "alpha":0.17443, "fx":[-2.68283,-4.82275,-2.89283,-2.89634], "fy":[-0.00054,0.15185,0.15449,-0.00238]}, + {"t":3.25882, "x":4.12212, "y":6.93914, "heading":-0.00071, "vx":0.98711, "vy":-0.01398, "omega":0.00864, "ax":-9.99476, "ay":0.14145, "alpha":-0.05737, "fx":[-130.60905,-129.76892,-130.48999,-130.48999], "fy":[1.8791,1.81025,1.80923,1.87969]}, + {"t":3.30819, "x":4.15868, "y":6.93862, "heading":-0.00029, "vx":0.49361, "vy":-0.00699, "omega":0.00581, "ax":-9.9969, "ay":0.14163, "alpha":-0.11766, "fx":[-130.53615,-130.53587,-130.37577,-130.02163], "fy":[0.49133,2.25305,2.25302,2.39036]}, + {"t":3.35757, "x":4.17086, "y":6.93845, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/FlexAdvKitBeta/src/main/deploy/choreo/New Path.traj b/FlexAdvKitBeta/src/main/deploy/choreo/New Path.traj deleted file mode 100644 index f746de3..0000000 --- a/FlexAdvKitBeta/src/main/deploy/choreo/New Path.traj +++ /dev/null @@ -1,27 +0,0 @@ -{ - "name":"New Path", - "version":1, - "snapshot":{ - "waypoints":[], - "constraints":[], - "targetDt":0.05 - }, - "params":{ - "waypoints":[], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":null, - "waypoints":[], - "samples":[], - "splits":[] - }, - "events":[] -} diff --git a/FlexAdvKitBeta/src/main/deploy/choreo/proj.chor b/FlexAdvKitBeta/src/main/deploy/choreo/proj.chor index d3f5f5a..364409a 100644 --- a/FlexAdvKitBeta/src/main/deploy/choreo/proj.chor +++ b/FlexAdvKitBeta/src/main/deploy/choreo/proj.chor @@ -9,64 +9,64 @@ "config":{ "frontLeft":{ "x":{ - "exp":"11 in", - "val":0.2794 + "exp":"9.75 in", + "val":0.24765 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9.75 in", + "val":0.24765 } }, "backLeft":{ "x":{ - "exp":"-11 in", - "val":-0.2794 + "exp":"-9.75 in", + "val":-0.24765 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9.75 in", + "val":0.24765 } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"115 lbs", + "val":52.16312255 }, "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 + "exp":"2 kg m ^ 2", + "val":2.0 }, "gearing":{ - "exp":"6.5", - "val":6.5 + "exp":"5.142857142857142", + "val":5.142857142857142 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"2.194 in", + "val":0.055727599999999995 }, "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 + "exp":"3000 RPM", + "val":314.1592653589793 }, "tmax":{ - "exp":"1.2 N * m", - "val":1.2 + "exp":"0.5 N * m", + "val":0.5 }, "cof":{ - "exp":"1.5", - "val":1.5 + "exp":"1.19", + "val":1.19 }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"19.5 in", + "val":0.4953 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"16.25 in", + "val":0.41275 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"19.5 in", + "val":0.4953 } }, "differentialTrackWidth":{ diff --git a/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/3Piece.auto b/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/3Piece.auto new file mode 100644 index 0000000..a9ac09b --- /dev/null +++ b/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/3Piece.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/Forward.auto b/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/Forward.auto new file mode 100644 index 0000000..5c629cf --- /dev/null +++ b/FlexAdvKitBeta/src/main/deploy/pathplanner/autos/Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/Example Path.path b/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/Example Path.path index 303dbb2..803a50c 100644 --- a/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/Example Path.path +++ b/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/Example Path.path @@ -8,24 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 3.013282910175676, - "y": 6.5274984191074985 + "x": 2.7671758370498094, + "y": 6.180035899502349 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 + "x": 4.604764701683467, + "y": 4.122214464115091 }, "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 + "x": 4.281114356699413, + "y": 4.300295004571826 }, "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 + "x": 5.61310871873312, + "y": 3.5673982857749618 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 2.8925766268080317 + }, + "prevControl": { + "x": 6.69180462659209, + "y": 3.3862861371701434 + }, + "nextControl": { + "x": 7.273504656701004, + "y": 2.4544394521694968 }, "isLocked": false, "linkedName": null @@ -36,8 +52,8 @@ "y": 1.0 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 7.0335721456920615, + "y": 1.8747505799602564 }, "nextControl": null, "isLocked": false, diff --git a/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/ForwardAuto.path b/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/ForwardAuto.path new file mode 100644 index 0000000..049632a --- /dev/null +++ b/FlexAdvKitBeta/src/main/deploy/pathplanner/paths/ForwardAuto.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.772678179111184, + "y": 7.011088339115795 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.372716908050121, + "y": 7.008368243449518 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/FlexAdvKitBeta/src/main/deploy/pathplanner/settings.json b/FlexAdvKitBeta/src/main/deploy/pathplanner/settings.json index f981b63..46e3cdb 100644 --- a/FlexAdvKitBeta/src/main/deploy/pathplanner/settings.json +++ b/FlexAdvKitBeta/src/main/deploy/pathplanner/settings.json @@ -4,18 +4,18 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "robotMass": 74.088, "robotMOI": 6.883, "robotWheelbase": 0.546, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, + "driveWheelRadius": 0.0557, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 + "driveCurrentLimit": 120.0, + "wheelCOF": 1.19 } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/Constants.java b/FlexAdvKitBeta/src/main/java/frc/robot/Constants.java index c42039f..0078715 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/Constants.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/Constants.java @@ -27,19 +27,27 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.subsystems.drive.Drive; import java.util.Map; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -63,14 +71,18 @@ public static enum Mode { public class IntakeConstants { public static enum IntakeState { - IDLE, - INTAKE, - EXTAKE, + IDLE(0.0), + INTAKE(0.8), + EXTAKE(-0.8); + + public double pct = 0.0; + + private IntakeState(double pct) { + this.pct = pct; + } } public static final int INTAKE_MOTOR_ID = 6; - public static final double INTAKE_OUTPUT_PERCENT = 0.8; - public static final int BEAM_BREAK_SENSOR_ID = 1; } @@ -86,7 +98,7 @@ public static enum SysIdRoutineMode { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(50) + .withKP(100) .withKI(0) .withKD(0.0) .withKS(0.03428) @@ -97,12 +109,12 @@ public static enum SysIdRoutineMode { // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.17527) + .withKP(0.4) .withKI(0) .withKD(0) - .withKS(0.066052) - .withKV(0.11653) - .withKA(0.014691); + .withKS(0.08223) + .withKV(0.097754) + .withKA(0.02484); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -117,7 +129,7 @@ public static enum SysIdRoutineMode { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -142,7 +154,7 @@ public static enum SysIdRoutineMode { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(7.0); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.573); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -195,7 +207,7 @@ public static enum SysIdRoutineMode { private static final int kFrontLeftDriveMotorId = 13; private static final int kFrontLeftSteerMotorId = 14; private static final int kFrontLeftEncoderId = 15; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.214111328125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.2119140625); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftCANcoderInverted = false; @@ -206,7 +218,7 @@ public static enum SysIdRoutineMode { private static final int kFrontRightDriveMotorId = 19; private static final int kFrontRightSteerMotorId = 20; private static final int kFrontRightEncoderId = 21; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.267822265625); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.26513671875); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightCANcoderInverted = false; @@ -217,7 +229,7 @@ public static enum SysIdRoutineMode { private static final int kBackLeftDriveMotorId = 16; private static final int kBackLeftSteerMotorId = 17; private static final int kBackLeftEncoderId = 18; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.356689453125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.35205078125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftCANcoderInverted = false; @@ -228,7 +240,7 @@ public static enum SysIdRoutineMode { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.326171875); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.32763671875); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightCANcoderInverted = false; @@ -279,6 +291,42 @@ public static enum SysIdRoutineMode { kInvertRightSide, kBackRightSteerMotorInverted, kBackRightCANcoderInverted); + + // PathPlanner config constants + public static final double WHEEL_COF = 1.19; + public static final Mass ROBOT_MASS = Pounds.of(115.0); + public static final double ROBOT_MOI = 2.0; // TODO: find more accurate value + + public static final RobotConfig PP_CONFIG = + new RobotConfig( + DriveConstants.ROBOT_MASS.in(Kilograms), + DriveConstants.ROBOT_MOI, + new ModuleConfig( + FrontLeft.WheelRadius, + kSpeedAt12Volts.in(MetersPerSecond), + DriveConstants.WHEEL_COF, + DCMotor.getKrakenX60Foc(1).withReduction(FrontLeft.DriveMotorGearRatio), + FrontLeft.SlipCurrent, + 1), + Drive.getModuleTranslations()); + + public static final DriveTrainSimulationConfig MAPLE_SIM_CONFIG = + DriveTrainSimulationConfig.Default() + .withRobotMass(DriveConstants.ROBOT_MASS) + .withCustomModuleTranslations(Drive.getModuleTranslations()) + .withGyro(COTS.ofPigeon2()) + .withSwerveModule( + () -> + new SwerveModuleSimulation( + DCMotor.getKrakenX60(1), + DCMotor.getFalcon500(1), + FrontLeft.DriveMotorGearRatio, + FrontLeft.SteerMotorGearRatio, + Volts.of(FrontLeft.DriveFrictionVoltage), + Volts.of(FrontLeft.SteerFrictionVoltage), + Meters.of(FrontLeft.WheelRadius), + KilogramSquareMeters.of(FrontLeft.SteerInertia), + DriveConstants.WHEEL_COF)); } public class VisionConstants { @@ -322,11 +370,19 @@ public class VisionConstants { public static class ShooterConstants { public static enum ShooterState { - IDLE, - SPEAKER, - AMP, - SHUTTLE, - EXTAKE + IDLE(RPM.of(0), RPM.of(0)), + SPEAKER(RPM.of(3000), RPM.of(5000)), + AMP(RPM.of(-1200), RPM.of(-1200)), + SHUTTLE(RPM.of(2900), RPM.of(2900)), + EXTAKE(RPM.of(1500), RPM.of(1500)); + + public AngularVelocity topRPM; + public AngularVelocity bottomRPM; + + private ShooterState(AngularVelocity topRPM, AngularVelocity bottomRPM) { + this.topRPM = topRPM; + this.bottomRPM = bottomRPM; + } } public static final int SHOOTER_TOP_MOTOR_ID = 2; @@ -340,17 +396,6 @@ public static enum ShooterState { public static final LinearVelocity ShooterSpeed = FeetPerSecond.of(28.06308713961776); // in ft/s - public static final AngularVelocity SPK_TOP_RPM = RPM.of(3000); - public static final AngularVelocity SPK_BOTTOM_RPM = RPM.of(5000); - - public static final AngularVelocity SHUTTLING_TOP_RPM = RPM.of(2900); - public static final AngularVelocity SHUTTLING_BOTTOM_RPM = RPM.of(2900); - - public static final AngularVelocity AMP_TOP_RPM = RPM.of(-1200); - public static final AngularVelocity AMP_BOTTOM_RPM = RPM.of(-1200); - - public static final AngularVelocity SHOOTER_OUTTAKE_RPM = RPM.of(1500); - public static final AngularVelocity SHOOTER_MAX_RPM = RPM.of(6784); // TODO: tune further: get both faster @@ -366,13 +411,24 @@ public static enum ShooterState { public static class PivotConstants { public static enum PivotState { - INTAKE, - SUBWOOFER, - AMP, - SHUTTLE, - EXTAKE, - MANUAL_OVERRIDE, - AIMING + INTAKE(Rotations.of(22.5)), + INTAKE_SIM(Rotations.of(-0.1)), + SUBWOOFER(Rotations.of(5.0947265625)), + SUBWOOFER_SIM(Rotations.of(-0.166)), + AMP(Rotations.of(99)), + AMP_SIM(Rotations.of(0.31)), + SHUTTLE(Rotations.of(14.853271484375)), + SHUTTLE_SIM(Rotations.of(-0.02)), + EXTAKE(Rotations.of(22.5)), + EXTAKE_SIM(Rotations.of(-0.1)), + MANUAL_OVERRIDE(Rotations.of(0)), + AIMING(Rotations.of(0)); + + public Angle angle; + + private PivotState(Angle pivotAngle) { + this.angle = pivotAngle; + } } public static final double PIVOT_GEAR_RATIO = 31.25; @@ -382,43 +438,34 @@ public static enum PivotState { public static final int PIVOT_MAIN_MOTOR_ID = 1; public static final int PIVOT_FOLLOWER_MOTOR_ID = 5; - public static final double PIVOT_kV = 9.0; - public static final double PIVOT_kP = 2.0; + public static final double PIVOT_kV = 0.0; + public static final double PIVOT_kP = 8.0; public static final double PIVOT_kI = 0.0; - public static final double PIVOT_kD = 0.0; + public static final double PIVOT_kD = 0.01; + public static final double PIVOT_kG = 0.001; // motion magic constraints public static final double PIVOT_CRUISE_VELOCITY = 2500; public static final double PIVOT_ACCELERATION = 2000; public static final double PIVOT_JERK = 50000; - public static final Angle PIVOT_STOW_POS_REAL = Rotations.of(22.5); - public static final Angle PIVOT_AMP_POS_REAL = Rotations.of(99); // found via empirical testing - public static final Angle PIVOT_SUB_POS_REAL = Rotations.of(5.0947265625); - public static final Angle PIVOT_SHUTTLING_POS_REAL = Rotations.of(14.853271484375); - // maps Z distances to april tag (meters) with pivot angles (rotations) - // public static final InterpolatingDoubleTreeMap PIVOT_ANGLES_MAP_REAL = - // InterpolatingDoubleTreeMap.ofEntries( - // Map.entry(1.1, PIVOT_SUB_POS.in(Rotations)), - // Map.entry(1.43, 21.68115234375), - // Map.entry(1.84, 16.1193359375), - // Map.entry(2.31, 12.78369140625), - // Map.entry(2.76, 10.56689453125), - // Map.entry(2.82, 10.32265625), - // Map.entry(3.02, 9.54326171875), - // Map.entry(3.46, 8.09541015625), - // Map.entry(4.00, 7.0)); - - public static final Angle PIVOT_STOW_POS_SIM = Rotations.of(-0.1); - public static final Angle PIVOT_AMP_POS_SIM = Rotations.of(0.31); - public static final Angle PIVOT_SUB_POS_SIM = Rotations.of(-0.166); - public static final Angle PIVOT_SHUTTLING_POS_SIM = Rotations.of(-0.02); + // these won't work now, since Flex's Limelight was recalibrated + public static final InterpolatingDoubleTreeMap PIVOT_ANGLES_MAP_REAL = + InterpolatingDoubleTreeMap.ofEntries( + Map.entry(1.1, PivotState.INTAKE.angle.in(Rotations)), + Map.entry(1.43, 21.68115234375), + Map.entry(1.84, 16.1193359375), + Map.entry(2.31, 12.78369140625), + Map.entry(2.76, 10.56689453125), + Map.entry(2.82, 10.32265625), + Map.entry(3.02, 9.54326171875), + Map.entry(3.46, 8.09541015625)); public static final InterpolatingDoubleTreeMap PIVOT_ANGLES_MAP_SIM = // maps distance from speaker to angle for pivot (in rotations) InterpolatingDoubleTreeMap.ofEntries( - Map.entry(1.176, PIVOT_SUB_POS_SIM.in(Rotations)), + Map.entry(1.176, PivotState.INTAKE_SIM.angle.in(Rotations)), Map.entry(1.5, -0.138044), Map.entry(2.0, -0.113583), Map.entry(2.533, -0.092616), @@ -435,11 +482,17 @@ public static class IndexConstants { public static final double INDEX_AMP_PCT = .30; public static enum IndexState { - IDLE, - SPEAKER, - INTAKE, - EXTAKE, - AMP, + IDLE(0.0), + SPEAKER(0.28), + INTAKE(0.28), + EXTAKE(0.28), + AMP(0.30); + + public double pct = 0.0; + + private IndexState(double pct) { + this.pct = pct; + } } } } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/Robot.java b/FlexAdvKitBeta/src/main/java/frc/robot/Robot.java index bdfa0e8..ef6732c 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/Robot.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/Robot.java @@ -57,7 +57,7 @@ public Robot() { switch (Constants.currentMode) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); break; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/RobotContainer.java b/FlexAdvKitBeta/src/main/java/frc/robot/RobotContainer.java index 3c5ffc2..e92c67e 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/RobotContainer.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import static frc.robot.Constants.VisionConstants.*; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -23,14 +22,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IndexConstants.IndexState; -import frc.robot.Constants.IntakeConstants.IntakeState; +import frc.robot.Constants.Mode; import frc.robot.Constants.PivotConstants.PivotState; -import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterConstants.ShooterState; +import frc.robot.commands.Autos; import frc.robot.commands.DriveCommands; +import frc.robot.commands.NoteCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.gyro.GyroIO; import frc.robot.subsystems.drive.gyro.GyroIOPigeon2; @@ -80,6 +81,9 @@ public class RobotContainer { private SwerveDriveSimulation driveSimulation = null; + private final Autos autos; + private final NoteCommands noteCommands; + // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -98,9 +102,7 @@ public RobotContainer() { new ModuleIOTalonFXReal(DriveConstants.FrontRight), new ModuleIOTalonFXReal(DriveConstants.BackLeft), new ModuleIOTalonFXReal(DriveConstants.BackRight)); - vision = - new Vision( - drive::addVisionMeasurement, new VisionIOLimelight(CAM_0_NAME, drive::getRotation)); + vision = new Vision(drive, new VisionIOLimelight(CAM_0_NAME, drive::getRotation)); intake = new Intake(new IntakeIOSparkFlex()); shooter = new Shooter(new ShooterIOSparkFlex()); @@ -111,7 +113,8 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations driveSimulation = - new SwerveDriveSimulation(Drive.MAPLE_SIM_CONFIG, new Pose2d(3, 1.5, new Rotation2d())); + new SwerveDriveSimulation( + DriveConstants.MAPLE_SIM_CONFIG, new Pose2d(3, 1.5, new Rotation2d())); Arena2024Crescendo.getInstance().addDriveTrainSimulation(driveSimulation); drive = new Drive( @@ -120,20 +123,20 @@ public RobotContainer() { new ModuleIOTalonFXSim(DriveConstants.FrontRight, driveSimulation.getModules()[1]), new ModuleIOTalonFXSim(DriveConstants.BackLeft, driveSimulation.getModules()[2]), new ModuleIOTalonFXSim(DriveConstants.BackRight, driveSimulation.getModules()[3])); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVisionSim( - CAM_1_NAME, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); + vision = null; + new Vision( + drive, + new VisionIOPhotonVisionSim( + CAM_1_NAME, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); shooter = new Shooter(new ShooterIOSim()); pivot = new Pivot(new PivotIOSim()); index = new Index( new IndexIOSim( - driveSimulation::getSimulatedDriveTrainPose, + drive::getPose, drive::getChassisSpeeds, - pivot::getPivotAngle, + pivot::getPosition, Arena2024Crescendo.getInstance())); intake = new Intake( @@ -156,7 +159,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}); + vision = new Vision(drive, new VisionIO() {}); intake = new Intake(new IntakeIO() {}); shooter = new Shooter(new ShooterIO() {}); pivot = new Pivot(new PivotIO() {}); @@ -164,9 +167,12 @@ public RobotContainer() { break; } - // Set up auto routines - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + autos = new Autos(drive, driveSimulation, intake, shooter, index, pivot); + noteCommands = new NoteCommands(shooter, pivot, intake, index); + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + autoChooser.addOption("Three Piece", autos.ThreePieceAuto()); // Set up SysId routines autoChooser.addOption( "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive)); @@ -227,66 +233,41 @@ private void configureButtonBindings() { // run intake mechanism while held controller .rightTrigger() - .whileTrue( - Commands.parallel( - shooter.setVelocityState(ShooterConstants.ShooterState.IDLE), // turn off shooter - // move pivot to intake position, run intake and index - pivot.setPositionState(PivotState.INTAKE), - intake.setVelocityState(IntakeState.INTAKE), - index.setVelocityState(IndexState.INTAKE))) - .onFalse( - Commands.parallel( - // turn off intake mechanism when trigger is released - intake.setVelocityState(IntakeState.IDLE), - index.setVelocityState(IndexState.IDLE))); + .whileTrue(noteCommands.intakeNoteCmd()) + .onFalse(noteCommands.turnOffIntakeCmd()); // shut off intake mechanism automatically when note enters intake - intake.noteInIntake.onTrue( - Commands.parallel( - intake.setVelocityState(IntakeState.IDLE), index.setVelocityState(IndexState.IDLE))); + intake.noteInIntake.and(RobotModeTriggers.teleop()).whileTrue(noteCommands.turnOffIntakeCmd()); // manually override pivot angle - controller - .povUp() - .onTrue(pivot.setPositionState(PivotState.MANUAL_OVERRIDE)) - .whileTrue(pivot.setPivotPctCommand(0.05)); + controller.povUp().whileTrue(pivot.setPivotPctCommand(0.05)); - controller - .povDown() - .onTrue(pivot.setPositionState(PivotState.MANUAL_OVERRIDE)) - .whileTrue(pivot.setPivotPctCommand(-0.05)); + controller.povDown().whileTrue(pivot.setPivotPctCommand(-0.05)); // extake thru intake controller .povLeft() - .whileTrue( - Commands.parallel( - pivot.setPositionState(PivotState.INTAKE), - index.setVelocityState(IndexState.EXTAKE), - intake.setVelocityState(IntakeState.EXTAKE))) - .onFalse( - Commands.parallel( - index.setVelocityState(IndexState.IDLE), - intake.setVelocityState(IntakeState.IDLE))); + .whileTrue(noteCommands.extakeThruIntakeCmd()) + .onFalse(noteCommands.extakeThruIntakeCmdEnd()); - // outtake thru shooter + // extake thru shooter controller .povRight() - .whileTrue( - Commands.parallel( - index.setVelocityState(IndexState.EXTAKE), - shooter.setVelocityState(ShooterState.EXTAKE))) - .onFalse( - Commands.parallel( - index.setVelocityState(IndexState.IDLE), - intake.setVelocityState(IntakeState.IDLE))); + .whileTrue(noteCommands.extakeThruShooterCmd()) + .onFalse(noteCommands.extakeThruShooterCmdEnd()); // turn off shooter manually - controller.y().onTrue(shooter.setVelocityState(ShooterConstants.ShooterState.IDLE)); + controller.y().onTrue(shooter.setState(ShooterState.IDLE)); // move to preset pivot positions - controller.a().onTrue(pivot.setPositionState(PivotState.SUBWOOFER)); - controller.leftBumper().onTrue(pivot.setPositionState(PivotState.AMP)); - controller.x().onTrue(pivot.setPositionState(PivotState.SHUTTLE)); + if (Constants.currentMode == Mode.SIM) { + controller.a().onTrue(pivot.setState(PivotState.SUBWOOFER_SIM)); + controller.leftBumper().onTrue(pivot.setState(PivotState.AMP_SIM)); + controller.x().onTrue(pivot.setState(PivotState.SHUTTLE_SIM)); + } else { + controller.a().onTrue(pivot.setState(PivotState.SUBWOOFER)); + controller.leftBumper().onTrue(pivot.setState(PivotState.AMP)); + controller.x().onTrue(pivot.setState(PivotState.SHUTTLE)); + } controller .start() @@ -296,30 +277,7 @@ private void configureButtonBindings() { driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d())))); // hold to rev shooter, let go to shoot - controller - .leftTrigger() - .onTrue( - shooter.run( - () -> { - switch (pivot.getPivotState()) { - case AMP -> shooter.setVelocityState(ShooterState.AMP); - case SHUTTLE -> shooter.setVelocityState(ShooterState.SHUTTLE); - case EXTAKE -> shooter.setVelocityState(ShooterState.EXTAKE); - case SUBWOOFER -> shooter.setVelocityState(ShooterState.SPEAKER); - case INTAKE -> shooter.setVelocityState(ShooterState.EXTAKE); - case MANUAL_OVERRIDE -> shooter.setVelocityState(ShooterState.SPEAKER); - case AIMING -> shooter.setVelocityState(ShooterState.SPEAKER); - } - })) - .onFalse( - index - .setVelocityState( - pivot.getPivotState() == PivotState.AMP ? IndexState.AMP : IndexState.SPEAKER) - .andThen(Commands.waitSeconds(1.0)) // account for delays in shooting - .andThen( - index - .setVelocityState(IndexState.IDLE) - .alongWith(shooter.setVelocityState(ShooterState.IDLE)))); + controller.leftTrigger().whileTrue(noteCommands.shootCmd()).onFalse(noteCommands.shootCmdEnd()); } /** @@ -335,7 +293,7 @@ public void resetSimulationField() { if (Constants.currentMode != Constants.Mode.SIM) return; driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d())); - System.out.println("Resetting simulation field"); + // System.out.println("Resetting simulation field"); Arena2024Crescendo.getInstance().resetFieldForAuto(); } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/commands/Autos.java b/FlexAdvKitBeta/src/main/java/frc/robot/commands/Autos.java new file mode 100644 index 0000000..f3a3559 --- /dev/null +++ b/FlexAdvKitBeta/src/main/java/frc/robot/commands/Autos.java @@ -0,0 +1,119 @@ +package frc.robot.commands; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.IndexConstants.IndexState; +import frc.robot.Constants.ShooterConstants.ShooterState; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.index.Index; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.pivot.Pivot; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.util.LocalADStarAK; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; + +public class Autos { + private final Drive drive; + private final Intake intake; + private final Shooter shooter; + private final Index index; + private final Pivot pivot; + private final SwerveDriveSimulation drivesim; + + Command forwardCmd; + NoteCommands noteCommands; + + public Autos( + Drive drive, + SwerveDriveSimulation driveSim, + Intake intake, + Shooter shooter, + Index index, + Pivot pivot) { + this.drive = drive; + this.intake = intake; + this.shooter = shooter; + this.index = index; + this.pivot = pivot; + this.drivesim = driveSim; + + // configure PathPlanner + AutoBuilder.configure( + drive::getPose, + pose -> { + drive.setPose(pose); + if (driveSim != null) driveSim.setSimulationWorldPose(pose); + }, + drive::getChassisSpeeds, + drive::runVelocity, + new PPHolonomicDriveController( + new PIDConstants(10.0, 0.0, 0.0), new PIDConstants(10.0, 0.0, 0.0)), + DriveConstants.PP_CONFIG, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + drive); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + noteCommands = new NoteCommands(shooter, pivot, intake, index); + } + + public Command ThreePieceAuto() { + PathPlannerPath[] pathSections = new PathPlannerPath[3]; + try { + for (int i = 0; i < 3; i++) { + pathSections[i] = PathPlannerPath.fromChoreoTrajectory("3Piece", i); + } + } catch (Exception e) { + e.printStackTrace(); + } + return Commands.sequence( + drive.runOnce( + () -> { + drive.setPose(pathSections[0].getStartingDifferentialPose()); + drivesim.setSimulationWorldPose(pathSections[0].getStartingDifferentialPose()); + }), + // aimAndShoot(), + followPathAndIntake(pathSections[0]), + aimAndShoot(), + followPathAndIntake(pathSections[1]), + aimAndShoot(), + followPathAndIntake(pathSections[2]), + aimAndShoot()); + } + + private Command aimAndShoot() { + return Commands.parallel( + shooter.setState(ShooterState.SPEAKER), + pivot.angleToSpeakerCommand(drive::getPose), + DriveCommands.centerOnSpeakerTag(drive, () -> 0.0, () -> 0.0)) + .until(() -> shooter.atSpeed.and(pivot.atAngle).getAsBoolean()) + .andThen(index.setState(IndexState.SPEAKER)); + } + + private Command followPathAndIntake(PathPlannerPath path) { + return Commands.sequence( + shooter.setState(ShooterState.IDLE), + noteCommands.intakeNoteCmd(), + AutoBuilder.followPath(path), + noteCommands.turnOffIntakeCmd()); + } +} diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/commands/DriveCommands.java b/FlexAdvKitBeta/src/main/java/frc/robot/commands/DriveCommands.java index 7a2a510..296bd18 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/commands/DriveCommands.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/commands/DriveCommands.java @@ -13,8 +13,8 @@ package frc.robot.commands; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; +import static frc.robot.Constants.VisionConstants.APRILTAG_LAYOUT; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; @@ -163,23 +163,23 @@ public static Command joystickDriveAtAngle( public static Command centerOnSpeakerTag( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); - - return joystickDriveAtAngle( - drive, - xSupplier, - ySupplier, - () -> - drive - .getPose() - .getTranslation() - .minus( - fieldLayout - .getTagPose(FieldMirroringUtils.isSidePresentedAsRed() ? 4 : 7) - .get() - .toPose2d() - .getTranslation()) - .getAngle()); + return Commands.runOnce(() -> drive.isAiming = true) + .andThen( + joystickDriveAtAngle( + drive, + xSupplier, + ySupplier, + () -> + drive + .getPose() + .getTranslation() + .minus( + APRILTAG_LAYOUT + .getTagPose(FieldMirroringUtils.isSidePresentedAsRed() ? 4 : 7) + .get() + .toPose2d() + .getTranslation()) + .getAngle())); } /** diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/commands/NoteCommands.java b/FlexAdvKitBeta/src/main/java/frc/robot/commands/NoteCommands.java new file mode 100644 index 0000000..b27c177 --- /dev/null +++ b/FlexAdvKitBeta/src/main/java/frc/robot/commands/NoteCommands.java @@ -0,0 +1,93 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.Constants.IndexConstants.IndexState; +import frc.robot.Constants.IntakeConstants.IntakeState; +import frc.robot.Constants.Mode; +import frc.robot.Constants.PivotConstants.PivotState; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterConstants.ShooterState; +import frc.robot.subsystems.index.Index; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.pivot.Pivot; +import frc.robot.subsystems.shooter.Shooter; +import java.util.Map; + +public class NoteCommands { + private final Shooter shooter; + private final Pivot pivot; + private final Intake intake; + private final Index index; + + public NoteCommands(Shooter shooter, Pivot pivot, Intake intake, Index index) { + this.shooter = shooter; + this.pivot = pivot; + this.intake = intake; + this.index = index; + } + + public Command intakeNoteCmd() { + return Commands.parallel( + shooter.setState(ShooterState.IDLE), + pivot.setState( + Constants.currentMode == Mode.SIM ? PivotState.INTAKE_SIM : PivotState.INTAKE), + intake.setState(IntakeState.INTAKE), + index.setState(IndexState.INTAKE)); + } + + public Command turnOffIntakeCmd() { + return Commands.sequence(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)); + } + + public Command extakeThruIntakeCmd() { + return Commands.sequence( + pivot.setState( + Constants.currentMode == Mode.SIM ? PivotState.EXTAKE_SIM : PivotState.EXTAKE), + intake.setState(IntakeState.EXTAKE), + index.setState(IndexState.EXTAKE)); + } + + public Command extakeThruIntakeCmdEnd() { + return Commands.parallel(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)); + } + + public Command extakeThruShooterCmd() { + return Commands.sequence( + shooter.setState(ShooterConstants.ShooterState.EXTAKE), index.setState(IndexState.EXTAKE)); + } + + public Command extakeThruShooterCmdEnd() { + return Commands.parallel( + shooter.setState(ShooterConstants.ShooterState.IDLE), index.setState(IndexState.IDLE)); + } + + public Command shootCmd() { + return Commands.select( + Map.ofEntries( + Map.entry(PivotState.AMP, shooter.setState(ShooterState.AMP)), + Map.entry(PivotState.AMP_SIM, shooter.setState(ShooterState.AMP)), + Map.entry(PivotState.SHUTTLE, shooter.setState(ShooterState.SHUTTLE)), + Map.entry(PivotState.SHUTTLE_SIM, shooter.setState(ShooterState.SHUTTLE)), + Map.entry(PivotState.SUBWOOFER, shooter.setState(ShooterState.SPEAKER)), + Map.entry(PivotState.SUBWOOFER_SIM, shooter.setState(ShooterState.SPEAKER)), + Map.entry(PivotState.INTAKE, shooter.setState(ShooterState.EXTAKE)), + Map.entry(PivotState.INTAKE_SIM, shooter.setState(ShooterState.EXTAKE)), + Map.entry(PivotState.EXTAKE, shooter.setState(ShooterState.EXTAKE)), + Map.entry(PivotState.MANUAL_OVERRIDE, shooter.setState(ShooterState.SPEAKER)), + Map.entry(PivotState.AIMING, shooter.setState(ShooterState.SPEAKER))), + pivot::getState); + } + + public Command shootCmdEnd() { + return Commands.sequence( + index.setState( + (pivot.getState() == PivotState.AMP || pivot.getState() == PivotState.AMP_SIM) + ? IndexState.AMP + : IndexState.SPEAKER), + Commands.waitSeconds(1.0), + index.setState(IndexState.IDLE), + shooter.setState(ShooterState.IDLE)); + } +} diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/Drive.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/Drive.java index 94b60a5..51a4184 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,18 +15,13 @@ import static edu.wpi.first.units.Units.*; +import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -38,14 +33,12 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.system.plant.DCMotor; -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.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; @@ -54,17 +47,13 @@ import frc.robot.subsystems.drive.gyro.GyroIOInputsAutoLogged; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; -import frc.robot.util.LocalADStarAK; +import frc.robot.subsystems.vision.Vision.VisionConsumer; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import org.ironmaple.simulation.drivesims.COTS; -import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.utils.FieldMirroringUtils; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { +public class Drive extends SubsystemBase implements VisionConsumer { // TunerConstants doesn't include these constants, so they are declared locally public static final double ODOMETRY_FREQUENCY = new CANBus(DriveConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; @@ -77,42 +66,6 @@ public class Drive extends SubsystemBase { Math.hypot(DriveConstants.BackLeft.LocationX, DriveConstants.BackLeft.LocationY), Math.hypot(DriveConstants.BackRight.LocationX, DriveConstants.BackRight.LocationY))); - // PathPlanner config constants - private static final Mass ROBOT_MASS = Pounds.of(115.0); - private static final double ROBOT_MOI = 2.0; // TODO: find more accurate value - private static final double WHEEL_COF = 1.19; - private static final RobotConfig PP_CONFIG = - new RobotConfig( - ROBOT_MASS.in(Kilograms), - ROBOT_MOI, - new ModuleConfig( - DriveConstants.FrontLeft.WheelRadius, - DriveConstants.kSpeedAt12Volts.in(MetersPerSecond), - WHEEL_COF, - DCMotor.getKrakenX60Foc(1) - .withReduction(DriveConstants.FrontLeft.DriveMotorGearRatio), - DriveConstants.FrontLeft.SlipCurrent, - 1), - getModuleTranslations()); - - public static final DriveTrainSimulationConfig MAPLE_SIM_CONFIG = - DriveTrainSimulationConfig.Default() - .withRobotMass(ROBOT_MASS) - .withCustomModuleTranslations(getModuleTranslations()) - .withGyro(COTS.ofPigeon2()) - .withSwerveModule( - () -> - new SwerveModuleSimulation( - DCMotor.getKrakenX60(1), - DCMotor.getFalcon500(1), - DriveConstants.FrontLeft.DriveMotorGearRatio, - DriveConstants.FrontLeft.SteerMotorGearRatio, - Volts.of(DriveConstants.FrontLeft.DriveFrictionVoltage), - Volts.of(DriveConstants.FrontLeft.SteerFrictionVoltage), - Meters.of(DriveConstants.FrontLeft.WheelRadius), - KilogramSquareMeters.of(DriveConstants.FrontLeft.SteerInertia), - WHEEL_COF)); - static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -133,6 +86,13 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + private final PIDController xControllerAuto = new PIDController(10.0, 0.0, 0.0); + private final PIDController yControllerAuto = new PIDController(10.0, 0.0, 0.0); + private final PIDController headingControllerAuto = new PIDController(7.5, 0.0, 0.0); + + public boolean isAiming = false; + public Trigger isAimingTrigger; + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -151,38 +111,36 @@ public Drive( // Start odometry thread PhoenixOdometryThread.getInstance().start(); - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); + headingControllerAuto.enableContinuousInput(-Math.PI, Math.PI); // Configure SysId sysId = new SysIdRoutine( new SysIdRoutine.Config( - null, - Volts.of(3.0), + Volts.of(0.5).per(Second), + Volts.of(1.0), Seconds.of(7.0), (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + + // isAimingTrigger = + // new Trigger( + // () -> + // isAiming + // && MathUtil.isNear( + // this.getPose() + // .getTranslation() + // .minus( + // APRILTAG_LAYOUT + // .getTagPose(FieldMirroringUtils.isSidePresentedAsRed() ? 4 : + // 7) + // .get() + // .toPose2d() + // .getTranslation()) + // .getAngle(), + // getPose().getRotation().getRadians(), + // 0.1)); } @Override @@ -207,7 +165,6 @@ public void periodic() { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - // Update odometry double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together @@ -365,7 +322,8 @@ public void setPose(Pose2d pose) { } /** Adds a new timestamped vision measurement. */ - public void addVisionMeasurement( + @Override + public void accept( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { @@ -393,13 +351,19 @@ public static Translation2d[] getModuleTranslations() { }; } - @AutoLogOutput(key = "Odometry/DistanceToSpeaker") - public double getDistanceToSpeaker() { - return getPose() - .getTranslation() - .minus( - FieldMirroringUtils.toCurrentAllianceTranslation(FieldMirroringUtils.SPEAKER_POSE_BLUE) - .toTranslation2d()) - .getNorm(); + public void followTrajectory(SwerveSample sample) { + // Get the current pose of the robot + Pose2d pose = getPose(); + + // Generate the next speeds for the robot + ChassisSpeeds speeds = + new ChassisSpeeds( + sample.vx + xControllerAuto.calculate(pose.getX(), sample.x), + sample.vy + yControllerAuto.calculate(pose.getY(), sample.y), + sample.omega + + headingControllerAuto.calculate(pose.getRotation().getRadians(), sample.heading)); + + // Apply the generated speeds + runVelocity(speeds); } } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/Module.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/Module.java index 1aa1ff6..8c4e95d 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -83,12 +83,12 @@ public void runSetpoint(SwerveModuleState state) { /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { // drive - // io.setDriveOpenLoop(output); - // io.setTurnPosition(new Rotation2d()); + io.setDriveOpenLoop(output); + io.setTurnPosition(new Rotation2d()); // turn - io.setDriveOpenLoop(0.0); - io.setTurnOpenLoop(output); + // io.setDriveOpenLoop(0.0); + // io.setTurnOpenLoop(output); // rotation // io.setDriveOpenLoop(output); diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index a615844..b8ad2f5 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -26,7 +26,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.Drive; @@ -92,8 +91,6 @@ protected ModuleIOTalonFX(SwerveModuleConstants constants) { var turnConfig = new TalonFXConfiguration(); turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; turnConfig.Slot0 = constants.SteerMotorGains; - if (Constants.currentMode == Constants.Mode.SIM) - turnConfig.Slot0.withKD(0.5).withKS(0); // during simulation, gains are slightly different turnConfig.Feedback.FeedbackRemoteSensorID = constants.CANcoderId; turnConfig.Feedback.FeedbackSensorSource = @@ -163,15 +160,17 @@ public void updateInputs(ModuleIOInputs inputs) { // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = drivePosition.getValue().in(Radians); - inputs.driveVelocityRadPerSec = driveVelocity.getValue().in(RadiansPerSecond); + inputs.drivePositionRad = drivePosition.getValue().in(Radians) / constants.DriveMotorGearRatio; + inputs.driveVelocityRadPerSec = + driveVelocity.getValue().in(RadiansPerSecond) / constants.DriveMotorGearRatio; inputs.driveAppliedVolts = driveAppliedVolts.getValue().in(Volts); inputs.driveCurrentAmps = driveCurrent.getValue().in(Amps); // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); - inputs.turnPosition = new Rotation2d(turnPosition.getValue()); + inputs.turnPosition = + new Rotation2d(turnPosition.getValue().in(Radians) / constants.SteerMotorGearRatio); inputs.turnAbsolutePosition = new Rotation2d(turnAbsolutePosition.getValue()); inputs.turnVelocityRadPerSec = turnVelocity.getValue().in(RadiansPerSecond); inputs.turnAppliedVolts = turnAppliedVolts.getValue().in(Volts); diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/Index.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/Index.java index aff0620..eae8675 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/Index.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/Index.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.index; -import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.IndexConstants.*; import edu.wpi.first.wpilibj.Alert; @@ -23,65 +22,20 @@ public Index(IndexIO io) { @Override public void periodic() { - setDefaultCommand(manageOutput()); io.updateInputs(inputs); Logger.processInputs("Index", inputs); indexMotorDisconnected.set(!inputs.indexMotorConnected); } - /** - * Instant Command: Gets the current index state. - * - * @return {@link IndexState} - */ public IndexState getIndexState() { return inputs.indexState; } - /** - * Instant Command: Sets the index velocity to a specific state. - * - * @param state - the {@link IndexState} to set the index to - * @return {@link Command} - */ - public Command setVelocityState(IndexState state) { + public Command setState(IndexState state) { return runOnce(() -> io.setIndexState(state)); } - /** - * Continuous Command: Sets the index velocity to a specific voltage. - * - *

Default command for the index. - * - * @return {@link Command} - */ - private Command manageOutput() { - return run( - () -> { - switch (inputs.indexState) { - case IDLE: - io.setOutputVolts(Volts.of(0.0)); - break; - case SPEAKER: - io.setOutputVolts(Volts.of(INDEX_RUN_PCT * 12.0)); - break; - case INTAKE: - io.setOutputVolts(Volts.of(INDEX_RUN_PCT * 12.0)); - break; - case EXTAKE: - io.setOutputVolts(Volts.of(-INDEX_RUN_PCT * 12.0)); - break; - case AMP: - io.setOutputVolts(Volts.of(INDEX_AMP_PCT * 12.0)); - break; - default: - setVelocityState(IndexState.IDLE); - break; - } - }); - } - /** * Instant Command: Sets the index state to a specific state. Used in simulation only. * diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIO.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIO.java index 55c8ca6..847a228 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIO.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.index; -import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.IndexConstants.IndexState; import org.littletonrobotics.junction.AutoLog; @@ -17,7 +16,5 @@ public static class IndexIOInputs { public default void updateInputs(IndexIOInputs inputs) {} - public default void setOutputVolts(Voltage volts) {} - public default void setIndexState(IndexState state) {} } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSim.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSim.java index b15e918..e42726f 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSim.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSim.java @@ -33,7 +33,7 @@ public class IndexIOSim implements IndexIO { private NoteOnFly noteOnFly; - private final Supplier robotSimulationWorldPose; + private final Supplier robotPose; private final Supplier chassisSpeedsFieldRelative; private double velocityRPM; private final Supplier pivotAngle; @@ -42,7 +42,7 @@ public class IndexIOSim implements IndexIO { private IndexState state = IndexState.IDLE; public IndexIOSim( - Supplier robotSimulationWorldPose, + Supplier robotPose, Supplier chassisSpeedsFieldRelative, Supplier pivotAngle, SimulatedArena simulatedArena) { @@ -52,7 +52,7 @@ public IndexIOSim( this.noteInIntake = null; - this.robotSimulationWorldPose = robotSimulationWorldPose; + this.robotPose = robotPose; this.chassisSpeedsFieldRelative = chassisSpeedsFieldRelative; this.pivotAngle = pivotAngle; this.simulatedArena = simulatedArena; @@ -64,11 +64,10 @@ public void setNoteInIntake(Trigger noteInIntake) { @Override public void updateInputs(IndexIOInputs inputs) { - inputs.indexMotorConnected = true; - - indexMotorSim.setInputVoltage(MathUtil.clamp(voltsToApply.in(Volts), -12.0, 12.0)); + indexMotorSim.setInputVoltage(MathUtil.clamp((inputs.indexState.pct * 12.0), -12.0, 12.0)); indexMotorSim.update(0.02); + inputs.indexMotorConnected = true; inputs.indexPositionRad = indexMotorSim.getAngularPositionRad(); inputs.indexVelocityRadPerSec = indexMotorSim.getAngularVelocityRadPerSec(); inputs.indexAppliedVolts = voltsToApply.in(Volts); @@ -80,11 +79,6 @@ public void updateInputs(IndexIOInputs inputs) { } } - @Override - public void setOutputVolts(Voltage volts) { - voltsToApply = volts; - } - public void launchNoteWithTrajectory() { if (state == SPEAKER) velocityRPM = 6000; else if (state == EXTAKE) velocityRPM = 500; @@ -92,15 +86,15 @@ public void launchNoteWithTrajectory() { noteOnFly = new NoteOnFly( // Specify the position of the chassis when the note is launched - robotSimulationWorldPose.get().getTranslation(), + robotPose.get().getTranslation(), // Specify the translation of the shooter from the robot center (in the shooter’s // reference frame) new Translation2d(0.2, 0), // Specify the field-relative speed of the chassis, adding it to the initial velocity of // the projectile chassisSpeedsFieldRelative.get(), - // The shooter facing direction is the same as the robot’s facing direction - robotSimulationWorldPose.get().getRotation().plus(Rotation2d.k180deg), + // The shooter facing direction is opposite the robot’s facing direction + robotPose.get().getRotation().plus(Rotation2d.k180deg), // Initial height of the flying note 0.7, // The launch speed is proportional to the RPM; assumed to be 16 meters/second at 6000 diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSparkFlex.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSparkFlex.java index 7b4411e..81d7467 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSparkFlex.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/index/IndexIOSparkFlex.java @@ -13,8 +13,8 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants.IndexConstants.IndexState; import java.util.function.DoubleSupplier; public class IndexIOSparkFlex implements IndexIO { @@ -30,7 +30,8 @@ public IndexIOSparkFlex() { config .inverted(false) .idleMode(IdleMode.kBrake) - .voltageCompensation(RobotController.getBatteryVoltage()); + .voltageCompensation(RobotController.getBatteryVoltage()) + .smartCurrentLimit(20); tryUntilOk( indexMotor, @@ -42,6 +43,8 @@ public IndexIOSparkFlex() { @Override public void updateInputs(IndexIOInputs inputs) { + indexMotor.setVoltage(inputs.indexState.pct * 12.0); + sparkStickyFault = false; ifOk(indexMotor, indexEncoder::getPosition, position -> inputs.indexPositionRad = position); ifOk( @@ -57,11 +60,6 @@ public void updateInputs(IndexIOInputs inputs) { inputs.indexState = state; } - @Override - public void setOutputVolts(Voltage volts) { - indexMotor.setVoltage(volts); - } - @Override public void setIndexState(IndexState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/Intake.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/Intake.java index 09f2fea..20dbc05 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,14 +1,10 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Volts; -import static frc.robot.Constants.IntakeConstants.INTAKE_OUTPUT_PERCENT; - import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakeConstants.IntakeState; import org.littletonrobotics.junction.Logger; @@ -30,7 +26,7 @@ public Intake(IntakeIO io) { @Override public void periodic() { - setDefaultCommand(manageIntake()); + io.updateInputs(inputs); Logger.processInputs("Intake", inputs); @@ -38,41 +34,7 @@ public void periodic() { beamBreakDisconnected.set(!inputs.beamBreakSensorConnected); } - /** - * Instant Command: Sets the intake velocity to a specific state. - * - * @param state - the {@link IntakeState} to set the intake to - * @return {@link Command} - */ - public Command setVelocityState(IntakeState state) { + public Command setState(IntakeState state) { return runOnce(() -> io.setIntakeState(state)); } - - /** - * Continuous Command: Sets the intake velocity to a specific percentage. - * - *

Default command for the intake. - * - * @param pct - the percentage to set the intake to - * @return {@link Command} - */ - private Command manageIntake() { - return run( - () -> { - switch (inputs.intakeState) { - case IDLE: - io.setOutputVolts(Volts.of(0.0)); - break; - case INTAKE: - io.setOutputVolts(Volts.of(INTAKE_OUTPUT_PERCENT * 12.0)); - break; - case EXTAKE: - io.setOutputVolts(Volts.of(-INTAKE_OUTPUT_PERCENT * 12.0)); - break; - default: - setVelocityState(IntakeConstants.IntakeState.IDLE); - break; - } - }); - } } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index a5e1110..6b1bbfc 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.IntakeConstants.IntakeState; import org.littletonrobotics.junction.AutoLog; @@ -20,7 +19,5 @@ public static class IntakeIOInputs { public default void updateInputs(IntakeIOInputs inputs) {} - public default void setOutputVolts(Voltage volts) {} - public default void setIntakeState(IntakeState state) {} } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 118614d..a2ee8fa 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -37,6 +37,13 @@ public IntakeIOSim(AbstractDriveTrainSimulation driveTrainSim, BooleanSupplier i @Override public void updateInputs(IntakeIOInputs inputs) { + appliedVolts = Volts.of(inputs.intakeState.pct * 12.0); + if (appliedVolts.equals(Volts.of(0.0))) { + intakePhysicsSim.stopIntake(); + } else { + intakePhysicsSim.startIntake(); + } + inputs.intakeMotorConnected = true; // Update motor sim @@ -57,16 +64,6 @@ public void updateInputs(IntakeIOInputs inputs) { } } - @Override - public void setOutputVolts(Voltage volts) { - appliedVolts = volts; - if (volts.equals(Volts.of(0.0))) { - intakePhysicsSim.stopIntake(); - } else { - intakePhysicsSim.startIntake(); - } - } - @Override public void setIntakeState(IntakeState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkFlex.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkFlex.java index 5029e05..2e0217b 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkFlex.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkFlex.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.IntakeConstants.BEAM_BREAK_SENSOR_ID; import static frc.robot.Constants.IntakeConstants.INTAKE_MOTOR_ID; import static frc.robot.util.SparkUtil.ifOk; @@ -14,7 +15,6 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants.IntakeConstants.IntakeState; @@ -41,7 +41,7 @@ public IntakeIOSparkFlex() { .inverted(false) .idleMode(IdleMode.kCoast) .voltageCompensation(RobotController.getBatteryVoltage()) - .smartCurrentLimit(80); + .smartCurrentLimit(30); tryUntilOk( intakeMotor, @@ -53,6 +53,8 @@ public IntakeIOSparkFlex() { @Override public void updateInputs(IntakeIOInputs inputs) { + intakeMotor.setVoltage(Volts.of(state.pct * 12.0)); + sparkStickyFault = false; ifOk(intakeMotor, intakeEncoder::getPosition, position -> inputs.intakePositionRad = position); ifOk( @@ -72,11 +74,6 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.intakeState = state; } - @Override - public void setOutputVolts(Voltage volts) { - intakeMotor.setVoltage(volts); - } - @Override public void setIntakeState(IntakeState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 53ead8f..e871fa0 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -65,6 +65,8 @@ public IntakeIOTalonFX() { @Override public void updateInputs(IntakeIOInputs inputs) { + intakeMotor.setControl(intakeVoltageOut.withOutput(Volts.of(state.pct * 12.0))); + var intakeStatus = BaseStatusSignal.refreshAll( intakePosition, intakeVelocity, intakeAppliedVoltage, intakeCurrent); @@ -80,11 +82,6 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.intakeState = state; } - @Override - public void setOutputVolts(Voltage volts) { - intakeMotor.setControl(intakeVoltageOut.withOutput(volts.in(Volts))); - } - @Override public void setIntakeState(IntakeState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/Pivot.java index a3fdda3..aec7af1 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Rotations; import static frc.robot.Constants.PivotConstants.*; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; @@ -11,7 +12,10 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.PivotConstants; import frc.robot.Constants.PivotConstants.PivotState; import java.util.function.Supplier; @@ -25,6 +29,11 @@ public class Pivot extends SubsystemBase { private final PivotIO io; private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + @AutoLogOutput + public Trigger atAngle = + new Trigger( + () -> MathUtil.isNear(inputs.pivotMainPositionRad, getPosition().in(Radians), 0.001)); + @AutoLogOutput private final LoggedMechanism2d pivotMech = new LoggedMechanism2d(2, 2); private final LoggedMechanismLigament2d pivotArm = pivotMech @@ -42,7 +51,6 @@ public Pivot(PivotIO io) { @Override public void periodic() { - setDefaultCommand(run(() -> io.managePosition())); io.updateInputs(inputs); Logger.processInputs("Pivot", inputs); @@ -52,27 +60,21 @@ public void periodic() { pivotFollowerDisconnected.set(!inputs.pivotFollowerConnected); } - /** - * Instant Command: Sets the pivot position to a specific angle - * - * @param state - the {@link PivotState} to set the pivot to - * @return - */ - public Command setPositionState(PivotState state) { - return runOnce(() -> io.setPivotState(state)); + public Command setState(PivotState state) { + return runOnce(() -> io.setState(state)); } /** * @return the current pivot angle as a {@link Angle} object */ - public Angle getPivotAngle() { + public Angle getPosition() { return Radians.of(inputs.pivotMainPositionRad); } /** * @return the current pivot state as a {@link PivotState} object */ - public PivotState getPivotState() { + public PivotState getState() { return inputs.pivotState; } @@ -80,10 +82,11 @@ public PivotState getPivotState() { * Continuous command: runs the pivot at specific duty cycle and stops it when the command ends * * @param pct - duty cycle percentage - * @return "run-end" {@link Command} + * @return "run-end" {@link RunCommand} */ public Command setPivotPctCommand(double pct) { - return runEnd(() -> io.setPivotPct(pct), () -> io.setPivotPct(0.0)); + return setState(PivotState.MANUAL_OVERRIDE) + .andThen(runEnd(() -> io.setPct(pct), () -> io.setPct(0.0))); } /** @@ -92,14 +95,14 @@ public Command setPivotPctCommand(double pct) { * localization is necessary for this to work well * * @param drivePoseSupplier - a supplier for the robot's current pose - * @return {@link Command} + * @return {@link SequentialCommandGroup} */ public Command angleToSpeakerCommand(Supplier drivePoseSupplier) { return Commands.sequence( - this.setPositionState(PivotState.AIMING), + this.setState(PivotState.AIMING), run( () -> - io.setPivotPosition( + io.setTargetPosition( Rotations.of( PivotConstants.PIVOT_ANGLES_MAP_SIM.get( drivePoseSupplier diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIO.java index abc5abd..c81e56a 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIO.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -25,11 +25,9 @@ public static class PivotIOInputs { public default void updateInputs(PivotIOInputs inputs) {} - public default void managePosition() {} + public default void setState(PivotState state) {} - public default void setPivotState(PivotState state) {} + public default void setPct(double pct) {} - public default void setPivotPct(double pct) {} - - public default void setPivotPosition(Angle position) {} + public default void setTargetPosition(Angle position) {} } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java index 14fdda2..474eca3 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -2,11 +2,7 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Volts; -import static frc.robot.Constants.PivotConstants.PIVOT_AMP_POS_SIM; import static frc.robot.Constants.PivotConstants.PIVOT_GEAR_RATIO; -import static frc.robot.Constants.PivotConstants.PIVOT_SHUTTLING_POS_SIM; -import static frc.robot.Constants.PivotConstants.PIVOT_STOW_POS_SIM; -import static frc.robot.Constants.PivotConstants.PIVOT_SUB_POS_SIM; import static frc.robot.Constants.PivotConstants.PIVOT_kD; import static frc.robot.Constants.PivotConstants.PIVOT_kI; import static frc.robot.Constants.PivotConstants.PIVOT_kP; @@ -31,8 +27,7 @@ public class PivotIOSim implements PivotIO { PIVOT_kP, PIVOT_kI, PIVOT_kD, new TrapezoidProfile.Constraints(2500, 2000)); private Angle pivotTargetPosition = Radians.of(0.0); - - private PivotState state = PivotState.SUBWOOFER; + private PivotState state = PivotState.INTAKE_SIM; private Voltage voltsToApply = Volts.of(0.0); @@ -47,9 +42,13 @@ public PivotIOSim() { @Override public void updateInputs(PivotIOInputs inputs) { + if (state != PivotState.MANUAL_OVERRIDE && state != PivotState.AIMING) + pivotTargetPosition = state.angle; + inputs.pivotMainConnected = true; inputs.pivotFollowerConnected = true; + // run closed-loop if not in manual override if (state != PivotState.MANUAL_OVERRIDE) { voltsToApply = Volts.of( @@ -60,7 +59,6 @@ public void updateInputs(PivotIOInputs inputs) { 12.0)); } pivotSim.setInputVoltage(voltsToApply.in(Volts)); - pivotSim.update(0.02); inputs.pivotMainPositionRad = pivotSim.getAngularPositionRad(); @@ -78,42 +76,17 @@ public void updateInputs(PivotIOInputs inputs) { } @Override - public void managePosition() { - switch (state) { - case INTAKE: - pivotTargetPosition = PIVOT_STOW_POS_SIM; - break; - case EXTAKE: - pivotTargetPosition = PIVOT_STOW_POS_SIM; - break; - case SUBWOOFER: - pivotTargetPosition = PIVOT_SUB_POS_SIM; - break; - case AMP: - pivotTargetPosition = PIVOT_AMP_POS_SIM; - break; - case SHUTTLE: - pivotTargetPosition = PIVOT_SHUTTLING_POS_SIM; - break; - case MANUAL_OVERRIDE: - break; - case AIMING: - break; - } - } - - @Override - public void setPivotState(PivotState state) { + public void setState(PivotState state) { this.state = state; } @Override - public void setPivotPct(double pct) { + public void setPct(double pct) { voltsToApply = Volts.of(pct * SimulatedBattery.getBatteryVoltage().in(Volts)); } @Override - public void setPivotPosition(Angle position) { + public void setTargetPosition(Angle position) { pivotTargetPosition = position; } } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java index 81aab0b..a81d6c5 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; @@ -44,7 +45,8 @@ public class PivotIOTalonFX implements PivotIO { private final MotionMagicVoltage pivotMotionMagicPositionRequest = new MotionMagicVoltage(0); - private PivotState state = PivotState.SUBWOOFER; + private Angle pivotTargetPosition = Radians.of(0.0); + private PivotState state = PivotState.INTAKE; public PivotIOTalonFX() { TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); @@ -55,6 +57,8 @@ public PivotIOTalonFX() { pivotConfig.Slot0.kP = PIVOT_kP; pivotConfig.Slot0.kI = PIVOT_kI; pivotConfig.Slot0.kD = PIVOT_kD; + pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + pivotConfig.Slot0.kG = PIVOT_kG; pivotConfig.MotionMagic.MotionMagicCruiseVelocity = PIVOT_CRUISE_VELOCITY; pivotConfig.MotionMagic.MotionMagicAcceleration = PIVOT_ACCELERATION; @@ -64,8 +68,9 @@ public PivotIOTalonFX() { tryUntilOk(5, () -> pivotFollowerMotor.getConfigurator().apply(pivotConfig, 0.25)); pivotFollowerMotor.setControl(new Follower(PIVOT_MAIN_MOTOR_ID, false)); - pivotMainMotor.setPosition(0.0); - pivotFollowerMotor.setPosition(0.0); + // don't do this, because it will mess up pivot position on second deploy if not rebooted + // pivotMainMotor.setPosition(0.0); + // pivotFollowerMotor.setPosition(0.0); pivotMainPosition = pivotMainMotor.getPosition(); pivotMainVelocity = pivotMainMotor.getVelocity(); @@ -92,6 +97,12 @@ public PivotIOTalonFX() { @Override public void updateInputs(PivotIOInputs inputs) { + if (state != PivotState.MANUAL_OVERRIDE && state != PivotState.AIMING) + pivotTargetPosition = state.angle; + + if (state != PivotState.MANUAL_OVERRIDE) + pivotMainMotor.setControl(pivotMotionMagicPositionRequest.withPosition(pivotTargetPosition)); + StatusCode pivotMainStatus = BaseStatusSignal.refreshAll( pivotMainPosition, pivotMainVelocity, pivotMainAppliedVoltage, pivotMainCurrent); @@ -114,50 +125,20 @@ public void updateInputs(PivotIOInputs inputs) { inputs.pivotFollowerAppliedVolts = pivotFollowerAppliedVoltage.getValue().in(Volts); inputs.pivotFollowerCurrentAmps = pivotFollowerCurrent.getValue().in(Amps); - inputs.pivotPositionSetpointRad = - pivotMotionMagicPositionRequest.getPositionMeasure().in(Radians); + inputs.pivotPositionSetpointRad = pivotTargetPosition.in(Radians); inputs.pivotState = state; } @Override - public void managePosition() { - Angle pivotTargetPosition = PIVOT_STOW_POS_REAL; - switch (state) { - case INTAKE: - pivotTargetPosition = PIVOT_STOW_POS_REAL; - break; - case EXTAKE: - pivotTargetPosition = PIVOT_STOW_POS_REAL; - break; - case SUBWOOFER: - pivotTargetPosition = PIVOT_SUB_POS_REAL; - break; - case AMP: - pivotTargetPosition = PIVOT_AMP_POS_REAL; - break; - case SHUTTLE: - pivotTargetPosition = PIVOT_SHUTTLING_POS_REAL; - break; - case AIMING: - return; // don't set closed-loop position, is controlled externally by vision output - case MANUAL_OVERRIDE: - return; // don't set closed-loop position - } - pivotMainMotor.setControl(pivotMotionMagicPositionRequest.withPosition(pivotTargetPosition)); - } - - @Override - public void setPivotState(PivotState state) { + public void setState(PivotState state) { this.state = state; } @Override - public void setPivotPct(double pct) { + public void setPct(double pct) { pivotMainMotor.set(pct); } @Override - public void setPivotPosition(Angle position) { - pivotMainMotor.setControl(pivotMotionMagicPositionRequest.withPosition(position)); - } + public void setTargetPosition(Angle position) {} } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 61603ca..3e61f2f 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static frc.robot.Constants.ShooterConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Alert; @@ -10,8 +9,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterConstants.ShooterState; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -23,18 +22,18 @@ public class Shooter extends SubsystemBase { private final Alert bottomShooterDisconnected = new Alert("Disconnected Bottom Shooter Motor.", AlertType.kError); - private final Trigger shooterAtSpeed = + @AutoLogOutput + public final Trigger atSpeed = new Trigger( - () -> { - return MathUtil.isNear( - inputs.topVelocitySetpointRadPerSec, - inputs.topShooterVelocityRadPerSec, - RPM.of(500.0).in(RadiansPerSecond)) - && MathUtil.isNear( - inputs.bottomVelocitySetpointRadPerSec, - inputs.bottomShooterVelocityRadPerSec, - RPM.of(500.0).in(RadiansPerSecond)); - }); + () -> + MathUtil.isNear( + inputs.topVelocitySetpointRadPerSec, + inputs.topShooterVelocityRadPerSec, + RPM.of(500.0).in(RadiansPerSecond)) + && MathUtil.isNear( + inputs.bottomVelocitySetpointRadPerSec, + inputs.bottomShooterVelocityRadPerSec, + RPM.of(500.0).in(RadiansPerSecond))); public Shooter(ShooterIO io) { this.io = io; @@ -42,7 +41,6 @@ public Shooter(ShooterIO io) { @Override public void periodic() { - setDefaultCommand(manageVelocity()); io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); @@ -50,46 +48,7 @@ public void periodic() { bottomShooterDisconnected.set(!inputs.bottomConnected); } - /** - * Instant Command: Sets the shooter velocity to a specific state. - * - * @param state - the {@link ShooterState} to set the shooter to - * @return {@link Command} - */ - public Command setVelocityState(ShooterState state) { + public Command setState(ShooterState state) { return runOnce(() -> io.setShooterState(state)); } - - /** - * Continuous Command: Sets the shooter velocity to a specific RPM. - * - *

Default command for the shooter. - * - * @return {@link Command} - */ - private Command manageVelocity() { - return run( - () -> { - switch (inputs.shooterState) { - case IDLE: - io.setVelocity(RPM.of(0.0), RPM.of(0.0)); - break; - case SPEAKER: - io.setVelocity(SPK_TOP_RPM, SPK_BOTTOM_RPM); - break; - case AMP: - io.setVelocity(AMP_TOP_RPM, AMP_BOTTOM_RPM); - break; - case SHUTTLE: - io.setVelocity(SHUTTLING_TOP_RPM, SHUTTLING_BOTTOM_RPM); - break; - case EXTAKE: - io.setVelocity(SHOOTER_OUTTAKE_RPM, SHOOTER_OUTTAKE_RPM); - break; - default: - setVelocityState(ShooterConstants.ShooterState.IDLE); - break; - } - }); - } } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 42ecdbc..463f4f9 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.ShooterConstants; import org.littletonrobotics.junction.AutoLog; @@ -26,7 +25,5 @@ public static class ShooterIOInputs { public default void updateInputs(ShooterIOInputs inputs) {} - public default void setVelocity(AngularVelocity topVelocity, AngularVelocity bottomVelocity) {} - public default void setShooterState(ShooterConstants.ShooterState state) {} } diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index fbe100e..809df5f 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -12,6 +12,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterConstants.ShooterState; public class ShooterIOSim implements ShooterIO { DCMotorSim topShooterSim, bottomShooterSim; @@ -27,6 +28,8 @@ public class ShooterIOSim implements ShooterIO { private SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(0.0, 0.017); private ShooterConstants.ShooterState state = ShooterConstants.ShooterState.IDLE; + Voltage voltsToApplyTop = Volts.of(0); + Voltage voltsToApplyBottom = Volts.of(0); public ShooterIOSim() { topShooterSim = @@ -39,24 +42,33 @@ public ShooterIOSim() { @Override public void updateInputs(ShooterIOInputs inputs) { - Voltage voltsToApplyTop = - Volts.of( - MathUtil.clamp( - topController.calculate( - topShooterSim.getAngularVelocityRadPerSec(), - topTargetVelocity.in(RadiansPerSecond)) - + topFeedforward.calculate(topTargetVelocity.in(RadiansPerSecond)), - -12.0, - 12.0)); - Voltage voltsToApplyBottom = - Volts.of( - MathUtil.clamp( - bottomController.calculate( - bottomShooterSim.getAngularVelocityRadPerSec(), - bottomTargetVelocity.in(RadiansPerSecond)) - + bottomFeedforward.calculate(bottomTargetVelocity.in(RadiansPerSecond)), - -12.0, - 12.0)); + topTargetVelocity = inputs.shooterState.topRPM; + bottomTargetVelocity = inputs.shooterState.bottomRPM; + + if (state == ShooterState.IDLE) { + voltsToApplyTop = Volts.zero(); + voltsToApplyBottom = Volts.zero(); + } else { + voltsToApplyTop = + Volts.of( + MathUtil.clamp( + topController.calculate( + topShooterSim.getAngularVelocityRadPerSec(), + topTargetVelocity.in(RadiansPerSecond)) + + topFeedforward.calculate(topTargetVelocity.in(RadiansPerSecond)), + -12.0, + 12.0)); + + voltsToApplyBottom = + Volts.of( + MathUtil.clamp( + bottomController.calculate( + bottomShooterSim.getAngularVelocityRadPerSec(), + bottomTargetVelocity.in(RadiansPerSecond)) + + bottomFeedforward.calculate(bottomTargetVelocity.in(RadiansPerSecond)), + -12.0, + 12.0)); + } topShooterSim.setInputVoltage(voltsToApplyTop.in(Volts)); bottomShooterSim.setInputVoltage(voltsToApplyBottom.in(Volts)); @@ -81,12 +93,6 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.shooterState = state; } - @Override - public void setVelocity(AngularVelocity topVelocity, AngularVelocity bottomVelocity) { - topTargetVelocity = topVelocity; - bottomTargetVelocity = bottomVelocity; - } - @Override public void setShooterState(ShooterConstants.ShooterState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java index c37f487..81c1a78 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkFlex.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.Constants.ShooterConstants.*; import static frc.robot.util.SparkUtil.ifOk; import static frc.robot.util.SparkUtil.sparkStickyFault; @@ -19,7 +18,6 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.ShooterConstants; import java.util.function.DoubleSupplier; @@ -81,6 +79,11 @@ public ShooterIOSparkFlex() { @Override public void updateInputs(ShooterIOInputs inputs) { + topTargetVelocity = inputs.shooterState.topRPM.in(RPM); + bottomTargetVelocity = inputs.shooterState.bottomRPM.in(RPM); + topController.setReference(topTargetVelocity, ControlType.kVelocity); + bottomController.setReference(bottomTargetVelocity, ControlType.kVelocity); + sparkStickyFault = false; ifOk(topShooter, topEncoder::getPosition, position -> inputs.topShooterPositionRad = position); ifOk( @@ -121,14 +124,6 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.shooterState = state; } - @Override - public void setVelocity(AngularVelocity topVelocity, AngularVelocity bottomVelocity) { - topTargetVelocity = topVelocity.in(RadiansPerSecond); - bottomTargetVelocity = bottomVelocity.in(RadiansPerSecond); - topController.setReference(topTargetVelocity, ControlType.kVelocity); - bottomController.setReference(bottomTargetVelocity, ControlType.kVelocity); - } - @Override public void setShooterState(ShooterConstants.ShooterState state) { this.state = state; diff --git a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 257a5b8..41ee235 100644 --- a/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/FlexAdvKitBeta/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -13,6 +13,8 @@ package frc.robot.subsystems.vision; +import static frc.robot.Constants.VisionConstants.APRILTAG_LAYOUT; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; @@ -83,6 +85,29 @@ public void updateInputs(VisionIOInputs inputs) { multitagResult.fiducialIDsUsed.size(), // Tag count totalTagDistance / result.targets.size(), // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + // Calculate robot pose + var tagPose = APRILTAG_LAYOUT.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // Add tag ID + tagIds.add((short) target.fiducialId); + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } } } diff --git a/FlexReboot/src/main/deploy/pathplanner/settings.json b/FlexReboot/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..3b5b754 --- /dev/null +++ b/FlexReboot/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.81, + "robotLength": 0.81, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 115.0, + "robotMOI": 6.883, + "robotWheelbase": 0.4953, + "robotTrackwidth": 0.4953, + "driveWheelRadius": 0.0557, + "driveGearing": 5.1428, + "maxDriveSpeed": 7.0, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 120.0, + "wheelCOF": 1.19 +} \ No newline at end of file