diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 5652a0a..bb7f515 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -8,6 +8,7 @@ #include #include #include +#include RobotContainer::RobotContainer() { ConfigureBindings(); @@ -37,19 +38,19 @@ void RobotContainer::ConfigureBindings() { driverController.Start().WhileTrue(intakeSubsystem.FakeNote()); } - driverController.LeftTrigger().OnTrue(IntakeNote()); - (!driverController.LeftTrigger() && !intakeSubsystem.TouchedNote()) + (driverController.LeftTrigger() && frc2::RobotModeTriggers::Teleop()).OnTrue(IntakeNote()); + (!driverController.LeftTrigger() && !intakeSubsystem.TouchedNote() && frc2::RobotModeTriggers::Teleop()) .OnTrue(frc2::cmd::Sequence(intakeSubsystem.Stop(), frc2::cmd::Print("Cancelled"))); - (shooterSubsystem.UpToSpeed() && driverController.RightTrigger()) + (shooterSubsystem.UpToSpeed() && driverController.RightTrigger() && frc2::RobotModeTriggers::Teleop()) .OnTrue(feederSubsystem.Feed()) .OnFalse(feederSubsystem.Stop()); - intakeSubsystem.TouchedNote().OnTrue(RumbleDriver([] { return .1_s; })); - feederSubsystem.GotNote().OnTrue(frc2::cmd::Parallel( - intakeSubsystem.Stop(), feederSubsystem.Stop(), - RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; }))); + // intakeSubsystem.TouchedNote().OnTrue(RumbleDriver([] { return .1_s; })); + // feederSubsystem.GotNote().OnTrue(frc2::cmd::Parallel( + // intakeSubsystem.Stop(), feederSubsystem.Stop(), + // RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; }))); driverController.A().WhileTrue(swerveSubsystem.AlignToAmp()); diff --git a/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp index 6828ce9..9bb8cde 100644 --- a/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/src/main/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -265,7 +265,7 @@ void PathPlannerPath::loadChoreoTrajectoryIntoCache( size_t splitStartIdx = splits[i]; size_t splitEndIdx = fullTrajStates.size(); - if (i < splits.size()) { + if (i < splits.size() - 1) { splitEndIdx = splits[i + 1]; } diff --git a/src/main/deploy/choreo/CloseFourSafe.traj b/src/main/deploy/choreo/CloseFourSafe.traj index 7de53f2..dc8a2d5 100644 --- a/src/main/deploy/choreo/CloseFourSafe.traj +++ b/src/main/deploy/choreo/CloseFourSafe.traj @@ -15,8 +15,8 @@ {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.005202293395996094}}, "enabled":true}, {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"PointAt", "props":{"x":-0.01297958567738533, "y":5.310976028442383, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}, - {"from":4, "to":5, "data":{"type":"PointAt", "props":{"x":-0.008655529469251633, "y":5.791820526123047, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"PointAt", "props":{"x":-0.01974187046289444, "y":5.372384548187256, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"PointAt", "props":{"x":-0.02268305607140064, "y":5.6723856925964355, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}, {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":0.1 }, @@ -34,8 +34,8 @@ {"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":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"5.202293395996094 mm", "val":0.005202293395996094}}}, "enabled":true}, {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"PointAt", "props":{"x":{"exp":"-12.97958567738533 mm", "val":-0.01297958567738533}, "y":{"exp":"5.310976028442383 m", "val":5.310976028442383}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}, - {"from":4, "to":5, "data":{"type":"PointAt", "props":{"x":{"exp":"-8.655529469251633 mm", "val":-0.008655529469251633}, "y":{"exp":"5.791820526123047 m", "val":5.791820526123047}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"PointAt", "props":{"x":{"exp":"-19.74187046289444 mm", "val":-0.01974187046289444}, "y":{"exp":"5.372384548187256 m", "val":5.372384548187256}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}, + {"from":4, "to":5, "data":{"type":"PointAt", "props":{"x":{"exp":"-22.683056071400642 mm", "val":-0.02268305607140064}, "y":{"exp":"5.6723856925964355 m", "val":5.6723856925964355}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}, {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":{ "exp":"0.1 s", @@ -43,68 +43,68 @@ } }, "trajectory":{ - "waypoints":[0.0,1.0405005932287497,1.8432814627511191,2.3083927877031587,3.7087838574850687,4.061519066353924], + "waypoints":[0.0,1.0405005932287497,1.8427683144730311,2.3078442342235768,3.707297079984401,4.060034341604896], "samples":[ - {"t":0.0, "x":1.2731446027755735, "y":5.5781683921813965, "heading":-3.1415926535897927, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.505500795340117, "ay":-0.000136331046865511, "alpha":-0.00009097022022561486, "fx":[63.24417416874877,63.24417483261289,63.24417413562843,63.24417486573166], "fy":[-0.0013661852871373536,-0.002461187265153369,-0.0024612460872977435,-0.0013661526362176823]}, - {"t":0.09459096302079543, "x":1.293300974963694, "y":5.5781677822737645, "heading":-3.1415926535897927, "vx":0.42617965912218136, "vy":-0.00001289568501264188, "omega":-8.604960737354751e-6, "ax":4.504981513683446, "ay":-0.00009592243293905437, "alpha":-0.00007270971798658265, "fx":[63.236884952259565,63.23688562987533,63.236884933638,63.23688564849618], "fy":[-0.0009089586539514576,-0.0017839789524504831,-0.0017840130232793115,-0.0009089412947874124]}, - {"t":0.18918192604159087, "x":1.3537677684046527, "y":5.5781661333279, "heading":3.141591839638271, "vx":0.8523101988923792, "vy":-0.000021969080319644703, "omega":-0.000015482642982676053, "ax":4.504116176009493, "ay":-0.00006122851875943258, "alpha":-0.000054418230247151104, "fx":[63.22473811498249,63.22473880065895,63.22473810608925,63.224738809551944], "fy":[-0.0005321382654872202,-0.001186798991177896,-0.00118681521281303,-0.0005321313980331182]}, - {"t":0.2837728890623863, "x":1.4545387886921368, "y":5.578163781331872, "heading":3.1415903751201615, "vx":1.2783588855386596, "vy":-0.00002776074487343627, "omega":-0.000020630115787641455, "ax":4.502385995404041, "ay":-0.00003308562615377868, "alpha":-0.000036052533140373, "fx":[63.20045139278101,63.200452079374514,63.20045138959863,63.20045208255684], "fy":[-0.00024772241636634697,-0.0006811281559326016,-0.0006811332351964608,-0.00024772143681219574]}, - {"t":0.37836385208318174, "x":1.5956024241907625, "y":5.578161007400283, "heading":3.1415884236976415, "vx":1.7042439127352704, "vy":-0.00003089034611346821, "omega":-0.00002404035961672848, "ax":4.497199426317788, "ay":-0.000011319103126459344, "alpha":-0.000017396241526518773, "fx":[63.12764700460344,63.12764766833042,63.1276470040774,63.127647668856454], "fy":[-0.00005455051666457257,-0.0002632238198080387,-0.00026322379969498746,-0.000054551487138551075]}, - {"t":0.4729548151039772, "x":1.7769277312635274, "y":5.578158034814139, "heading":3.141586149696874, "vx":2.129638337367239, "vy":-0.0000319610309787317, "omega":-0.000025685886855664243, "ax":-2.884168391140556e-6, "ay":0.0006757453954288452, "alpha":0.000543072695145864, "fx":[0.00151358456271755,-0.0015945552783092223,0.001513599201200274,-0.0015945699498802735], "fy":[0.008360130732565762,0.01061088343298241,0.01061086319715624,0.008360150969182283]}, - {"t":0.5675457781247726, "x":1.9783722595781237, "y":5.578158034688605, "heading":3.1415837200441006, "vx":2.129638064550973, "vy":0.00003195837673175099, "omega":0.000025683882368481884, "ax":-4.4971988602857795, "ay":-0.00001130109896467031, "alpha":-0.000017381424550597293, "fx":[-63.127639723138735,-63.12763905889697,-63.12763972262031,-63.1276390594154], "fy":[-0.00005438575765788252,-0.000262884075853203,-0.00026288030746422574,-0.000054388577304607536]}, - {"t":0.662136741145568, "x":2.1596975433772068, "y":5.578161007104227, "heading":3.1415861495072677, "vx":1.7042436934605176, "vy":0.00003088939489748951, "omega":0.000024039756681567588, "ax":-4.502385424990097, "ay":-0.00003306979429774427, "alpha":-0.00003604088750094197, "fx":[-63.20044407552104,-63.2004433827169,-63.20044407234566,-63.20044338589236], "fy":[-0.00024756662872948347,-0.0006808435751961708,-0.000680833245531708,-0.0002475728613343542]}, - {"t":0.7567277041663634, "x":2.300761160686297, "y":5.578163781016666, "heading":3.141588423451003, "vx":1.278358720219911, "vy":0.000027761291207966267, "omega":0.00002063061442472934, "ax":-4.5041155999808264, "ay":-0.00006121816418992179, "alpha":-0.00005441184376245093, "fx":[-63.2247307237932,-63.22473002030161,-63.224730714905306,-63.224730029189786], "fy":[-0.0005320222669255808,-0.0011866336465225068,-0.0011866128697409636,-0.0005320336917261252]}, - {"t":0.8513186671871588, "x":2.401532167913116, "y":5.578166133110696, "heading":3.141590374920689, "vx":0.8523100880607368, "vy":0.000021970606102876392, "omega":0.000015483745723502044, "ax":-4.5049809311848765, "ay":-0.00009592235289002769, "alpha":-0.00007271074025340404, "fx":[-63.236877471986816,-63.23687675696363,-63.236877453366894,-63.2368767755843], "fy":[-0.0009089327507392896,-0.0017840193202455596,-0.0017839824952774878,-0.0009089528635808984]}, - {"t":0.9459096302079542, "x":2.461998953476341, "y":5.578167782201244, "heading":3.1415918395431084, "vx":0.4261796033896395, "vy":0.000012897218367788096, "omega":8.60596678097764e-6, "ax":-4.505500206144911, "ay":-0.00013634725724206732, "alpha":-0.00009098085595251132, "fx":[-63.24416659528168,-63.24416586489193,-63.24416656215554,-63.24416589801968], "fy":[-0.0013663156865609122,-0.0024615381334282985,-0.002461478066303576,-0.0013663495763540413]}, - {"t":1.0405005932287497, "x":2.4821553230285645, "y":5.5781683921813965, "heading":-3.1415926535897927, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.897944881918209, "ay":4.017206401658027, "alpha":2.2816428811654195, "fx":[-20.046229724460076,-32.12810768340165,-15.49250324202557,-38.899743405634794], "fy":[59.95285314717523,54.46351050998857,61.30122950383173,49.842151881758646]}, - {"t":1.097842083908919, "x":2.4790350574647744, "y":5.5847727730130785, "heading":-3.1415926535897927, "vx":-0.10883098875798784, "vy":0.23035260344099115, "omega":0.13083280400579758, "ax":-1.8222017029960405, "ay":4.051981388433268, "alpha":2.284452828444842, "fx":[-18.72912166833696,-31.224255761617773,-14.497152647474392,-37.863195046234075], "fy":[60.369032878200215,54.98009140291215,61.53788551798052,50.625295397815066]}, - {"t":1.1551835745890882, "x":2.4697987843226046, "y":5.60464308639583, "heading":-3.1340905055782353, "vx":-0.2133187507277237, "vy":0.4626992564620578, "omega":0.2618267345773314, "ax":-1.7207521973609292, "ay":4.095604262513181, "alpha":2.2962363924906004, "fx":[-16.87969900665645,-30.07586205464848,-13.19874662728446,-36.46318917646983], "fy":[60.90141237316008,55.60790171145965,61.821168887914624,51.63117716701135]}, - {"t":1.2125250652692574, "x":2.454737812499931, "y":5.637908220237743, "heading":-3.1190769703176526, "vx":-0.31198924681557594, "vy":0.6975473101106198, "omega":0.39349635227677615, "ax":-1.5782098199216823, "ay":4.151932796693678, "alpha":2.3129931833327357, "fx":[-14.265079410104018,-28.48521651474196,-11.41580261891463,-34.44787072069676], "fy":[61.552137994201615,56.427231384460775,62.16399417056047,52.98105412330419]}, - {"t":1.2698665559494269, "x":2.4342532703318565, "y":5.684732496980538, "heading":-3.0965133029008975, "vx":-0.4024861504959664, "vy":0.9356253258769202, "omega":0.5261268293421273, "ax":-1.3641984247097063, "ay":4.226613788847277, "alpha":2.3236324230420995, "fx":[-10.451018453102032,-26.050041485883323,-8.799332283509354,-31.297177854623445], "fy":[62.293263622082854,57.57438366970053,62.57204379923659,54.87794541705243]}, - {"t":1.327208046629596, "x":2.4089313405200183, "y":5.745331299334976, "heading":-3.066344406219594, "vx":-0.4807113217523594, "vy":1.1779856610587816, "omega":0.6593673762721212, "ax":-1.0104488794166069, "ay":4.325752586485812, "alpha":2.294104883872778, "fx":[-4.544216164247994,-21.823391741953618,-4.588369232616751,-25.779119052873018], "fy":[62.97028196583737,59.27762452534443,62.996121247796545,57.64009445177244]}, - {"t":1.3845495373097652, "x":2.3797054352662155, "y":5.81999039108143, "heading":-3.028535297958284, "vx":-0.5386519667542141, "vy":1.426030762681476, "omega":0.7909147700900372, "ax":-0.3350778909404974, "ay":4.435515896906786, "alpha":2.103301061439376, "fx":[5.5021569354285,-13.069177141434771,3.171039422515623,-14.418109430745826], "fy":[62.844572961276555,61.75019897023195,63.03972579204853,61.41265963097941]}, - {"t":1.4418910279899344, "x":2.348267452682506, "y":5.909053212148106, "heading":-2.9831830660403647, "vx":-0.5578658325147103, "vy":1.680369856145698, "omega":0.911521188302164, "ax":1.211093420419379, "ay":4.3012285106901, "alpha":1.3073290510114126, "fx":[24.421213626306542,10.03764220386429,20.419131447425737,13.122980514350814], "fy":[58.075036853014325,62.20841415552515,59.63218162787963,61.59150173473471]}, - {"t":1.4992325186701037, "x":2.318269660020021, "y":6.012479444383554, "heading":-2.930915082316565, "vx":-0.4884199304349226, "vy":1.927008710704712, "omega":0.986485384896691, "ax":3.914027651793856, "ay":2.1812328134768904, "alpha":-0.2926673465125419, "fx":[53.82295224023921,55.98651243889381,55.399974715240766,54.55698307884881], "fy":[32.57045486181914,28.73317458366881,29.796757059065527,31.37236398286829]}, - {"t":1.556574009350273, "x":2.2966976856962207, "y":6.126562993926317, "heading":-2.8743485398123916, "vx":-0.26398375031766297, "vy":2.0520838517499818, "omega":0.9697034029743058, "ax":4.360196695199902, "ay":-0.9451130159290068, "alpha":-1.287291539611522, "fx":[61.97388998667362,61.37076401518739,58.56653021349979,62.90691985308701], "fy":[-11.168386667526413,-14.50131258098357,-23.23333433223644,-4.163557874835601]}, - {"t":1.6139155000304422, "x":2.2887287287956153, "y":6.242678753189054, "heading":-2.818744301168212, "vx":-0.013963572156150322, "vy":1.997889662555384, "omega":0.8958881871530695, "ax":3.8546525093671167, "ay":-2.2187724416771055, "alpha":-1.881348994336835, "fx":[53.10294610885322,56.01827939003022,47.343164308570515,59.968212929216094], "fy":[-34.029440353621204,-29.12803716663307,-41.73325073520028,-19.6898126015]}, - {"t":1.6712569907106114, "x":2.2942651752019967, "y":6.3535930111146355, "heading":-2.7673727370340973, "vx":0.207067948785018, "vy":1.8706619432695395, "omega":0.7880088313282165, "ax":3.5002851350294093, "ay":-2.732867607245527, "alpha":-2.182473834277982, "fx":[45.731944449193946,52.63878712969524,41.073592586891245,57.09111456804095], "fy":[-43.51922488957331,-34.93470710979414,-47.98295009227712,-27.00928399817471]}, - {"t":1.7285984813907809, "x":2.311893310294532, "y":6.45636665754189, "heading":-2.722187135976592, "vx":0.40777951623324415, "vy":1.7139552408385323, "omega":0.6628625283003092, "ax":3.2775491900282905, "ay":-2.9914314435719636, "alpha":-2.342801702839804, "fx":[40.87839452289637,50.59923285798276,37.58233025340141,54.969212328891246], "fy":[-48.15218817340335,-37.86434637389072,-50.79799033643125,-31.149589045417404]}, - {"t":1.78593997207095, "x":2.3406643627831594, "y":6.549729423086739, "heading":-2.684177610487817, "vx":0.5957190725670494, "vy":1.5424221025965834, "omega":0.5285227862914912, "ax":3.129723081027856, "ay":-3.14357460423193, "alpha":-2.4318581487728386, "fx":[37.611190353879586,49.27431972171728,35.45192473824169,53.3915344319794], "fy":[-50.77233405166598,-39.59501213223467,-52.32744218660784,-33.811921854980106]}, - {"t":1.8432814627511191, "x":2.3799691200256348, "y":6.6330060958862305, "heading":-2.653871326063413, "vx":0.7751820594493202, "vy":1.3621648487255993, "omega":0.3890764149181926, "ax":2.746107041303926, "ay":-3.5055147542380625, "alpha":-2.145514914048793, "fx":[31.444226584948428,44.391497366651414,31.093191860072253,47.26062342658446], "fy":[-54.82038668911764,-45.00666154180544,-55.03726634423301,-41.964758952956075]}, - {"t":1.9097259377442677, "x":2.437537535393499, "y":6.715776231207257, "heading":-2.6280193479419514, "vx":0.9576457000837476, "vy":1.1292427612995148, "omega":0.2465188028642617, "ax":1.5314283027251443, "ay":-4.17971413289965, "alpha":-2.151227756198208, "fx":[10.81171987539892,30.310430769323062,14.745473728880656,30.11963716500595], "fy":[-62.248419583486715,-55.45063870299402,-61.45282830181987,-55.53240889109741]}, - {"t":1.976170412737416, "x":2.5045483282658845, "y":6.78158172999704, "heading":-2.6116395355096897, "vx":1.0594006496479678, "vy":0.8515238501175544, "omega":0.10358160401298684, "ax":-0.6630943411313196, "ay":-4.425129541724452, "alpha":-1.396060723673523, "fx":[-18.5057709882083,-0.2891158584278576,-11.657681868001516,-6.779123055828091], "fy":[-60.40246363988015,-63.171473466748736,-62.09981856877916,-62.79022436817826]}, - {"t":2.0426148877305645, "x":2.573475911160077, "y":6.828392603188554, "heading":-2.604757110212089, "vx":1.015341694280569, "vy":0.5574984409410018, "omega":0.01082108216993723, "ax":-2.812164520819077, "ay":-3.508710041438915, "alpha":-0.49831215500429665, "fx":[-42.33606477046985,-36.36288111218844,-38.64997590594528,-40.54963977707303], "fy":[-46.90648251065772,-51.66613762332808,-49.990283338513905,-48.44558034582436]}, - {"t":2.109059362723713, "x":2.6347320890368287, "y":6.857690048113692, "heading":-2.604038109088441, "vx":0.8284888991003869, "vy":0.32436404433440497, "omega":-0.022289007352042894, "ax":-3.844625448182057, "ay":-2.343451364328255, "alpha":-0.07580299250372934, "fx":[-54.20595779461576,-53.724051425437736,-53.73651942155684,-54.20307057014829], "fy":[-32.50517946168819,-33.293040457564665,-33.27552004435603,-32.507323941848114]}, - {"t":2.1755038377168616, "x":2.6812938415493677, "y":6.874069232225913, "heading":-2.6055190904800525, "vx":0.5730347796506321, "vy":0.1686546487596365, "omega":-0.027325697391883813, "ax":-4.235101381369376, "ay":-1.5312815878116142, "alpha":0.1393721001779105, "fx":[-59.24583853101558,-59.64715685318504,-59.7925108325405,-59.10869510127517], "fy":[-22.059756168516884,-20.95670833755555,-20.532701274471556,-22.429857949204123]}, - {"t":2.24194831271001, "x":2.7100201293089325, "y":6.8818951985805334, "heading":-2.6073347320970712, "vx":0.2916356918227859, "vy":0.06690944759081893, "omega":-0.018065191366895102, "ax":-4.389163912467639, "ay":-1.0069979121321706, "alpha":0.2718840260049396, "fx":[-61.43384339759198,-61.79830008118982,-62.066767337146544,-61.145656393375745], "fy":[-14.96272116727333,-13.393285611618069,-12.074805399971217,-16.110517692903798]}, - {"t":2.3083927877031587, "x":2.7197089195251465, "y":6.88411808013916, "heading":-2.6085350642530876, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.738647867793861, "ay":-4.1093784923370125, "alpha":-1.6552757578985586, "fx":[-35.12950048983887,-12.533912093983728,-25.04134104066828,-24.917556649845817], "fy":[-52.58109335612239,-61.97699245889323,-58.07122340293761,-58.1057520009947]}, - {"t":2.3750780767403925, "x":2.7158430987789153, "y":6.874981025463477, "heading":-2.6085350642530907, "vx":-0.11594223559780438, "vy":-0.27403509252488517, "omega":-0.11038254235180832, "ax":-1.721717942022949, "ay":-4.11650999335826, "alpha":-1.6516850816090618, "fx":[-34.886276972853466,-12.333511427307284,-24.83975737422128,-24.612176116220255], "fy":[-52.73823153896432,-62.01291125552546,-58.15404941978046,-58.23029143541479]}, - {"t":2.4417633657776263, "x":2.7042832796187826, "y":6.847554004801446, "heading":-2.6158959559944845, "vx":-0.2307554942021966, "vy":-0.5485457512566416, "omega":-0.22052563941741296, "ax":-1.700462324137455, "ay":-4.1249455561436825, "alpha":-1.6583561050755882, "fx":[-34.640802582171084,-12.04738975359917,-24.66429531946559,-24.12576512484241], "fy":[-52.894164353386785,-62.0639881535194,-58.22417787085028,-58.42679669497669]}, - {"t":2.50844865481486, "x":2.6851143662220727, "y":6.801802405348866, "heading":-2.630601771999162, "vx":-0.34415131578422964, "vy":-0.8236189379309357, "omega":-0.331113595611056, "ax":-1.6733519031380704, "ay":-4.13516697250912, "alpha":-1.6762743694181228, "fx":[-34.37437951493519,-11.65404860557169,-24.496865402165135,-23.43075227145224], "fy":[-53.06056779031169,-62.132706814715355,-58.289181677334135,-58.70058704929567]}, - {"t":2.575133943852094, "x":2.658443898729604, "y":6.737684743986356, "heading":-2.652682177826645, "vx":-0.45573927110599755, "vy":-1.0993737427099288, "omega":-0.442896436441426, "ax":-1.6379553925955683, "ay":-4.14789922130438, "alpha":-1.7071799167253074, "fx":[-34.05735253944832,-11.120366503887103,-24.31003232656155,-22.48083669985578], "fy":[-53.25533646386832,-62.222384753878224,-58.360085081717386,-59.06013253541984]}, - {"t":2.6418192328893277, "x":2.624410859046346, "y":6.655149984068356, "heading":-2.6822168547043113, "vx":-0.5649667998913291, "vy":-1.3759776011799278, "omega":-0.5567402226268294, "ax":-1.590161919949434, "ay":-4.164297774863583, "alpha":-1.7539343564013936, "fx":[-33.63992806257308,-10.3902422772172,-24.05845061050851,-21.196439705895987], "fy":[-53.507468957991584,-62.337840230845345,-58.45452799829373,-59.51885432761473]}, - {"t":2.7085045219265615, "x":2.583200217095678, "y":6.554133354307833, "heading":-2.719343237368841, "vx":-0.6710072071391602, "vy":-1.6536750019338151, "omega":-0.6737018421358067, "ax":-1.522439433699588, "ay":-4.186320992117699, "alpha":-1.8213294794144008, "fx":[-33.031245479106104,-9.359623636854945,-23.65963955467612,-19.43204218026803], "fy":[-53.86758310032358,-62.48616926021934,-58.60341145513725,-60.09809629230687]}, - {"t":2.7751898109637954, "x":2.5350688184405756, "y":6.434549425284907, "heading":-2.7642692394365955, "vx":-0.7725315208171001, "vy":-1.932841027295823, "omega":-0.7951577249026073, "ax":-1.4193828448621728, "ay":-4.2175184620654775, "alpha":-1.9180675672912677, "fx":[-32.04460245455299,-7.813999427993708,-22.944615467585617,-16.892870078443067], "fy":[-54.43325806615935,-62.67635118069884,-58.866408407420195,-60.83093073253235]}, - {"t":2.841875100001029, "x":2.48039638418715, "y":6.296279862723547, "heading":-2.817294562151925, "vx":-0.8671834760812255, "vy":-2.2140874649585287, "omega":-0.9230646150203854, "ax":-1.2440013968323247, "ay":-4.264715410250725, "alpha":-2.062324144168671, "fx":[-30.222032624555485,-5.241544692645217,-21.504276058100704,-12.880844910780752], "fy":[-55.420144220638285,-62.90769868684976,-59.37310664039483,-61.75603235352814]}, - {"t":2.908560389038263, "x":2.4198020112551384, "y":6.139150359466091, "heading":-2.878849392804616, "vx":-0.950140068791712, "vy":-2.498481244752643, "omega":-1.0605912966627542, "ax":-0.8813989213345087, "ay":-4.33976396012493, "alpha":-2.2994991920183376, "fx":[-26.05833375925274,-0.10401327652131084,-18.04257193181343,-5.284227493773326], "fy":[-57.40260762673982,-63.055022989305435,-60.44554603916663,-62.767661862694496]}, - {"t":2.975245678075497, "x":2.354481887470287, "y":5.962889107062256, "heading":-2.9495752299729605, "vx":-1.00891641061801, "vy":-2.7878796587869448, "omega":-1.2139340649233947, "ax":0.24303615548737564, "ay":-4.389643056942526, "alpha":-2.7012592237377193, "fx":[-10.656453134636582,14.560574176139268,-5.587109054455592,15.3290812894343], "fy":[-61.85696460562957,-61.15280010661529,-62.62935969492124,-60.83234600172205]}, - {"t":3.0419309671127306, "x":2.287742387128775, "y":5.767218333401137, "heading":-3.030526773964536, "vx":-0.992709474342837, "vy":-3.0806042748094447, "omega":-1.3940683170228323, "ax":4.435566177309857, "ay":-0.25714717899298845, "alpha":-0.25747047148003016, "fx":[62.30437125293196,62.24097961951379,62.12247420710711,62.382155439582], "fy":[-2.4549715586446146,-4.6760384769066805,-5.4424340558128685,-1.8649606936582548]}, - {"t":3.1086162561499644, "x":2.2314055901158234, "y":5.561215589459999, "heading":-3.123490622622869, "vx":-0.6969224617651503, "vy":-3.0977522087657103, "omega":-1.411237809831991, "ax":3.220498475726086, "ay":3.089363171073363, "alpha":1.1937847218578306, "fx":[50.822802385616335,39.133421194815696,46.18146425867487,44.6881753267838], "fy":[37.13610984975816,49.27818552303616,42.81325652436073,44.23527264782637]}, - {"t":3.175301545187198, "x":2.1920917763753565, "y":5.361510175497405, "heading":3.0655858833077914, "vx":-0.48216259006738366, "vy":-2.8917371327617007, "omega":-1.3316299306066297, "ax":2.6849339894616584, "ay":3.5608447185004115, "alpha":1.6537032067571915, "fx":[45.88930764480007,29.59343531710793,40.73530649436007,34.53672029358626], "fy":[43.282364134388295,55.71199770995361,48.19733735982893,52.74406374549788]}, - {"t":3.241986834224432, "x":2.1659084784582907, "y":5.176591258618251, "heading":2.9767857564946496, "vx":-0.3031169909342387, "vy":-2.6542811734917953, "omega":-1.2213522542821875, "ax":2.457226087959617, "ay":3.718376279784227, "alpha":1.8786267097359488, "fx":[43.615444441439415,26.0076986777479,38.86593346874905,29.48025694788064], "fy":[45.64971945014809,57.54083265321615,49.78208187447634,55.80827538618683]}, - {"t":3.308672123261666, "x":2.151158587774491, "y":5.007857426754202, "heading":2.8953395284015677, "vx":-0.1392561590288195, "vy":-2.406320176525192, "omega":-1.09607548915034, "ax":2.3346924639067983, "ay":3.794297085218485, "alpha":2.011683472573062, "fx":[42.233902567047195,24.34286370392693,38.09154331481552,26.4209560030304], "fy":[46.97124348793809,58.299354291699984,50.41089525966446,57.36224870249961]}, - {"t":3.3753574122988996, "x":2.1470633549401636, "y":4.855827752811985, "heading":2.8222474176009484, "vx":0.016433482739857747, "vy":-2.1532963787042654, "omega":-0.9619257953303414, "ax":2.2593007108059155, "ay":3.838292696155866, "alpha":2.0938111895387252, "fx":[41.24253569039785,23.49826618149628,37.74908605562393,24.366250729924296], "fy":[47.868988552877234,58.66584315123263,50.68857021107977,58.29062335049118]}, - {"t":3.4420427013361334, "x":2.153182700026887, "y":4.720768866612861, "heading":2.7581011179069823, "vx":0.16709560366197893, "vy":-1.89733872085161, "omega":-0.8222993909665198, "ax":2.2087493298720813, "ay":3.8668591850601577, "alpha":2.143393364894117, "fx":[40.46006123063887,23.056222224595157,37.595062384220725,22.90641324399093], "fy":[48.54912086881411,58.854954967863215,50.81694553402106,58.89696677609291]}, - {"t":3.5087279903733672, "x":2.1692365930243205, "y":4.60284210736539, "heading":2.7032658453452423, "vx":0.3143866911352961, "vy":-1.6394760984295926, "omega":-0.6793665849080305, "ax":2.1726839906804085, "ay":3.886929993565867, "alpha":2.1715887777853937, "fx":[39.813152968830174,22.821571882549858,37.52289236445775,21.83513047919712], "fy":[49.09335915822404,58.956288756651475,50.880419904611585,59.314864277080126]}, - {"t":3.575413279410601, "x":2.1950324447832563, "y":4.502155618343927, "heading":2.657962088268423, "vx":0.4592727510403909, "vy":-1.3802750483411612, "omega":-0.5345535595913623, "ax":2.1456607461701807, "ay":3.901873428223942, "alpha":2.1862622731275834, "fx":[39.271281076703936,22.69007708268728,37.48099827101031,21.033079099781897], "fy":[49.53730837252062,59.01443945553433,50.919013475525134,59.61322086274388]}, - {"t":3.642098568447835, "x":2.2304299801161513, "y":4.418787252453675, "heading":2.6223152296412, "vx":0.6023567580745971, "vy":-1.120077490993347, "omega":-0.38876202799661463, "ax":2.124560809623353, "ay":3.913495765101088, "alpha":2.1932088964239465, "fx":[38.82215497909296,22.60094458671981,37.442795681577216,20.424812431628897], "fy":[49.8975315342227,59.05436079087151,50.95320540832905,59.831460148633944]}, - {"t":3.7087838574850687, "x":2.2753221988677987, "y":4.3527960777282715, "heading":2.596390521437557, "vx":0.7440337097415111, "vy":-0.8591048947515922, "omega":-0.2425072588195172, "ax":1.7609925999004936, "ay":4.104606326507409, "alpha":1.8527010462732956, "fx":[32.01815184624958,18.802936547773932,32.154599602378674,15.90124341837194], "fy":[54.49840259539828,60.35979488558022,54.43101773418256,61.177896488575755]}, - {"t":3.7675730589632113, "x":2.3221064916186145, "y":4.309383096035475, "heading":2.582133713339049, "vx":0.8475610584986021, "vy":-0.6177983664340558, "omega":-0.13358844373241588, "ax":0.4837278329154911, "ay":4.442710575176965, "alpha":1.6664878043699651, "fx":[11.963505310167006,2.5712451387630293,15.897718127206655,-3.2719207343655365], "fy":[62.03526946280875,63.148923580516126,61.16179670151144,63.10513734240204]}, - {"t":3.826362260441354, "x":2.3727698523147596, "y":4.280740605370071, "heading":2.574280155405393, "vx":0.8759990315284342, "vy":-0.35661495932078263, "omega":-0.03561695644347868, "ax":-1.949508782286021, "ay":4.040942572164644, "alpha":0.9067529051914868, "fx":[-28.114214055027908,-27.111763925511312,-20.930409615519302,-33.305427387994726], "fy":[56.55094739527283,57.06612898262334,59.59505632294346,53.680360533681394]}, - {"t":3.885151461919497, "x":2.420900218784779, "y":4.266758589346444, "heading":2.572186262976905, "vx":0.7613889669431464, "vy":-0.11905117228408404, "omega":0.017690322786263927, "ax":-3.9914640028756017, "ay":2.078296602392951, "alpha":0.20410087171288205, "fx":[-56.66679350757237,-55.4146443062853,-55.5232069584012,-56.50970555446682], "fy":[27.93491490406332,30.35263121563157,30.143239204144766,28.262260760498023]}, - {"t":3.943940663397639, "x":2.4587640786742484, "y":4.263351139395654, "heading":2.573226262927041, "vx":0.5267339854853706, "vy":0.0031302254054562796, "omega":0.02968925005087548, "ax":-4.482471958944181, "ay":0.42241933206545695, "alpha":-0.17224367421798922, "fx":[-62.79179272181103,-63.03848056801679,-62.94943551098336,-62.90395669455554], "fy":[7.237846976339996,4.60076426676694,5.715309317126565,6.164252546951046]}, - {"t":4.002729864875781, "x":2.481984256045524, "y":4.264265139403598, "heading":2.5749716702293894, "vx":0.26321303837091753, "vy":0.02796392062660433, "omega":0.01956318197934503, "ax":-4.477234453825943, "ay":-0.4756642363443265, "alpha":-0.3327682891167336, "fx":[-63.08106771064706,-62.56509444954148,-62.885790958678136,-62.85763480093662], "fy":[-4.265698514896327,-9.104626820639742,-6.554486984166413,-6.782977409888858]}, - {"t":4.061519066353924, "x":2.4897212982177725, "y":4.265087127685547, "heading":2.5761217740753297, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.2731446027755735, "y":5.5781683921813965, "heading":-3.1415926535897927, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.505500795340116, "ay":-0.00013633104687162933, "alpha":-0.00009097022022549216, "fx":[63.24417416874877,63.24417483261289,63.24417413562843,63.24417486573166], "fy":[-0.0013661852872244456,-0.0024611872652380447,-0.0024612460873824214,-0.001366152636304772]}, + {"t":0.09459096302079543, "x":1.2933009749636943, "y":5.5781677822737645, "heading":-3.1415926535897927, "vx":0.42617965912218136, "vy":-0.00001289568501322062, "omega":-8.604960737343147e-6, "ax":4.504981513683446, "ay":-0.0000959224329450282, "alpha":-0.00007270971798615821, "fx":[63.236884952259565,63.23688562987533,63.236884933638,63.23688564849618], "fy":[-0.000908958654036442,-0.00178397895253321,-0.00178401302336204,-0.0009089412948723952]}, + {"t":0.18918192604159087, "x":1.353767768404653, "y":5.5781661333279, "heading":3.141591839638271, "vx":0.8523101988923791, "vy":-0.000021969080320788512, "omega":-0.000015482642982624302, "ax":4.504116176009493, "ay":-0.00006122851876513098, "alpha":-0.00005441823024803384, "fx":[63.22473811498249,63.22473880065895,63.22473810608925,63.224738809551944], "fy":[-0.000532138265568194,-0.0011867989912569003,-0.0011868152128920355,-0.0005321313981140908]}, + {"t":0.2837728890623863, "x":1.454538788692137, "y":5.578163781331872, "heading":3.1415903751201615, "vx":1.2783588855386596, "vy":-0.000027760744875119097, "omega":-0.00002063011578767321, "ax":4.502385995404041, "ay":-0.000033085626158894096, "alpha":-0.00003605253313978146, "fx":[63.20045139278101,63.200452079374514,63.20045138959863,63.20045208255684], "fy":[-0.00024772241643881233,-0.0006811281560037475,-0.0006811332352676076,-0.0002477214368846603]}, + {"t":0.37836385208318174, "x":1.5956024241907625, "y":5.578161007400284, "heading":3.1415884236976415, "vx":1.7042439127352704, "vy":-0.00003089034611563491, "omega":-0.00002404035961670427, "ax":4.497199426317788, "ay":-0.000011319103130312204, "alpha":-0.000017396241526915466, "fx":[63.12764700460344,63.12764766833042,63.1276470040774,63.127647668856454], "fy":[-0.00005455051671788044,-0.0002632238198628969,-0.00026322379974984587,-0.00005455148719185837]}, + {"t":0.4729548151039772, "x":1.7769277312635274, "y":5.57815803481414, "heading":3.141586149696874, "vx":2.129638337367239, "vy":-0.000031961030981262844, "omega":-0.00002568588685567756, "ax":-2.884168393713032e-6, "ay":0.0006757453954131626, "alpha":0.0005430726951462432, "fx":[0.0015135845577785034,-0.0015945552734667108,0.0015135992068228165,-0.0015945699555507211], "fy":[0.008360130725692543,0.010610883439196432,0.01061086318946274,0.008360150976654431]}, + {"t":0.5675457781247726, "x":1.9783722595781237, "y":5.578158034688606, "heading":3.1415837200441006, "vx":2.129638064550973, "vy":0.000031958376727736397, "omega":0.00002568388236850445, "ax":-4.4971988602857795, "ay":-0.000011301098963488138, "alpha":-0.00001738142455002673, "fx":[-63.127639723138735,-63.12763905889697,-63.12763972262031,-63.1276390594154], "fy":[-0.00005438575763755793,-0.0002628840758403389,-0.0002628803074513615,-0.000054388577284283265]}, + {"t":0.662136741145568, "x":2.159697543377207, "y":5.578161007104225, "heading":3.1415861495072677, "vx":1.7042436934605174, "vy":0.00003088939489358674, "omega":0.00002403975668164412, "ax":-4.502385424990097, "ay":-0.00003306979429408158, "alpha":-0.00003604088750144403, "fx":[-63.20044407552104,-63.20044338271689,-63.20044407234566,-63.20044338589236], "fy":[-0.00024756662867577936,-0.0006808435751470482,-0.0006808332454825856,-0.0002475728612806497]}, + {"t":0.7567277041663634, "x":2.300761160686297, "y":5.578163781016665, "heading":3.141588423451003, "vx":1.278358720219911, "vy":0.00002776129120440996, "omega":0.000020630614424758383, "ax":-4.5041155999808264, "ay":-0.0000612181641819451, "alpha":-0.00005441184376202829, "fx":[-63.2247307237932,-63.22473002030161,-63.224730714905306,-63.224730029189786], "fy":[-0.0005320222668116429,-0.0011866336464125054,-0.0011866128696309637,-0.0005320336916121858]}, + {"t":0.8513186671871588, "x":2.401532167913116, "y":5.578166133110695, "heading":3.141590374920689, "vx":0.8523100880607368, "vy":0.000021970606100074604, "omega":0.000015483745723571067, "ax":-4.504980931184877, "ay":-0.00009592235287739936, "alpha":-0.00007271074025308092, "fx":[-63.236877471986816,-63.23687675696363,-63.236877453366894,-63.2368767755843], "fy":[-0.000908932750560206,-0.0017840193200701132,-0.0017839824951020448,-0.000908952863401811]}, + {"t":0.9459096302079542, "x":2.461998953476341, "y":5.578167782201243, "heading":3.1415918395431084, "vx":0.4261796033896395, "vy":0.000012897218366180834, "omega":8.605966781077228e-6, "ax":-4.505500206144911, "ay":-0.00013634725722515428, "alpha":-0.00009098085595344607, "fx":[-63.24416659528168,-63.24416586489193,-63.24416656215554,-63.24416589801968], "fy":[-0.0013663156863217565,-0.002461538133192635,-0.0024614780660679176,-0.0013663495761148798]}, + {"t":1.0405005932287497, "x":2.4821553230285645, "y":5.5781683921813965, "heading":-3.1415926535897927, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.8998722096373628, "ay":4.022815499129762, "alpha":2.1693706742228716, "fx":[-20.437816339057353,-31.89605314032918,-16.02042919486477,-38.320501764866776], "fy":[59.82118165969276,54.5993848998633,61.165251507808485,50.2888688701159]}, + {"t":1.097805430460484, "x":2.4790358806987483, "y":5.584773542195893, "heading":-3.1415926535897927, "vx":-0.1088718677343648, "vy":0.23052678739093016, "omega":0.12431543338163228, "ax":-1.8239407764545923, "ay":4.057569489360194, "alpha":2.173004774204488, "fx":[-19.128469229132705,-30.986744518646685,-15.012563396037118,-37.28359418636463], "fy":[60.24443698446629,55.113916928977034,61.414029376918585,51.05368487870257]}, + {"t":1.1551102676922185, "x":2.4698022272139863, "y":5.604646055586961, "heading":-3.134468777914466, "vx":-0.2133924970494186, "vy":0.4630451465351685, "omega":0.24883911827119545, "ax":-1.722237552198593, "ay":4.101210140783639, "alpha":2.1849240494410194, "fx":[-17.298545104525694,-29.825251254034995,-13.694637234559364,-35.882463581752305], "fy":[60.784582896760966,55.74229359931523,61.71302774975789,52.03651703541032]}, + {"t":1.2124151049239529, "x":2.4547460248591673, "y":5.637914650255921, "heading":-3.1202090927450477, "vx":-0.31208503965253975, "vy":0.6980643261059142, "omega":0.37404583528810886, "ax":-1.5793282501673391, "ay":4.1575638664830326, "alpha":2.201671115649442, "fx":[-14.712847666966242,-28.21370819160362,-11.882255956504927,-33.86795552740872], "fy":[61.44751700301509,56.5630076081971,62.07628370813383,53.35378495856712]}, + {"t":1.2697199421556873, "x":2.4342689083679496, "y":5.684743509188957, "heading":-3.098774457036656, "vx":-0.40258818796385915, "vy":0.9363128467552654, "omega":0.5002122402082059, "ax":-1.3647672769462478, "ay":4.232189413865262, "alpha":2.213465694726016, "fx":[-10.931113211815775,-25.75037805977012,-9.22089237332458,-30.72712657981579], "fy":[62.21171885731976,57.708525839092914,62.510990017040285,55.19946426434758]}, + {"t":1.3270247793874217, "x":2.4089578161157754, "y":5.745347690160326, "heading":-3.070109876030205, "vx":-0.4807959546284613, "vy":1.1788377722506842, "omega":0.6270545315625041, "ax":-1.0102459347078458, "ay":4.330933506544813, "alpha":2.189748779988195, "fx":[-5.04326697914614,-21.492804201030765,-4.938676442390735,-25.24895354676516], "fy":[62.93315299415712,59.397776482402065,62.969184646172366,57.87490848257418]}, + {"t":1.384329616619156, "x":2.3797471369815337, "y":5.820011852628266, "heading":-3.034176618163596, "vx":-0.5386879334809158, "vy":1.4270212119047, "omega":0.7525377289781151, "ax":-0.3343366948998697, "ay":4.439265806988591, "alpha":2.01963830177586, "fx":[5.04889281555332,-12.733298688518062,2.9398430343438533,-14.027910397737209], "fy":[62.88326831787523,61.81997378164974,63.05034851311738,61.50411822404757]}, + {"t":1.4416344538508905, "x":2.348328757798069, "y":5.909075999916649, "heading":-2.991052566093768, "vx":-0.5578470433627493, "vy":1.681412616402585, "omega":0.8682727731283607, "ax":1.2088306501752888, "ay":4.302695428436486, "alpha":1.2839277670561584, "fx":[24.223835189924294,10.164027019714071,20.365960068229473,13.12009456665082], "fy":[58.1577638958531,62.18826664158655,59.65003880311409,61.59343012708531]}, + {"t":1.498939291082625, "x":2.3183462296404875, "y":6.012493767298566, "heading":-2.9412963361569022, "vx":-0.4885751997137238, "vy":1.9279778775868663, "omega":0.9418480448368312, "ax":3.9114847838938296, "ay":2.186793008972618, "alpha":-0.2444758452299215, "fx":[53.9718183011984,55.78886775707678,55.27007901263518,54.592879417349685], "fy":[32.32630629999774,29.111249697721096,30.04226035930175,31.305130257014564]}, + {"t":1.556244128314359, "x":2.2967708609887003, "y":6.126566769715672, "heading":-2.887323887250502, "vx":-0.26442820083828256, "vy":2.053291695025538, "omega":0.9278383963188565, "ax":4.364718597756994, "ay":-0.9434454159599098, "alpha":-1.1863308092365898, "fx":[61.976006370148006,61.36257059567952,58.8753654012174,62.85805933222032], "fy":[-11.180562236226963,-14.52488161230889,-22.439833621007587,-4.8276809021851985]}, + {"t":1.6135489655460935, "x":2.2887843742774607, "y":6.2426812521295085, "heading":-2.8341542589720956, "vx":-0.014308712031492848, "vy":1.9992277090269304, "omega":0.8598559023925799, "ax":3.8604984916758736, "ay":-2.220902600889836, "alpha":-1.7507294734846888, "fx":[53.342777875307654,55.87467038536325,47.84212388260628,59.70127319465518], "fy":[-33.65748428953355,-29.39940603575615,-41.159115316140046,-20.484140264626983]}, + {"t":1.670853802777828, "x":2.2943030539824423, "y":6.353600121333173, "heading":-2.784880356442742, "vx":0.20691652566735044, "vy":1.8719592469754025, "omega":0.7595306348777577, "ax":3.505787767886492, "ay":-2.737400410780314, "alpha":-2.036750710110143, "fx":[46.18298371009311,52.39362229674625,41.552861129246025,56.714935673815106], "fy":[-43.04264800812506,-35.299676206210165,-47.567470255065125,-27.79088132378227]}, + {"t":1.7281586400095623, "x":2.3119166025186333, "y":6.456377842821637, "heading":-2.741355577038554, "vx":0.4078151230750923, "vy":1.7150929619975528, "omega":0.6428149669532941, "ax":3.2822801936218235, "ay":-2.9974920556778932, "alpha":-2.1905249680807435, "fx":[41.45055135817435,50.28824007693655,38.01394693879307,54.54206991044379], "fy":[-47.66184936694225,-38.275368882704434,-50.474952879556355,-31.89223652267504]}, + {"t":1.785463477241297, "x":2.34067563043464, "y":6.549739317140388, "heading":-2.7045191699871696, "vx":0.5959056553195374, "vy":1.5433221676435134, "omega":0.5172872902053938, "ax":3.13365216342106, "ay":-3.150604160240399, "alpha":-2.277639845731396, "fx":[38.25002363366874,48.91962186251556,35.838825884825276,52.941109587048274], "fy":[-50.29360760825308,-40.03168150194951,-52.06250713011425,-34.51361236571295]}, + {"t":1.8427683144730311, "x":2.3799691200256348, "y":6.6330060958862305, "heading":-2.6748761060199, "vx":0.7754790824852543, "vy":1.3627773090593112, "omega":0.3867675095732595, "ax":2.7460635012124803, "ay":-3.5117282645755825, "alpha":-2.033775102729744, "fx":[31.960645358671744,44.02850117647088,31.30995742598545,46.88799057029898], "fy":[-54.52123415348526,-45.36153729444134,-54.91377833126023,-42.381402463987875]}, + {"t":1.9092077315802525, "x":2.4375523297060373, "y":6.715797497262229, "heading":-2.6491794981278405, "vx":0.9579259408452256, "vy":1.1294601301219571, "omega":0.25164467722071937, "ax":1.528054177392674, "ay":-4.184509245379921, "alpha":-2.081057915221667, "fx":[11.253062923968333,29.8923484370102,14.764831210322914,29.887567200671455], "fy":[-62.17006125536048,-55.67713917223275,-61.447731378543374,-55.65860160638203]}, + {"t":1.9756471486874732, "x":2.5045689362776975, "y":6.781602547662396, "heading":-2.6324603724551596, "vx":1.0594489696994485, "vy":0.8514437749791387, "omega":0.11338040236702436, "ax":-0.665050074423415, "ay":-4.425230794929647, "alpha":-1.3938763388571445, "fx":[-18.441184950144276,-0.45380912058750744,-11.81033051242392,-6.636178498612009], "fy":[-60.42196396806449,-63.170874163871325,-62.07083737472134,-62.80598974323057]}, + {"t":2.042086565794694, "x":2.5734902775418163, "y":6.828405057412514, "heading":-2.6249274446105098, "vx":1.0152634304076429, "vy":0.5574340203990894, "omega":0.020772070893806945, "ax":-2.8115850601008767, "ay":-3.5086310899077775, "alpha":-0.5240133714525708, "fx":[-42.48084757220351,-36.18533487954884,-38.684268626266416,-40.51557469069069], "fy":[-46.77552641386079,-51.79069003747352,-49.964128839922886,-48.47370552426505]}, + {"t":2.1085259829019147, "x":2.6347383441007897, "y":6.857696755886955, "heading":-2.623547360328214, "vx":0.8284633578671702, "vy":0.3243226159413437, "omega":-0.01404307206189464, "ax":-3.8444535926761314, "ay":-2.343486690661899, "alpha":-0.11000454240929862, "fx":[-54.31837838056509,-53.60122374361094,-53.641356841034295,-54.29899083344257], "fy":[-32.3175624158608,-33.4899054716665,-33.429343888733946,-32.34623564659895]}, + {"t":2.1749654000091354, "x":2.6812958805769123, "y":6.874072256486256, "heading":-2.624480373850397, "vx":0.573040102074007, "vy":0.1686227262152372, "omega":-0.021351709738718576, "ax":-4.2353434185098715, "ay":-1.5311892778191056, "alpha":0.09731250197539408, "fx":[-59.30427806399855,-59.597292231976425,-59.688986154508825,-59.21723486770738], "fy":[-21.903125434647656,-21.097186838854636,-20.83243592315027,-22.14109247395115]}, + {"t":2.241404817116356, "x":2.710020512641244, "y":6.881895967222901, "heading":-2.6258989689996786, "vx":0.291646354099307, "vy":0.06689140311610935, "omega":-0.01488632383023246, "ax":-4.389658530998912, "ay":-1.0068029797455784, "alpha":0.22405861577944453, "fx":[-61.46117179453489,-61.78048166915176,-61.99319295781629,-61.23749283102038], "fy":[-14.851350847790552,-13.474007054133326,-12.44778558762314,-15.757241238996931]}, + {"t":2.3078442342235768, "x":2.7197089195251465, "y":6.88411808013916, "heading":-2.6268880076778256, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.7410395740175106, "ay":-4.114032990152588, "alpha":-1.5605910089459936, "fx":[-34.559558817817134,-13.39063690567614,-25.180340006327075,-24.626065041962764], "fy":[-52.95704692807103,-61.79780296805115,-58.01066939477579,-58.230884571572055]}, + {"t":2.374484845926473, "x":2.7158429662845696, "y":6.8749829292744264, "heading":-2.6268880076778274, "vx":-0.11602394221147797, "vy":-0.27416167502966127, "omega":-0.103998739454236, "ax":-1.7239958845523504, "ay":-4.121136696117811, "alpha":-1.5579555457139005, "fx":[-34.3175859787987,-13.180650038260714,-24.97539072367529,-24.325997996965945], "fy":[-53.109620257228734,-61.83866771090629,-58.09547259099445,-58.351505085924856]}, + {"t":2.4411254576293695, "x":2.7042829518292533, "y":6.847561703003679, "heading":-2.633818547291389, "vx":-0.23091208253132345, "vy":-0.5487967453702026, "omega":-0.20782185002656436, "ax":-1.7026837592963102, "ay":-4.129565626827544, "alpha":-1.5647180450343912, "fx":[-34.06649947364837,-12.888070706098604,-24.787845847237534,-23.86056680128185], "fy":[-53.26539842214614,-61.89516442811321,-58.17118256296012,-58.53679127438963]}, + {"t":2.507766069332266, "x":2.6851140446920856, "y":6.801819911331409, "heading":-2.647667922502394, "vx":-0.3443799697874176, "vy":-0.8239935248092419, "omega":-0.3120956176902499, "ax":-1.6755667501928708, "ay":-4.139810784998948, "alpha":-1.58176167859978, "fx":[-33.78746829998938,-12.49094650344566,-24.599513717782308,-23.202477409539004], "fy":[-53.43571786834504,-61.97017530196677,-58.245370970985206,-58.79252186984964]}, + {"t":2.5744066810351622, "x":2.658443781066791, "y":6.7377160887128325, "heading":-2.6684661653750705, "vx":-0.4560407629693055, "vy":-1.0998730478558163, "omega":-0.41750518352037286, "ax":-1.6402028071598025, "ay":-4.15261308570717, "alpha":-1.610630079539864, "fx":[-33.45054670736153,-11.95528967286448,-24.3825835775639,-22.306356862410095], "fy":[-53.63802617539488,-62.0676585088622,-58.32916541051754,-59.127764723006266]}, + {"t":2.6410472927380586, "x":2.6244108990056185, "y":6.65519905859822, "heading":-2.6962889661940093, "vx":-0.5653448813552433, "vy":-1.3766057240527916, "omega":-0.5248385572480271, "ax":-1.5924630769760055, "ay":-4.1691541199330215, "alpha":-1.6538695367539258, "fx":[-33.00532311041575,-11.22364799637928,-24.090898431982353,-21.094397421580062], "fy":[-53.90065822191639,-62.19345216289903,-58.44040899316404,-59.55684818230097]}, + {"t":2.707687904440955, "x":2.583199929015267, "y":6.554203664535949, "heading":-2.7312645286942945, "vx":-0.6714675949192015, "vy":-1.654440704888776, "omega":-0.6350534348541276, "ax":-1.5247814255636791, "ay":-4.191435934601253, "alpha":-1.7157821993357862, "fx":[-32.35962292481045,-10.189918173545475,-23.640363199746247,-19.424145668093345], "fy":[-54.27309204353587,-62.35635229951819,-58.61028437875809,-60.10272726488862]}, + {"t":2.7743285161438513, "x":2.5350671626073553, "y":6.4346437009504935, "heading":-2.7735848780570143, "vx":-0.7730799618319799, "vy":-1.933760559484102, "omega":-0.7493942101668383, "ax":-1.4216783540019546, "ay":-4.223086529418797, "alpha":-1.8042713062688724, "fx":[-31.32382831162336,-8.636191501781548,-22.8586423197964,-17.006314479683546], "fy":[-54.850570348556694,-62.56844641251809,-58.89871469912816,-60.801855045494904]}, + {"t":2.8409691278467477, "x":2.4803918247938106, "y":6.2963994117051545, "heading":-2.823524966629157, "vx":-0.8678214769874375, "vy":-2.21518962907883, "omega":-0.8696319536946121, "ax":-1.245964681589609, "ay":-4.271089027766561, "alpha":-1.9360229216948792, "fx":[-29.43289355981037,-6.043585834048996,-21.328697767306412,-13.153756438968612], "fy":[-55.84266125988171,-62.83555764337504,-59.43490750624356,-61.70172397018425]}, + {"t":2.907609739549644, "x":2.4197930241296484, "y":6.139293928256428, "heading":-2.88147777197977, "vx":-0.950853325528774, "vy":-2.49981761452672, "omega":-0.9986497054672178, "ax":-0.8820070064297043, "ay":-4.347645989224414, "alpha":-2.15305625738112, "fx":[-25.162201296111903,-0.8518255289880377,-17.730678865770642,-5.778583780662676], "fy":[-57.800767496874464,-63.0485814149876,-60.535585968036344,-62.72846702568088]}, + {"t":2.9742503512525404, "x":2.3544690930514935, "y":5.963050668121943, "heading":-2.948028399229044, "vx":-1.009630811963491, "vy":-2.7895474027162774, "omega":-1.1421306914898663, "ax":0.2494858845444272, "ay":-4.399221263815051, "alpha":-2.5235347312496095, "fx":[-9.592488904728697,14.029295218370882,-4.991505545076138,14.56293452807716], "fy":[-62.03347859240343,-61.27382971885202,-62.67603391114822,-61.02592925621341]}, + {"t":3.040890962955437, "x":2.2877406579532233, "y":5.7673851155212645, "heading":-3.024140687154603, "vx":-0.993004920006218, "vy":-3.082714198753302, "omega":-1.310300589633825, "ax":4.435818071468198, "ay":-0.25708095877631787, "alpha":-0.25086698872461743, "fx":[62.30596003910187,62.24586541353015,62.129545816613046,62.38275270592669], "fy":[-2.5009058523333607,-4.633402228518494,-5.400961561030389,-1.899416983380519]}, + {"t":3.107531574658333, "x":2.23141587265255, "y":5.561380311053381, "heading":-3.1114599199624884, "vx":-0.6973992903208175, "vy":-3.0998462311033683, "omega":-1.3270185192184163, "ax":3.224843162066139, "ay":3.0892003088589584, "alpha":1.0993814677235405, "fx":[50.47154957394162,39.653609978400176,46.09320659274382,44.85144424312073], "fy":[37.611715955327114,48.86059402831459,42.90486154430043,44.07650856293725]}, + {"t":3.1741721863612296, "x":2.192101475032156, "y":5.36166418671807, "heading":3.083292061355298, "vx":-0.4824937693548206, "vy":-2.893980032848243, "omega":-1.253755065714412, "ax":2.6896120817214593, "ay":3.5634106933614595, "alpha":1.524732196225399, "fx":[45.37861116192915,30.17716564888096,40.47217861359836,34.98948175945463], "fy":[43.81809038137478,55.39706174728166,48.41672491724755,52.44796130780795]}, + {"t":3.240812798064126, "x":2.1659200398999197, "y":5.1767200890765475, "heading":2.999741056850474, "vx":-0.30325637498539826, "vy":-2.6565121644940004, "omega":-1.1521459794747722, "ax":2.46136388087331, "ay":3.722638401441861, "alpha":1.7319652688793958, "fx":[43.05110350870704,26.545204332613025,38.48526059647987,30.120095583766343], "fy":[46.18320826973365,57.29389005019214,50.07585631296352,55.467266077958705]}, + {"t":3.3074534097670223, "x":2.151176272533488, "y":5.007954558269128, "heading":2.922961344007241, "vx":-0.13922958034058314, "vy":-2.4084332642732287, "omega":-1.0367267545084002, "ax":2.3380558613561453, "ay":3.799737792071913, "alpha":1.8557271936607345, "fx":[41.65901310430244,24.81318832841118,37.624395342626435,27.18151822655443], "fy":[47.482862907902074,58.099844078886704,50.75977615566659,57.00674562889175]}, + {"t":3.3740940214699187, "x":2.14708954742058, "y":4.855892355206892, "heading":2.853873238918043, "vx":0.016579892455735115, "vy":-2.1552164134989495, "omega":-0.9130599591690726, "ax":2.2618373801444753, "ay":3.8445793243472086, "alpha":1.933998662995584, "fx":[40.67917852452663,23.89949800536653,37.216171345100456,25.203720728051504], "fy":[48.34968163449467,58.50272793348844,51.08062129387216,57.933978569994515]}, + {"t":3.440734633172815, "x":2.153216818846679, "y":4.720804247948824, "heading":2.793026364717603, "vx":0.1673101190410419, "vy":-1.8990112955841496, "omega":-0.7841771052343876, "ax":2.2105011740318283, "ay":3.873749755121265, "alpha":1.9832419289119725, "fx":[39.91954832588459,23.393631395879275,37.011893526337005,23.791049096407043], "fy":[48.99555577846257,58.72092939147125,51.24289417624933,58.54550334143658]}, + {"t":3.5073752448757114, "x":2.169274853469952, "y":4.602854578990443, "heading":2.740768322741387, "vx":0.3146192694484959, "vy":-1.640862242318929, "omega":-0.6520126499367871, "ax":2.1737466096233184, "ay":3.8942456506848058, "alpha":2.0130845290056074, "fx":[39.299674195100664,23.103961174102068,36.901601597253865,22.747175090793153], "fy":[49.50636129555786,58.8455407468563,51.3325486275023,58.9712439291418]}, + {"t":3.574015856578608, "x":2.1950680470061292, "y":4.5021536316924, "heading":2.6973178009115912, "vx":0.4594790732008933, "vy":-1.3813473300359551, "omega":-0.5178594655141437, "ax":2.146156483203825, "ay":3.909486380589403, "alpha":2.030171805632574, "fx":[38.783791649495434,22.928135729277553,36.83119969757509,21.960143299182324], "fy":[49.92085149481827,58.92176189385507,51.39080783642928,59.27801609858328]}, + {"t":3.640656468281504, "x":2.2304535229981024, "y":4.4187807587156565, "heading":2.662807329353614, "vx":0.6025002540517355, "vy":-1.120816766189341, "omega":-0.3825675745247438, "ax":2.1246210580599896, "ay":3.9213197596242417, "alpha":2.039384539750917, "fx":[38.35569229571829,22.80635559945654,36.77217046613659,21.359872171523836], "fy":[50.25788484499299,58.97479519626342,51.4391434344136,59.50403924497604]}, + {"t":3.707297079984401, "x":2.2753221988677983, "y":4.3527960777282715, "heading":2.6373127921696176, "vx":0.7440863009977104, "vy":-0.8594976187253323, "omega":-0.2466617412982424, "ax":1.7596108086754303, "ay":4.109610604505918, "alpha":1.7521927723228714, "fx":[31.769257282401373,18.86192935854829,31.57739284519953,16.590766550811388], "fy":[54.64482802337728,60.34090230148758,54.76794676953785,60.99441685336344]}, + {"t":3.766086623587817, "x":2.3221074855260966, "y":4.309368444527029, "heading":2.622811660974328, "vx":0.8475330173593904, "vy":-0.6178954868986488, "omega":-0.1436511279088812, "ax":0.48214889573638664, "ay":4.444960354723043, "alpha":1.6267933800044825, "fx":[12.078961086743163,2.412890074959227,15.424296509846677,-2.844254639102705], "fy":[62.01398057705011,63.15472307693894,61.28329448850698,63.125450485155675]}, + {"t":3.8248761671912326, "x":2.372766768828322, "y":4.280724010042883, "heading":2.614366476726463, "vx":0.8758783308886109, "vy":-0.35657829630912835, "omega":-0.048012687563337514, "ax":-1.947204238734864, "ay":4.041664677620103, "alpha":0.930668893258717, "fx":[-27.782614749678515,-27.32947426076432,-20.822778967464657,-33.39755055401811], "fy":[56.71463755534605,56.962298014545034,59.63339091336207,53.622711823176616]}, + {"t":3.8836657107946486, "x":2.420894282346845, "y":4.266745356565006, "heading":2.6115438327373406, "vx":0.76140308239071, "vy":-0.11897067451372507, "omega":0.006700911914387456, "ax":-3.9907256192627023, "ay":2.078856498054835, "alpha":0.246931424628033, "fx":[-56.76352540061375,-55.31254258933245,-55.35937058272096,-56.63745268952035], "fy":[27.736467239963485,30.539491627968825,30.442067341214596,28.00645712538856]}, + {"t":3.9424552543980647, "x":2.458760428290384, "y":4.263343607671061, "heading":2.611937776290248, "vx":0.5267901445878157, "vy":0.0032443502240012173, "omega":0.02121789766691995, "ax":-4.482874702583453, "ay":0.42168127130257554, "alpha":-0.10067485283551426, "fx":[-62.85246516679099,-62.99688017662742,-62.945185054286355,-62.911748511937674], "fy":[6.6881610749615135,5.142183343850803,5.755367241633609,6.091020509715102]}, + {"t":4.001244798001481, "x":2.4819833012978445, "y":4.264263051145531, "heading":2.613185166809864, "vx":0.263243986791654, "vy":0.028034799710057717, "omega":0.015299269013719556, "ax":-4.477734825897365, "ay":-0.4768671092092846, "alpha":-0.26023792814082874, "fx":[-63.04385107207008,-62.63386296010892,-62.8731704627761,-62.866798520183416], "fy":[-4.783637261597021,-8.619127694815889,-6.671103200315867,-6.701460969540904]}, + {"t":4.060034341604896, "x":2.489721298217773, "y":4.265087127685547, "heading":2.6140846038519396, "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,11,32] }, "events":[] diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index e7bf774..fce06f0 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -d4cefa7 \ No newline at end of file +6355b9e \ No newline at end of file diff --git a/src/main/include/Autos.h b/src/main/include/Autos.h index 2d39816..d32954b 100644 --- a/src/main/include/Autos.h +++ b/src/main/include/Autos.h @@ -21,7 +21,10 @@ class Autos { public: frc2::CommandPtr IntakeNote() { - return frc2::cmd::Parallel(intakeSub.IntakeNote(), feederSub.Feed()).Until([this] { return feederSub.HasNote(); }); + return frc2::cmd::Sequence( + frc2::cmd::Parallel(m_intakeSub.IntakeNote(), m_feederSub.Feed()).WithName("IntakeAndFeed").Until([this] { return m_feederSub.HasNote(); }).WithName("IntakeAndFeedUntilNote"), + frc2::cmd::Print("Got Note!") + ).WithName("Intake Note Command Auto"); } frc2::CommandPtr CloseFourSafe() { @@ -32,30 +35,33 @@ class Autos { safefourloop.Enabled().OnTrue( frc2::cmd::Sequence( frc2::cmd::Print("RESET POSE"), - frc2::cmd::RunOnce([this] { swerveSub.ResetPose(mtocenter.GetInitialPose().value()); }), + frc2::cmd::RunOnce([this] { m_swerveSub.ResetPose(mtocenter.GetInitialPose().value()); }).WithName("ResetPoseCmd"), frc2::cmd::Print("SPIN UP SUB SHOT"), - shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; }), + m_shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; }), + m_feederSub.Feed().Until([this] { return !m_feederSub.HasNote(); }), frc2::cmd::Print("SHOOT"), - feederSub.Feed().Until([this] { return !feederSub.HasNote(); }), frc2::cmd::Print("CENTER NOTE AND INTAKE"), - frc2::cmd::Deadline(IntakeNote(), mtocenter.Cmd()), + IntakeNote().DeadlineFor(mtocenter.Cmd()), + m_swerveSub.Stop(), frc2::cmd::Print("SPIN UP SHOT"), - shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return swerveSub.GetDistanceToSpeaker(SpeakerSide::CENTER); }), + m_shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::CENTER); }), + m_feederSub.Feed().Until([this] { return !m_feederSub.HasNote(); }), frc2::cmd::Print("SHOOT"), - feederSub.Feed().Until([this] { return !feederSub.HasNote(); }), frc2::cmd::Print("AMP NOTE AND INTAKE"), - frc2::cmd::Deadline(IntakeNote(), ctoamp.Cmd()), + IntakeNote().DeadlineFor(ctoamp.Cmd()), + m_swerveSub.Stop(), frc2::cmd::Print("SPIN UP SHOT"), - shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return swerveSub.GetDistanceToSpeaker(SpeakerSide::AMP_SIDE); }), + m_shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::AMP_SIDE); }), + m_feederSub.Feed().Until([this] { return !m_feederSub.HasNote(); }), frc2::cmd::Print("SHOOT"), - feederSub.Feed().Until([this] { return !feederSub.HasNote(); }), frc2::cmd::Print("SOURCE NOTE AND INTAKE"), - frc2::cmd::Deadline(IntakeNote(), atosource.Cmd()), + IntakeNote().DeadlineFor(atosource.Cmd()), + m_swerveSub.Stop(), frc2::cmd::Print("SPIN UP SHOT"), - shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return swerveSub.GetDistanceToSpeaker(SpeakerSide::SOURCE); }), - frc2::cmd::Print("SHOOT"), - feederSub.Feed().Until([this] { return !feederSub.HasNote(); }) - )); + m_shooterSub.RunShooter([] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::SOURCE); }), + m_feederSub.Feed().Until([this] { return !m_feederSub.HasNote(); }), + frc2::cmd::Print("SHOOT") + ).WithName("WHOLE AUTO COMMAND")); return safefourloop.Cmd(); } @@ -65,7 +71,7 @@ class Autos { straightTraj = factory.Trajectory("ChoreoTest", loop); loop.Enabled().OnTrue(frc2::cmd::RunOnce([this] { - swerveSub.ResetPose( + m_swerveSub.ResetPose( straightTraj.GetInitialPose().value()); }) .AndThen(straightTraj.Cmd()) @@ -75,19 +81,16 @@ class Autos { explicit Autos(SwerveSubsystem& swerveSub, ShooterSubsystem& shooterSub, IntakeSubsystem& intakeSub, FeederSubsystem& feederSub) - : swerveSub{swerveSub}, - shooterSub{shooterSub}, - intakeSub{intakeSub}, - feederSub{feederSub}, + : m_swerveSub{swerveSub}, + m_shooterSub{shooterSub}, + m_intakeSub{intakeSub}, + m_feederSub{feederSub}, factory{swerveSub.GetFactory()}, loop{factory.NewLoop("Auto Routine Loops")}, safefourloop{factory.NewLoop("Safe Four Loop")} { BindCommandsAndTriggers(); - pathplanner::NamedCommands::registerCommand( - "Print", frc2::cmd::Print("Test Named Command")); - selectCommand = frc2::cmd::Select( [this] { return autoChooser.GetSelected(); }, std::pair{CHOREO_TEST, TestChoreoAuto()}, @@ -105,32 +108,32 @@ class Autos { private: void BindCommandsAndTriggers() { - factory.Bind("SpinUpShooterCenter", [this] { return shooterSub.RunShooter( + factory.Bind("SpinUpShooterCenter", [this] { return m_shooterSub.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { - return swerveSub.GetDistanceToSpeaker(SpeakerSide::CENTER); + return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::CENTER); }); } ); - factory.Bind("SpinUpShooterAmp", [this] { return shooterSub.RunShooter( + factory.Bind("SpinUpShooterAmp", [this] { return m_shooterSub.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { - return swerveSub.GetDistanceToSpeaker(SpeakerSide::AMP_SIDE); + return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::AMP_SIDE); }); } ); - factory.Bind("SpinUpShooterSource", [this] { return shooterSub.RunShooter( + factory.Bind("SpinUpShooterSource", [this] { return m_shooterSub.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::SPEAKER_DIST; }, [this] { - return swerveSub.GetDistanceToSpeaker(SpeakerSide::SOURCE); + return m_swerveSub.GetDistanceToSpeaker(SpeakerSide::SOURCE); }); } ); @@ -140,10 +143,10 @@ class Autos { frc::SendableChooser autoChooser; - SwerveSubsystem& swerveSub; - ShooterSubsystem& shooterSub; - IntakeSubsystem& intakeSub; - FeederSubsystem& feederSub; + SwerveSubsystem& m_swerveSub; + ShooterSubsystem& m_shooterSub; + IntakeSubsystem& m_intakeSub; + FeederSubsystem& m_feederSub; choreo::AutoLoop loop; choreo::AutoTrajectory straightTraj;