From 55c71bf9390406860ea82ec3993ebb8dca70dc89 Mon Sep 17 00:00:00 2001 From: John Joseph Date: Thu, 14 Dec 2023 09:14:12 -0500 Subject: [PATCH] Change led length to 12 per strip, add one piece taxi auto Note: The leds have 36 physical leds but I think one led controller controls 3 leds --- simgui.json | 3 + src/main/deploy/choreo/1PieceTaxi.json | 1339 +++++++++++++++++ .../holonomic/SwerveOrthogonalCommand.kt | 27 +- .../robot2023/auto/routines/OnePieceTaxi.kt | 43 + .../robot2023/auto/routines/RoutineChooser.kt | 10 +- .../robot2023/commands/light/BlairChasing.kt | 8 +- .../robot2023/commands/light/BreatheHue.kt | 12 +- .../constants/subsystem/LightConstants.kt | 8 +- 8 files changed, 1423 insertions(+), 27 deletions(-) create mode 100644 src/main/deploy/choreo/1PieceTaxi.json create mode 100644 src/main/kotlin/frc/team449/robot2023/auto/routines/OnePieceTaxi.kt diff --git a/simgui.json b/simgui.json index e5c2de8..7c4e2c1 100644 --- a/simgui.json +++ b/simgui.json @@ -109,6 +109,9 @@ "open": true } }, + "driveCommand": { + "open": true + }, "intake": { "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": { "open": true diff --git a/src/main/deploy/choreo/1PieceTaxi.json b/src/main/deploy/choreo/1PieceTaxi.json new file mode 100644 index 0000000..3356b1c --- /dev/null +++ b/src/main/deploy/choreo/1PieceTaxi.json @@ -0,0 +1,1339 @@ +{ + "version": "v0.1.1", + "robotConfiguration": { + "mass": 60, + "rotationalInertia": 6, + "wheelMaxTorque": 2, + "wheelMaxVelocity": 80, + "wheelbase": 0.6223, + "trackWidth": 0.5461, + "bumperLength": 0.9271, + "bumperWidth": 0.851, + "wheelRadius": 0.0508 + }, + "paths": { + "1": { + "waypoints": [ + { + "x": 0.4425, + "y": 5, + "heading": 1.57079632679, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 0.9745361804962158, + "y": 5.795419692993164, + "heading": 1.571, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 40 + }, + { + "x": 0.785, + "y": 7.225, + "heading": 2.35619, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "timestamp": 0, + "x": 0.4425, + "y": 5, + "heading": 1.57079632679, + "velocityX": -1.1469877523974064e-18, + "velocityY": 3.987579383498826e-20, + "angularVelocity": -2.5955693290719213e-29 + }, + { + "timestamp": 0.0656457951054257, + "x": 0.4512813775788706, + "y": 5.007128739630323, + "heading": 1.57079632679, + "velocityX": 0.13376907941731708, + "velocityY": 0.1085940023862049, + "angularVelocity": -1.2584575036891587e-20 + }, + { + "timestamp": 0.1312915902108514, + "x": 0.4685834409610889, + "y": 5.021695854775544, + "heading": 1.57079632679, + "velocityX": 0.2635669711126451, + "velocityY": 0.22190477123212884, + "angularVelocity": -1.2584574976451283e-20 + }, + { + "timestamp": 0.1969373853162771, + "x": 0.4940754911838283, + "y": 5.044063971625227, + "heading": 1.57079632679, + "velocityX": 0.3883272368291027, + "velocityY": 0.34073952206322466, + "angularVelocity": -1.2584574615874057e-20 + }, + { + "timestamp": 0.2625831804217028, + "x": 0.5273287998290616, + "y": 5.074659747554664, + "heading": 1.57079632679, + "velocityX": 0.506556567588667, + "velocityY": 0.4660736591018717, + "angularVelocity": -1.2584575251190582e-20 + }, + { + "timestamp": 0.3282289755271285, + "x": 0.5677741748744144, + "y": 5.113985109961858, + "heading": 1.57079632679, + "velocityX": 0.6161152436404872, + "velocityY": 0.5990537907879476, + "angularVelocity": -1.2584575574420175e-20 + }, + { + "timestamp": 0.39387477063255416, + "x": 0.6146371888727673, + "y": 5.162624193475235, + "heading": 1.57079632679, + "velocityX": 0.7138768587247969, + "velocityY": 0.7409322019066734, + "angularVelocity": -1.2584574551033388e-20 + }, + { + "timestamp": 0.45952056573797984, + "x": 0.6668410573860795, + "y": 5.221233570526957, + "heading": 1.57079632679, + "velocityX": 0.7952355277207609, + "velocityY": 0.8928123569467977, + "angularVelocity": -1.2584574827338083e-20 + }, + { + "timestamp": 0.5251663608434055, + "x": 0.7228726150054193, + "y": 5.290486267748315, + "heading": 1.57079632679, + "velocityX": 0.8535437422816555, + "velocityY": 1.0549449071359331, + "angularVelocity": -1.2584575018821742e-20 + }, + { + "timestamp": 0.5908121559488312, + "x": 0.7806420346729138, + "y": 5.370915336965035, + "heading": 1.57079632679, + "velocityX": 0.8800170608752337, + "velocityY": 1.2251975787261382, + "angularVelocity": -1.2584574951435764e-20 + }, + { + "timestamp": 0.6564579510542569, + "x": 0.8374641534643532, + "y": 5.462615345195544, + "heading": 1.57079632679, + "velocityX": 0.8655865726081035, + "velocityY": 1.396890815066536, + "angularVelocity": -1.2584574949383493e-20 + }, + { + "timestamp": 0.7221037461596825, + "x": 0.8903699666449546, + "y": 5.564926385041506, + "heading": 1.57079632679, + "velocityX": 0.8059284390665953, + "velocityY": 1.55853150505151, + "angularVelocity": -1.2584575032602156e-20 + }, + { + "timestamp": 0.7877495412651082, + "x": 0.9367038482268095, + "y": 5.676442921161932, + "heading": 1.57079632679, + "velocityX": 0.7058164427354948, + "velocityY": 1.6987612982877642, + "angularVelocity": -1.258457452647393e-20 + }, + { + "timestamp": 0.8533953363705339, + "x": 0.9745361804962158, + "y": 5.795419692993164, + "heading": 1.57079632679, + "velocityX": 0.5763100623375512, + "velocityY": 1.8124050693598495, + "angularVelocity": -3.745141012797808e-21 + }, + { + "timestamp": 0.9147895182879412, + "x": 1.0019643228622268, + "y": 5.911364726783633, + "heading": 1.5809700254463517, + "velocityX": 0.4467547495446619, + "velocityY": 1.8885345511476659, + "angularVelocity": 0.1657111201520421 + }, + { + "timestamp": 0.9761837002053485, + "x": 1.0211045580239033, + "y": 6.0312045998873085, + "heading": 1.6018648712174086, + "velocityX": 0.3117597557929079, + "velocityY": 1.951974427558838, + "angularVelocity": 0.34033918391756685 + }, + { + "timestamp": 1.037577882122756, + "x": 1.0316185298421663, + "y": 6.153828939667901, + "heading": 1.6342547527327569, + "velocityX": 0.1712535535110973, + "velocityY": 1.9973283453073198, + "angularVelocity": 0.527572491460547 + }, + { + "timestamp": 1.0989720640401632, + "x": 1.0332344477389297, + "y": 6.277656329526426, + "heading": 1.6789201930062343, + "velocityX": 0.026320375095758353, + "velocityY": 2.016923851597345, + "angularVelocity": 0.7275191048813916 + }, + { + "timestamp": 1.1603662459575705, + "x": 1.0258577180854473, + "y": 6.400415051786288, + "heading": 1.7354201354662555, + "velocityX": -0.1201535621633652, + "velocityY": 1.9995171924435224, + "angularVelocity": 0.9202817057816595 + }, + { + "timestamp": 1.2217604278749779, + "x": 1.0101036197043256, + "y": 6.518800606468701, + "heading": 1.8002840194461396, + "velocityX": -0.25660572205873317, + "velocityY": 1.9282862151608164, + "angularVelocity": 1.0565151607874534 + }, + { + "timestamp": 1.2831546097923852, + "x": 0.9881846323135212, + "y": 6.629604828230481, + "heading": 1.8695552797317774, + "velocityX": -0.3570205955393545, + "velocityY": 1.8048000364406491, + "angularVelocity": 1.128303336280741 + }, + { + "timestamp": 1.3445487917097925, + "x": 0.9628103531834049, + "y": 6.731144757298989, + "heading": 1.9397858357757938, + "velocityX": -0.41330103826860554, + "velocityY": 1.6539014919216304, + "angularVelocity": 1.1439285262974286 + }, + { + "timestamp": 1.4059429736271998, + "x": 0.9361531514365301, + "y": 6.8228940733108665, + "heading": 2.00818783184271, + "velocityX": -0.4341975235167435, + "velocityY": 1.4944301421803503, + "angularVelocity": 1.1141445969413895 + }, + { + "timestamp": 1.467337155544607, + "x": 0.9097137925068551, + "y": 6.904829465138113, + "heading": 2.0728553769549345, + "velocityX": -0.4306492586747602, + "velocityY": 1.3345790963950488, + "angularVelocity": 1.0533171563263184 + }, + { + "timestamp": 1.5287313374620144, + "x": 0.8845141640120975, + "y": 6.977088601290756, + "heading": 2.1324944761296076, + "velocityX": -0.4104562958206423, + "velocityY": 1.1769704212339163, + "angularVelocity": 0.9714128816783414 + }, + { + "timestamp": 1.5901255193794217, + "x": 0.8612708627578636, + "y": 7.0398445670221985, + "heading": 2.186201594820018, + "velocityX": -0.3785912692102121, + "velocityY": 1.0221809912846034, + "angularVelocity": 0.8747916661331482 + }, + { + "timestamp": 1.651519701296829, + "x": 0.8405055352126447, + "y": 7.093264859340472, + "heading": 2.233325796844512, + "velocityX": -0.33822956665753895, + "velocityY": 0.8701197841537902, + "angularVelocity": 0.767567879443183 + }, + { + "timestamp": 1.7129138832142363, + "x": 0.8226113020257952, + "y": 7.13749998054338, + "heading": 2.2733812031707337, + "velocityX": -0.2914646409153611, + "velocityY": 0.7205099867999959, + "angularVelocity": 0.6524300035483459 + }, + { + "timestamp": 1.7743080651316436, + "x": 0.8078932237385573, + "y": 7.1726820887210065, + "heading": 2.3059890839161508, + "velocityX": -0.23973083161264336, + "velocityY": 0.573052805312347, + "angularVelocity": 0.5311233039847939 + }, + { + "timestamp": 1.835702247049051, + "x": 0.7965937050620064, + "y": 7.19892684216443, + "heading": 2.3308381093208506, + "velocityX": -0.18404868871372718, + "velocityY": 0.42747948785651213, + "angularVelocity": 0.40474560664605813 + }, + { + "timestamp": 1.8970964289664582, + "x": 0.7889089647229168, + "y": 7.2163360105719825, + "heading": 2.3476561858773746, + "velocityX": -0.12517049823105034, + "velocityY": 0.2835638144176846, + "angularVelocity": 0.2739359990031175 + }, + { + "timestamp": 1.9584906108838656, + "x": 0.785, + "y": 7.225, + "heading": 2.35619, + "velocityX": -0.06366995374537969, + "velocityY": 0.14112069185436435, + "angularVelocity": 0.13900037195879023 + }, + { + "timestamp": 2.019884792801273, + "x": 0.785, + "y": 7.225, + "heading": 2.35619, + "velocityX": 1.0427087556385988e-18, + "velocityY": -1.775000129906374e-18, + "angularVelocity": 8.024234162766743e-20 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "WptZeroVelocity", + "uuid": "c905fd04-3fd8-46c9-8623-06ac5fcc9f1b" + }, + { + "scope": [ + "last" + ], + "type": "WptZeroVelocity", + "uuid": "7a2c09ef-4ca0-4071-9e5b-8533614efe5f" + }, + { + "scope": [ + 0, + 1 + ], + "type": "ZeroAngularVelocity", + "uuid": "6a6ef220-b60b-4fd8-a1e8-2ba408cb7f91" + }, + { + "scope": [ + 2, + 2 + ], + "type": "ZeroAngularVelocity", + "uuid": "5a00d15b-33dd-4d19-8249-03ee23e5d2d2" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40 + }, + "2": { + "waypoints": [ + { + "x": 0.9481441378593445, + "y": 7.18, + "heading": 2.35619, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 100 + }, + { + "x": 0.75, + "y": 2.35, + "heading": 1.57079632679, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 100 + } + ], + "trajectory": [ + { + "timestamp": 0, + "x": 0.9481441378593444, + "y": 7.18, + "heading": 2.35619, + "velocityX": 0, + "velocityY": -6.018531076210112e-36, + "angularVelocity": -0.5394714268555183 + }, + { + "timestamp": 0.02714388554549102, + "x": 0.9480648490413264, + "y": 7.1780677934583865, + "heading": 2.3415466493294486, + "velocityX": -0.002921056305115401, + "velocityY": -0.07118385974532045, + "angularVelocity": -0.539471427036468 + }, + { + "timestamp": 0.05428777109098204, + "x": 0.9479062712871635, + "y": 7.174203380400841, + "heading": 2.326903741464538, + "velocityX": -0.005842116962103935, + "velocityY": -0.14236771854451655, + "angularVelocity": -0.5394551137625324 + }, + { + "timestamp": 0.08143165663647306, + "x": 0.9476684044775645, + "y": 7.168406760866707, + "heading": 2.312261741314886, + "velocityX": -0.008763182013873853, + "velocityY": -0.21355157589433246, + "angularVelocity": -0.5394216728884695 + }, + { + "timestamp": 0.10857554218196408, + "x": 0.9473512484922447, + "y": 7.160677934910843, + "heading": 2.2976211373138353, + "velocityX": -0.011684251497007095, + "velocityY": -0.28473543122303413, + "angularVelocity": -0.5393702377835082 + }, + { + "timestamp": 0.1357194277274551, + "x": 0.946954803210118, + "y": 7.15101690260574, + "heading": 2.282982442961913, + "velocityX": -0.014605325441050915, + "velocityY": -0.3559192838811419, + "angularVelocity": -0.5392998849548987 + }, + { + "timestamp": 0.16286331327294612, + "x": 0.9464790685095086, + "y": 7.1394236640439175, + "heading": 2.268346198502419, + "velocityX": -0.017526403867710615, + "velocityY": -0.42710313313076664, + "angularVelocity": -0.5392096291797928 + }, + { + "timestamp": 0.19000719881843714, + "x": 0.9459240442683896, + "y": 7.125898219340658, + "heading": 2.253712972744365, + "velocityX": -0.020447486789925185, + "velocityY": -0.49828697813330686, + "angularVelocity": -0.5390984180772257 + }, + { + "timestamp": 0.21715108436392816, + "x": 0.9452897303646499, + "y": 7.110440568637127, + "heading": 2.2390833650500936, + "velocityX": -0.023368574210803877, + "velocityY": -0.5694708179352157, + "angularVelocity": -0.5389651260407238 + }, + { + "timestamp": 0.24429496990941918, + "x": 0.944576126676394, + "y": 7.093050712103942, + "heading": 2.2244580075074762, + "velocityX": -0.026289666122397033, + "velocityY": -0.6406546514514866, + "angularVelocity": -0.5388085474386464 + }, + { + "timestamp": 0.2714388554549102, + "x": 0.9437832330822812, + "y": 7.073728649945259, + "heading": 2.2098375673095076, + "velocityX": -0.029210762504268687, + "velocityY": -0.7118384774464348, + "angularVelocity": -0.5386273889733045 + }, + { + "timestamp": 0.2985827410004012, + "x": 0.9429110494619096, + "y": 7.052474382403478, + "heading": 2.1952227493676237, + "velocityX": -0.0321318633218313, + "velocityY": -0.7830222945112588, + "angularVelocity": -0.5384202610705772 + }, + { + "timestamp": 0.32572662654589224, + "x": 0.9419595756962532, + "y": 7.029287909764649, + "heading": 2.180614299189169, + "velocityX": -0.03505296852439394, + "velocityY": -0.8542061010377544, + "angularVelocity": -0.5381856681484056 + }, + { + "timestamp": 0.35287051209138326, + "x": 0.9409288116681617, + "y": 7.0041692323647275, + "heading": 2.1660130060543294, + "velocityX": -0.037974078042863874, + "velocityY": -0.9253898951874127, + "angularVelocity": -0.5379219975842324 + }, + { + "timestamp": 0.3800143976368743, + "x": 0.9398187572629331, + "y": 6.9771183505968155, + "heading": 2.1514197065336718, + "velocityX": -0.04089519178702727, + "velocityY": -0.9965736748549535, + "angularVelocity": -0.5376275071669206 + }, + { + "timestamp": 0.4071582831823653, + "x": 0.9386294123689755, + "y": 6.948135264919573, + "heading": 2.13683528839439, + "velocityX": -0.04381630964231639, + "velocityY": -1.067757437625124, + "angularVelocity": -0.5373003107763752 + }, + { + "timestamp": 0.4343021687278563, + "x": 0.9373607768785728, + "y": 6.917219975867027, + "heading": 2.122260694951757, + "velocityX": -0.04673743146594764, + "velocityY": -1.1389411807213015, + "angularVelocity": -0.5369383619819672 + }, + { + "timestamp": 0.46144605427334734, + "x": 0.9360128506887763, + "y": 6.884372484060052, + "heading": 2.1076969299324073, + "velocityX": -0.04965855708228441, + "velocityY": -1.210124900944078, + "angularVelocity": -0.5365394351862701 + }, + { + "timestamp": 0.48858993981883836, + "x": 0.9345856337024482, + "y": 6.849592790219844, + "heading": 2.0931450629284045, + "velocityX": -0.05257968627724053, + "velocityY": -1.281308594597527, + "angularVelocity": -0.536101103860135 + }, + { + "timestamp": 0.5157338253643293, + "x": 0.93307912582949, + "y": 6.812880895183809, + "heading": 2.078606235536111, + "velocityX": -0.05550081879148892, + "velocityY": -1.3524922574002418, + "angularVelocity": -0.535620715314189 + }, + { + "timestamp": 0.5428777109098204, + "x": 0.9314933269882956, + "y": 6.774236799924393, + "heading": 2.0640816682923866, + "velocityX": -0.05842195431217405, + "velocityY": -1.423675884377431, + "angularVelocity": -0.5350953613244046 + }, + { + "timestamp": 0.5700215964553115, + "x": 0.9298282371074794, + "y": 6.733660505571467, + "heading": 2.049572668543588, + "velocityX": -0.06134309246273847, + "velocityY": -1.4948594697293027, + "angularVelocity": -0.5345218437673782 + }, + { + "timestamp": 0.5971654820008025, + "x": 0.928083856127948, + "y": 6.691152013439112, + "heading": 2.035080639411364, + "velocityX": -0.06426423279035642, + "velocityY": -1.566043006669555, + "angularVelocity": -0.5338966342134303 + }, + { + "timestamp": 0.6243093675462936, + "x": 0.9262601840053934, + "y": 6.646711325057805, + "heading": 2.0206070900550825, + "velocityX": -0.06718537475030886, + "velocityY": -1.637226487225917, + "angularVelocity": -0.5332158261577086 + }, + { + "timestamp": 0.6514532530917847, + "x": 0.9243572207133192, + "y": 6.6003384422133475, + "heading": 2.006153647476024, + "velocityX": -0.07010651768641886, + "velocityY": -1.7084099019921217, + "angularVelocity": -0.5324750782205918 + }, + { + "timestamp": 0.6785971386372758, + "x": 0.9223749662467348, + "y": 6.55203336699422, + "heading": 1.9917220701662, + "velocityX": -0.0730276608063702, + "velocityY": -1.779593239817202, + "angularVelocity": -0.5316695461902743 + }, + { + "timestamp": 0.7057410241827669, + "x": 0.9203134206267041, + "y": 6.501796101849591, + "heading": 1.9773142639789139, + "velocityX": -0.07594880315032046, + "velocityY": -1.8507764874131822, + "angularVelocity": -0.5307938011721701 + }, + { + "timestamp": 0.7328849097282579, + "x": 0.9181725839059847, + "y": 6.449626649660883, + "heading": 1.9629323006945676, + "velocityX": -0.07886994355064084, + "velocityY": -1.9219596288554668, + "angularVelocity": -0.5298417302942984 + }, + { + "timestamp": 0.760028795273749, + "x": 0.9159524561760831, + "y": 6.395525013830777, + "heading": 1.9485784398816015, + "velocityX": -0.08179108057979151, + "velocityY": -1.9931426449406684, + "angularVelocity": -0.5288064153120962 + }, + { + "timestamp": 0.7871726808192401, + "x": 0.9136530375761607, + "y": 6.339491198394834, + "heading": 1.9342551548200577, + "velocityX": -0.08471221248215123, + "velocityY": -2.0643255123528617, + "angularVelocity": -0.5276799829385402 + }, + { + "timestamp": 0.8143165663647312, + "x": 0.9112743283043847, + "y": 6.281525208162837, + "heading": 1.9199651634801256, + "velocityX": -0.08763333708387652, + "velocityY": -2.135508202569197, + "angularVelocity": -0.5264534186150152 + }, + { + "timestamp": 0.8414604519102222, + "x": 0.9088163286325561, + "y": 6.221627048899592, + "heading": 1.9057114658537209, + "velocityX": -0.0905544516722693, + "velocityY": -2.206690680406122, + "angularVelocity": -0.525116332461052 + }, + { + "timestamp": 0.8686043374557133, + "x": 0.9062790389251755, + "y": 6.1597967275588275, + "heading": 1.8914973893586489, + "velocityX": -0.09347555283219539, + "velocityY": -2.277872902062644, + "angularVelocity": -0.5236566618745908 + }, + { + "timestamp": 0.8957482230012044, + "x": 0.90366245966462, + "y": 6.096034252589647, + "heading": 1.8773266446252284, + "velocityX": -0.09639663622101008, + "velocityY": -2.3490548124482267, + "angularVelocity": -0.5220602890347809 + }, + { + "timestamp": 0.9228921085466955, + "x": 0.9009665914848618, + "y": 6.030339634343619, + "heading": 1.8632033948163824, + "velocityX": -0.0993176962538356, + "velocityY": -2.4202363414747605, + "angularVelocity": -0.5203105423194304 + }, + { + "timestamp": 0.9500359940921865, + "x": 0.8981914352173501, + "y": 5.962712885624053, + "heading": 1.8491323428538153, + "velocityX": -0.1022387256554861, + "velocityY": -2.4914173988181463, + "angularVelocity": -0.5183875366321403 + }, + { + "timestamp": 0.9771798796376776, + "x": 0.895336991954562, + "y": 5.893154022440242, + "heading": 1.8351188427352954, + "velocityX": -0.10515971480950784, + "velocityY": -2.5625978663679576, + "angularVelocity": -0.5162672858688164 + }, + { + "timestamp": 1.0043237651831687, + "x": 0.8924032631398172, + "y": 5.82166306506384, + "heading": 1.821169043882116, + "velocityX": -0.10808065079059799, + "velocityY": -2.6337775870955715, + "angularVelocity": -0.5139204860624911 + }, + { + "timestamp": 1.0314676507286598, + "x": 0.8893902506971634, + "y": 5.748240039542391, + "heading": 1.807290081755592, + "velocityX": -0.11100151588841312, + "velocityY": -2.704956348213193, + "angularVelocity": -0.5113108107992671 + }, + { + "timestamp": 1.0586115362741508, + "x": 0.8862979572242544, + "y": 5.672884979925212, + "heading": 1.7934903349052074, + "velocityX": -0.11392228528681338, + "velocityY": -2.776133854929808, + "angularVelocity": -0.5083924638291151 + }, + { + "timestamp": 1.085755421819642, + "x": 0.8831263862877836, + "y": 5.595597931638242, + "heading": 1.779779780164289, + "velocityX": -0.1168429232856968, + "velocityY": -2.8473096881226443, + "angularVelocity": -0.5051065632414197 + }, + { + "timestamp": 1.112899307365133, + "x": 0.8798755428928904, + "y": 5.516378956788517, + "heading": 1.7661704978047128, + "velocityX": -0.1197633768917069, + "velocityY": -2.9184832332481836, + "angularVelocity": -0.5013756168677626 + }, + { + "timestamp": 1.140043192910624, + "x": 0.876545434262605, + "y": 5.435228142869693, + "heading": 1.7526774151753128, + "velocityX": -0.12268356439614257, + "velocityY": -2.9896535550453955, + "angularVelocity": -0.4970947363732658 + }, + { + "timestamp": 1.1671870784561151, + "x": 0.8731360712040808, + "y": 5.352145617824372, + "heading": 1.739319448499104, + "velocityX": -0.1256033537573871, + "velocityY": -3.0608191633463337, + "angularVelocity": -0.49211696880329975 + }, + { + "timestamp": 1.1943309640016062, + "x": 0.86964747067212, + "y": 5.267131577887475, + "heading": 1.72612135075843, + "velocityX": -0.12852251849169202, + "velocityY": -3.1319775422136114, + "angularVelocity": -0.48622728380343705 + }, + { + "timestamp": 1.2214748495470973, + "x": 0.8660796610245576, + "y": 5.18018634364508, + "heading": 1.713116912069963, + "velocityX": -0.1314406385032288, + "velocityY": -3.20312411046225, + "angularVelocity": -0.4790927469339034 + }, + { + "timestamp": 1.2486187350925884, + "x": 0.8624326941738264, + "y": 5.0913104866410315, + "heading": 1.7003550297612484, + "velocityX": -0.1343568460239118, + "velocityY": -3.274249622630286, + "angularVelocity": -0.4701567978287094 + }, + { + "timestamp": 1.2757626206380794, + "x": 0.858706679038428, + "y": 5.000505166210723, + "heading": 1.6879128060383959, + "velocityX": -0.13726904090971828, + "velocityY": -3.3453324240598534, + "angularVelocity": -0.45838034875239686 + }, + { + "timestamp": 1.3029065061835705, + "x": 0.8549019034412932, + "y": 4.9077732872278785, + "heading": 1.6759300641529111, + "velocityX": -0.14017063219455378, + "velocityY": -3.416308207880525, + "angularVelocity": -0.4414527118968651 + }, + { + "timestamp": 1.3300503917290616, + "x": 0.8510196037686495, + "y": 4.813125903283162, + "heading": 1.6647403548593838, + "velocityX": -0.14302667413362238, + "velocityY": -3.4868767695790317, + "angularVelocity": -0.41223682861408706 + }, + { + "timestamp": 1.3571942772745527, + "x": 0.8470909786246714, + "y": 4.716731933645248, + "heading": 1.6562793310793322, + "velocityX": -0.14473333736228755, + "velocityY": -3.551222225579694, + "angularVelocity": -0.3117101185173759 + }, + { + "timestamp": 1.3843381628200437, + "x": 0.8432112361843821, + "y": 4.622095789564364, + "heading": 1.650459969800491, + "velocityX": -0.14293246388006348, + "velocityY": -3.4864626850220812, + "angularVelocity": -0.21438939790411776 + }, + { + "timestamp": 1.4114820483655348, + "x": 0.8394086254881836, + "y": 4.529373563299637, + "heading": 1.6454716067064712, + "velocityX": -0.14009087570867307, + "velocityY": -3.4159525948974805, + "angularVelocity": -0.18377483524501192 + }, + { + "timestamp": 1.438625933911026, + "x": 0.8356845251489661, + "y": 4.43857675912213, + "heading": 1.6409878614802318, + "velocityX": -0.13719849846040313, + "velocityY": -3.3450186792651584, + "angularVelocity": -0.16518435500809991 + }, + { + "timestamp": 1.465769819456517, + "x": 0.83203929287561, + "y": 4.349708572611897, + "heading": 1.6368707878531328, + "velocityX": -0.13429294296320643, + "velocityY": -3.273967036196571, + "angularVelocity": -0.15167591317034243 + }, + { + "timestamp": 1.492913705002008, + "x": 0.8284730769396671, + "y": 4.262770340533613, + "heading": 1.6330440514887725, + "velocityX": -0.13138192503686008, + "velocityY": -3.202866145769035, + "angularVelocity": -0.14097968243899844 + }, + { + "timestamp": 1.5200575905474991, + "x": 0.8249859542558534, + "y": 4.177762751738168, + "heading": 1.629459148691146, + "velocityX": -0.12846807351781545, + "velocityY": -3.1317398775859546, + "angularVelocity": -0.13207036227817726 + }, + { + "timestamp": 1.5472014760929902, + "x": 0.8215779704161437, + "y": 4.094686209630369, + "heading": 1.6260825212914087, + "velocityX": -0.1255525423580826, + "velocityY": -3.060598747683654, + "angularVelocity": -0.12439734886451492 + }, + { + "timestamp": 1.5743453616384813, + "x": 0.8182491549579317, + "y": 4.013540971880442, + "heading": 1.6228895665465475, + "velocityX": -0.12263592301980276, + "velocityY": -2.98944812502742, + "angularVelocity": -0.117630717957236 + }, + { + "timestamp": 1.6014892471839723, + "x": 0.8149995282740337, + "y": 3.934327213758722, + "heading": 1.6198614723014202, + "velocityX": -0.11971855239570846, + "velocityY": -2.9182910452876487, + "angularVelocity": -0.11155714019116433 + }, + { + "timestamp": 1.6286331327294634, + "x": 0.8118291051328418, + "y": 3.8570450603125566, + "heading": 1.616983386832351, + "velocityX": -0.1168006376935721, + "velocityY": -2.847129358714929, + "angularVelocity": -0.10603071046148076 + }, + { + "timestamp": 1.6557770182749545, + "x": 0.8087378966349528, + "y": 3.7816946041611508, + "heading": 1.6142432866844063, + "velocityX": -0.11388231403757894, + "velocityY": -2.775964259984961, + "angularVelocity": -0.10094723334125827 + }, + { + "timestamp": 1.6829209038204456, + "x": 0.8057259113762254, + "y": 3.7082759160059124, + "heading": 1.6116312386258167, + "velocityX": -0.11096367370404653, + "velocityY": -2.704796556564997, + "angularVelocity": -0.09622970352628825 + }, + { + "timestamp": 1.7100647893659366, + "x": 0.8027931561770109, + "y": 3.636789051173458, + "heading": 1.6091388979151229, + "velocityX": -0.10804478210394167, + "velocityY": -2.633626814873201, + "angularVelocity": -0.09181959990663927 + }, + { + "timestamp": 1.7372086749114277, + "x": 0.7999396365596929, + "y": 3.5672340538679626, + "heading": 1.6067591552911868, + "velocityX": -0.10512568705529957, + "velocityY": -2.562455444668251, + "angularVelocity": -0.08767140651078023 + }, + { + "timestamp": 1.7643525604569188, + "x": 0.797165357072916, + "y": 3.4996109600357874, + "heading": 1.6044858813725507, + "velocityX": -0.10220642443137137, + "velocityY": -2.491282750173872, + "angularVelocity": -0.08374902387616041 + }, + { + "timestamp": 1.7914964460024099, + "x": 0.7944703215185437, + "y": 3.4339197993548356, + "heading": 1.6023137370299276, + "velocityX": -0.09928702174409075, + "velocityY": -2.420108962324443, + "angularVelocity": -0.08002333855216454 + }, + { + "timestamp": 1.818640331547901, + "x": 0.7918545331147353, + "y": 3.3701605966526005, + "heading": 1.6002380297333685, + "velocityX": -0.09636750049753819, + "velocityY": -2.3489342598125864, + "angularVelocity": -0.07647052936048246 + }, + { + "timestamp": 1.845784217093392, + "x": 0.7893179946158024, + "y": 3.3083333729401136, + "heading": 1.5982546027353055, + "velocityX": -0.09344787778019543, + "velocityY": -2.277758783239393, + "angularVelocity": -0.07307085769809388 + }, + { + "timestamp": 1.872928102638883, + "x": 0.7868607084020597, + "y": 3.2484381461809475, + "heading": 1.5963597482134015, + "velocityX": -0.09052816737037092, + "velocityY": -2.2065826448754615, + "angularVelocity": -0.06980778484102053 + }, + { + "timestamp": 1.9000719881843742, + "x": 0.7844826765483673, + "y": 3.190474931873266, + "heading": 1.5945501382289813, + "velocityX": -0.08760838052122408, + "velocityY": -2.135405935548165, + "angularVelocity": -0.06666731560566856 + }, + { + "timestamp": 1.9272158737298652, + "x": 0.7821839008772349, + "y": 3.134443743497303, + "heading": 1.5928227691555061, + "velocityX": -0.0846885265295369, + "velocityY": -2.064228729599524, + "angularVelocity": -0.06363750210283013 + }, + { + "timestamp": 1.9543597592753563, + "x": 0.7799643830005373, + "y": 3.0803445928642126, + "heading": 1.5911749164444566, + "velocityX": -0.08176861315522627, + "velocityY": -1.9930510885195294, + "angularVelocity": -0.06070806290030923 + }, + { + "timestamp": 1.9815036448208474, + "x": 0.7778241243526975, + "y": 3.028177490391485, + "heading": 1.5896040974314454, + "velocityX": -0.07884864693575805, + "velocityY": -1.9218730636518435, + "angularVelocity": -0.05787008681509019 + }, + { + "timestamp": 2.0086475303663383, + "x": 0.7757631262173792, + "y": 2.977942445322863, + "heading": 1.5881080404719419, + "velocityX": -0.07592863342521819, + "velocityY": -1.8506946982380998, + "angularVelocity": -0.05511579972591108 + }, + { + "timestamp": 2.035791415911829, + "x": 0.7737813897491795, + "y": 2.9296394659057845, + "heading": 1.5866846591149384, + "velocityX": -0.07300857737848829, + "velocityY": -1.7795160289829102, + "angularVelocity": -0.05243837897203983 + }, + { + "timestamp": 2.06293530145732, + "x": 0.7718789159914262, + "y": 2.8832685595359084, + "heading": 1.5853320303268048, + "velocityX": -0.07008848289483084, + "velocityY": -1.7083370872663899, + "angularVelocity": -0.04983180414135624 + }, + { + "timestamp": 2.090079187002811, + "x": 0.7700557058909012, + "y": 2.8388297328758623, + "heading": 1.5840483760012258, + "velocityX": -0.06716835353105532, + "velocityY": -1.6371579000939427, + "angularVelocity": -0.047290736008748224 + }, + { + "timestamp": 2.1172230725483017, + "x": 0.7683117603101198, + "y": 2.79632299195359, + "heading": 1.5828320471578223, + "velocityX": -0.0642481923916052, + "velocityY": -1.5659784908477714, + "angularVelocity": -0.044810417483138555 + }, + { + "timestamp": 2.1443669580937925, + "x": 0.7666470800376448, + "y": 2.755748342244408, + "heading": 1.5816815103578157, + "velocityX": -0.061328002200936295, + "velocityY": -1.4947988798870493, + "angularVelocity": -0.04238659192998638 + }, + { + "timestamp": 2.1715108436392834, + "x": 0.7650616657968096, + "y": 2.7171057887399406, + "heading": 1.5805953359610199, + "velocityX": -0.05840778536216369, + "velocityY": -1.4236190850313657, + "angularVelocity": -0.04001543533550987 + }, + { + "timestamp": 2.1986547291847742, + "x": 0.7635555182531394, + "y": 2.6803953360063972, + "heading": 1.5795721879223468, + "velocityX": -0.05548754400495877, + "velocityY": -1.3524391219532605, + "angularVelocity": -0.03769349958989415 + }, + { + "timestamp": 2.225798614730265, + "x": 0.7621286380207051, + "y": 2.6456169882341385, + "heading": 1.5786108148834967, + "velocityX": -0.052567280024954206, + "velocityY": -1.2812590044993153, + "angularVelocity": -0.03541766477178523 + }, + { + "timestamp": 2.252942500275756, + "x": 0.7607810256675891, + "y": 2.612770749280059, + "heading": 1.5777100423606045, + "velocityX": -0.04964699511638458, + "velocityY": -1.2100787449546195, + "angularVelocity": -0.033185098772472256 + }, + { + "timestamp": 2.280086385821247, + "x": 0.7595126817206141, + "y": 2.5818566227040227, + "heading": 1.576868765864294, + "velocityX": -0.046726690799296755, + "velocityY": -1.1388983542620164, + "angularVelocity": -0.03099322294534166 + }, + { + "timestamp": 2.3072302713667376, + "x": 0.7583236066694515, + "y": 2.552874611800333, + "heading": 1.5760859448170281, + "velocityX": -0.04380636844236904, + "velocityY": -1.0677178422049574, + "angularVelocity": -0.02883968273286212 + }, + { + "timestamp": 2.3343741569122285, + "x": 0.7572138009702076, + "y": 2.5258247196250387, + "heading": 1.5753605971554836, + "velocityX": -0.040886029282155255, + "velocityY": -0.9965372175608755, + "angularVelocity": -0.026722322429941197 + }, + { + "timestamp": 2.3615180424577193, + "x": 0.7561832650485678, + "y": 2.5007069490197322, + "heading": 1.5746917945241172, + "velocityX": -0.037965674439399306, + "velocityY": -0.9253564882305212, + "angularVelocity": -0.024639163403798615 + }, + { + "timestamp": 2.38866192800321, + "x": 0.7552319993025618, + "y": 2.47752130263237, + "heading": 1.5740786579811215, + "velocityX": -0.03504530493293495, + "velocityY": -0.8541756613475749, + "angularVelocity": -0.022588385217395694 + }, + { + "timestamp": 2.415805813548701, + "x": 0.754360004105008, + "y": 2.4562677829355595, + "heading": 1.5735203541502267, + "velocityX": -0.03212492169158344, + "velocityY": -0.7829947433719892, + "angularVelocity": -0.020568309203988742 + }, + { + "timestamp": 2.442949699094192, + "x": 0.7535672798056785, + "y": 2.436946392242682, + "heading": 1.5730160917619267, + "velocityX": -0.02920452556438263, + "velocityY": -0.7118137401698306, + "angularVelocity": -0.01857738412054292 + }, + { + "timestamp": 2.4700935846396828, + "x": 0.7528538267332249, + "y": 2.419557132722148, + "heading": 1.5725651185360554, + "velocityX": -0.02628411732941817, + "velocityY": -0.6406326570818668, + "angularVelocity": -0.016614173572078315 + }, + { + "timestamp": 2.4972374701851736, + "x": 0.7522196451968968, + "y": 2.4041000064100535, + "heading": 1.5721667183645969, + "velocityX": -0.023363697701478097, + "velocityY": -0.5694514989827211, + "angularVelocity": -0.014677344950952577 + }, + { + "timestamp": 2.5243813557306645, + "x": 0.7516647354880791, + "y": 2.3905750152214384, + "heading": 1.5718202087594069, + "velocityX": -0.02044326733871249, + "velocityY": -0.4982702703320893, + "angularVelocity": -0.012765659677243838 + }, + { + "timestamp": 2.5515252412761553, + "x": 0.7511890978816715, + "y": 2.378982160960346, + "heading": 1.5715249385344021, + "velocityX": -0.01752282684844818, + "velocityY": -0.42708897521924366, + "angularVelocity": -0.010877964560803057 + }, + { + "timestamp": 2.578669126821646, + "x": 0.7507927326373299, + "y": 2.3693214453288225, + "heading": 1.5712802856958759, + "velocityX": -0.014602376792282998, + "velocityY": -0.35590761740183907, + "angularVelocity": -0.009013184133766632 + }, + { + "timestamp": 2.605813012367137, + "x": 0.7504756400005856, + "y": 2.361592869934995, + "heading": 1.5710856555180701, + "velocityX": -0.011681917690563183, + "velocityY": -0.2847262003398624, + "angularVelocity": -0.0071703138255794245 + }, + { + "timestamp": 2.632956897912628, + "x": 0.7502378202038584, + "y": 2.355796436300345, + "heading": 1.5709404787840837, + "velocityX": -0.008761450026330823, + "velocityY": -0.21354472722542603, + "angularVelocity": -0.005348413871837114 + }, + { + "timestamp": 2.6601007834581187, + "x": 0.7500792734673746, + "y": 2.351932145866267, + "heading": 1.5708442101747173, + "velocityX": -0.0058409742488142805, + "velocityY": -0.14236320100899352, + "angularVelocity": -0.0035466038642719827 + }, + { + "timestamp": 2.6872446690036096, + "x": 0.75, + "y": 2.35, + "heading": 1.57079632679, + "velocityX": -0.002920490776523157, + "velocityY": -0.07118162442253091, + "angularVelocity": -0.00176405786257998 + }, + { + "timestamp": 2.7143885545491004, + "x": 0.75, + "y": 2.35, + "heading": 1.57079632679, + "velocityX": -1.7174671634974605e-35, + "velocityY": 0, + "angularVelocity": 0 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "WptZeroVelocity", + "uuid": "f1ce62b9-a3c1-4d6d-8728-9901ead6eccf" + }, + { + "scope": [ + "last" + ], + "type": "WptZeroVelocity", + "uuid": "46994585-c3c8-4be5-8310-9caae164e938" + }, + { + "scope": [ + 1, + 1 + ], + "type": "ZeroAngularVelocity", + "uuid": "cafb41a4-6155-47b2-9c55-2149ac586e57" + } + ], + "usesControlIntervalGuessing": false, + "defaultControlIntervalCount": 100 + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt index aee6bf1..d044573 100644 --- a/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt +++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt @@ -37,7 +37,7 @@ class SwerveOrthogonalCommand( private var atGoal = true - private var rotRamp = SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT, RobotConstants.NEG_ROT_RATE_LIM, 0.0) + private var rotRamp = SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT) private val timer = Timer() @@ -45,6 +45,8 @@ class SwerveOrthogonalCommand( private var skewConstant = 0.5 + private var desiredVel = doubleArrayOf(0.0, 0.0, 0.0) + init { addRequirements(drive) rotCtrl.enableContinuousInput(-PI, PI) @@ -65,7 +67,7 @@ class SwerveOrthogonalCommand( rotRamp = SlewRateLimiter( RobotConstants.ROT_RATE_LIMIT, - -RobotConstants.NEG_ROT_RATE_LIM, + RobotConstants.NEG_ROT_RATE_LIM, drive.currentSpeeds.omegaRadiansPerSecond ) @@ -131,14 +133,20 @@ class SwerveOrthogonalCommand( * by rotating it with offset proportional to how much we are rotating **/ vel.rotateBy(Rotation2d(-rotScaled * dt * skewConstant)) + + val desVel = ChassisSpeeds.fromFieldRelativeSpeeds( + vel.x * directionCompensation.invoke(), + vel.y * directionCompensation.invoke(), + rotScaled, + drive.heading + ) drive.set( - ChassisSpeeds.fromFieldRelativeSpeeds( - vel.x * directionCompensation.invoke(), - vel.y * directionCompensation.invoke(), - rotScaled, - drive.heading - ) + desVel ) + + desiredVel[0] = desVel.vxMetersPerSecond + desiredVel[1] = desVel.vyMetersPerSecond + desiredVel[2] = desVel.omegaRadiansPerSecond } else { drive.set( ChassisSpeeds( @@ -168,5 +176,8 @@ class SwerveOrthogonalCommand( builder.publishConstString("4.0", "Turning Skew") builder.addDoubleProperty("4.1 skew constant", { skewConstant }, { k: Double -> skewConstant = k }) + + builder.publishConstString("5.0", "Given Speeds") + builder.addDoubleArrayProperty("Chassis Speed", { desiredVel }, null) } } diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePieceTaxi.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePieceTaxi.kt new file mode 100644 index 0000000..51fd46a --- /dev/null +++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePieceTaxi.kt @@ -0,0 +1,43 @@ +package frc.team449.robot2023.auto.routines + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import frc.team449.control.auto.ChoreoRoutine +import frc.team449.control.auto.ChoreoRoutineStructure +import frc.team449.control.auto.ChoreoTrajectory +import frc.team449.robot2023.Robot +import frc.team449.robot2023.auto.AutoUtil + +class OnePieceTaxi( + robot: Robot, + isRed: Boolean +) : ChoreoRoutineStructure { + + override val routine = + ChoreoRoutine( + drive = robot.drive, + parallelEventMap = hashMapOf( + 0 to robot.intake.extend().andThen( + robot.elevator.high() + ), + 1 to robot.elevator.stow() + ), + stopEventMap = hashMapOf( + 1 to SequentialCommandGroup( + WaitCommand(1.0), + robot.manipulator.outtake(), + WaitCommand(1.0), + robot.manipulator.stop() + ) + ) + ) + + override val trajectory: MutableList = + if (isRed) { + AutoUtil.transformForRed( + ChoreoTrajectory.createTrajectory("1PieceTaxi") + ) + } else { + ChoreoTrajectory.createTrajectory("1PieceTaxi") + } +} diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt index 7870daa..dfdeede 100644 --- a/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt +++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt @@ -11,8 +11,8 @@ class RoutineChooser(private val robot: Robot) : SendableChooser() { "DoNothing" to DoNothing(robot).createCommand(), "BlueOnePiecePick" to OnePiecePick(robot, false).createCommand(), "RedOnePiecePick" to OnePiecePick(robot, true).createCommand(), - "BlueOnePieceDrop" to OnePieceDrop(robot, false).createCommand(), - "RedOnePieceDrop" to OnePieceDrop(robot, true).createCommand() + "BlueOnePieceTaxi" to OnePieceTaxi(robot, false).createCommand(), + "RedOnePieceTaxi" to OnePieceTaxi(robot, true).createCommand() ) } @@ -34,11 +34,11 @@ class RoutineChooser(private val robot: Robot) : SendableChooser() { ) this.addOption( - "One Piece and Drop", + "One Piece and Taxi", if (isRed) { - "RedOnePieceDrop" + "RedOnePieceTaxi" } else { - "BlueOnePieceDrop" + "BlueOnePieceTaxi" } ) } diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt index e320b2b..1cf81fb 100644 --- a/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt +++ b/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt @@ -24,21 +24,21 @@ class BlairChasing( override fun execute() { for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) { // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle) - val saturation = MathUtil.inputModulus(firstSaturation1 + i * 300.0 / led.buffer.length, 0.0, 255.0) + val saturation = MathUtil.inputModulus(firstSaturation1 + i * 200.0 / led.buffer.length, 0.0, 255.0) led.setHSV(i, 0, saturation.toInt(), 255) // The i * 255.0 relates to how fast it will cycle in between the high and low intensity - firstSaturation1 += 0.0375 + firstSaturation1 += 0.135 firstSaturation1 = MathUtil.inputModulus(firstSaturation1, 0.0, 255.0) } for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) { // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle) - val saturation = MathUtil.inputModulus(firstSaturation2 + (i - LightConstants.SECTION_2_START) * 300.0 / led.buffer.length, 0.0, 255.0) + val saturation = MathUtil.inputModulus(firstSaturation2 + (i - LightConstants.SECTION_2_START) * 200.0 / led.buffer.length, 0.0, 255.0) led.setHSV(i, 0, saturation.toInt(), 255) // The i * 255.0 relates to how fast it will cycle in between the high and low intensity - firstSaturation2 += 0.0375 + firstSaturation2 += 0.135 firstSaturation2 = MathUtil.inputModulus(firstSaturation2, 0.0, 255.0) } } diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt index d81a623..4fc7a9f 100644 --- a/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt +++ b/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt @@ -26,22 +26,22 @@ class BreatheHue( override fun execute() { for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) { // This number is related to how many lights will show up between the high and low intensity - val intensity = MathUtil.inputModulus(firstIntensity1 + i * 32.5 / led.buffer.length, 175.0, 255.0) + val intensity = MathUtil.inputModulus(firstIntensity1 + i * 32.5 / led.buffer.length, 125.0, 255.0) led.setHSV(i, hue, 255, intensity.toInt()) // The i * 255.0 relates to how fast it will cycle in between the high and low intensity - firstIntensity1 += 0.045 - firstIntensity1 = MathUtil.inputModulus(firstIntensity1, 175.0, 255.0) + firstIntensity1 += 0.1 + firstIntensity1 = MathUtil.inputModulus(firstIntensity1, 125.0, 255.0) } for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) { // This number is related to how many lights will show up between the high and low intensity - val intensity = MathUtil.inputModulus(firstIntensity2 + (i - LightConstants.SECTION_2_START) * 32.5 / led.buffer.length, 175.0, 255.0) + val intensity = MathUtil.inputModulus(firstIntensity2 + (i - LightConstants.SECTION_2_START) * 32.5 / led.buffer.length, 125.0, 255.0) led.setHSV(i, hue, 255, intensity.toInt()) // The i * 255.0 relates to how fast it will cycle in between the high and low intensity - firstIntensity2 += 0.045 - firstIntensity2 = MathUtil.inputModulus(firstIntensity2, 175.0, 255.0) + firstIntensity2 += 0.1 + firstIntensity2 = MathUtil.inputModulus(firstIntensity2, 125.0, 255.0) } } } diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt index 43a04e5..7576685 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt @@ -2,10 +2,10 @@ package frc.team449.robot2023.constants.subsystem object LightConstants { const val LIGHT_PORT = 9 - const val LIGHT_LENGTH = 72 + const val LIGHT_LENGTH = 24 const val SECTION_1_START = 0 - const val SECTION_1_END = 35 - const val SECTION_2_START = 36 - const val SECTION_2_END = 71 + const val SECTION_1_END = 11 + const val SECTION_2_START = 12 + const val SECTION_2_END = 23 }