diff --git a/.gitignore b/.gitignore index 5528d4f..2b45ed8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,10 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +# Custom +src/main/deploy/ +Choreo/demos + ### C++ ### # Prerequisites *.d diff --git a/.vscode/settings.json b/.vscode/settings.json index da0bd81..ba49bd3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,5 +27,6 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "java.debug.settings.onBuildFailureProceed": true, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "java.debug.settings.showHex": false } diff --git a/AdvantageScope/assets/Robot_619/ARM-00-00.bin b/AdvantageScope/assets/Robot_619/ARM-00-00.bin new file mode 100644 index 0000000..7e66c7e Binary files /dev/null and b/AdvantageScope/assets/Robot_619/ARM-00-00.bin differ diff --git a/AdvantageScope/assets/Robot_619/ARM-00-00.gltf b/AdvantageScope/assets/Robot_619/ARM-00-00.gltf new file mode 100644 index 0000000..9f3a090 --- /dev/null +++ b/AdvantageScope/assets/Robot_619/ARM-00-00.gltf @@ -0,0 +1 @@ +{"accessors":[{"bufferView":0,"byteOffset":0,"componentType":5126,"count":20416,"max":[0.2684859375000001,0.00635,0.582464238513265],"min":[-0.2683600681181053,0.0,-0.1541357614867355],"type":"VEC3"},{"bufferView":0,"byteOffset":244992,"componentType":5126,"count":14144,"max":[0.26848593750000007,0.012699999999999996,0.582464238513265],"min":[-0.26839218163026137,0.0063500000000000019,-0.1541060885944417],"type":"VEC3"},{"bufferView":0,"byteOffset":414720,"componentType":5126,"count":1256,"max":[-0.0941964822461537,0.019049999999999999,0.582464238513265],"min":[-0.2177875375,0.012700000000000003,0.3851068625],"type":"VEC3"},{"bufferView":0,"byteOffset":429792,"componentType":5126,"count":640,"max":[0.0254,0.0508,0.012700000000000017],"min":[-0.02540000000000001,-0.05080000000000001,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":437472,"componentType":5126,"count":296,"max":[0.012699999999999989,0.05715,0.012699999999999976],"min":[-0.012699999999999989,0.0,-0.01270000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":441024,"componentType":5126,"count":11956,"max":[0.025500000000000003,0.00725,0.025499999999999889],"min":[-0.02550000000000003,-0.00725,-0.025500000000000114],"type":"VEC3"},{"bufferView":0,"byteOffset":584496,"componentType":5126,"count":11956,"max":[0.025500000000000003,0.011,0.025500000000000003],"min":[-0.025500000000000003,-0.011,-0.025500000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":727968,"componentType":5126,"count":3572,"max":[0.020258045073549496,0.006324600000000004,0.020319200606876168],"min":[-0.020258045073549514,-0.006324600000000004,-0.020319200606876168],"type":"VEC3"},{"bufferView":0,"byteOffset":770832,"componentType":5126,"count":3916,"max":[0.050799999999999987,0.0508,0.012699999999999996],"min":[-0.050799999999999987,-0.0508,-0.0127],"type":"VEC3"},{"bufferView":0,"byteOffset":817824,"componentType":5126,"count":924,"max":[0.06984999999999998,0.0,0.582464238513265],"min":[0.044449999999999979,-0.0254,0.41229279999999998],"type":"VEC3"},{"bufferView":1,"byteOffset":0,"componentType":5126,"count":20416,"type":"VEC3"},{"bufferView":1,"byteOffset":244992,"componentType":5126,"count":14144,"type":"VEC3"},{"bufferView":1,"byteOffset":414720,"componentType":5126,"count":1256,"type":"VEC3"},{"bufferView":1,"byteOffset":429792,"componentType":5126,"count":640,"type":"VEC3"},{"bufferView":1,"byteOffset":437472,"componentType":5126,"count":296,"type":"VEC3"},{"bufferView":1,"byteOffset":441024,"componentType":5126,"count":11956,"type":"VEC3"},{"bufferView":1,"byteOffset":584496,"componentType":5126,"count":11956,"type":"VEC3"},{"bufferView":1,"byteOffset":727968,"componentType":5126,"count":3572,"type":"VEC3"},{"bufferView":1,"byteOffset":770832,"componentType":5126,"count":3916,"type":"VEC3"},{"bufferView":1,"byteOffset":817824,"componentType":5126,"count":924,"type":"VEC3"},{"bufferView":2,"byteOffset":0,"componentType":5123,"count":59322,"type":"SCALAR"},{"bufferView":2,"byteOffset":118644,"componentType":5123,"count":40860,"type":"SCALAR"},{"bufferView":2,"byteOffset":200364,"componentType":5123,"count":3720,"type":"SCALAR"},{"bufferView":2,"byteOffset":207804,"componentType":5123,"count":1872,"type":"SCALAR"},{"bufferView":2,"byteOffset":211548,"componentType":5123,"count":864,"type":"SCALAR"},{"bufferView":2,"byteOffset":213276,"componentType":5123,"count":34608,"type":"SCALAR"},{"bufferView":2,"byteOffset":282492,"componentType":5123,"count":34608,"type":"SCALAR"},{"bufferView":2,"byteOffset":351708,"componentType":5123,"count":10296,"type":"SCALAR"},{"bufferView":2,"byteOffset":372300,"componentType":5123,"count":11064,"type":"SCALAR"},{"bufferView":2,"byteOffset":394428,"componentType":5123,"count":2724,"type":"SCALAR"}],"asset":{"generator":"Open CASCADE Technology 7.6 [dev.opencascade.org]","version":"2.0","extras":{"Author":"","Originating system":"SolidWorks 2024","Organization":"","Comments":"STEP AP214"}},"bufferViews":[{"buffer":0,"byteLength":828912,"byteOffset":0,"byteStride":12,"target":34962},{"buffer":0,"byteLength":828912,"byteOffset":828912,"byteStride":12,"target":34962},{"buffer":0,"byteLength":399876,"byteOffset":1657824,"target":34963}],"buffers":[{"byteLength":2057700,"uri":"ARM-00-00.bin"}],"materials":[{"name":"mat_0","pbrMetallicRoughness":{"baseColorFactor":[0.5906188488006592,0.637596845626831,0.8549926280975342,1.0],"roughnessFactor":0.2121320366859436},"doubleSided":true}],"meshes":[{"name":"ARM-00-01","primitives":[{"attributes":{"NORMAL":10,"POSITION":0},"indices":20,"material":0,"mode":4}]},{"name":"ARM-00-02","primitives":[{"attributes":{"NORMAL":11,"POSITION":1},"indices":21,"material":0,"mode":4}]},{"name":"ARM-00-03","primitives":[{"attributes":{"NORMAL":12,"POSITION":2},"indices":22,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":13,"POSITION":3},"indices":23,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":13,"POSITION":3},"indices":23,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":13,"POSITION":3},"indices":23,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":13,"POSITION":3},"indices":23,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":14,"POSITION":4},"indices":24,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":14,"POSITION":4},"indices":24,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":14,"POSITION":4},"indices":24,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":14,"POSITION":4},"indices":24,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":15,"POSITION":5},"indices":25,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":16,"POSITION":6},"indices":26,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":17,"POSITION":7},"indices":27,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":17,"POSITION":7},"indices":27,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":17,"POSITION":7},"indices":27,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":17,"POSITION":7},"indices":27,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":18,"POSITION":8},"indices":28,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":19,"POSITION":9},"indices":29,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":19,"POSITION":9},"indices":29,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":19,"POSITION":9},"indices":29,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":19,"POSITION":9},"indices":29,"material":0,"mode":4}]}],"nodes":[{"children":[1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39],"name":"ARM-00-00"},{"mesh":0,"name":"NAUO1"},{"translation":[-1.212830901789648e-15,0.05715,4.9786563760534359e-15],"mesh":1,"name":"NAUO2"},{"translation":[2.9514585220269399e-14,0.08889999999999988,1.2578046243438262e-14],"mesh":2,"name":"NAUO3"},{"rotation":[-0.49999999999999836,-0.5,0.49999999999999836,0.5000000000000036],"translation":[-0.1779015999999993,0.03213242365947713,0.10624820000000111],"mesh":3,"name":"NAUO4"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[0.1779015999999975,0.03304822263134287,0.30624780000000159],"mesh":4,"name":"NAUO5"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[-0.17790160000000033,0.03185153328977558,0.3062477999999995],"mesh":5,"name":"NAUO6"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[0.17790160000000003,0.03174999999999936,0.10624820000000059],"mesh":6,"name":"NAUO7"},{"rotation":[7.571150543111076e-17,0.8715956043360786,-1.1750589177415768e-16,-0.4902255628810333],"translation":[-0.1923875375,0.006350000000000009,0.4745142385132649],"mesh":7,"name":"NAUO8"},{"rotation":[0.0,0.3219675184639639,0.0,0.9467507153702907],"translation":[0.19238753749999996,0.0063500000000000019,0.4745142385132649],"mesh":8,"name":"NAUO9"},{"rotation":[0.0,0.6344455211665221,0.0,0.7729675806084887],"translation":[-0.19812017549278602,0.0063500000000000019,-0.04556295053321841],"mesh":9,"name":"NAUO10"},{"rotation":[0.0,0.16087598606864818,0.0,0.9869746284005684],"translation":[0.19812017549278605,0.006350000000000008,-0.04556295053321853],"mesh":10,"name":"NAUO11"},{"rotation":[0.0,-0.0997365048088287,0.0,0.9950138841285176],"translation":[-0.13408241974616215,0.08027500000000001,0.5204532085796465],"mesh":11,"name":"NAUO12"},{"rotation":[0.0,0.14502937294898669,0.0,0.9894273500272892],"translation":[-0.14638547383637558,0.08027500000000001,0.4464017652164423],"mesh":12,"name":"NAUO13"},{"translation":[-0.2286,0.05317499999999999,-0.1142498239867352],"mesh":13,"name":"NAUO14"},{"translation":[0.22860000000000139,0.05317499999999999,-0.11424982398672599],"mesh":14,"name":"NAUO15"},{"translation":[0.16509999999999146,0.08027500000000004,0.5425783010132701],"mesh":15,"name":"NAUO16"},{"translation":[-0.17790160000000056,0.053175,2.220446049250313e-15],"mesh":16,"name":"NAUO17"},{"translation":[0.17790159999999875,0.05317499999999999,9.441556214278942e-15],"mesh":17,"name":"NAUO18"},{"rotation":[0.0,-0.673510355071642,0.0,0.739177787552677],"translation":[0.1779015999999932,0.08027500000000004,0.42499280000000547],"mesh":18,"name":"NAUO19"},{"translation":[-0.17790159999999997,0.034925,0.21249640000000004],"mesh":19,"name":"NAUO20"},{"translation":[0.1779015999999944,0.034925,0.21249640000000809],"mesh":20,"name":"NAUO21"},{"rotation":[0.0,-0.018098543678665165,0.0,0.999836207944437],"translation":[0.17790159999999323,0.034925,0.42499280000000558],"mesh":21,"name":"NAUO22"},{"translation":[-0.17790160000000053,0.034925,2.220446049250313e-15],"mesh":22,"name":"NAUO23"},{"translation":[-0.17790159999999997,0.034925,0.42499280000000008],"mesh":23,"name":"NAUO24"},{"translation":[0.17790159999999875,0.034925,9.441556214278942e-15],"mesh":24,"name":"NAUO25"},{"translation":[-0.14638547383637558,0.09527540000000001,0.44640176521644228],"mesh":25,"name":"NAUO26"},{"translation":[-0.1779016000000085,0.09527540000000001,0.4249928000000005],"mesh":26,"name":"NAUO27"},{"rotation":[0.0,-0.2777037341443709,0.0,0.9606667663879461],"translation":[-0.16510000000000875,0.09527540000000001,0.5425783010132659],"mesh":27,"name":"NAUO28"},{"rotation":[0.0,0.881785320353058,0.0,-0.47165098198758678],"translation":[-0.1340824197461621,0.09527540000000001,0.5204532085796465],"mesh":28,"name":"NAUO29"},{"rotation":[-0.7071067811865475,0.0,0.0,0.7071067811865475],"translation":[0.2286,0.03322499999999999,-0.1142498239867354],"mesh":29,"name":"NAUO30"},{"rotation":[0.7023653799149907,-0.08174884156286734,0.08174884156286734,0.7023653799149907],"translation":[0.1651,0.022224999999999999,0.542578301013265],"mesh":30,"name":"NAUO31"},{"rotation":[-0.7071067811865475,0.0,0.0,0.7071067811865475],"translation":[0.1651,0.047625,0.542578301013265],"mesh":31,"name":"NAUO32"},{"rotation":[-0.7071067811865475,0.0,0.0,0.7071067811865475],"translation":[-0.2286,0.03322499999999999,-0.1142498239867352],"mesh":32,"name":"NAUO33"},{"rotation":[0.7023653799149907,-0.08174884156286734,0.08174884156286734,0.7023653799149907],"translation":[-0.16510000000000853,0.022224999999999989,0.5425783010132653],"mesh":33,"name":"NAUO34"},{"rotation":[0.7023653799149907,-0.08174884156286734,0.08174884156286734,0.7023653799149907],"translation":[-0.16510000000000853,0.047624999999999997,0.5425783010132653],"mesh":34,"name":"NAUO35"},{"mesh":35,"name":"NAUO36"},{"translation":[-0.08254999999999999,0.0,0.0],"mesh":36,"name":"NAUO37"},{"rotation":[0.0,1.0,0.0,-3.4914813388431335e-15],"translation":[3.989863994746656e-15,0.0,0.994757038513264],"mesh":37,"name":"NAUO38"},{"rotation":[0.0,1.0,0.0,-3.4914813388431335e-15],"translation":[0.08255000000000296,0.0,0.994757038513264],"mesh":38,"name":"NAUO39"}],"scene":0,"scenes":[{"nodes":[0]}]} \ No newline at end of file diff --git a/AdvantageScope/assets/Robot_619/RBT-00-00.bin b/AdvantageScope/assets/Robot_619/RBT-00-00.bin new file mode 100644 index 0000000..8972d26 Binary files /dev/null and b/AdvantageScope/assets/Robot_619/RBT-00-00.bin differ diff --git a/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.bin b/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.bin new file mode 100644 index 0000000..5f889fc Binary files /dev/null and b/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.bin differ diff --git a/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.gltf b/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.gltf new file mode 100644 index 0000000..c6e62e9 --- /dev/null +++ b/AdvantageScope/assets/Robot_619/RBT-00-00_no_arm.gltf @@ -0,0 +1 @@ +{"accessors":[{"bufferView":0,"byteOffset":0,"componentType":5126,"count":163533,"max":[0.0,0.0,-0.0006558304516347846],"min":[-0.19434485504900027,-0.19429796936844088,-0.17286604999999997],"type":"VEC3"},{"bufferView":0,"byteOffset":1962396,"componentType":5126,"count":5376,"max":[0.012699999999999989,0.45085,0.025399999999999993],"min":[-0.012699999999999989,0.0063499999999999949,-0.025400000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":2026908,"componentType":5126,"count":60,"max":[0.0762,0.090551,0.08343900000000002],"min":[0.0,-0.090551,-0.08343900000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2027628,"componentType":5126,"count":622,"max":[0.038099999999999998,0.012699999999999989,0.0],"min":[-0.03810000000000001,-0.012699999999999989,-0.00635],"type":"VEC3"},{"bufferView":0,"byteOffset":2035092,"componentType":5126,"count":776,"max":[0.7429499999999999,0.41275,0.0],"min":[-0.08255000000000001,-5.551115123125783e-17,-0.127],"type":"VEC3"},{"bufferView":0,"byteOffset":2044404,"componentType":5126,"count":8928,"max":[0.012700000000000007,0.6604,0.025399999999999993],"min":[-0.012700000000000003,0.0,-0.02540000000000002],"type":"VEC3"},{"bufferView":0,"byteOffset":2151540,"componentType":5126,"count":518,"max":[0.0,0.17145,3.552713678800501e-18],"min":[-0.025400000000000007,0.0,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2157756,"componentType":5126,"count":374,"max":[0.2413,0.0,0.0],"min":[0.0,-0.025400000000000007,-0.025399999999999993],"type":"VEC3"},{"bufferView":0,"byteOffset":2162244,"componentType":5126,"count":628,"max":[0.025399999999999979,0.0381,0.0],"min":[0.0,-0.0381,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2169780,"componentType":5126,"count":480,"max":[0.0254,0.0,0.0],"min":[-0.0254,-0.025399999999999979,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2175540,"componentType":5126,"count":2120,"max":[0.2921,0.025400000000000007,0.012700000000000017],"min":[-0.2921,-0.025400000000000007,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2200980,"componentType":5126,"count":2120,"max":[0.2921,0.025400000000000007,0.012700000000000017],"min":[-0.29209999999999999,-0.025400000000000007,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2226420,"componentType":5126,"count":1232,"max":[0.3556,0.0254,0.012700000000000017],"min":[-0.05080000000000001,-0.5080000000000001,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2241204,"componentType":5126,"count":1232,"max":[0.3556,0.0254,0.012700000000000047],"min":[-0.05080000000000001,-0.5080000000000001,-0.012700000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2255988,"componentType":5126,"count":1342,"max":[0.015557500000000033,0.015557499999999976,1.4210854715202005e-17],"min":[-0.015557499999999992,-0.015557500000000006,-0.007937500000000014],"type":"VEC3"},{"bufferView":0,"byteOffset":2272092,"componentType":5126,"count":194,"max":[0.007239095848374177,0.025400000000000007,0.006337300000000014],"min":[-0.007239095848374177,-0.22860000000000003,-0.0063373000000000278],"type":"VEC3"},{"bufferView":0,"byteOffset":2274420,"componentType":5126,"count":5292,"max":[0.023394639261530644,0.006286500000000004,0.023854228666702824],"min":[-0.023394639261530644,-0.006286500000000018,-0.023854228666702824],"type":"VEC3"},{"bufferView":0,"byteOffset":2337924,"componentType":5126,"count":72,"max":[0.0146646968374165,0.012699999999999989,1.4210854715202005e-17],"min":[-0.014664696837416528,-0.012700000000000017,-0.15976600000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2338788,"componentType":5126,"count":72,"max":[0.0146646968374165,0.01270000000000003,0.0],"min":[-0.0146646968374165,-0.012700000000000017,-0.009144000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":2339652,"componentType":5126,"count":49220,"max":[0.0572002,0.021,0.0],"min":[-0.03476400000000004,-0.02100000000000003,-0.04445000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2930292,"componentType":5126,"count":5689,"max":[0.032,0.032,0.011525000000000006],"min":[-0.03199999999999999,-0.032,-0.009825000000000018],"type":"VEC3"},{"bufferView":0,"byteOffset":2998560,"componentType":5126,"count":16435,"max":[0.029991120151092136,0.03500000000000003,0.029991120151092164],"min":[-0.03814108252475668,-0.05835000000000003,-0.03802030086559549],"type":"VEC3"},{"bufferView":0,"byteOffset":3195780,"componentType":5126,"count":1904,"max":[0.08890000000000004,9.769962616701377e-18,2.842170943040401e-17],"min":[-0.08890000000000002,-0.10159999999999998,-0.0031750000000000117],"type":"VEC3"},{"bufferView":0,"byteOffset":3218628,"componentType":5126,"count":936,"max":[0.012700000000000003,0.044449999999999999,0.025400000000000007],"min":[-0.012700000000000017,-0.04445,-0.025400000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":3229860,"componentType":5126,"count":36,"max":[0.127,0.0073323484187083069,0.006350000000000051],"min":[-0.127,-0.0073323484187083069,-0.006350000000000023],"type":"VEC3"},{"bufferView":0,"byteOffset":3230292,"componentType":5126,"count":16684,"max":[0.09982888152266571,0.0019939000000000108,0.09982873372984733],"min":[-0.09982888152266571,-0.0019939000000000108,-0.09982873372984727],"type":"VEC3"},{"bufferView":0,"byteOffset":3430500,"componentType":5126,"count":3002,"max":[0.058528594780987878,0.017462500000000007,-0.0000071761424943588289],"min":[0.0,-0.007937500000000014,-0.05215928371273964],"type":"VEC3"},{"bufferView":0,"byteOffset":3466524,"componentType":5126,"count":1510,"max":[0.06348890832287806,0.0015875000000000058,0.14561940674229144],"min":[-0.0635,-0.0015875000000000058,0.0],"type":"VEC3"},{"bufferView":0,"byteOffset":3484644,"componentType":5126,"count":72,"max":[0.013335000000000009,0.0146646968374165,0.012699999999999989],"min":[-0.013335000000000009,-0.014664696837416613,-0.012700000000000074],"type":"VEC3"},{"bufferView":0,"byteOffset":3485508,"componentType":5126,"count":72,"max":[0.0238125,0.014664696837416613,0.012700000000000047],"min":[-0.023812500000000009,-0.014664696837416443,-0.012700000000000047],"type":"VEC3"},{"bufferView":0,"byteOffset":3486372,"componentType":5126,"count":2416,"max":[0.025400000000000035,0.1016,0.0127],"min":[-0.025399999999999979,-0.1016,-0.012700000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3515364,"componentType":5126,"count":3150,"max":[0.047625,0.01976755020390027,0.019767550000000003],"min":[0.0,-0.01976754999999997,-0.019767550000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3553164,"componentType":5126,"count":1548,"max":[0.38959898794721339,0.01976755000000003,0.01976755000000003],"min":[-0.0009154879472028484,-0.01976755000000003,-0.01976755000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3571740,"componentType":5126,"count":2212,"max":[0.04445,0.019767550000000003,0.01976754999999997],"min":[0.0,-0.019767550000000003,-0.01976754999999997],"type":"VEC3"},{"bufferView":0,"byteOffset":3598284,"componentType":5126,"count":304,"max":[0.004762499999999999,0.012700000000000047,0.004746233703793023],"min":[-0.004762499999999999,-0.012700000000000047,-0.0047625],"type":"VEC3"},{"bufferView":0,"byteOffset":3601932,"componentType":5126,"count":634,"max":[0.4255769999999999,0.019145249999999976,0.019145250000000004],"min":[-0.025399999999999979,-0.019145250000000035,-0.019145250000000004],"type":"VEC3"},{"bufferView":0,"byteOffset":3609540,"componentType":5126,"count":1384,"max":[0.1016,4.0389678347315809e-31,0.0005855214026497038],"min":[0.0,-0.10160000000000001,-0.0031750000000000686],"type":"VEC3"},{"bufferView":0,"byteOffset":3626148,"componentType":5126,"count":48,"max":[0.02540000000000001,0.012700000000000047,0.254],"min":[-0.02539999999999999,-0.012700000000000017,-0.25400000000000008],"type":"VEC3"},{"bufferView":0,"byteOffset":3626724,"componentType":5126,"count":184,"max":[0.025400000000000035,0.0254,0.012700000000000047],"min":[-0.025400000000000035,-0.025400000000000004,-0.012699999999999989],"type":"VEC3"},{"bufferView":1,"byteOffset":0,"componentType":5126,"count":163533,"type":"VEC3"},{"bufferView":1,"byteOffset":1962396,"componentType":5126,"count":5376,"type":"VEC3"},{"bufferView":1,"byteOffset":2026908,"componentType":5126,"count":60,"type":"VEC3"},{"bufferView":1,"byteOffset":2027628,"componentType":5126,"count":622,"type":"VEC3"},{"bufferView":1,"byteOffset":2035092,"componentType":5126,"count":776,"type":"VEC3"},{"bufferView":1,"byteOffset":2044404,"componentType":5126,"count":8928,"type":"VEC3"},{"bufferView":1,"byteOffset":2151540,"componentType":5126,"count":518,"type":"VEC3"},{"bufferView":1,"byteOffset":2157756,"componentType":5126,"count":374,"type":"VEC3"},{"bufferView":1,"byteOffset":2162244,"componentType":5126,"count":628,"type":"VEC3"},{"bufferView":1,"byteOffset":2169780,"componentType":5126,"count":480,"type":"VEC3"},{"bufferView":1,"byteOffset":2175540,"componentType":5126,"count":2120,"type":"VEC3"},{"bufferView":1,"byteOffset":2200980,"componentType":5126,"count":2120,"type":"VEC3"},{"bufferView":1,"byteOffset":2226420,"componentType":5126,"count":1232,"type":"VEC3"},{"bufferView":1,"byteOffset":2241204,"componentType":5126,"count":1232,"type":"VEC3"},{"bufferView":1,"byteOffset":2255988,"componentType":5126,"count":1342,"type":"VEC3"},{"bufferView":1,"byteOffset":2272092,"componentType":5126,"count":194,"type":"VEC3"},{"bufferView":1,"byteOffset":2274420,"componentType":5126,"count":5292,"type":"VEC3"},{"bufferView":1,"byteOffset":2337924,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":2338788,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":2339652,"componentType":5126,"count":49220,"type":"VEC3"},{"bufferView":1,"byteOffset":2930292,"componentType":5126,"count":5689,"type":"VEC3"},{"bufferView":1,"byteOffset":2998560,"componentType":5126,"count":16435,"type":"VEC3"},{"bufferView":1,"byteOffset":3195780,"componentType":5126,"count":1904,"type":"VEC3"},{"bufferView":1,"byteOffset":3218628,"componentType":5126,"count":936,"type":"VEC3"},{"bufferView":1,"byteOffset":3229860,"componentType":5126,"count":36,"type":"VEC3"},{"bufferView":1,"byteOffset":3230292,"componentType":5126,"count":16684,"type":"VEC3"},{"bufferView":1,"byteOffset":3430500,"componentType":5126,"count":3002,"type":"VEC3"},{"bufferView":1,"byteOffset":3466524,"componentType":5126,"count":1510,"type":"VEC3"},{"bufferView":1,"byteOffset":3484644,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":3485508,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":3486372,"componentType":5126,"count":2416,"type":"VEC3"},{"bufferView":1,"byteOffset":3515364,"componentType":5126,"count":3150,"type":"VEC3"},{"bufferView":1,"byteOffset":3553164,"componentType":5126,"count":1548,"type":"VEC3"},{"bufferView":1,"byteOffset":3571740,"componentType":5126,"count":2212,"type":"VEC3"},{"bufferView":1,"byteOffset":3598284,"componentType":5126,"count":304,"type":"VEC3"},{"bufferView":1,"byteOffset":3601932,"componentType":5126,"count":634,"type":"VEC3"},{"bufferView":1,"byteOffset":3609540,"componentType":5126,"count":1384,"type":"VEC3"},{"bufferView":1,"byteOffset":3626148,"componentType":5126,"count":48,"type":"VEC3"},{"bufferView":1,"byteOffset":3626724,"componentType":5126,"count":184,"type":"VEC3"},{"bufferView":2,"byteOffset":0,"componentType":5125,"count":490617,"type":"SCALAR"},{"bufferView":2,"byteOffset":1962468,"componentType":5123,"count":16080,"type":"SCALAR"},{"bufferView":2,"byteOffset":1994628,"componentType":5123,"count":108,"type":"SCALAR"},{"bufferView":2,"byteOffset":1994844,"componentType":5123,"count":1806,"type":"SCALAR"},{"bufferView":2,"byteOffset":1998456,"componentType":5123,"count":2916,"type":"SCALAR"},{"bufferView":2,"byteOffset":2004288,"componentType":5123,"count":26736,"type":"SCALAR"},{"bufferView":2,"byteOffset":2057760,"componentType":5123,"count":1500,"type":"SCALAR"},{"bufferView":2,"byteOffset":2060760,"componentType":5123,"count":1068,"type":"SCALAR"},{"bufferView":2,"byteOffset":2062896,"componentType":5123,"count":1836,"type":"SCALAR"},{"bufferView":2,"byteOffset":2066568,"componentType":5123,"count":1392,"type":"SCALAR"},{"bufferView":2,"byteOffset":2069352,"componentType":5123,"count":6312,"type":"SCALAR"},{"bufferView":2,"byteOffset":2081976,"componentType":5123,"count":6312,"type":"SCALAR"},{"bufferView":2,"byteOffset":2094600,"componentType":5123,"count":3648,"type":"SCALAR"},{"bufferView":2,"byteOffset":2101896,"componentType":5123,"count":3648,"type":"SCALAR"},{"bufferView":2,"byteOffset":2109192,"componentType":5123,"count":4392,"type":"SCALAR"},{"bufferView":2,"byteOffset":2117976,"componentType":5123,"count":492,"type":"SCALAR"},{"bufferView":2,"byteOffset":2118960,"componentType":5123,"count":13716,"type":"SCALAR"},{"bufferView":2,"byteOffset":2146392,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2146680,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2146968,"componentType":5123,"count":178920,"type":"SCALAR"},{"bufferView":2,"byteOffset":2504808,"componentType":5123,"count":20532,"type":"SCALAR"},{"bufferView":2,"byteOffset":2545872,"componentType":5123,"count":44766,"type":"SCALAR"},{"bufferView":2,"byteOffset":2635404,"componentType":5123,"count":5592,"type":"SCALAR"},{"bufferView":2,"byteOffset":2646588,"componentType":5123,"count":2760,"type":"SCALAR"},{"bufferView":2,"byteOffset":2652108,"componentType":5123,"count":60,"type":"SCALAR"},{"bufferView":2,"byteOffset":2652228,"componentType":5123,"count":44646,"type":"SCALAR"},{"bufferView":2,"byteOffset":2741520,"componentType":5123,"count":8604,"type":"SCALAR"},{"bufferView":2,"byteOffset":2758728,"componentType":5123,"count":4476,"type":"SCALAR"},{"bufferView":2,"byteOffset":2767680,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2767968,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2768256,"componentType":5123,"count":7200,"type":"SCALAR"},{"bufferView":2,"byteOffset":2782656,"componentType":5123,"count":11412,"type":"SCALAR"},{"bufferView":2,"byteOffset":2805480,"componentType":5123,"count":5772,"type":"SCALAR"},{"bufferView":2,"byteOffset":2817024,"componentType":5123,"count":6984,"type":"SCALAR"},{"bufferView":2,"byteOffset":2830992,"componentType":5123,"count":864,"type":"SCALAR"},{"bufferView":2,"byteOffset":2832720,"componentType":5123,"count":1818,"type":"SCALAR"},{"bufferView":2,"byteOffset":2836356,"componentType":5123,"count":4056,"type":"SCALAR"},{"bufferView":2,"byteOffset":2844468,"componentType":5123,"count":96,"type":"SCALAR"},{"bufferView":2,"byteOffset":2844660,"componentType":5123,"count":504,"type":"SCALAR"}],"asset":{"generator":"Open CASCADE Technology 7.6 [dev.opencascade.org]","version":"2.0","extras":{"Author":"","Originating system":"SolidWorks 2024","Organization":"","Comments":"STEP AP214"}},"bufferViews":[{"buffer":0,"byteLength":3628932,"byteOffset":0,"byteStride":12,"target":34962},{"buffer":0,"byteLength":3628932,"byteOffset":3628932,"byteStride":12,"target":34962},{"buffer":0,"byteLength":2845668,"byteOffset":7257864,"target":34963}],"buffers":[{"byteLength":10103532,"uri":"RBT-00-00_no_arm.bin"}],"materials":[{"name":"mat_0","pbrMetallicRoughness":{"baseColorFactor":[0.5906188488006592,0.637596845626831,0.8549926280975342,1.0],"roughnessFactor":0.2121320366859436},"doubleSided":true}],"meshes":[{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":39,"POSITION":0},"indices":78,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":39,"POSITION":0},"indices":78,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":39,"POSITION":0},"indices":78,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":39,"POSITION":0},"indices":78,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":40,"POSITION":1},"indices":79,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":40,"POSITION":1},"indices":79,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":40,"POSITION":1},"indices":79,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":40,"POSITION":1},"indices":79,"material":0,"mode":4}]},{"name":"12Vbattery","primitives":[{"attributes":{"NORMAL":41,"POSITION":2},"indices":80,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":42,"POSITION":3},"indices":81,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":42,"POSITION":3},"indices":81,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":42,"POSITION":3},"indices":81,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":42,"POSITION":3},"indices":81,"material":0,"mode":4}]},{"name":"CHS-00-06","primitives":[{"attributes":{"NORMAL":43,"POSITION":4},"indices":82,"material":0,"mode":4}]},{"name":"CHS-00-06","primitives":[{"attributes":{"NORMAL":43,"POSITION":4},"indices":82,"material":0,"mode":4}]},{"name":"CHS-00-09","primitives":[{"attributes":{"NORMAL":44,"POSITION":5},"indices":83,"material":0,"mode":4}]},{"name":"CHS-00-10","primitives":[{"attributes":{"NORMAL":45,"POSITION":6},"indices":84,"material":0,"mode":4}]},{"name":"CHS-00-10","primitives":[{"attributes":{"NORMAL":45,"POSITION":6},"indices":84,"material":0,"mode":4}]},{"name":"CHS-00-11","primitives":[{"attributes":{"NORMAL":46,"POSITION":7},"indices":85,"material":0,"mode":4}]},{"name":"CHS-00-11","primitives":[{"attributes":{"NORMAL":46,"POSITION":7},"indices":85,"material":0,"mode":4}]},{"name":"CHS-00-09","primitives":[{"attributes":{"NORMAL":44,"POSITION":5},"indices":83,"material":0,"mode":4}]},{"name":"CHS-00-12","primitives":[{"attributes":{"NORMAL":47,"POSITION":8},"indices":86,"material":0,"mode":4}]},{"name":"CHS-00-12","primitives":[{"attributes":{"NORMAL":47,"POSITION":8},"indices":86,"material":0,"mode":4}]},{"name":"CHS-00-13","primitives":[{"attributes":{"NORMAL":48,"POSITION":9},"indices":87,"material":0,"mode":4}]},{"name":"CHS-00-13","primitives":[{"attributes":{"NORMAL":48,"POSITION":9},"indices":87,"material":0,"mode":4}]},{"name":"STR-00-01","primitives":[{"attributes":{"NORMAL":49,"POSITION":10},"indices":88,"material":0,"mode":4}]},{"name":"STR-00-02","primitives":[{"attributes":{"NORMAL":50,"POSITION":11},"indices":89,"material":0,"mode":4}]},{"name":"STR-00-04","primitives":[{"attributes":{"NORMAL":51,"POSITION":12},"indices":90,"material":0,"mode":4}]},{"name":"STR-00-06","primitives":[{"attributes":{"NORMAL":52,"POSITION":13},"indices":91,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":53,"POSITION":14},"indices":92,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":53,"POSITION":14},"indices":92,"material":0,"mode":4}]},{"name":"STR-04-01","primitives":[{"attributes":{"NORMAL":54,"POSITION":15},"indices":93,"material":0,"mode":4}]},{"name":"STR-04-03","primitives":[{"attributes":{"NORMAL":55,"POSITION":16},"indices":94,"material":0,"mode":4}]},{"name":"STR-04-03","primitives":[{"attributes":{"NORMAL":55,"POSITION":16},"indices":94,"material":0,"mode":4}]},{"name":"STR-04-04","primitives":[{"attributes":{"NORMAL":56,"POSITION":17},"indices":95,"material":0,"mode":4}]},{"name":"STR-04-05","primitives":[{"attributes":{"NORMAL":57,"POSITION":18},"indices":96,"material":0,"mode":4}]},{"name":"STR-04-05","primitives":[{"attributes":{"NORMAL":57,"POSITION":18},"indices":96,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":53,"POSITION":14},"indices":92,"material":0,"mode":4}]},{"name":"STR-05-01","primitives":[{"attributes":{"NORMAL":58,"POSITION":19},"indices":97,"material":0,"mode":4}]},{"name":"STR-05-02","primitives":[{"attributes":{"NORMAL":59,"POSITION":20},"indices":98,"material":0,"mode":4}]},{"name":"STR-05-03","primitives":[{"attributes":{"NORMAL":60,"POSITION":21},"indices":99,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":61,"POSITION":22},"indices":100,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":61,"POSITION":22},"indices":100,"material":0,"mode":4}]},{"name":"STR-00-08","primitives":[{"attributes":{"NORMAL":62,"POSITION":23},"indices":101,"material":0,"mode":4}]},{"name":"STR-00-08","primitives":[{"attributes":{"NORMAL":62,"POSITION":23},"indices":101,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":61,"POSITION":22},"indices":100,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":61,"POSITION":22},"indices":100,"material":0,"mode":4}]},{"name":"STR-05-01","primitives":[{"attributes":{"NORMAL":58,"POSITION":19},"indices":97,"material":0,"mode":4}]},{"name":"STR-05-02","primitives":[{"attributes":{"NORMAL":59,"POSITION":20},"indices":98,"material":0,"mode":4}]},{"name":"STR-05-03","primitives":[{"attributes":{"NORMAL":60,"POSITION":21},"indices":99,"material":0,"mode":4}]},{"name":"STR-03-01","primitives":[{"attributes":{"NORMAL":63,"POSITION":24},"indices":102,"material":0,"mode":4}]},{"name":"STR-02-01","primitives":[{"attributes":{"NORMAL":64,"POSITION":25},"indices":103,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":65,"POSITION":26},"indices":104,"material":0,"mode":4}]},{"name":"STR-02-01","primitives":[{"attributes":{"NORMAL":64,"POSITION":25},"indices":103,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":65,"POSITION":26},"indices":104,"material":0,"mode":4}]},{"name":"str-00-05","primitives":[{"attributes":{"NORMAL":66,"POSITION":27},"indices":105,"material":0,"mode":4}]},{"name":"str-00-05","primitives":[{"attributes":{"NORMAL":66,"POSITION":27},"indices":105,"material":0,"mode":4}]},{"name":"STR-03-03","primitives":[{"attributes":{"NORMAL":67,"POSITION":28},"indices":106,"material":0,"mode":4}]},{"name":"STR-03-03","primitives":[{"attributes":{"NORMAL":67,"POSITION":28},"indices":106,"material":0,"mode":4}]},{"name":"STR-03-04","primitives":[{"attributes":{"NORMAL":68,"POSITION":29},"indices":107,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":65,"POSITION":26},"indices":104,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":65,"POSITION":26},"indices":104,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":53,"POSITION":14},"indices":92,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":53,"POSITION":14},"indices":92,"material":0,"mode":4}]},{"name":"hng-00-01","primitives":[{"attributes":{"NORMAL":69,"POSITION":30},"indices":108,"material":0,"mode":4}]},{"name":"hng-00-01","primitives":[{"attributes":{"NORMAL":69,"POSITION":30},"indices":108,"material":0,"mode":4}]},{"name":"17R000-0_rod_guide","primitives":[{"attributes":{"NORMAL":70,"POSITION":31},"indices":109,"material":0,"mode":4}]},{"name":"17R-15.30250SR","primitives":[{"attributes":{"NORMAL":71,"POSITION":32},"indices":110,"material":0,"mode":4}]},{"name":"17P0000_rear_head","primitives":[{"attributes":{"NORMAL":72,"POSITION":33},"indices":111,"material":0,"mode":4}]},{"name":"D-358","primitives":[{"attributes":{"NORMAL":73,"POSITION":34},"indices":112,"material":0,"mode":4}]},{"name":"SR-1714-DP0000_rod","primitives":[{"attributes":{"NORMAL":74,"POSITION":35},"indices":113,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":75,"POSITION":36},"indices":114,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":75,"POSITION":36},"indices":114,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":75,"POSITION":36},"indices":114,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":75,"POSITION":36},"indices":114,"material":0,"mode":4}]},{"name":"17R000-0_rod_guide","primitives":[{"attributes":{"NORMAL":70,"POSITION":31},"indices":109,"material":0,"mode":4}]},{"name":"17R-15.30250SR","primitives":[{"attributes":{"NORMAL":71,"POSITION":32},"indices":110,"material":0,"mode":4}]},{"name":"17P0000_rear_head","primitives":[{"attributes":{"NORMAL":72,"POSITION":33},"indices":111,"material":0,"mode":4}]},{"name":"D-358","primitives":[{"attributes":{"NORMAL":73,"POSITION":34},"indices":112,"material":0,"mode":4}]},{"name":"SR-1714-DP0000_rod","primitives":[{"attributes":{"NORMAL":74,"POSITION":35},"indices":113,"material":0,"mode":4}]},{"name":"HNG-00-10","primitives":[{"attributes":{"NORMAL":76,"POSITION":37},"indices":115,"material":0,"mode":4}]},{"name":"HNG-00-10","primitives":[{"attributes":{"NORMAL":76,"POSITION":37},"indices":115,"material":0,"mode":4}]},{"name":"HNG-00-13","primitives":[{"attributes":{"NORMAL":77,"POSITION":38},"indices":116,"material":0,"mode":4}]},{"name":"HNG-00-13","primitives":[{"attributes":{"NORMAL":77,"POSITION":38},"indices":116,"material":0,"mode":4}]}],"nodes":[{"children":[1,27,56,71,72,73],"name":"RBT-00-00_no_arm"},{"children":[2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26],"translation":[0.21214138642130749,0.06542792188427198,0.13567741509886556],"name":"NAUO1"},{"translation":[0.11805861357869255,0.2647720781157281,-0.02772741509886506],"mesh":0,"name":"NAUO61"},{"rotation":[0.0,-0.0,0.7071067811865479,0.7071067811865471],"translation":[-0.5423413864213077,0.2647720781157279,-0.027727415098865048],"mesh":1,"name":"NAUO62"},{"rotation":[0.0,0.0,-0.7071067811865475,0.7071067811865476],"translation":[0.11805861357869259,-0.3956279218842718,-0.02772741509886506],"mesh":2,"name":"NAUO63"},{"rotation":[0.0,0.0,1.0,-2.220446049250312e-16],"translation":[-0.5423413864213073,-0.395627921884272,-0.02772741509886506],"mesh":3,"name":"NAUO64"},{"translation":[0.10535861357869254,-0.2940279218842719,-0.09757741509886506],"mesh":4,"name":"NAUO65"},{"rotation":[0.0,0.0,-0.7071067811865474,0.7071067811865477],"translation":[-0.44074138642130736,0.25207207811572776,-0.09757741509886506],"mesh":5,"name":"NAUO66"},{"translation":[-0.5296413864213073,-0.29402792188427237,-0.09757741509886506],"mesh":6,"name":"NAUO67"},{"rotation":[0.0,0.0,-0.7071067811865474,0.7071067811865477],"translation":[-0.44074138642130708,-0.38292792188427196,-0.09757741509886506],"mesh":7,"name":"NAUO68"},{"rotation":[-0.5000000000000001,0.49999999999999997,0.5,0.5],"translation":[-0.2121413864213072,-0.14391392188427247,-0.1547274150988651],"mesh":8,"name":"NAUO69"},{"translation":[-0.09784138642130702,-0.3829279218842717,-0.12297741509886505],"mesh":9,"name":"NAUO70"},{"translation":[-0.3264413864213071,-0.38292792188427179,-0.12297741509886505],"mesh":10,"name":"NAUO71"},{"translation":[-0.0978413864213073,0.25207207811572798,-0.12297741509886505],"mesh":11,"name":"NAUO72"},{"translation":[-0.3264413864213073,0.25207207811572776,-0.12297741509886505],"mesh":12,"name":"NAUO73"},{"translation":[-0.5423413864213074,-0.06542792188427239,-0.053127415098865028],"mesh":13,"name":"NAUO74"},{"rotation":[6.986417961653758e-19,-5.143899031570203e-17,1.0,-3.061786935099064e-16],"translation":[0.11805861357869255,-0.06542792188427155,-0.05312741509886506],"mesh":14,"name":"NAUO75"},{"rotation":[-1.7539188962154345e-16,0.7071067811865475,2.563835365164946e-16,0.7071067811865476],"translation":[-0.11054138642130684,-0.3956279218842718,-0.1420274150988651],"mesh":15,"name":"NAUO76"},{"translation":[-0.3042163864213074,-0.22735292188427246,-0.15472741509886504],"mesh":16,"name":"NAUO77"},{"rotation":[-8.673617379884036e-19,-3.5300453521074238e-19,1.0,-8.673617379884038e-19],"translation":[-0.12006638642130749,-0.0559029218842725,-0.15472741509886504],"mesh":17,"name":"NAUO78"},{"translation":[-0.33279138642130726,-0.22735292188427243,-0.15472741509886504],"mesh":18,"name":"NAUO79"},{"rotation":[4.986098557373419e-19,-2.672492477452662e-19,1.0,8.673617379884034e-19],"translation":[-0.09149138642130712,-0.05590292188427244,-0.15472741509886504],"mesh":19,"name":"NAUO80"},{"rotation":[2.3565581545043415e-16,-0.7071067811865476,2.3537192215469846e-16,0.7071067811865475],"translation":[-0.3137413864213071,-0.3956279218842718,-0.14202741509886505],"mesh":20,"name":"NAUO81"},{"translation":[-0.5423413864213077,-0.06542792188427003,-0.12297741509886505],"mesh":21,"name":"NAUO82"},{"rotation":[0.0,0.0,1.0,3.4914813388431335e-15],"translation":[0.11805861357869209,-0.06542792188427277,-0.12297741509886505],"mesh":22,"name":"NAUO83"},{"translation":[-0.21214138642130729,0.2647720781157278,-0.122977415098865],"mesh":23,"name":"NAUO84"},{"rotation":[0.0,0.0,1.0,3.4914813388431335e-15],"translation":[-0.2121413864213104,-0.39562792188427,-0.12297741509886505],"mesh":24,"name":"NAUO85"},{"children":[28,29,30,31,32,33,34,41,42,46,47,48,49,50,51,52],"rotation":[-6.623827560072138e-17,-3.189250306701401e-17,0.707106781186549,0.707106781186546],"translation":[0.11429999999998147,0.22148268122384316,-0.1587893249326159],"name":"NAUO2"},{"rotation":[0.4999999999999966,0.4999999999999966,-0.5000000000000053,0.5000000000000018],"translation":[-0.005582681223843088,-1.3877787807814366e-17,-0.15236067506738402],"mesh":25,"name":"NAUO36"},{"rotation":[-0.5,0.49999999999999658,0.5,0.5000000000000036],"translation":[-0.005582681223841479,0.22860000000000303,-0.15236067506738258],"mesh":26,"name":"NAUO37"},{"rotation":[2.468850131082263e-15,0.7071067811865488,0.7071067811865462,2.468850131082254e-15],"translation":[-0.030982681223842763,-0.025399999999997218,0.13973932493261616],"mesh":27,"name":"NAUO38"},{"rotation":[2.468850131082263e-15,0.7071067811865488,0.7071067811865462,2.468850131082254e-15],"translation":[-0.030982681223840796,0.25400000000000269,0.13973932493261519],"mesh":28,"name":"NAUO39"},{"rotation":[-0.18301270189221817,-0.6830127018922145,0.6830127018922247,-0.18301270189221839],"translation":[-0.005582681223842478,0.006350000000002298,0.012739324932615872],"mesh":29,"name":"NAUO40"},{"rotation":[-0.18301270189221828,0.6830127018922247,0.6830127018922146,0.18301270189221817],"translation":[-0.005582681223841729,0.22225000000000209,0.012739324932612874],"mesh":30,"name":"NAUO41"},{"children":[35,36,37,38,39,40],"rotation":[-6.234669935009758e-15,0.2588190451025223,-3.4870081323131555e-15,0.9659258262890678],"translation":[-0.0028941953486844326,0.1855345726296745,-0.02255829890053291],"name":"NAUO42"},{"rotation":[1.996804166340685e-30,5.296612495676417e-59,-8.776077570583752e-30,1.0],"translation":[-0.019977108982175249,0.030365427370327364,0.02917899599515297],"mesh":31,"name":"NAUO55"},{"rotation":[1.996804166340685e-30,5.296612495676417e-59,-8.776077570583752e-30,1.0],"translation":[-0.019969776633756543,-0.15734776313485789,0.029166295995152928],"mesh":32,"name":"NAUO56"},{"rotation":[5.2557785042231248e-17,0.5000000000000001,-6.580388297704567e-17,0.8660254037844386],"translation":[-0.019955111936919235,0.01487861787551252,0.02919169599515291],"mesh":33,"name":"NAUO57"},{"rotation":[0.6123724356957946,0.3535533905932737,-0.3535533905932738,0.6123724356957946],"translation":[-0.019955111936919124,-0.1511175726296727,0.029191695995152996],"mesh":34,"name":"NAUO58"},{"rotation":[-0.7071067811865475,1.2326874191699776e-16,-5.511807515714702e-18,0.7071067811865476],"translation":[-0.019969776633756543,0.030309117875512549,0.02916629599515297],"mesh":35,"name":"NAUO59"},{"rotation":[-6.275350074315773e-30,0.7071067811865475,0.7071067811865475,-6.2753500743157748e-30],"translation":[-0.01999910602743138,-0.1636342631348579,0.02916629599515297],"mesh":36,"name":"NAUO60"},{"rotation":[0.6830127018922146,-0.18301270189221814,0.18301270189221814,0.6830127018922247],"translation":[-0.005582681223841702,0.23495000000000214,0.012739324932612682],"mesh":37,"name":"NAUO43"},{"children":[43,44,45],"rotation":[-3.2295213397925705e-31,-8.668877364968815e-32,0.7071067811865438,0.7071067811865511],"translation":[0.0033605090523277927,0.0025670869535548009,0.01793770752310858],"name":"NAUO44"},{"rotation":[-5.824012151826996e-31,3.396267472520518e-31,2.051038353574629e-29,1.0],"translation":[-0.03189208695355272,0.008943190276170325,0.01702661740950738],"mesh":38,"name":"NAUO52"},{"rotation":[0.7071067811865475,9.813077866780638e-18,-0.7071067811865475,-9.813077866766135e-18],"translation":[-0.0724170869535527,0.008943190276170325,-0.005198382590492656],"mesh":39,"name":"NAUO53"},{"rotation":[-0.5,-0.5,-0.5000000000000001,0.5000000000000001],"translation":[-0.08394208695355277,0.008943190276170325,-0.005198382590492656],"mesh":40,"name":"NAUO54"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223841506,-0.01587499999999846,0.165139324932616],"mesh":41,"name":"NAUO45"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223842562,0.24130000000000136,0.1651393249326168],"mesh":42,"name":"NAUO46"},{"rotation":[0.49999999999999658,-0.49999999999999836,0.5000000000000106,-0.4999999999999948],"translation":[-0.005582681223840037,0.20320000000000339,0.0952893249326131],"mesh":43,"name":"NAUO47"},{"rotation":[-0.4999999999999966,0.49999999999999836,-0.5000000000000036,0.5000000000000018],"translation":[-0.005582681223843505,0.025400000000002113,0.09528932493261593],"mesh":44,"name":"NAUO48"},{"rotation":[0.7071067811865426,-9.937183757282918e-15,-2.5381230457357168e-15,0.7071067811865525],"translation":[-0.0055826812238409799,0.1873250000000043,0.16513932493261339],"mesh":45,"name":"NAUO49"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223842562,0.038099999999981978,0.16513932493261616],"mesh":46,"name":"NAUO50"},{"children":[53,54,55],"rotation":[0.4999999999999974,-0.5000000000000061,-0.4999999999999957,0.5000000000000008],"translation":[-0.010781063814334443,0.22603291304644933,0.0037961346564424067],"name":"NAUO51"},{"rotation":[-5.824012151826996e-31,3.396267472520518e-31,2.051038353574629e-29,1.0],"translation":[-0.03189208695355272,0.008943190276170325,0.01702661740950738],"mesh":47,"name":"NAUO52"},{"rotation":[0.7071067811865475,9.813077866780638e-18,-0.7071067811865475,-9.813077866766135e-18],"translation":[-0.0724170869535527,0.008943190276170325,-0.005198382590492656],"mesh":48,"name":"NAUO53"},{"rotation":[-0.5,-0.5,-0.5000000000000001,0.5000000000000001],"translation":[-0.08394208695355277,0.008943190276170325,-0.005198382590492656],"mesh":49,"name":"NAUO54"},{"children":[57,58,61,64,65,66,67,68,69,70],"rotation":[-0.45029449496087689,-1.100501664646917e-15,3.322546818077275e-16,0.8928800971059491],"translation":[-1.4070901337315319e-14,0.21589774466199347,-0.5651301275705946],"name":"NAUO3"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"mesh":50,"name":"NAUO24"},{"children":[59,60],"rotation":[0.1830127018922198,0.18301270189221847,-0.6830127018922219,0.6830127018922171],"translation":[0.08613140000000001,-5.551115123125782e-16,-1.1102230246251556e-16],"name":"NAUO25"},{"rotation":[3.944304526105062e-30,5.5511151231257817e-17,-5.127595883936578e-30,1.0],"mesh":51,"name":"NAUO34"},{"rotation":[1.5508251547067147e-30,-0.2588190451025206,-4.498986871965784e-30,0.9659258262890683],"translation":[-0.03839320962149228,-0.001993899999999993,0.007951989730066944],"mesh":52,"name":"NAUO35"},{"children":[62,63],"rotation":[-0.5000000000000052,0.500000000000007,0.49999999999999647,0.4999999999999913],"translation":[-0.0861314,-3.33066907387547e-16,5.828670879282072e-16],"name":"NAUO26"},{"rotation":[3.944304526105062e-30,5.5511151231257817e-17,-5.127595883936578e-30,1.0],"mesh":53,"name":"NAUO34"},{"rotation":[1.5508251547067147e-30,-0.2588190451025206,-4.498986871965784e-30,0.9659258262890683],"translation":[-0.03839320962149228,-0.001993899999999993,0.007951989730066944],"mesh":54,"name":"NAUO35"},{"rotation":[-0.009100500670791337,-0.009100500670791191,-0.7070482168052904,0.7070482168052903],"translation":[0.03968749999999996,-0.0029418500950466614,-0.11426213510178374],"mesh":55,"name":"NAUO27"},{"rotation":[-0.0091005006707913,-0.009100500670791229,-0.7070482168052904,0.7070482168052903],"translation":[-0.03968750000000003,-0.0029418500950466614,-0.11426213510178361],"mesh":56,"name":"NAUO28"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"translation":[-0.06286499999999999,0.0,2.7755575615628916e-17],"mesh":57,"name":"NAUO29"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"translation":[0.062865,0.0,-2.7755575615628916e-17],"mesh":58,"name":"NAUO30"},{"rotation":[-0.5000000000000002,2.7940404561633018e-30,3.794496104309507e-31,0.8660254037844385],"mesh":59,"name":"NAUO31"},{"rotation":[-0.7071067811865462,0.7071067811865488,-2.4728956224269327e-15,-2.4728956224269576e-15],"translation":[0.04127500000000008,0.02927349999999973,-0.026083229927617057],"mesh":60,"name":"NAUO32"},{"rotation":[0.7071067811865511,0.7071067811865438,-2.4688501310822525e-15,-2.4688501310822785e-15],"translation":[-0.04127499999999962,-0.029273499999999509,-0.026083229927616526],"mesh":61,"name":"NAUO33"},{"rotation":[0.22129280107253516,-0.6715872960334054,-0.22129280107253733,0.6715872960334203],"translation":[-0.1079500000000319,0.21590000000000193,-0.5651499999999974],"mesh":62,"name":"NAUO4"},{"rotation":[0.22129280107253603,0.6715872960334129,0.2212928010725361,0.6715872960334129],"translation":[0.10794999999998989,0.2159000000000022,-0.5651499999999999],"mesh":63,"name":"NAUO5"},{"children":[74,75,76,82,83,84,85,86,92,93,94,95],"name":"NAUO6"},{"translation":[-0.3048000000000008,-2.6330028493691965e-18,-1.0846837446788913e-29],"mesh":64,"name":"NAUO7"},{"rotation":[-6.98296267768624e-15,6.98296267768634e-15,1.0,3.491481338843087e-15],"translation":[0.3048,-2.1280637800386269e-15,4.736828266594918e-15],"mesh":65,"name":"NAUO8"},{"children":[77,78,79,80,81],"rotation":[0.5000000000000027,0.49999999999999747,0.5000000000000027,-0.49999999999999747],"translation":[-0.29916754999999997,3.1422831228713813e-15,-0.4827270000000002],"name":"NAUO9"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":66,"name":"NAUO19"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"translation":[0.04222749999999997,0.0,0.0],"mesh":67,"name":"NAUO20"},{"rotation":[0.7071067811865475,-1.0197966064698899e-45,-1.0197966064698899e-45,0.7071067811865475],"translation":[0.4255769999999999,0.0,0.0],"mesh":68,"name":"NAUO21"},{"rotation":[1.0,1.4422101917317764e-45,0.0,0.0],"translation":[0.4605020000000001,0.0,0.0],"mesh":69,"name":"NAUO22"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":70,"name":"NAUO23"},{"rotation":[2.4688501310822698e-15,0.7071067811865487,-4.937700262164522e-15,0.7071067811865462],"translation":[-0.2762249999999999,2.9326593936879757e-15,0.012700000000000973],"mesh":71,"name":"NAUO10"},{"rotation":[0.7071067811865475,5.8545825673967158e-33,-0.7071067811865475,1.1576945416813776e-32],"translation":[-0.2788144785973512,2.815246350435978e-18,0.012699999999999996],"mesh":72,"name":"NAUO11"},{"rotation":[2.0807720770400135e-15,0.7071067811865427,-2.826565994914841e-15,0.7071067811865525],"translation":[0.2788144785973503,-1.9017702002227868e-15,0.012700000000004354],"mesh":73,"name":"NAUO12"},{"rotation":[-0.7071067811865475,6.20667528484673e-17,0.7071067811865476,-5.96945585410674e-17],"translation":[0.276225,3.797182960523879e-18,0.012700000000000428],"mesh":74,"name":"NAUO13"},{"children":[87,88,89,90,91],"rotation":[0.5000000000000001,-0.49999999999999997,0.5,0.5],"translation":[0.29916755000000008,9.936975113543188e-18,-0.4827269999999997],"name":"NAUO14"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":75,"name":"NAUO19"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"translation":[0.04222749999999997,0.0,0.0],"mesh":76,"name":"NAUO20"},{"rotation":[0.7071067811865475,-1.0197966064698899e-45,-1.0197966064698899e-45,0.7071067811865475],"translation":[0.4255769999999999,0.0,0.0],"mesh":77,"name":"NAUO21"},{"rotation":[1.0,1.4422101917317764e-45,0.0,0.0],"translation":[0.4605020000000001,0.0,0.0],"mesh":78,"name":"NAUO22"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":79,"name":"NAUO23"},{"rotation":[0.0,0.0,-0.7071067811865488,0.7071067811865462],"translation":[0.2629394785973505,-8.835338985750676e-16,-0.2412999999999996],"mesh":80,"name":"NAUO15"},{"rotation":[-1.7334705305548197e-16,3.173068402831953e-16,0.7071067811865475,0.7071067811865475],"translation":[-0.2629394785973513,-1.3786260431350188e-16,-0.2412999999999999],"mesh":81,"name":"NAUO16"},{"rotation":[2.520843109358146e-16,4.2107967123869179e-16,1.0,8.326672684688684e-17],"translation":[-0.30075505000000327,2.3624337269355368e-15,-0.45732700000000006],"mesh":82,"name":"NAUO17"},{"translation":[0.3007550500000001,1.965344605669299e-17,-0.45732699999999967],"mesh":83,"name":"NAUO18"}],"scene":0,"scenes":[{"nodes":[0]}]} \ No newline at end of file diff --git a/AdvantageScope/assets/Robot_619/config.json b/AdvantageScope/assets/Robot_619/config.json new file mode 100644 index 0000000..3981dce --- /dev/null +++ b/AdvantageScope/assets/Robot_619/config.json @@ -0,0 +1,34 @@ +{ + "name": "Robot_619", + "disableSimplification": false, + "rotations": [ + { + "axis": "x", + "degrees": 180 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [0, 0, 0.11], + "cameras": [ + { + "name": "Front Camera", + "rotations": [ + { + "axis": "y", + "degrees": 0 + } + ], + "position": [0.2, 0, 0.8], + "resolution": [960, 720], + "fov": 100 + } + ], + "components": [] +} diff --git a/AdvantageScope/assets/Robot_619/model.glb b/AdvantageScope/assets/Robot_619/model.glb new file mode 100644 index 0000000..1ac7eca --- /dev/null +++ b/AdvantageScope/assets/Robot_619/model.glb @@ -0,0 +1 @@ +{"accessors":[{"bufferView":0,"byteOffset":0,"componentType":5126,"count":163401,"max":[0.0,0.0,-0.0006558304516347846],"min":[-0.19434485504900027,-0.19429796936844088,-0.17286604999999997],"type":"VEC3"},{"bufferView":0,"byteOffset":1960812,"componentType":5126,"count":5376,"max":[0.012699999999999989,0.45085,0.025399999999999993],"min":[-0.012699999999999989,0.0063499999999999949,-0.025400000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":2025324,"componentType":5126,"count":60,"max":[0.0762,0.090551,0.08343900000000002],"min":[0.0,-0.090551,-0.08343900000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2026044,"componentType":5126,"count":622,"max":[0.038099999999999998,0.012699999999999989,0.0],"min":[-0.03810000000000001,-0.012699999999999989,-0.00635],"type":"VEC3"},{"bufferView":0,"byteOffset":2033508,"componentType":5126,"count":776,"max":[0.7429499999999999,0.41275,0.0],"min":[-0.08255000000000001,-5.551115123125783e-17,-0.127],"type":"VEC3"},{"bufferView":0,"byteOffset":2042820,"componentType":5126,"count":8928,"max":[0.012700000000000007,0.6604,0.025399999999999993],"min":[-0.012700000000000003,0.0,-0.02540000000000002],"type":"VEC3"},{"bufferView":0,"byteOffset":2149956,"componentType":5126,"count":518,"max":[0.0,0.17145,3.552713678800501e-18],"min":[-0.025400000000000007,0.0,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2156172,"componentType":5126,"count":374,"max":[0.2413,0.0,0.0],"min":[0.0,-0.025400000000000007,-0.025399999999999993],"type":"VEC3"},{"bufferView":0,"byteOffset":2160660,"componentType":5126,"count":628,"max":[0.025399999999999979,0.0381,0.0],"min":[0.0,-0.0381,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2168196,"componentType":5126,"count":480,"max":[0.0254,0.0,0.0],"min":[-0.0254,-0.025399999999999979,-0.0254],"type":"VEC3"},{"bufferView":0,"byteOffset":2173956,"componentType":5126,"count":2120,"max":[0.2921,0.025400000000000007,0.012700000000000017],"min":[-0.2921,-0.025400000000000007,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2199396,"componentType":5126,"count":2120,"max":[0.2921,0.025400000000000007,0.012700000000000017],"min":[-0.29209999999999999,-0.025400000000000007,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2224836,"componentType":5126,"count":1232,"max":[0.3556,0.0254,0.012700000000000017],"min":[-0.05080000000000001,-0.5080000000000001,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":2239620,"componentType":5126,"count":1232,"max":[0.3556,0.0254,0.012700000000000047],"min":[-0.05080000000000001,-0.5080000000000001,-0.012700000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2254404,"componentType":5126,"count":1342,"max":[0.015557500000000033,0.015557499999999976,1.4210854715202005e-17],"min":[-0.015557499999999992,-0.015557500000000006,-0.007937500000000014],"type":"VEC3"},{"bufferView":0,"byteOffset":2270508,"componentType":5126,"count":194,"max":[0.007239095848374177,0.025400000000000007,0.006337300000000014],"min":[-0.007239095848374177,-0.22860000000000003,-0.0063373000000000278],"type":"VEC3"},{"bufferView":0,"byteOffset":2272836,"componentType":5126,"count":5292,"max":[0.023394639261530644,0.006286500000000004,0.023854228666702824],"min":[-0.023394639261530644,-0.006286500000000018,-0.023854228666702824],"type":"VEC3"},{"bufferView":0,"byteOffset":2336340,"componentType":5126,"count":72,"max":[0.0146646968374165,0.012699999999999989,1.4210854715202005e-17],"min":[-0.014664696837416528,-0.012700000000000017,-0.15976600000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2337204,"componentType":5126,"count":72,"max":[0.0146646968374165,0.01270000000000003,0.0],"min":[-0.0146646968374165,-0.012700000000000017,-0.009144000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":2338068,"componentType":5126,"count":49414,"max":[0.0572002,0.021,0.0],"min":[-0.03476400000000004,-0.02100000000000003,-0.04445000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":2931036,"componentType":5126,"count":5691,"max":[0.032,0.032,0.011525000000000006],"min":[-0.03199999999999999,-0.032,-0.009825000000000018],"type":"VEC3"},{"bufferView":0,"byteOffset":2999328,"componentType":5126,"count":16435,"max":[0.029991120151092136,0.03500000000000003,0.029991120151092164],"min":[-0.03814108252475668,-0.05835000000000003,-0.03802030086559549],"type":"VEC3"},{"bufferView":0,"byteOffset":3196548,"componentType":5126,"count":1904,"max":[0.08890000000000004,9.769962616701377e-18,2.842170943040401e-17],"min":[-0.08890000000000002,-0.10159999999999998,-0.0031750000000000117],"type":"VEC3"},{"bufferView":0,"byteOffset":3219396,"componentType":5126,"count":936,"max":[0.012700000000000003,0.044449999999999999,0.025400000000000007],"min":[-0.012700000000000017,-0.04445,-0.025400000000000007],"type":"VEC3"},{"bufferView":0,"byteOffset":3230628,"componentType":5126,"count":36,"max":[0.127,0.0073323484187083069,0.006350000000000051],"min":[-0.127,-0.0073323484187083069,-0.006350000000000023],"type":"VEC3"},{"bufferView":0,"byteOffset":3231060,"componentType":5126,"count":16684,"max":[0.09982888152266571,0.0019939000000000108,0.09982873372984733],"min":[-0.09982888152266571,-0.0019939000000000108,-0.09982873372984727],"type":"VEC3"},{"bufferView":0,"byteOffset":3431268,"componentType":5126,"count":3002,"max":[0.058528594780987878,0.017462500000000007,-0.0000071761424943588289],"min":[0.0,-0.007937500000000014,-0.05215928371273964],"type":"VEC3"},{"bufferView":0,"byteOffset":3467292,"componentType":5126,"count":1510,"max":[0.06348890832287806,0.0015875000000000058,0.14561940674229144],"min":[-0.0635,-0.0015875000000000058,0.0],"type":"VEC3"},{"bufferView":0,"byteOffset":3485412,"componentType":5126,"count":72,"max":[0.013335000000000009,0.0146646968374165,0.012699999999999989],"min":[-0.013335000000000009,-0.014664696837416613,-0.012700000000000074],"type":"VEC3"},{"bufferView":0,"byteOffset":3486276,"componentType":5126,"count":72,"max":[0.0238125,0.014664696837416613,0.012700000000000047],"min":[-0.023812500000000009,-0.014664696837416443,-0.012700000000000047],"type":"VEC3"},{"bufferView":0,"byteOffset":3487140,"componentType":5126,"count":2416,"max":[0.025400000000000035,0.1016,0.0127],"min":[-0.025399999999999979,-0.1016,-0.012700000000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3516132,"componentType":5126,"count":3150,"max":[0.047625,0.01976755020390027,0.019767550000000003],"min":[0.0,-0.01976754999999997,-0.019767550000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3553932,"componentType":5126,"count":1548,"max":[0.38959898794721339,0.01976755000000003,0.01976755000000003],"min":[-0.0009154879472028484,-0.01976755000000003,-0.01976755000000003],"type":"VEC3"},{"bufferView":0,"byteOffset":3572508,"componentType":5126,"count":2212,"max":[0.04445,0.019767550000000003,0.01976754999999997],"min":[0.0,-0.019767550000000003,-0.01976754999999997],"type":"VEC3"},{"bufferView":0,"byteOffset":3599052,"componentType":5126,"count":304,"max":[0.004762499999999999,0.012700000000000047,0.004746233703793023],"min":[-0.004762499999999999,-0.012700000000000047,-0.0047625],"type":"VEC3"},{"bufferView":0,"byteOffset":3602700,"componentType":5126,"count":630,"max":[0.4255769999999999,0.019145249999999976,0.019145250000000004],"min":[-0.025399999999999979,-0.019145250000000035,-0.019145250000000004],"type":"VEC3"},{"bufferView":0,"byteOffset":3610260,"componentType":5126,"count":1384,"max":[0.1016,4.0389678347315809e-31,0.0005855214026497038],"min":[0.0,-0.10160000000000001,-0.0031750000000000686],"type":"VEC3"},{"bufferView":0,"byteOffset":3626868,"componentType":5126,"count":48,"max":[0.02540000000000001,0.012700000000000047,0.254],"min":[-0.02539999999999999,-0.012700000000000017,-0.25400000000000008],"type":"VEC3"},{"bufferView":0,"byteOffset":3627444,"componentType":5126,"count":184,"max":[0.025400000000000035,0.0254,0.012700000000000047],"min":[-0.025400000000000035,-0.025400000000000004,-0.012699999999999989],"type":"VEC3"},{"bufferView":0,"byteOffset":3629652,"componentType":5126,"count":20416,"max":[0.2684859375000001,0.006350000000000137,0.5824642385132652],"min":[-0.2683600681181053,-1.1368683772161604e-16,-0.15413576148673553],"type":"VEC3"},{"bufferView":0,"byteOffset":3874644,"componentType":5126,"count":14144,"max":[0.26848593750000007,0.012700000000000047,0.5824642385132652],"min":[-0.2683921816302614,0.006349999999999852,-0.15410608859444173],"type":"VEC3"},{"bufferView":0,"byteOffset":4044372,"componentType":5126,"count":1256,"max":[-0.09419648224615369,0.019050000000000068,0.5824642385132652],"min":[-0.21778753750000005,0.012699999999999875,0.38510686250000006],"type":"VEC3"},{"bufferView":0,"byteOffset":4059444,"componentType":5126,"count":640,"max":[0.025400000000000035,0.05080000000000004,0.012699999999999989],"min":[-0.025400000000000094,-0.05080000000000004,-0.012700000000000047],"type":"VEC3"},{"bufferView":0,"byteOffset":4067124,"componentType":5126,"count":296,"max":[0.012700000000000047,0.05715000000000009,0.012699999999999989],"min":[-0.012699999999999932,-5.684341886080802e-17,-0.012700000000000017],"type":"VEC3"},{"bufferView":0,"byteOffset":4070676,"componentType":5126,"count":11956,"max":[0.025499999999999986,0.007250000000000057,0.025500000000000003],"min":[-0.025500000000000038,-0.007250000000000114,-0.025500000000000114],"type":"VEC3"},{"bufferView":0,"byteOffset":4214148,"componentType":5126,"count":11956,"max":[0.025500000000000003,0.011,0.025500000000000003],"min":[-0.02550000000000003,-0.011000000000000057,-0.025500000000000059],"type":"VEC3"},{"bufferView":0,"byteOffset":4357620,"componentType":5126,"count":3572,"max":[0.020258045073549514,0.006324600000000089,0.020319200606876168],"min":[-0.020258045073549514,-0.006324600000000033,-0.020319200606876168],"type":"VEC3"},{"bufferView":0,"byteOffset":4400484,"componentType":5126,"count":3916,"max":[0.050799999999999987,0.0508,0.012700000000000047],"min":[-0.050799999999999987,-0.050800000000000029,-0.012700000000000047],"type":"VEC3"},{"bufferView":0,"byteOffset":4447476,"componentType":5126,"count":924,"max":[0.06985,5.684341886080802e-17,0.5824642385132652],"min":[0.04444999999999997,-0.025399999999999979,0.41229280000000009],"type":"VEC3"},{"bufferView":1,"byteOffset":0,"componentType":5126,"count":163401,"type":"VEC3"},{"bufferView":1,"byteOffset":1960812,"componentType":5126,"count":5376,"type":"VEC3"},{"bufferView":1,"byteOffset":2025324,"componentType":5126,"count":60,"type":"VEC3"},{"bufferView":1,"byteOffset":2026044,"componentType":5126,"count":622,"type":"VEC3"},{"bufferView":1,"byteOffset":2033508,"componentType":5126,"count":776,"type":"VEC3"},{"bufferView":1,"byteOffset":2042820,"componentType":5126,"count":8928,"type":"VEC3"},{"bufferView":1,"byteOffset":2149956,"componentType":5126,"count":518,"type":"VEC3"},{"bufferView":1,"byteOffset":2156172,"componentType":5126,"count":374,"type":"VEC3"},{"bufferView":1,"byteOffset":2160660,"componentType":5126,"count":628,"type":"VEC3"},{"bufferView":1,"byteOffset":2168196,"componentType":5126,"count":480,"type":"VEC3"},{"bufferView":1,"byteOffset":2173956,"componentType":5126,"count":2120,"type":"VEC3"},{"bufferView":1,"byteOffset":2199396,"componentType":5126,"count":2120,"type":"VEC3"},{"bufferView":1,"byteOffset":2224836,"componentType":5126,"count":1232,"type":"VEC3"},{"bufferView":1,"byteOffset":2239620,"componentType":5126,"count":1232,"type":"VEC3"},{"bufferView":1,"byteOffset":2254404,"componentType":5126,"count":1342,"type":"VEC3"},{"bufferView":1,"byteOffset":2270508,"componentType":5126,"count":194,"type":"VEC3"},{"bufferView":1,"byteOffset":2272836,"componentType":5126,"count":5292,"type":"VEC3"},{"bufferView":1,"byteOffset":2336340,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":2337204,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":2338068,"componentType":5126,"count":49414,"type":"VEC3"},{"bufferView":1,"byteOffset":2931036,"componentType":5126,"count":5691,"type":"VEC3"},{"bufferView":1,"byteOffset":2999328,"componentType":5126,"count":16435,"type":"VEC3"},{"bufferView":1,"byteOffset":3196548,"componentType":5126,"count":1904,"type":"VEC3"},{"bufferView":1,"byteOffset":3219396,"componentType":5126,"count":936,"type":"VEC3"},{"bufferView":1,"byteOffset":3230628,"componentType":5126,"count":36,"type":"VEC3"},{"bufferView":1,"byteOffset":3231060,"componentType":5126,"count":16684,"type":"VEC3"},{"bufferView":1,"byteOffset":3431268,"componentType":5126,"count":3002,"type":"VEC3"},{"bufferView":1,"byteOffset":3467292,"componentType":5126,"count":1510,"type":"VEC3"},{"bufferView":1,"byteOffset":3485412,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":3486276,"componentType":5126,"count":72,"type":"VEC3"},{"bufferView":1,"byteOffset":3487140,"componentType":5126,"count":2416,"type":"VEC3"},{"bufferView":1,"byteOffset":3516132,"componentType":5126,"count":3150,"type":"VEC3"},{"bufferView":1,"byteOffset":3553932,"componentType":5126,"count":1548,"type":"VEC3"},{"bufferView":1,"byteOffset":3572508,"componentType":5126,"count":2212,"type":"VEC3"},{"bufferView":1,"byteOffset":3599052,"componentType":5126,"count":304,"type":"VEC3"},{"bufferView":1,"byteOffset":3602700,"componentType":5126,"count":630,"type":"VEC3"},{"bufferView":1,"byteOffset":3610260,"componentType":5126,"count":1384,"type":"VEC3"},{"bufferView":1,"byteOffset":3626868,"componentType":5126,"count":48,"type":"VEC3"},{"bufferView":1,"byteOffset":3627444,"componentType":5126,"count":184,"type":"VEC3"},{"bufferView":1,"byteOffset":3629652,"componentType":5126,"count":20416,"type":"VEC3"},{"bufferView":1,"byteOffset":3874644,"componentType":5126,"count":14144,"type":"VEC3"},{"bufferView":1,"byteOffset":4044372,"componentType":5126,"count":1256,"type":"VEC3"},{"bufferView":1,"byteOffset":4059444,"componentType":5126,"count":640,"type":"VEC3"},{"bufferView":1,"byteOffset":4067124,"componentType":5126,"count":296,"type":"VEC3"},{"bufferView":1,"byteOffset":4070676,"componentType":5126,"count":11956,"type":"VEC3"},{"bufferView":1,"byteOffset":4214148,"componentType":5126,"count":11956,"type":"VEC3"},{"bufferView":1,"byteOffset":4357620,"componentType":5126,"count":3572,"type":"VEC3"},{"bufferView":1,"byteOffset":4400484,"componentType":5126,"count":3916,"type":"VEC3"},{"bufferView":1,"byteOffset":4447476,"componentType":5126,"count":924,"type":"VEC3"},{"bufferView":2,"byteOffset":0,"componentType":5125,"count":490221,"type":"SCALAR"},{"bufferView":2,"byteOffset":1960884,"componentType":5123,"count":16080,"type":"SCALAR"},{"bufferView":2,"byteOffset":1993044,"componentType":5123,"count":108,"type":"SCALAR"},{"bufferView":2,"byteOffset":1993260,"componentType":5123,"count":1806,"type":"SCALAR"},{"bufferView":2,"byteOffset":1996872,"componentType":5123,"count":2916,"type":"SCALAR"},{"bufferView":2,"byteOffset":2002704,"componentType":5123,"count":26736,"type":"SCALAR"},{"bufferView":2,"byteOffset":2056176,"componentType":5123,"count":1500,"type":"SCALAR"},{"bufferView":2,"byteOffset":2059176,"componentType":5123,"count":1068,"type":"SCALAR"},{"bufferView":2,"byteOffset":2061312,"componentType":5123,"count":1836,"type":"SCALAR"},{"bufferView":2,"byteOffset":2064984,"componentType":5123,"count":1392,"type":"SCALAR"},{"bufferView":2,"byteOffset":2067768,"componentType":5123,"count":6312,"type":"SCALAR"},{"bufferView":2,"byteOffset":2080392,"componentType":5123,"count":6312,"type":"SCALAR"},{"bufferView":2,"byteOffset":2093016,"componentType":5123,"count":3648,"type":"SCALAR"},{"bufferView":2,"byteOffset":2100312,"componentType":5123,"count":3648,"type":"SCALAR"},{"bufferView":2,"byteOffset":2107608,"componentType":5123,"count":4392,"type":"SCALAR"},{"bufferView":2,"byteOffset":2116392,"componentType":5123,"count":492,"type":"SCALAR"},{"bufferView":2,"byteOffset":2117376,"componentType":5123,"count":13716,"type":"SCALAR"},{"bufferView":2,"byteOffset":2144808,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2145096,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2145384,"componentType":5123,"count":180000,"type":"SCALAR"},{"bufferView":2,"byteOffset":2505384,"componentType":5123,"count":20544,"type":"SCALAR"},{"bufferView":2,"byteOffset":2546472,"componentType":5123,"count":44766,"type":"SCALAR"},{"bufferView":2,"byteOffset":2636004,"componentType":5123,"count":5592,"type":"SCALAR"},{"bufferView":2,"byteOffset":2647188,"componentType":5123,"count":2760,"type":"SCALAR"},{"bufferView":2,"byteOffset":2652708,"componentType":5123,"count":60,"type":"SCALAR"},{"bufferView":2,"byteOffset":2652828,"componentType":5123,"count":44646,"type":"SCALAR"},{"bufferView":2,"byteOffset":2742120,"componentType":5123,"count":8604,"type":"SCALAR"},{"bufferView":2,"byteOffset":2759328,"componentType":5123,"count":4476,"type":"SCALAR"},{"bufferView":2,"byteOffset":2768280,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2768568,"componentType":5123,"count":144,"type":"SCALAR"},{"bufferView":2,"byteOffset":2768856,"componentType":5123,"count":7200,"type":"SCALAR"},{"bufferView":2,"byteOffset":2783256,"componentType":5123,"count":11412,"type":"SCALAR"},{"bufferView":2,"byteOffset":2806080,"componentType":5123,"count":5772,"type":"SCALAR"},{"bufferView":2,"byteOffset":2817624,"componentType":5123,"count":6984,"type":"SCALAR"},{"bufferView":2,"byteOffset":2831592,"componentType":5123,"count":864,"type":"SCALAR"},{"bufferView":2,"byteOffset":2833320,"componentType":5123,"count":1497,"type":"SCALAR"},{"bufferView":2,"byteOffset":2836316,"componentType":5123,"count":4056,"type":"SCALAR"},{"bufferView":2,"byteOffset":2844428,"componentType":5123,"count":96,"type":"SCALAR"},{"bufferView":2,"byteOffset":2844620,"componentType":5123,"count":504,"type":"SCALAR"},{"bufferView":2,"byteOffset":2845628,"componentType":5123,"count":59322,"type":"SCALAR"},{"bufferView":2,"byteOffset":2964272,"componentType":5123,"count":40860,"type":"SCALAR"},{"bufferView":2,"byteOffset":3045992,"componentType":5123,"count":3720,"type":"SCALAR"},{"bufferView":2,"byteOffset":3053432,"componentType":5123,"count":1872,"type":"SCALAR"},{"bufferView":2,"byteOffset":3057176,"componentType":5123,"count":864,"type":"SCALAR"},{"bufferView":2,"byteOffset":3058904,"componentType":5123,"count":34608,"type":"SCALAR"},{"bufferView":2,"byteOffset":3128120,"componentType":5123,"count":34608,"type":"SCALAR"},{"bufferView":2,"byteOffset":3197336,"componentType":5123,"count":10296,"type":"SCALAR"},{"bufferView":2,"byteOffset":3217928,"componentType":5123,"count":11064,"type":"SCALAR"},{"bufferView":2,"byteOffset":3240056,"componentType":5123,"count":2724,"type":"SCALAR"}],"asset":{"generator":"Open CASCADE Technology 7.6 [dev.opencascade.org]","version":"2.0","extras":{"Author":"","Originating system":"SolidWorks 2024","Organization":"","Comments":"STEP AP214"}},"bufferViews":[{"buffer":0,"byteLength":4458564,"byteOffset":0,"byteStride":12,"target":34962},{"buffer":0,"byteLength":4458564,"byteOffset":4458564,"byteStride":12,"target":34962},{"buffer":0,"byteLength":3245504,"byteOffset":8917128,"target":34963}],"buffers":[{"byteLength":12162632,"uri":"RBT-00-00.bin"}],"materials":[{"name":"mat_0","pbrMetallicRoughness":{"baseColorFactor":[0.5906188488006592,0.637596845626831,0.8549926280975342,1.0],"roughnessFactor":0.2121320366859436},"doubleSided":true}],"meshes":[{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":49,"POSITION":0},"indices":98,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":49,"POSITION":0},"indices":98,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":49,"POSITION":0},"indices":98,"mode":4}]},{"name":"SDS MK4i","primitives":[{"attributes":{"NORMAL":49,"POSITION":0},"indices":98,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":50,"POSITION":1},"indices":99,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":50,"POSITION":1},"indices":99,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":50,"POSITION":1},"indices":99,"material":0,"mode":4}]},{"name":"CHS-00-01","primitives":[{"attributes":{"NORMAL":50,"POSITION":1},"indices":99,"material":0,"mode":4}]},{"name":"12Vbattery","primitives":[{"attributes":{"NORMAL":51,"POSITION":2},"indices":100,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":52,"POSITION":3},"indices":101,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":52,"POSITION":3},"indices":101,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":52,"POSITION":3},"indices":101,"material":0,"mode":4}]},{"name":"CHS-00-08","primitives":[{"attributes":{"NORMAL":52,"POSITION":3},"indices":101,"material":0,"mode":4}]},{"name":"CHS-00-06","primitives":[{"attributes":{"NORMAL":53,"POSITION":4},"indices":102,"material":0,"mode":4}]},{"name":"CHS-00-06","primitives":[{"attributes":{"NORMAL":53,"POSITION":4},"indices":102,"material":0,"mode":4}]},{"name":"CHS-00-09","primitives":[{"attributes":{"NORMAL":54,"POSITION":5},"indices":103,"material":0,"mode":4}]},{"name":"CHS-00-10","primitives":[{"attributes":{"NORMAL":55,"POSITION":6},"indices":104,"material":0,"mode":4}]},{"name":"CHS-00-10","primitives":[{"attributes":{"NORMAL":55,"POSITION":6},"indices":104,"material":0,"mode":4}]},{"name":"CHS-00-11","primitives":[{"attributes":{"NORMAL":56,"POSITION":7},"indices":105,"material":0,"mode":4}]},{"name":"CHS-00-11","primitives":[{"attributes":{"NORMAL":56,"POSITION":7},"indices":105,"material":0,"mode":4}]},{"name":"CHS-00-09","primitives":[{"attributes":{"NORMAL":54,"POSITION":5},"indices":103,"material":0,"mode":4}]},{"name":"CHS-00-12","primitives":[{"attributes":{"NORMAL":57,"POSITION":8},"indices":106,"material":0,"mode":4}]},{"name":"CHS-00-12","primitives":[{"attributes":{"NORMAL":57,"POSITION":8},"indices":106,"material":0,"mode":4}]},{"name":"CHS-00-13","primitives":[{"attributes":{"NORMAL":58,"POSITION":9},"indices":107,"material":0,"mode":4}]},{"name":"CHS-00-13","primitives":[{"attributes":{"NORMAL":58,"POSITION":9},"indices":107,"material":0,"mode":4}]},{"name":"STR-00-01","primitives":[{"attributes":{"NORMAL":59,"POSITION":10},"indices":108,"material":0,"mode":4}]},{"name":"STR-00-02","primitives":[{"attributes":{"NORMAL":60,"POSITION":11},"indices":109,"material":0,"mode":4}]},{"name":"STR-00-04","primitives":[{"attributes":{"NORMAL":61,"POSITION":12},"indices":110,"material":0,"mode":4}]},{"name":"STR-00-06","primitives":[{"attributes":{"NORMAL":62,"POSITION":13},"indices":111,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":63,"POSITION":14},"indices":112,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":63,"POSITION":14},"indices":112,"material":0,"mode":4}]},{"name":"STR-04-01","primitives":[{"attributes":{"NORMAL":64,"POSITION":15},"indices":113,"material":0,"mode":4}]},{"name":"STR-04-03","primitives":[{"attributes":{"NORMAL":65,"POSITION":16},"indices":114,"material":0,"mode":4}]},{"name":"STR-04-03","primitives":[{"attributes":{"NORMAL":65,"POSITION":16},"indices":114,"material":0,"mode":4}]},{"name":"STR-04-04","primitives":[{"attributes":{"NORMAL":66,"POSITION":17},"indices":115,"material":0,"mode":4}]},{"name":"STR-04-05","primitives":[{"attributes":{"NORMAL":67,"POSITION":18},"indices":116,"material":0,"mode":4}]},{"name":"STR-04-05","primitives":[{"attributes":{"NORMAL":67,"POSITION":18},"indices":116,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":63,"POSITION":14},"indices":112,"material":0,"mode":4}]},{"name":"STR-05-01","primitives":[{"attributes":{"NORMAL":68,"POSITION":19},"indices":117,"material":0,"mode":4}]},{"name":"STR-05-02","primitives":[{"attributes":{"NORMAL":69,"POSITION":20},"indices":118,"material":0,"mode":4}]},{"name":"STR-05-03","primitives":[{"attributes":{"NORMAL":70,"POSITION":21},"indices":119,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":71,"POSITION":22},"indices":120,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":71,"POSITION":22},"indices":120,"material":0,"mode":4}]},{"name":"STR-00-08","primitives":[{"attributes":{"NORMAL":72,"POSITION":23},"indices":121,"material":0,"mode":4}]},{"name":"STR-00-08","primitives":[{"attributes":{"NORMAL":72,"POSITION":23},"indices":121,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":71,"POSITION":22},"indices":120,"material":0,"mode":4}]},{"name":"T-forbolts","primitives":[{"attributes":{"NORMAL":71,"POSITION":22},"indices":120,"material":0,"mode":4}]},{"name":"STR-05-01","primitives":[{"attributes":{"NORMAL":68,"POSITION":19},"indices":117,"material":0,"mode":4}]},{"name":"STR-05-02","primitives":[{"attributes":{"NORMAL":69,"POSITION":20},"indices":118,"material":0,"mode":4}]},{"name":"STR-05-03","primitives":[{"attributes":{"NORMAL":70,"POSITION":21},"indices":119,"material":0,"mode":4}]},{"name":"STR-03-01","primitives":[{"attributes":{"NORMAL":73,"POSITION":24},"indices":122,"material":0,"mode":4}]},{"name":"STR-02-01","primitives":[{"attributes":{"NORMAL":74,"POSITION":25},"indices":123,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":75,"POSITION":26},"indices":124,"material":0,"mode":4}]},{"name":"STR-02-01","primitives":[{"attributes":{"NORMAL":74,"POSITION":25},"indices":123,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":75,"POSITION":26},"indices":124,"material":0,"mode":4}]},{"name":"str-00-05","primitives":[{"attributes":{"NORMAL":76,"POSITION":27},"indices":125,"material":0,"mode":4}]},{"name":"str-00-05","primitives":[{"attributes":{"NORMAL":76,"POSITION":27},"indices":125,"material":0,"mode":4}]},{"name":"STR-03-03","primitives":[{"attributes":{"NORMAL":77,"POSITION":28},"indices":126,"material":0,"mode":4}]},{"name":"STR-03-03","primitives":[{"attributes":{"NORMAL":77,"POSITION":28},"indices":126,"material":0,"mode":4}]},{"name":"STR-03-04","primitives":[{"attributes":{"NORMAL":78,"POSITION":29},"indices":127,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":75,"POSITION":26},"indices":124,"material":0,"mode":4}]},{"name":"STR-02-02","primitives":[{"attributes":{"NORMAL":75,"POSITION":26},"indices":124,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":63,"POSITION":14},"indices":112,"material":0,"mode":4}]},{"name":"STR-03-02","primitives":[{"attributes":{"NORMAL":63,"POSITION":14},"indices":112,"material":0,"mode":4}]},{"name":"hng-00-01","primitives":[{"attributes":{"NORMAL":79,"POSITION":30},"indices":128,"material":0,"mode":4}]},{"name":"hng-00-01","primitives":[{"attributes":{"NORMAL":79,"POSITION":30},"indices":128,"material":0,"mode":4}]},{"name":"17R000-0_rod_guide","primitives":[{"attributes":{"NORMAL":80,"POSITION":31},"indices":129,"material":0,"mode":4}]},{"name":"17R-15.30250SR","primitives":[{"attributes":{"NORMAL":81,"POSITION":32},"indices":130,"material":0,"mode":4}]},{"name":"17P0000_rear_head","primitives":[{"attributes":{"NORMAL":82,"POSITION":33},"indices":131,"material":0,"mode":4}]},{"name":"D-358","primitives":[{"attributes":{"NORMAL":83,"POSITION":34},"indices":132,"material":0,"mode":4}]},{"name":"SR-1714-DP0000_rod","primitives":[{"attributes":{"NORMAL":84,"POSITION":35},"indices":133,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":85,"POSITION":36},"indices":134,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":85,"POSITION":36},"indices":134,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":85,"POSITION":36},"indices":134,"material":0,"mode":4}]},{"name":"HNG-00-06","primitives":[{"attributes":{"NORMAL":85,"POSITION":36},"indices":134,"material":0,"mode":4}]},{"name":"17R000-0_rod_guide","primitives":[{"attributes":{"NORMAL":80,"POSITION":31},"indices":129,"material":0,"mode":4}]},{"name":"17R-15.30250SR","primitives":[{"attributes":{"NORMAL":81,"POSITION":32},"indices":130,"material":0,"mode":4}]},{"name":"17P0000_rear_head","primitives":[{"attributes":{"NORMAL":82,"POSITION":33},"indices":131,"material":0,"mode":4}]},{"name":"D-358","primitives":[{"attributes":{"NORMAL":83,"POSITION":34},"indices":132,"material":0,"mode":4}]},{"name":"SR-1714-DP0000_rod","primitives":[{"attributes":{"NORMAL":84,"POSITION":35},"indices":133,"material":0,"mode":4}]},{"name":"HNG-00-10","primitives":[{"attributes":{"NORMAL":86,"POSITION":37},"indices":135,"material":0,"mode":4}]},{"name":"HNG-00-10","primitives":[{"attributes":{"NORMAL":86,"POSITION":37},"indices":135,"material":0,"mode":4}]},{"name":"HNG-00-13","primitives":[{"attributes":{"NORMAL":87,"POSITION":38},"indices":136,"material":0,"mode":4}]},{"name":"HNG-00-13","primitives":[{"attributes":{"NORMAL":87,"POSITION":38},"indices":136,"material":0,"mode":4}]},{"name":"ARM-00-01","primitives":[{"attributes":{"NORMAL":88,"POSITION":39},"indices":137,"material":0,"mode":4}]},{"name":"ARM-00-02","primitives":[{"attributes":{"NORMAL":89,"POSITION":40},"indices":138,"material":0,"mode":4}]},{"name":"ARM-00-03","primitives":[{"attributes":{"NORMAL":90,"POSITION":41},"indices":139,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":91,"POSITION":42},"indices":140,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":91,"POSITION":42},"indices":140,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":91,"POSITION":42},"indices":140,"material":0,"mode":4}]},{"name":"ARM-00-04","primitives":[{"attributes":{"NORMAL":91,"POSITION":42},"indices":140,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":92,"POSITION":43},"indices":141,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":92,"POSITION":43},"indices":141,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":92,"POSITION":43},"indices":141,"material":0,"mode":4}]},{"name":"ARM-00-05","primitives":[{"attributes":{"NORMAL":92,"POSITION":43},"indices":141,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-06","primitives":[{"attributes":{"NORMAL":93,"POSITION":44},"indices":142,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-07","primitives":[{"attributes":{"NORMAL":94,"POSITION":45},"indices":143,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":95,"POSITION":46},"indices":144,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":95,"POSITION":46},"indices":144,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":95,"POSITION":46},"indices":144,"material":0,"mode":4}]},{"name":"ARM-00-08","primitives":[{"attributes":{"NORMAL":95,"POSITION":46},"indices":144,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"ARM-00-09","primitives":[{"attributes":{"NORMAL":96,"POSITION":47},"indices":145,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":97,"POSITION":48},"indices":146,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":97,"POSITION":48},"indices":146,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":97,"POSITION":48},"indices":146,"material":0,"mode":4}]},{"name":"L-bracket^ARM-00-00","primitives":[{"attributes":{"NORMAL":97,"POSITION":48},"indices":146,"material":0,"mode":4}]}],"nodes":[{"children":[1,27,56,71,72,73,96],"name":"RBT-00-00"},{"children":[2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26],"translation":[0.21214138642130749,0.06542792188427198,0.13567741509886556],"name":"NAUO1"},{"translation":[0.11805861357869255,0.2647720781157281,-0.02772741509886506],"mesh":0,"name":"NAUO101"},{"rotation":[0.0,-0.0,0.7071067811865479,0.7071067811865471],"translation":[-0.5423413864213077,0.2647720781157279,-0.027727415098865048],"mesh":1,"name":"NAUO102"},{"rotation":[0.0,0.0,-0.7071067811865475,0.7071067811865476],"translation":[0.11805861357869259,-0.3956279218842718,-0.02772741509886506],"mesh":2,"name":"NAUO103"},{"rotation":[0.0,0.0,1.0,-2.220446049250312e-16],"translation":[-0.5423413864213073,-0.395627921884272,-0.02772741509886506],"mesh":3,"name":"NAUO104"},{"translation":[0.10535861357869254,-0.2940279218842719,-0.09757741509886506],"mesh":4,"name":"NAUO105"},{"rotation":[0.0,0.0,-0.7071067811865474,0.7071067811865477],"translation":[-0.44074138642130736,0.25207207811572776,-0.09757741509886506],"mesh":5,"name":"NAUO106"},{"translation":[-0.5296413864213073,-0.29402792188427237,-0.09757741509886506],"mesh":6,"name":"NAUO107"},{"rotation":[0.0,0.0,-0.7071067811865474,0.7071067811865477],"translation":[-0.44074138642130708,-0.38292792188427196,-0.09757741509886506],"mesh":7,"name":"NAUO108"},{"rotation":[-0.5000000000000001,0.49999999999999997,0.5,0.5],"translation":[-0.2121413864213072,-0.14391392188427247,-0.1547274150988651],"mesh":8,"name":"NAUO109"},{"translation":[-0.09784138642130702,-0.3829279218842717,-0.12297741509886505],"mesh":9,"name":"NAUO110"},{"translation":[-0.3264413864213071,-0.38292792188427179,-0.12297741509886505],"mesh":10,"name":"NAUO111"},{"translation":[-0.0978413864213073,0.25207207811572798,-0.12297741509886505],"mesh":11,"name":"NAUO112"},{"translation":[-0.3264413864213073,0.25207207811572776,-0.12297741509886505],"mesh":12,"name":"NAUO113"},{"translation":[-0.5423413864213074,-0.06542792188427239,-0.053127415098865028],"mesh":13,"name":"NAUO114"},{"rotation":[6.986417961653758e-19,-5.143899031570203e-17,1.0,-3.061786935099064e-16],"translation":[0.11805861357869255,-0.06542792188427155,-0.05312741509886506],"mesh":14,"name":"NAUO115"},{"rotation":[-1.7539188962154345e-16,0.7071067811865475,2.563835365164946e-16,0.7071067811865476],"translation":[-0.11054138642130684,-0.3956279218842718,-0.1420274150988651],"mesh":15,"name":"NAUO116"},{"translation":[-0.3042163864213074,-0.22735292188427246,-0.15472741509886504],"mesh":16,"name":"NAUO117"},{"rotation":[-8.673617379884036e-19,-3.5300453521074238e-19,1.0,-8.673617379884038e-19],"translation":[-0.12006638642130749,-0.0559029218842725,-0.15472741509886504],"mesh":17,"name":"NAUO118"},{"translation":[-0.33279138642130726,-0.22735292188427243,-0.15472741509886504],"mesh":18,"name":"NAUO119"},{"rotation":[4.986098557373419e-19,-2.672492477452662e-19,1.0,8.673617379884034e-19],"translation":[-0.09149138642130712,-0.05590292188427244,-0.15472741509886504],"mesh":19,"name":"NAUO120"},{"rotation":[2.3565581545043415e-16,-0.7071067811865476,2.3537192215469846e-16,0.7071067811865475],"translation":[-0.3137413864213071,-0.3956279218842718,-0.14202741509886505],"mesh":20,"name":"NAUO121"},{"translation":[-0.5423413864213077,-0.06542792188427003,-0.12297741509886505],"mesh":21,"name":"NAUO122"},{"rotation":[0.0,0.0,1.0,3.4914813388431335e-15],"translation":[0.11805861357869209,-0.06542792188427277,-0.12297741509886505],"mesh":22,"name":"NAUO123"},{"translation":[-0.21214138642130729,0.2647720781157278,-0.122977415098865],"mesh":23,"name":"NAUO124"},{"rotation":[0.0,0.0,1.0,3.4914813388431335e-15],"translation":[-0.2121413864213104,-0.39562792188427,-0.12297741509886505],"mesh":24,"name":"NAUO125"},{"children":[28,29,30,31,32,33,34,41,42,46,47,48,49,50,51,52],"rotation":[-6.623827560072138e-17,-3.189250306701401e-17,0.707106781186549,0.707106781186546],"translation":[0.11429999999998147,0.22148268122384316,-0.1587893249326159],"name":"NAUO2"},{"rotation":[0.4999999999999966,0.4999999999999966,-0.5000000000000053,0.5000000000000018],"translation":[-0.005582681223843088,-1.3877787807814366e-17,-0.15236067506738402],"mesh":25,"name":"NAUO76"},{"rotation":[-0.5,0.49999999999999658,0.5,0.5000000000000036],"translation":[-0.005582681223841479,0.22860000000000303,-0.15236067506738258],"mesh":26,"name":"NAUO77"},{"rotation":[2.468850131082263e-15,0.7071067811865488,0.7071067811865462,2.468850131082254e-15],"translation":[-0.030982681223842763,-0.025399999999997218,0.13973932493261616],"mesh":27,"name":"NAUO78"},{"rotation":[2.468850131082263e-15,0.7071067811865488,0.7071067811865462,2.468850131082254e-15],"translation":[-0.030982681223840796,0.25400000000000269,0.13973932493261519],"mesh":28,"name":"NAUO79"},{"rotation":[-0.18301270189221817,-0.6830127018922145,0.6830127018922247,-0.18301270189221839],"translation":[-0.005582681223842478,0.006350000000002298,0.012739324932615872],"mesh":29,"name":"NAUO80"},{"rotation":[-0.18301270189221828,0.6830127018922247,0.6830127018922146,0.18301270189221817],"translation":[-0.005582681223841729,0.22225000000000209,0.012739324932612874],"mesh":30,"name":"NAUO81"},{"children":[35,36,37,38,39,40],"rotation":[-6.234669935009758e-15,0.2588190451025223,-3.4870081323131555e-15,0.9659258262890678],"translation":[-0.0028941953486844326,0.1855345726296745,-0.02255829890053291],"name":"NAUO82"},{"rotation":[1.996804166340685e-30,5.296612495676417e-59,-8.776077570583752e-30,1.0],"translation":[-0.019977108982175249,0.030365427370327364,0.02917899599515297],"mesh":31,"name":"NAUO95"},{"rotation":[1.996804166340685e-30,5.296612495676417e-59,-8.776077570583752e-30,1.0],"translation":[-0.019969776633756543,-0.15734776313485789,0.029166295995152928],"mesh":32,"name":"NAUO96"},{"rotation":[5.2557785042231248e-17,0.5000000000000001,-6.580388297704567e-17,0.8660254037844386],"translation":[-0.019955111936919235,0.01487861787551252,0.02919169599515291],"mesh":33,"name":"NAUO97"},{"rotation":[0.6123724356957946,0.3535533905932737,-0.3535533905932738,0.6123724356957946],"translation":[-0.019955111936919124,-0.1511175726296727,0.029191695995152996],"mesh":34,"name":"NAUO98"},{"rotation":[-0.7071067811865475,1.2326874191699776e-16,-5.511807515714702e-18,0.7071067811865476],"translation":[-0.019969776633756543,0.030309117875512549,0.02916629599515297],"mesh":35,"name":"NAUO99"},{"rotation":[-6.275350074315773e-30,0.7071067811865475,0.7071067811865475,-6.2753500743157748e-30],"translation":[-0.01999910602743138,-0.1636342631348579,0.02916629599515297],"mesh":36,"name":"NAUO100"},{"rotation":[0.6830127018922146,-0.18301270189221814,0.18301270189221814,0.6830127018922247],"translation":[-0.005582681223841702,0.23495000000000214,0.012739324932612682],"mesh":37,"name":"NAUO83"},{"children":[43,44,45],"rotation":[-3.2295213397925705e-31,-8.668877364968815e-32,0.7071067811865438,0.7071067811865511],"translation":[0.0033605090523277927,0.0025670869535548009,0.01793770752310858],"name":"NAUO84"},{"rotation":[-5.824012151826996e-31,3.396267472520518e-31,2.051038353574629e-29,1.0],"translation":[-0.03189208695355272,0.008943190276170325,0.01702661740950738],"mesh":38,"name":"NAUO92"},{"rotation":[0.7071067811865475,9.813077866780638e-18,-0.7071067811865475,-9.813077866766135e-18],"translation":[-0.0724170869535527,0.008943190276170325,-0.005198382590492656],"mesh":39,"name":"NAUO93"},{"rotation":[-0.5,-0.5,-0.5000000000000001,0.5000000000000001],"translation":[-0.08394208695355277,0.008943190276170325,-0.005198382590492656],"mesh":40,"name":"NAUO94"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223841506,-0.01587499999999846,0.165139324932616],"mesh":41,"name":"NAUO85"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223842562,0.24130000000000136,0.1651393249326168],"mesh":42,"name":"NAUO86"},{"rotation":[0.49999999999999658,-0.49999999999999836,0.5000000000000106,-0.4999999999999948],"translation":[-0.005582681223840037,0.20320000000000339,0.0952893249326131],"mesh":43,"name":"NAUO87"},{"rotation":[-0.4999999999999966,0.49999999999999836,-0.5000000000000036,0.5000000000000018],"translation":[-0.005582681223843505,0.025400000000002113,0.09528932493261593],"mesh":44,"name":"NAUO88"},{"rotation":[0.7071067811865426,-9.937183757282918e-15,-2.5381230457357168e-15,0.7071067811865525],"translation":[-0.0055826812238409799,0.1873250000000043,0.16513932493261339],"mesh":45,"name":"NAUO89"},{"rotation":[0.7071067811865487,6.05963491551116e-30,5.654351889878287e-30,0.7071067811865462],"translation":[-0.005582681223842562,0.038099999999981978,0.16513932493261616],"mesh":46,"name":"NAUO90"},{"children":[53,54,55],"rotation":[0.4999999999999974,-0.5000000000000061,-0.4999999999999957,0.5000000000000008],"translation":[-0.010781063814334443,0.22603291304644933,0.0037961346564424067],"name":"NAUO91"},{"rotation":[-5.824012151826996e-31,3.396267472520518e-31,2.051038353574629e-29,1.0],"translation":[-0.03189208695355272,0.008943190276170325,0.01702661740950738],"mesh":47,"name":"NAUO92"},{"rotation":[0.7071067811865475,9.813077866780638e-18,-0.7071067811865475,-9.813077866766135e-18],"translation":[-0.0724170869535527,0.008943190276170325,-0.005198382590492656],"mesh":48,"name":"NAUO93"},{"rotation":[-0.5,-0.5,-0.5000000000000001,0.5000000000000001],"translation":[-0.08394208695355277,0.008943190276170325,-0.005198382590492656],"mesh":49,"name":"NAUO94"},{"children":[57,58,61,64,65,66,67,68,69,70],"rotation":[-0.45029449496087689,-1.100501664646917e-15,3.322546818077275e-16,0.8928800971059491],"translation":[-1.4070901337315319e-14,0.21589774466199347,-0.5651301275705946],"name":"NAUO3"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"mesh":50,"name":"NAUO64"},{"children":[59,60],"rotation":[0.1830127018922198,0.18301270189221847,-0.6830127018922219,0.6830127018922171],"translation":[0.08613140000000001,-5.551115123125782e-16,-1.1102230246251556e-16],"name":"NAUO65"},{"rotation":[3.944304526105062e-30,5.5511151231257817e-17,-5.127595883936578e-30,1.0],"mesh":51,"name":"NAUO74"},{"rotation":[1.5508251547067147e-30,-0.2588190451025206,-4.498986871965784e-30,0.9659258262890683],"translation":[-0.03839320962149228,-0.001993899999999993,0.007951989730066944],"mesh":52,"name":"NAUO75"},{"children":[62,63],"rotation":[-0.5000000000000052,0.500000000000007,0.49999999999999647,0.4999999999999913],"translation":[-0.0861314,-3.33066907387547e-16,5.828670879282072e-16],"name":"NAUO66"},{"rotation":[3.944304526105062e-30,5.5511151231257817e-17,-5.127595883936578e-30,1.0],"mesh":53,"name":"NAUO74"},{"rotation":[1.5508251547067147e-30,-0.2588190451025206,-4.498986871965784e-30,0.9659258262890683],"translation":[-0.03839320962149228,-0.001993899999999993,0.007951989730066944],"mesh":54,"name":"NAUO75"},{"rotation":[-0.009100500670791337,-0.009100500670791191,-0.7070482168052904,0.7070482168052903],"translation":[0.03968749999999996,-0.0029418500950466614,-0.11426213510178374],"mesh":55,"name":"NAUO67"},{"rotation":[-0.0091005006707913,-0.009100500670791229,-0.7070482168052904,0.7070482168052903],"translation":[-0.03968750000000003,-0.0029418500950466614,-0.11426213510178361],"mesh":56,"name":"NAUO68"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"translation":[-0.06286499999999999,0.0,2.7755575615628916e-17],"mesh":57,"name":"NAUO69"},{"rotation":[1.4099018988904146e-60,-4.930380657631324e-31,2.859620781426167e-30,1.0],"translation":[0.062865,0.0,-2.7755575615628916e-17],"mesh":58,"name":"NAUO70"},{"rotation":[-0.5000000000000002,2.7940404561633018e-30,3.794496104309507e-31,0.8660254037844385],"mesh":59,"name":"NAUO71"},{"rotation":[-0.7071067811865462,0.7071067811865488,-2.4728956224269327e-15,-2.4728956224269576e-15],"translation":[0.04127500000000008,0.02927349999999973,-0.026083229927617057],"mesh":60,"name":"NAUO72"},{"rotation":[0.7071067811865511,0.7071067811865438,-2.4688501310822525e-15,-2.4688501310822785e-15],"translation":[-0.04127499999999962,-0.029273499999999509,-0.026083229927616526],"mesh":61,"name":"NAUO73"},{"rotation":[0.22129280107253516,-0.6715872960334054,-0.22129280107253733,0.6715872960334203],"translation":[-0.1079500000000319,0.21590000000000193,-0.5651499999999974],"mesh":62,"name":"NAUO4"},{"rotation":[0.22129280107253603,0.6715872960334129,0.2212928010725361,0.6715872960334129],"translation":[0.10794999999998989,0.2159000000000022,-0.5651499999999999],"mesh":63,"name":"NAUO5"},{"children":[74,75,76,82,83,84,85,86,92,93,94,95],"name":"NAUO6"},{"translation":[-0.3048000000000008,-2.6330028493691965e-18,-1.0846837446788913e-29],"mesh":64,"name":"NAUO47"},{"rotation":[-6.98296267768624e-15,6.98296267768634e-15,1.0,3.491481338843087e-15],"translation":[0.3048,-2.1280637800386269e-15,4.736828266594918e-15],"mesh":65,"name":"NAUO48"},{"children":[77,78,79,80,81],"rotation":[0.5000000000000027,0.49999999999999747,0.5000000000000027,-0.49999999999999747],"translation":[-0.29916754999999997,3.1422831228713813e-15,-0.4827270000000002],"name":"NAUO49"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":66,"name":"NAUO59"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"translation":[0.04222749999999997,0.0,0.0],"mesh":67,"name":"NAUO60"},{"rotation":[0.7071067811865475,-1.0197966064698899e-45,-1.0197966064698899e-45,0.7071067811865475],"translation":[0.4255769999999999,0.0,0.0],"mesh":68,"name":"NAUO61"},{"rotation":[1.0,1.4422101917317764e-45,0.0,0.0],"translation":[0.4605020000000001,0.0,0.0],"mesh":69,"name":"NAUO62"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":70,"name":"NAUO63"},{"rotation":[2.4688501310822698e-15,0.7071067811865487,-4.937700262164522e-15,0.7071067811865462],"translation":[-0.2762249999999999,2.9326593936879757e-15,0.012700000000000973],"mesh":71,"name":"NAUO50"},{"rotation":[0.7071067811865475,5.8545825673967158e-33,-0.7071067811865475,1.1576945416813776e-32],"translation":[-0.2788144785973512,2.815246350435978e-18,0.012699999999999996],"mesh":72,"name":"NAUO51"},{"rotation":[2.0807720770400135e-15,0.7071067811865427,-2.826565994914841e-15,0.7071067811865525],"translation":[0.2788144785973503,-1.9017702002227868e-15,0.012700000000004354],"mesh":73,"name":"NAUO52"},{"rotation":[-0.7071067811865475,6.20667528484673e-17,0.7071067811865476,-5.96945585410674e-17],"translation":[0.276225,3.797182960523879e-18,0.012700000000000428],"mesh":74,"name":"NAUO53"},{"children":[87,88,89,90,91],"rotation":[0.5000000000000001,-0.49999999999999997,0.5,0.5],"translation":[0.29916755000000008,9.936975113543188e-18,-0.4827269999999997],"name":"NAUO54"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":75,"name":"NAUO59"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"translation":[0.04222749999999997,0.0,0.0],"mesh":76,"name":"NAUO60"},{"rotation":[0.7071067811865475,-1.0197966064698899e-45,-1.0197966064698899e-45,0.7071067811865475],"translation":[0.4255769999999999,0.0,0.0],"mesh":77,"name":"NAUO61"},{"rotation":[1.0,1.4422101917317764e-45,0.0,0.0],"translation":[0.4605020000000001,0.0,0.0],"mesh":78,"name":"NAUO62"},{"rotation":[0.0,0.0,1.4422101917317764e-45,1.0],"mesh":79,"name":"NAUO63"},{"rotation":[0.0,0.0,-0.7071067811865488,0.7071067811865462],"translation":[0.2629394785973505,-8.835338985750676e-16,-0.2412999999999996],"mesh":80,"name":"NAUO55"},{"rotation":[-1.7334705305548197e-16,3.173068402831953e-16,0.7071067811865475,0.7071067811865475],"translation":[-0.2629394785973513,-1.3786260431350188e-16,-0.2412999999999999],"mesh":81,"name":"NAUO56"},{"rotation":[2.520843109358146e-16,4.2107967123869179e-16,1.0,8.326672684688684e-17],"translation":[-0.30075505000000327,2.3624337269355368e-15,-0.45732700000000006],"mesh":82,"name":"NAUO57"},{"translation":[0.3007550500000001,1.965344605669299e-17,-0.45732699999999967],"mesh":83,"name":"NAUO58"},{"children":[97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135],"rotation":[0.9537169507482262,5.979913941133367e-15,-1.5317460723878594e-14,-0.30070579950427536],"translation":[-1.4775782558445607e-14,-0.17185965600997736,-0.21147373804735579],"name":"NAUO7"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"mesh":84,"name":"NAUO8"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-1.2128309017896654e-15,0.057150000000000037,4.98212582300539e-15],"mesh":85,"name":"NAUO9"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[2.951458522026938e-14,0.08889999999999993,1.2587153541687713e-14],"mesh":86,"name":"NAUO10"},{"rotation":[-0.49999999999999836,-0.5,0.49999999999999836,0.5000000000000036],"translation":[-0.17790159999999928,0.032132423659477157,0.10624820000000117],"mesh":87,"name":"NAUO11"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[0.17790159999999753,0.03304822263134288,0.3062478000000017],"mesh":88,"name":"NAUO12"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[-0.1779016000000003,0.03185153328977558,0.3062477999999997],"mesh":89,"name":"NAUO13"},{"rotation":[-0.5,0.5000000000000018,0.5,-0.4999999999999982],"translation":[0.17790160000000003,0.03174999999999939,0.1062482000000006],"mesh":90,"name":"NAUO14"},{"rotation":[1.0517621765889297e-16,0.8715956043360786,-1.3191138484208325e-16,-0.4902255628810333],"translation":[-0.19238753749999994,0.006350000000000022,0.4745142385132651],"mesh":91,"name":"NAUO15"},{"rotation":[-4.2669914795297227e-29,0.32196751846396395,1.2547157722606882e-28,0.9467507153702907],"translation":[0.1923875375,0.006350000000000134,0.4745142385132652],"mesh":92,"name":"NAUO16"},{"rotation":[-8.408219704766098e-29,0.6344455211665221,1.024403360980117e-28,0.7729675806084887],"translation":[-0.19812017549278597,0.006350000000000022,-0.045562950533218408],"mesh":93,"name":"NAUO17"},{"rotation":[-2.739404892930054e-17,0.16087598606864818,-4.465205595936425e-18,0.9869746284005684],"translation":[0.19812017549278605,0.006350000000000022,-0.0455629505332185],"mesh":94,"name":"NAUO18"},{"rotation":[1.321794255046816e-29,-0.09973650480882869,1.318678289613043e-28,0.9950138841285174],"translation":[-0.13408241974616207,0.08027500000000004,0.5204532085796464],"mesh":95,"name":"NAUO19"},{"rotation":[-1.9220544407933126e-29,0.1450293729489867,1.3112745323881628e-28,0.9894273500272892],"translation":[-0.1463854738363756,0.08027499999999999,0.4464017652164423],"mesh":96,"name":"NAUO20"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.2286,0.05317500000000002,-0.1142498239867352],"mesh":97,"name":"NAUO21"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[0.22860000000000139,0.05317500000000002,-0.11424982398672599],"mesh":98,"name":"NAUO22"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[0.16509999999999148,0.0802750000000001,0.5425783010132702],"mesh":99,"name":"NAUO23"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.17790160000000056,0.05317500000000002,2.2343238370581277e-15],"mesh":100,"name":"NAUO24"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[0.17790159999999875,0.05317500000000002,9.492406860545089e-15],"mesh":101,"name":"NAUO25"},{"rotation":[1.6348545851843093e-17,-0.6735103550716421,-2.326778429634253e-17,0.739177787552677],"translation":[0.17790159999999326,0.08027500000000015,0.4249928000000057],"mesh":102,"name":"NAUO26"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.17790159999999995,0.03492500000000004,0.21249640000000009],"mesh":103,"name":"NAUO27"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[0.1779015999999944,0.03492500000000004,0.2124964000000082],"mesh":104,"name":"NAUO28"},{"rotation":[2.773322151996486e-17,-0.018098543678665169,-1.4861183033140006e-18,0.9998362079444371],"translation":[0.17790159999999317,0.03492500000000004,0.4249928000000056],"mesh":105,"name":"NAUO29"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.1779016000000005,0.03492500000000004,2.248201624865942e-15],"mesh":106,"name":"NAUO30"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.17790159999999995,0.03492500000000004,0.4249928000000002],"mesh":107,"name":"NAUO31"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[0.1779015999999988,0.03492500000000004,9.46465128492946e-15],"mesh":108,"name":"NAUO32"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.14638547383637558,0.09527540000000007,0.4464017652164424],"mesh":109,"name":"NAUO33"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.17790160000000844,0.09527540000000007,0.42499280000000069],"mesh":110,"name":"NAUO34"},{"rotation":[-2.666385907586555e-17,-0.2777037341443709,7.707826991913912e-18,0.9606667663879461],"translation":[-0.16510000000000867,0.09527540000000007,0.5425783010132659],"mesh":111,"name":"NAUO35"},{"rotation":[-1.1686180229208462e-28,0.881785320353058,-6.250725946064994e-29,-0.47165098198758678],"translation":[-0.134082419746162,0.09527540000000007,0.5204532085796465],"mesh":112,"name":"NAUO36"},{"rotation":[-0.7071067811865475,-3.084683192085888e-28,-4.9589210809482e-28,0.7071067811865475],"translation":[0.2286,0.033225000000000007,-0.11424982398673542],"mesh":113,"name":"NAUO37"},{"rotation":[0.7023653799149907,-0.08174884156286733,0.08174884156286731,0.7023653799149907],"translation":[0.16510000000000003,0.022225000000000107,0.5425783010132651],"mesh":114,"name":"NAUO38"},{"rotation":[-0.7071067811865475,-3.084683192085888e-28,-4.9589210809482e-28,0.7071067811865475],"translation":[0.16510000000000006,0.04762500000000003,0.542578301013265],"mesh":115,"name":"NAUO39"},{"rotation":[-0.7071067811865475,-3.084683192085888e-28,-4.9589210809482e-28,0.7071067811865475],"translation":[-0.2286,0.033225000000000007,-0.11424982398673521],"mesh":116,"name":"NAUO40"},{"rotation":[0.7023653799149907,-0.08174884156286733,0.08174884156286731,0.7023653799149907],"translation":[-0.16510000000000855,0.022224999999999996,0.5425783010132655],"mesh":117,"name":"NAUO41"},{"rotation":[0.7023653799149907,-0.08174884156286733,0.08174884156286731,0.7023653799149907],"translation":[-0.16510000000000853,0.047624999999999927,0.5425783010132655],"mesh":118,"name":"NAUO42"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"mesh":119,"name":"NAUO43"},{"rotation":[-7.537813945767643e-56,5.687687126643495e-28,1.3252863207712996e-28,1.0],"translation":[-0.08254999999999999,0.0,2.7755575615628916e-17],"mesh":120,"name":"NAUO44"},{"rotation":[-1.3252863207712996e-28,1.0,-4.627212457597823e-43,-3.4914813388437038e-15],"translation":[3.989863994745521e-15,0.0,0.9947570385132643],"mesh":121,"name":"NAUO45"},{"rotation":[-1.3252863207712996e-28,1.0,-4.627212457597823e-43,-3.4914813388437038e-15],"translation":[0.08255000000000296,1.6653345369377349e-16,0.9947570385132643],"mesh":122,"name":"NAUO46"}],"scene":0,"scenes":[{"nodes":[0]}]} \ No newline at end of file diff --git a/Choreo/TestPath.chor b/Choreo/TestPath.chor new file mode 100644 index 0000000..dcacbe5 --- /dev/null +++ b/Choreo/TestPath.chor @@ -0,0 +1,1388 @@ +{ + "version": "v0.4", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 4704, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "TestPath": { + "waypoints": [ + { + "x": 0.723591148853302, + "y": 6.720295429229736, + "heading": -2.111215526396717, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 33 + }, + { + "x": 8.283105850219727, + "y": 7.439164638519287, + "heading": -3.125593998990179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7235911488533018, + "y": 6.720295429229736, + "heading": -2.111215526396717, + "angularVelocity": 1.0953254453084821e-17, + "velocityX": -1.1931304681766194e-16, + "velocityY": 6.936441479720161e-18, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 0.7577272241858162, + "y": 6.725953311496341, + "heading": -2.1236849565019162, + "angularVelocity": -0.15243353741778315, + "velocityX": 0.41729916063416983, + "velocityY": 0.06916523056092934, + "moduleForcesX": [ + 95.13238870926358, + 91.73935445924184, + 96.23804098831587, + 94.83570858070391 + ], + "moduleForcesY": [ + 14.657952739314128, + 29.16477970427033, + 2.263152317387709, + 16.556673891184168 + ], + "timestamp": 0.08180240593020663 + }, + { + "x": 0.8260058702121654, + "y": 6.737271294018102, + "heading": -2.1482853052603463, + "angularVelocity": -0.30072891473954527, + "velocityX": 0.8346777326040308, + "velocityY": 0.13835757608917584, + "moduleForcesX": [ + 95.05923910974651, + 91.85369295006373, + 96.22290178816708, + 94.8815813358542 + ], + "moduleForcesY": [ + 15.071445617945367, + 28.778322852661553, + 2.565256431685438, + 16.2520916183596 + ], + "timestamp": 0.16360481186041326 + }, + { + "x": 0.9284353599886986, + "y": 6.754252183346831, + "heading": -2.184565358581994, + "angularVelocity": -0.4435083896267034, + "velocityX": 1.2521574434575131, + "velocityY": 0.20758422659886122, + "moduleForcesX": [ + 94.95679965958053, + 92.00758596457024, + 96.20058551703323, + 94.94404493021392 + ], + "moduleForcesY": [ + 15.635629710673738, + 28.249598619606644, + 2.9847014017825204, + 15.828256617541507 + ], + "timestamp": 0.24540721779061989 + }, + { + "x": 1.0650265493929258, + "y": 6.776899486366739, + "heading": -2.231893766998151, + "angularVelocity": -0.5785698828536732, + "velocityX": 1.6697697918762886, + "velocityY": 0.276853753213816, + "moduleForcesX": [ + 94.8301569217571, + 92.22024415100945, + 96.16422147911038, + 95.01452261907505 + ], + "moduleForcesY": [ + 16.295674353798006, + 27.50023040101659, + 3.6221304131259098, + 15.318983826749179 + ], + "timestamp": 0.3272096237208265 + }, + { + "x": 1.2357941680243119, + "y": 6.80521768263762, + "heading": -2.289318503612598, + "angularVelocity": -0.7019932477710497, + "velocityX": 2.0875623455747143, + "velocityY": 0.3461780346143984, + "moduleForcesX": [ + 94.69205253647733, + 92.5242335987186, + 96.09756500714057, + 95.07850491285728 + ], + "moduleForcesY": [ + 16.951798752318002, + 26.385132123418188, + 4.660851032087815, + 14.78882818954975 + ], + "timestamp": 0.4090120296510331 + }, + { + "x": 1.4407586381713822, + "y": 6.839212930641203, + "heading": -2.355228903144546, + "angularVelocity": -0.80572690720207, + "velocityX": 2.5056043452290693, + "velocityY": 0.41557760681259437, + "moduleForcesX": [ + 94.57320592479594, + 92.9842154377374, + 95.95447243910232, + 95.10638403930349 + ], + "moduleForcesY": [ + 17.400554734461085, + 24.5801326461777, + 6.492170204562177, + 14.381942959316689 + ], + "timestamp": 0.4908144355812397 + }, + { + "x": 1.6799436862541839, + "y": 6.878894655290743, + "heading": -2.4263208762365993, + "angularVelocity": -0.8690694641962708, + "velocityX": 2.9239366297404237, + "velocityY": 0.48509238978381664, + "moduleForcesX": [ + 94.5506886638703, + 93.7629446356306, + 95.55628442602168, + 95.01126947226477 + ], + "moduleForcesY": [ + 17.1315041907569, + 21.0933166843758, + 10.212702268001271, + 14.521623148642945 + ], + "timestamp": 0.5726168415114463 + }, + { + "x": 1.9532656630777232, + "y": 6.9242675587031295, + "heading": -2.492242611011071, + "angularVelocity": -0.8058654757638622, + "velocityX": 3.3412461481838056, + "velocityY": 0.554664655990544, + "moduleForcesX": [ + 94.8422610463681, + 95.3108652848089, + 93.564377928277, + 94.23736947387722 + ], + "moduleForcesY": [ + 14.202552505226105, + 10.332766213991022, + 20.965628177900896, + 17.51026163388106 + ], + "timestamp": 0.6544192474416529 + }, + { + "x": 2.252224229767989, + "y": 6.97425005510552, + "heading": -2.492316321749889, + "angularVelocity": -0.0009010827735485682, + "velocityX": 3.6546426695870893, + "velocityY": 0.6110149915277239, + "moduleForcesX": [ + 89.93494974840866, + 71.39063573539269, + 69.80705643514082, + 52.70881734329257 + ], + "moduleForcesY": [ + 1.1140058065512373, + -48.60829657851154, + 54.327840310050156, + 44.2026310712367 + ], + "timestamp": 0.7362216533718595 + }, + { + "x": 2.5512585929561014, + "y": 7.023816952888593, + "heading": -2.4923752160986172, + "angularVelocity": -0.0007199586376259815, + "velocityX": 3.6555692929186328, + "velocityY": 0.6059344736780476, + "moduleForcesX": [ + 0.21786351005909635, + 0.20870530736507098, + 0.21091345871254757, + 0.20175524144047333 + ], + "moduleForcesY": [ + -1.1514520209173875, + -1.1584011007075974, + -1.1422970446285325, + -1.1492461543598806 + ], + "timestamp": 0.8180240593020661 + }, + { + "x": 2.850297433788597, + "y": 7.073356818345146, + "heading": -2.492434109928239, + "angularVelocity": -0.0007199522917783797, + "velocityX": 3.65562405690461, + "velocityY": 0.6056040137079765, + "moduleForcesX": [ + 0.012400140340371013, + 0.012399819516533697, + 0.012399896874506431, + 0.012399576050669119 + ], + "moduleForcesY": [ + -0.07482397077647213, + -0.07482421424217461, + -0.0748236499531071, + -0.07482389341880961 + ], + "timestamp": 0.8998264652322727 + }, + { + "x": 3.14933657103151, + "y": 7.122894929862625, + "heading": -2.492493003811556, + "angularVelocity": -0.0007199529481734056, + "velocityX": 3.655627607666916, + "velocityY": 0.6055825796779969, + "moduleForcesX": [ + 0.0008039471572979603, + 0.0008039803437903328, + 0.0008039723386991704, + 0.0008040055251915429 + ], + "moduleForcesY": [ + -0.004853165969804552, + -0.00485314078840341, + -0.0048531991562967185, + -0.004853173974895575 + ], + "timestamp": 0.9816288711624793 + }, + { + "x": 3.4483757071920436, + "y": 7.172432928367156, + "heading": -2.4925518977563463, + "angularVelocity": -0.0007199536996633355, + "velocityX": 3.655627837949571, + "velocityY": 0.6055811894639996, + "moduleForcesX": [ + 0.000052108013388694385, + 0.00005214600945410682, + 0.000052136840695771065, + 0.00005217483676118351 + ], + "moduleForcesY": [ + -0.0003147726569071702, + -0.00031474382960009384, + -0.0003148106529725816, + -0.00031478182566550524 + ], + "timestamp": 1.063431277092686 + }, + { + "x": 3.747414869116877, + "y": 7.2219709190187, + "heading": -2.492610791763129, + "angularVelocity": -0.0007199544574856417, + "velocityX": 3.6556278528684367, + "velocityY": 0.6055810993037084, + "moduleForcesX": [ + 0.000003344290161493637, + 0.000003382608109684488, + 0.000003373358123408973, + 0.000003411676071599823 + ], + "moduleForcesY": [ + -0.000020409791648895615, + -0.000020380723686980284, + -0.000020448109597086467, + -0.000020419041635171126 + ], + "timestamp": 1.1452336830228926 + }, + { + "x": 4.046454017061653, + "y": 7.271508909696975, + "heading": -2.492669685831927, + "angularVelocity": -0.000719955215611945, + "velocityX": 3.6556278538193374, + "velocityY": 0.6055810934618735, + "moduleForcesX": [ + 1.8160022363983863e-7, + 2.199352571610246e-7, + 2.1067758736106308e-7, + 2.490126208822491e-7 + ], + "moduleForcesY": [ + -0.0000013181003716116259, + -0.0000012890230078904014, + -0.0000013564354051328119, + -0.0000013273580414115874 + ], + "timestamp": 1.2270360889530993 + }, + { + "x": 4.345493171916041, + "y": 7.32104689979607, + "heading": -2.492728579962766, + "angularVelocity": -0.0007199559740240183, + "velocityX": 3.655627853873855, + "velocityY": 0.6055810930310193, + "moduleForcesX": [ + -2.1374592405069132e-8, + 1.6976602568068766e-8, + 7.7114742335871e-9, + 4.6062669206725e-8 + ], + "moduleForcesY": [ + -9.292300015532494e-8, + -6.383693351666872e-8, + -1.3127419512846282e-7, + -1.021881284898066e-7 + ], + "timestamp": 1.308838494883306 + }, + { + "x": 4.644532338221993, + "y": 7.370584890433222, + "heading": -2.492787474155635, + "angularVelocity": -0.0007199567323240461, + "velocityX": 3.6556278540182605, + "velocityY": 0.6055810920576199, + "moduleForcesX": [ + -1.0166114532307651e-9, + 3.733063203040434e-8, + 2.8062898670513328e-8, + 6.641014215414841e-8 + ], + "moduleForcesY": [ + -2.1576670992600522e-7, + -1.8668719980226116e-7, + -2.541139534096403e-7, + -2.2503444328589622e-7 + ], + "timestamp": 1.3906409008135128 + }, + { + "x": 4.943571482063242, + "y": 7.4201228793349525, + "heading": -2.492846368410551, + "angularVelocity": -0.0007199574908202425, + "velocityX": 3.655627856450527, + "velocityY": 0.60558107727333, + "moduleForcesX": [ + 5.170006516399061e-7, + 5.553595266840418e-7, + 5.460854264126991e-7, + 5.844443014568349e-7 + ], + "moduleForcesY": [ + -0.000003342874907899535, + -0.0000033137901331267416, + -0.0000033812337829436708, + -0.0000033521490081708774 + ], + "timestamp": 1.4724433067437195 + }, + { + "x": 5.242610646117307, + "y": 7.4696608499797295, + "heading": -2.492905262727423, + "angularVelocity": -0.000719958248198361, + "velocityX": 3.655627894215044, + "velocityY": 0.6055808492039377, + "moduleForcesX": [ + 0.000008517105719077667, + 0.000008555409761070612, + 0.000008546145364671363, + 0.000008584449406664307 + ], + "moduleForcesY": [ + -0.0000516356579026866, + -0.000051606618257092915, + -0.000051673961944679516, + -0.00005164492229908583 + ], + "timestamp": 1.5542457126739262 + }, + { + "x": 5.5416498369418346, + "y": 7.519198532525922, + "heading": -2.4929641571050025, + "angularVelocity": -0.0007199589903281, + "velocityX": 3.655628476722988, + "velocityY": 0.6055773327481002, + "moduleForcesX": [ + 0.00013186054495488035, + 0.00013189807949265153, + 0.00013188899773175296, + 0.0001319265322695241 + ], + "moduleForcesY": [ + -0.0007962040065681791, + -0.0007961755537913086, + -0.000796241541105944, + -0.0007962130883290733 + ], + "timestamp": 1.636048118604133 + }, + { + "x": 5.840689788714502, + "y": 7.568731780332111, + "heading": -2.4930230515239487, + "angularVelocity": -0.0007199594960188154, + "velocityX": 3.6556374574858914, + "velocityY": 0.6055231169021489, + "moduleForcesX": [ + 0.0020334340769035566, + 0.0020334596542576017, + 0.002033453463254187, + 0.0020334790406082316 + ], + "moduleForcesY": [ + -0.012275743266230177, + -0.012275723879879885, + -0.012275768843583206, + -0.012275749457232912 + ], + "timestamp": 1.7178505245343396 + }, + { + "x": 6.13974104317783, + "y": 7.618196649659188, + "heading": -2.4930819457408244, + "angularVelocity": -0.0007199570257876147, + "velocityX": 3.655775817191724, + "velocityY": 0.6046872200301471, + "moduleForcesX": [ + 0.03132801083671379, + 0.03132788588860738, + 0.031327916144162526, + 0.031327791196056116 + ], + "moduleForcesY": [ + -0.18926678400677058, + -0.18926687869893, + -0.18926665905984474, + -0.1892667537520042 + ], + "timestamp": 1.7996529304645463 + }, + { + "x": 6.438964639990969, + "y": 7.666607945276391, + "heading": -2.493140846849599, + "angularVelocity": -0.0007200412763564167, + "velocityX": 3.6578826805954936, + "velocityY": 0.5918077244194747, + "moduleForcesX": [ + 0.4770398224007296, + 0.47704408886545374, + 0.47704305336350017, + 0.4770473198282167 + ], + "moduleForcesY": [ + -2.9162210920332026, + -2.916217864276581, + -2.916225348956017, + -2.9162221211994117 + ], + "timestamp": 1.881455336394753 + }, + { + "x": 6.7400869789705276, + "y": 7.701294700323264, + "heading": -2.4932010186878983, + "angularVelocity": -0.0007355754102317422, + "velocityX": 3.6810939391212796, + "velocityY": 0.4240309855462845, + "moduleForcesX": [ + 5.254784560404537, + 5.255715903329803, + 5.25543696959438, + 5.256368308240926 + ], + "moduleForcesY": [ + -37.98856022681178, + -37.98802186640095, + -37.989200499013286, + -37.98866214312402 + ], + "timestamp": 1.9632577423249598 + }, + { + "x": 7.042051526481832, + "y": 7.703854164932228, + "heading": -2.496083506103539, + "angularVelocity": -0.035237196065060886, + "velocityX": 3.6913892776063815, + "velocityY": 0.03128838043669109, + "moduleForcesX": [ + -0.5131181103743951, + 2.8144249926569724, + 1.8764683611619357, + 5.146654665852247 + ], + "moduleForcesY": [ + -88.94598545278514, + -88.70624701966364, + -89.17904565592751, + -88.87348097115982 + ], + "timestamp": 2.0450601482551662 + }, + { + "x": 7.3159128076819595, + "y": 7.695470072126158, + "heading": -2.564557795212664, + "angularVelocity": -0.8370693787027762, + "velocityX": 3.3478390552326878, + "velocityY": -0.10249201171946311, + "moduleForcesX": [ + -95.16504284068242, + -89.8727533881857, + -71.77682575685434, + -54.336873900065136 + ], + "moduleForcesY": [ + -11.987099194062607, + 32.55720054677347, + -63.56562077075268, + -78.1686261193333 + ], + "timestamp": 2.1268625541853727 + }, + { + "x": 7.556311034412061, + "y": 7.6790347107736, + "heading": -2.649049475519964, + "angularVelocity": -1.032875247938158, + "velocityX": 2.9387673211916443, + "velocityY": -0.20091537300352555, + "moduleForcesX": [ + -94.9084342470495, + -95.8282615383345, + -89.32763449621226, + -90.4296285719666 + ], + "moduleForcesY": [ + -15.068782312431582, + -6.394953748196941, + -35.398885148440776, + -32.2788611774532 + ], + "timestamp": 2.2086649601155792 + }, + { + "x": 7.762744704458051, + "y": 7.655403771202389, + "heading": -2.7379426173904067, + "angularVelocity": -1.0866812639508867, + "velocityX": 2.523564703055092, + "velocityY": -0.28887829689307887, + "moduleForcesX": [ + -94.57661276830882, + -94.83247775739406, + -93.19246452145045, + -93.44511093002149 + ], + "moduleForcesY": [ + -17.406383668867363, + -15.889376348780091, + -23.71730489148478, + -22.65445841343478 + ], + "timestamp": 2.2904673660457857 + }, + { + "x": 7.935092302435521, + "y": 7.624985450298367, + "heading": -2.8260897872684283, + "angularVelocity": -1.077562070159109, + "velocityX": 2.1068769587213065, + "velocityY": -0.37185117632421305, + "moduleForcesX": [ + -94.24821923368096, + -94.22331177998737, + -94.47307334472593, + -94.44713200088614 + ], + "moduleForcesY": [ + -19.285904135245183, + -19.41394264633224, + -18.153319119476812, + -18.29490233989464 + ], + "timestamp": 2.372269771975992 + }, + { + "x": 8.073313135874166, + "y": 7.588017849593726, + "heading": -2.9105632356686155, + "angularVelocity": -1.0326523705481847, + "velocityX": 1.6896915370741834, + "velocityY": -0.45191336796574816, + "moduleForcesX": [ + -93.92962533071282, + -93.92006235182441, + -95.02590532426422, + -94.96688687854932 + ], + "moduleForcesY": [ + -20.886167520727035, + -20.953306957000848, + -15.137025278692608, + -15.535374995968011 + ], + "timestamp": 2.4540721779061987 + }, + { + "x": 8.177391969807777, + "y": 7.544656680913358, + "heading": -2.9894662981032174, + "angularVelocity": -0.9645567454596974, + "velocityX": 1.2723199028996386, + "velocityY": -0.5300720425079714, + "moduleForcesX": [ + -93.63054251216636, + -93.78893587985038, + -95.29968094257883, + -95.29197221759159 + ], + "moduleForcesY": [ + -22.255074584683925, + -21.60676410729548, + -13.412316192927724, + -13.513715183042004 + ], + "timestamp": 2.535874583836405 + }, + { + "x": 8.247323315092254, + "y": 7.49501173678298, + "heading": -3.0614700110183795, + "angularVelocity": -0.8802150999912234, + "velocityX": 0.8548813600752648, + "velocityY": -0.6068885582449909, + "moduleForcesX": [ + -93.35875637815658, + -93.75385753876255, + -95.4445391321016, + -95.51457728088783 + ], + "moduleForcesY": [ + -23.414893013893387, + -21.80867916490578, + -12.421603089211969, + -11.927109151127686 + ], + "timestamp": 2.6176769897666117 + }, + { + "x": 8.283105850219727, + "y": 7.439164638519287, + "heading": -3.125593998990179, + "angularVelocity": -0.7838887773877783, + "velocityX": 0.43742639841990505, + "velocityY": -0.6827072833806145, + "moduleForcesX": [ + -93.1187700450084, + -93.77083871108206, + -95.52263726576938, + -95.67435477806364 + ], + "moduleForcesY": [ + -24.385127065875785, + -21.771991689483382, + -11.87048635120808, + -10.640986001481616 + ], + "timestamp": 2.699479395696818 + }, + { + "x": 8.275898245132629, + "y": 7.361846362147694, + "heading": -3.191010868155049, + "angularVelocity": -0.6551331823095494, + "velocityX": -0.07218231700436661, + "velocityY": -0.774322727838524, + "moduleForcesX": [ + -92.9089458924044, + -93.83763069600508, + -95.56352646300107, + -95.80534842812062 + ], + "moduleForcesY": [ + -25.232613867087057, + -21.545751167591764, + -11.665466913153518, + -9.532272266434608 + ], + "timestamp": 2.799332175148509 + }, + { + "x": 8.217789777207303, + "y": 7.275476721098813, + "heading": -3.2436371761542473, + "angularVelocity": -0.5270389896802727, + "velocityX": -0.5819414168057278, + "velocityY": -0.8649698219168481, + "moduleForcesX": [ + -92.903588592184, + -93.96151379686765, + -95.52285840742927, + -95.83907171208575 + ], + "moduleForcesY": [ + -25.227520329041567, + -20.972125046118922, + -11.936712741166867, + -9.121257127428992 + ], + "timestamp": 2.8991849546002 + }, + { + "x": 8.108760004308806, + "y": 7.180190329323991, + "heading": -3.283555808706276, + "angularVelocity": -0.3997748762832392, + "velocityX": -1.091905232613487, + "velocityY": -0.9542687957197068, + "moduleForcesX": [ + -92.93731213024337, + -94.07473097235011, + -95.50023614869534, + -95.866646809436 + ], + "moduleForcesY": [ + -25.067964224128147, + -20.419767904766065, + -12.037931780772084, + -8.731683695063731 + ], + "timestamp": 2.9990377340518912 + }, + { + "x": 7.948779341315781, + "y": 7.076187580845669, + "heading": -3.310880239000986, + "angularVelocity": -0.2736471678063067, + "velocityX": -1.6021653534899039, + "velocityY": -1.0415608828486471, + "moduleForcesX": [ + -93.02012672348597, + -94.18592804778983, + -95.50167347150334, + -95.89104788722628 + ], + "moduleForcesY": [ + -24.705640250061386, + -19.841338914754612, + -11.909295005918898, + -8.312019503273916 + ], + "timestamp": 3.0988905135035822 + }, + { + "x": 7.737800987525542, + "y": 6.963798526731024, + "heading": -3.3257881664902467, + "angularVelocity": -0.1492990738062935, + "velocityX": -2.1128941435110864, + "velocityY": -1.1255475647607016, + "moduleForcesX": [ + -93.17981821761498, + -94.31443307151197, + -95.53616352094721, + -95.91610080841099 + ], + "moduleForcesY": [ + -24.00567642399376, + -19.119657574468487, + -11.429975968225625, + -7.760465240295746 + ], + "timestamp": 3.1987432929552733 + }, + { + "x": 7.475739881792017, + "y": 6.843669004629576, + "heading": -3.3286214140139636, + "angularVelocity": -0.028374247959601157, + "velocityX": -2.6244748301340293, + "velocityY": -1.2030664104890665, + "moduleForcesX": [ + -93.49694581572524, + -94.51204039840655, + -95.62179794832846, + -95.94781494882054 + ], + "moduleForcesY": [ + -22.550099126216203, + -17.904977932919422, + -10.263423866721734, + -6.798320249867356 + ], + "timestamp": 3.2985960724069643 + }, + { + "x": 7.162400592151543, + "y": 6.7176290311369, + "heading": -3.3203583608518663, + "angularVelocity": 0.08275236009960128, + "velocityX": -3.1380126981548124, + "velocityY": -1.262258020697683, + "moduleForcesX": [ + -94.28109580745485, + -94.97229266492786, + -95.79508980007127, + -95.98229486276374 + ], + "moduleForcesY": [ + -18.35760590231681, + -14.54231668075914, + -6.872120919312799, + -4.146480147937392 + ], + "timestamp": 3.3984488518586553 + }, + { + "x": 6.80601537289982, + "y": 6.618111915454223, + "heading": -3.320313068048379, + "angularVelocity": 0.00045359582113106104, + "velocityX": -3.5691066470153108, + "velocityY": -0.9966384088782563, + "moduleForcesX": [ + -78.95467888636652, + -76.95520120222969, + -82.73624014699872, + -81.21356460648425 + ], + "moduleForcesY": [ + 51.277036005904584, + 53.921241048671874, + 44.784632790420325, + 47.09943277699262 + ], + "timestamp": 3.4983016313103463 + }, + { + "x": 6.442926260897238, + "y": 6.546762452334811, + "heading": -3.3202777142315334, + "angularVelocity": 0.00035405941667094167, + "velocityX": -3.636244415351703, + "velocityY": -0.7145465890265993, + "moduleForcesX": [ + -12.455832844815848, + -12.449451390978085, + -12.4577244126878, + -12.451342678563787 + ], + "moduleForcesY": [ + 52.32794150283485, + 52.32799328195423, + 52.32413746080307, + 52.32418925677162 + ], + "timestamp": 3.5981544107620373 + }, + { + "x": 6.0795333305691415, + "y": 6.476976723504877, + "heading": -3.3202423729046426, + "angularVelocity": 0.00035393433297164096, + "velocityX": -3.6392870923962186, + "velocityY": -0.6988861912629167, + "moduleForcesX": [ + -0.5643979963447718, + -0.56439159116073, + -0.5643991552960914, + -0.5643927501120266 + ], + "moduleForcesY": [ + 2.904898348816943, + 2.9048971964131893, + 2.904891954019758, + 2.9048908016159896 + ], + "timestamp": 3.6980071902137284 + }, + { + "x": 5.716129009250863, + "y": 6.407250328199235, + "heading": -3.320207031543497, + "angularVelocity": 0.00035393467602626457, + "velocityX": -3.6394011532386092, + "velocityY": -0.698291987196345, + "moduleForcesX": [ + -0.021157483652564316, + -0.021157501205314427, + -0.02115748048361132, + -0.021157498036361425 + ], + "moduleForcesY": [ + 0.1102207002240024, + 0.11022070339292976, + 0.11022071777671147, + 0.11022072094563878 + ], + "timestamp": 3.7978599696654194 + }, + { + "x": 5.352724258498889, + "y": 6.3375261800976155, + "heading": -3.3201716901716383, + "angularVelocity": 0.0003539347833152721, + "velocityX": -3.63940547318192, + "velocityY": -0.6982694718699336, + "moduleForcesX": [ + -0.0008013171090822399, + -0.000801322598665709, + -0.000801316118201953, + -0.000801321607785422 + ], + "moduleForcesY": [ + 0.004176432795334672, + 0.004176433786214948, + 0.004176438284918122, + 0.004176439275798398 + ], + "timestamp": 3.8977127491171104 + }, + { + "x": 4.989319488930932, + "y": 6.26780211882884, + "heading": -3.3201363487909856, + "angularVelocity": 0.0003539348713868943, + "velocityX": -3.639405636864583, + "velocityY": -0.6982686187433157, + "moduleForcesX": [ + -0.00003036014590563864, + -0.000030364652235018398, + -0.000030359332668939323, + -0.000030363838998319076 + ], + "moduleForcesY": [ + 0.0001582463784475217, + 0.00015824719168422102, + 0.00015825088477690145, + 0.00015825169801360074 + ], + "timestamp": 3.9975655285688014 + }, + { + "x": 4.625914720026027, + "y": 6.198078059112632, + "heading": -3.320101007401615, + "angularVelocity": 0.00035393495868955816, + "velocityX": -3.6394056430662434, + "velocityY": -0.6982685864138914, + "moduleForcesX": [ + -0.000001148534287448773, + -0.0000011530013002516125, + -0.0000011477283092472209, + -0.0000011521953220500606 + ], + "moduleForcesY": [ + 0.000005994246259781661, + 0.000005995052237983213, + 0.0000059987132725845, + 0.000005999519250786051 + ], + "timestamp": 4.097418308020493 + }, + { + "x": 4.262509951038118, + "y": 6.128354000339291, + "heading": -3.3200656660035315, + "angularVelocity": 0.0003539350459510133, + "velocityX": -3.639405643318748, + "velocityY": -0.6982685850917263, + "moduleForcesX": [ + -4.5008107633990304e-8, + -4.947304034975265e-8, + -4.4202667796223006e-8, + -4.866760051198534e-8 + ], + "moduleForcesY": [ + 2.4261721596538975e-7, + 2.4342265580315705e-7, + 2.470821486811521e-7, + 2.4788758851891935e-7 + ], + "timestamp": 4.197271087472184 + }, + { + "x": 3.899105182546151, + "y": 6.058629942355329, + "heading": -3.3200303245967318, + "angularVelocity": 0.00035393513323821684, + "velocityX": -3.6394056438191997, + "velocityY": -0.6982685824772484, + "moduleForcesX": [ + -9.099983784292496e-8, + -9.546611644631589e-8, + -9.019431849061925e-8, + -9.466059709401017e-8 + ], + "moduleForcesY": [ + 4.823315062045864e-7, + 4.831370255568921e-7, + 4.867977848079773e-7, + 4.87603304160283e-7 + ], + "timestamp": 4.297123866923875 + }, + { + "x": 3.5357004118221393, + "y": 5.988905889397698, + "heading": -3.319994983181212, + "angularVelocity": 0.000353935220572087, + "velocityX": -3.6394056568219217, + "velocityY": -0.6982685147003905, + "moduleForcesX": [ + -0.0000024100828073008026, + -0.0000024145515024069127, + -0.0000024092770142660406, + -0.000002413745709372151 + ], + "moduleForcesY": [ + 0.000012569497256119221, + 0.000012570303049153985, + 0.000012573965951225331, + 0.000012574771744260094 + ], + "timestamp": 4.396976646375566 + }, + { + "x": 3.1722956084102734, + "y": 5.919182016036882, + "heading": -3.319959641753852, + "angularVelocity": 0.0003539353391446321, + "velocityX": -3.6394059999801684, + "velocityY": -0.69826672600501, + "moduleForcesX": [ + -0.00006365097504612675, + -0.00006365704219861527, + -0.00006364988124242055, + -0.00006365594839490907 + ], + "moduleForcesY": [ + 0.00033178693886780613, + 0.00033178803267151227, + 0.00033179300602029455, + 0.0003317940998240007 + ], + "timestamp": 4.496829425827257 + }, + { + "x": 2.8088900399053154, + "y": 5.849462882527497, + "heading": -3.3199240650931148, + "angularVelocity": 0.00035629114114364946, + "velocityX": -3.639413644585623, + "velocityY": -0.6982192486493579, + "moduleForcesX": [ + -0.0013686131704105154, + -0.0014891562687215552, + -0.0013468857334453688, + -0.0014674288317313169 + ], + "moduleForcesY": [ + 0.008735583117033041, + 0.008757310553014873, + 0.008856126213492287, + 0.008877853649459875 + ], + "timestamp": 4.596682205278948 + }, + { + "x": 2.4532247267194442, + "y": 5.781353045466849, + "heading": -3.299135360037311, + "angularVelocity": 0.20819355424606867, + "velocityX": -3.561896973670374, + "velocityY": -0.6821025813017123, + "moduleForcesX": [ + 18.868598944322862, + 8.32571363330943, + 20.348816615507218, + 9.972078331030568 + ], + "moduleForcesY": [ + -3.285533267638885, + -1.6202335960945649, + 7.437451110648959, + 9.42643335735913 + ], + "timestamp": 4.696534984730639 + }, + { + "x": 2.1481491038692218, + "y": 5.722895709329591, + "heading": -3.2630668401523923, + "angularVelocity": 0.36121698447193346, + "velocityX": -3.055254191420303, + "velocityY": -0.5854352373206007, + "moduleForcesX": [ + 95.62438307992944, + 95.16549067183493, + 93.35192749436273, + 91.77301060402911 + ], + "moduleForcesY": [ + 8.698058484871185, + 12.37287648011562, + 22.527375059298596, + 28.126162716809013 + ], + "timestamp": 4.79638776418233 + }, + { + "x": 1.893918919733568, + "y": 5.67416974242098, + "heading": -3.2266605845857983, + "angularVelocity": 0.3645993207835013, + "velocityX": -2.5460501436061524, + "velocityY": -0.4879780693566059, + "moduleForcesX": [ + 94.49456074129621, + 94.47644542180454, + 94.43142039606398, + 94.41277288887017 + ], + "moduleForcesY": [ + 17.86520147577287, + 17.957985013996876, + 18.196759690401734, + 18.290553923527458 + ], + "timestamp": 4.896240543634021 + }, + { + "x": 1.6905463426612732, + "y": 5.63518580098643, + "heading": -3.194492220115979, + "angularVelocity": 0.3221579273641397, + "velocityX": -2.0367242719846113, + "velocityY": -0.3904141835695623, + "moduleForcesX": [ + 93.95193830631428, + 94.19640950574573, + 94.79615129087857, + 94.96109021202487 + ], + "moduleForcesY": [ + 20.758019444351795, + 19.639392628256566, + 16.472251298678543, + 15.520018379563828 + ], + "timestamp": 4.996093323085712 + }, + { + "x": 1.5380260018243401, + "y": 5.605946840237555, + "heading": -3.168787387706304, + "angularVelocity": 0.25742730999325175, + "velocityX": -1.5274521085978272, + "velocityY": -0.2928207049112749, + "moduleForcesX": [ + 93.66310688465127, + 94.03395023535653, + 94.98702800360682, + 95.18165416889012 + ], + "moduleForcesY": [ + 22.131228901998117, + 20.52067691520685, + 15.485662425458505, + 14.274070597153358 + ], + "timestamp": 5.095946102537403 + }, + { + "x": 1.4363512552558055, + "y": 5.5864538973635876, + "heading": -3.150863748064947, + "angularVelocity": 0.17950065826642012, + "velocityX": -1.0182465405134766, + "velocityY": -0.19521682614201732, + "moduleForcesX": [ + 93.48733465079248, + 93.92658984979096, + 95.10458301157264, + 95.29781991554545 + ], + "moduleForcesY": [ + 22.923872857045062, + 21.07263381336957, + 14.842494589006758, + 13.580354171192587 + ], + "timestamp": 5.195798881989094 + }, + { + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "angularVelocity": 0.09284763554946453, + "velocityX": -0.5090991956375953, + "velocityY": -0.09760904249885234, + "moduleForcesX": [ + 93.36693167786208, + 93.85448481654781, + 95.18105538852447, + 95.3706555686842 + ], + "moduleForcesY": [ + 23.44951629201878, + 21.433762366300886, + 14.409999663578962, + 13.128974415173587 + ], + "timestamp": 5.295651661440785 + }, + { + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "angularVelocity": -5.523755381949148e-18, + "velocityX": -7.397578436541153e-17, + "velocityY": 5.726973296434314e-18, + "moduleForcesX": [ + 93.2742477353994, + 93.80974821349238, + 95.22986105500625, + 95.42354502309182 + ], + "moduleForcesY": [ + 23.843716595149832, + 21.65816851036705, + 14.131988322823306, + 12.7893133457974 + ], + "timestamp": 5.395504440892476 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.723591148853302, + "y": 6.720295429229736, + "heading": -2.111215526396717, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 33 + }, + { + "timestamp": 2.699479395696818, + "isStopPoint": false, + "x": 8.283105850219727, + "y": 7.439164638519287, + "heading": -3.125593998990179, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "timestamp": 5.395504440892476, + "isStopPoint": true, + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + } + }, + "splitTrajectoriesAtStopPoints": false, + "usesObstacles": false +} \ No newline at end of file diff --git a/Choreo/deploy/choreo/TestPath.traj b/Choreo/deploy/choreo/TestPath.traj new file mode 100644 index 0000000..a5146b5 --- /dev/null +++ b/Choreo/deploy/choreo/TestPath.traj @@ -0,0 +1,1286 @@ +{ + "samples": [ + { + "x": 0.7235911488533018, + "y": 6.720295429229736, + "heading": -2.111215526396717, + "angularVelocity": 1.0953254453084821e-17, + "velocityX": -1.1931304681766194e-16, + "velocityY": 6.936441479720161e-18, + "moduleForcesX": [ + 0, + 0, + 0, + 0 + ], + "moduleForcesY": [ + 0, + 0, + 0, + 0 + ], + "timestamp": 0 + }, + { + "x": 0.7577272241858162, + "y": 6.725953311496341, + "heading": -2.1236849565019162, + "angularVelocity": -0.15243353741778315, + "velocityX": 0.41729916063416983, + "velocityY": 0.06916523056092934, + "moduleForcesX": [ + 95.13238870926358, + 91.73935445924184, + 96.23804098831587, + 94.83570858070391 + ], + "moduleForcesY": [ + 14.657952739314128, + 29.16477970427033, + 2.263152317387709, + 16.556673891184168 + ], + "timestamp": 0.08180240593020663 + }, + { + "x": 0.8260058702121654, + "y": 6.737271294018102, + "heading": -2.1482853052603463, + "angularVelocity": -0.30072891473954527, + "velocityX": 0.8346777326040308, + "velocityY": 0.13835757608917584, + "moduleForcesX": [ + 95.05923910974651, + 91.85369295006373, + 96.22290178816708, + 94.8815813358542 + ], + "moduleForcesY": [ + 15.071445617945367, + 28.778322852661553, + 2.565256431685438, + 16.2520916183596 + ], + "timestamp": 0.16360481186041326 + }, + { + "x": 0.9284353599886986, + "y": 6.754252183346831, + "heading": -2.184565358581994, + "angularVelocity": -0.4435083896267034, + "velocityX": 1.2521574434575131, + "velocityY": 0.20758422659886122, + "moduleForcesX": [ + 94.95679965958053, + 92.00758596457024, + 96.20058551703323, + 94.94404493021392 + ], + "moduleForcesY": [ + 15.635629710673738, + 28.249598619606644, + 2.9847014017825204, + 15.828256617541507 + ], + "timestamp": 0.24540721779061989 + }, + { + "x": 1.0650265493929258, + "y": 6.776899486366739, + "heading": -2.231893766998151, + "angularVelocity": -0.5785698828536732, + "velocityX": 1.6697697918762886, + "velocityY": 0.276853753213816, + "moduleForcesX": [ + 94.8301569217571, + 92.22024415100945, + 96.16422147911038, + 95.01452261907505 + ], + "moduleForcesY": [ + 16.295674353798006, + 27.50023040101659, + 3.6221304131259098, + 15.318983826749179 + ], + "timestamp": 0.3272096237208265 + }, + { + "x": 1.2357941680243119, + "y": 6.80521768263762, + "heading": -2.289318503612598, + "angularVelocity": -0.7019932477710497, + "velocityX": 2.0875623455747143, + "velocityY": 0.3461780346143984, + "moduleForcesX": [ + 94.69205253647733, + 92.5242335987186, + 96.09756500714057, + 95.07850491285728 + ], + "moduleForcesY": [ + 16.951798752318002, + 26.385132123418188, + 4.660851032087815, + 14.78882818954975 + ], + "timestamp": 0.4090120296510331 + }, + { + "x": 1.4407586381713822, + "y": 6.839212930641203, + "heading": -2.355228903144546, + "angularVelocity": -0.80572690720207, + "velocityX": 2.5056043452290693, + "velocityY": 0.41557760681259437, + "moduleForcesX": [ + 94.57320592479594, + 92.9842154377374, + 95.95447243910232, + 95.10638403930349 + ], + "moduleForcesY": [ + 17.400554734461085, + 24.5801326461777, + 6.492170204562177, + 14.381942959316689 + ], + "timestamp": 0.4908144355812397 + }, + { + "x": 1.6799436862541839, + "y": 6.878894655290743, + "heading": -2.4263208762365993, + "angularVelocity": -0.8690694641962708, + "velocityX": 2.9239366297404237, + "velocityY": 0.48509238978381664, + "moduleForcesX": [ + 94.5506886638703, + 93.7629446356306, + 95.55628442602168, + 95.01126947226477 + ], + "moduleForcesY": [ + 17.1315041907569, + 21.0933166843758, + 10.212702268001271, + 14.521623148642945 + ], + "timestamp": 0.5726168415114463 + }, + { + "x": 1.9532656630777232, + "y": 6.9242675587031295, + "heading": -2.492242611011071, + "angularVelocity": -0.8058654757638622, + "velocityX": 3.3412461481838056, + "velocityY": 0.554664655990544, + "moduleForcesX": [ + 94.8422610463681, + 95.3108652848089, + 93.564377928277, + 94.23736947387722 + ], + "moduleForcesY": [ + 14.202552505226105, + 10.332766213991022, + 20.965628177900896, + 17.51026163388106 + ], + "timestamp": 0.6544192474416529 + }, + { + "x": 2.252224229767989, + "y": 6.97425005510552, + "heading": -2.492316321749889, + "angularVelocity": -0.0009010827735485682, + "velocityX": 3.6546426695870893, + "velocityY": 0.6110149915277239, + "moduleForcesX": [ + 89.93494974840866, + 71.39063573539269, + 69.80705643514082, + 52.70881734329257 + ], + "moduleForcesY": [ + 1.1140058065512373, + -48.60829657851154, + 54.327840310050156, + 44.2026310712367 + ], + "timestamp": 0.7362216533718595 + }, + { + "x": 2.5512585929561014, + "y": 7.023816952888593, + "heading": -2.4923752160986172, + "angularVelocity": -0.0007199586376259815, + "velocityX": 3.6555692929186328, + "velocityY": 0.6059344736780476, + "moduleForcesX": [ + 0.21786351005909635, + 0.20870530736507098, + 0.21091345871254757, + 0.20175524144047333 + ], + "moduleForcesY": [ + -1.1514520209173875, + -1.1584011007075974, + -1.1422970446285325, + -1.1492461543598806 + ], + "timestamp": 0.8180240593020661 + }, + { + "x": 2.850297433788597, + "y": 7.073356818345146, + "heading": -2.492434109928239, + "angularVelocity": -0.0007199522917783797, + "velocityX": 3.65562405690461, + "velocityY": 0.6056040137079765, + "moduleForcesX": [ + 0.012400140340371013, + 0.012399819516533697, + 0.012399896874506431, + 0.012399576050669119 + ], + "moduleForcesY": [ + -0.07482397077647213, + -0.07482421424217461, + -0.0748236499531071, + -0.07482389341880961 + ], + "timestamp": 0.8998264652322727 + }, + { + "x": 3.14933657103151, + "y": 7.122894929862625, + "heading": -2.492493003811556, + "angularVelocity": -0.0007199529481734056, + "velocityX": 3.655627607666916, + "velocityY": 0.6055825796779969, + "moduleForcesX": [ + 0.0008039471572979603, + 0.0008039803437903328, + 0.0008039723386991704, + 0.0008040055251915429 + ], + "moduleForcesY": [ + -0.004853165969804552, + -0.00485314078840341, + -0.0048531991562967185, + -0.004853173974895575 + ], + "timestamp": 0.9816288711624793 + }, + { + "x": 3.4483757071920436, + "y": 7.172432928367156, + "heading": -2.4925518977563463, + "angularVelocity": -0.0007199536996633355, + "velocityX": 3.655627837949571, + "velocityY": 0.6055811894639996, + "moduleForcesX": [ + 0.000052108013388694385, + 0.00005214600945410682, + 0.000052136840695771065, + 0.00005217483676118351 + ], + "moduleForcesY": [ + -0.0003147726569071702, + -0.00031474382960009384, + -0.0003148106529725816, + -0.00031478182566550524 + ], + "timestamp": 1.063431277092686 + }, + { + "x": 3.747414869116877, + "y": 7.2219709190187, + "heading": -2.492610791763129, + "angularVelocity": -0.0007199544574856417, + "velocityX": 3.6556278528684367, + "velocityY": 0.6055810993037084, + "moduleForcesX": [ + 0.000003344290161493637, + 0.000003382608109684488, + 0.000003373358123408973, + 0.000003411676071599823 + ], + "moduleForcesY": [ + -0.000020409791648895615, + -0.000020380723686980284, + -0.000020448109597086467, + -0.000020419041635171126 + ], + "timestamp": 1.1452336830228926 + }, + { + "x": 4.046454017061653, + "y": 7.271508909696975, + "heading": -2.492669685831927, + "angularVelocity": -0.000719955215611945, + "velocityX": 3.6556278538193374, + "velocityY": 0.6055810934618735, + "moduleForcesX": [ + 1.8160022363983863e-7, + 2.199352571610246e-7, + 2.1067758736106308e-7, + 2.490126208822491e-7 + ], + "moduleForcesY": [ + -0.0000013181003716116259, + -0.0000012890230078904014, + -0.0000013564354051328119, + -0.0000013273580414115874 + ], + "timestamp": 1.2270360889530993 + }, + { + "x": 4.345493171916041, + "y": 7.32104689979607, + "heading": -2.492728579962766, + "angularVelocity": -0.0007199559740240183, + "velocityX": 3.655627853873855, + "velocityY": 0.6055810930310193, + "moduleForcesX": [ + -2.1374592405069132e-8, + 1.6976602568068766e-8, + 7.7114742335871e-9, + 4.6062669206725e-8 + ], + "moduleForcesY": [ + -9.292300015532494e-8, + -6.383693351666872e-8, + -1.3127419512846282e-7, + -1.021881284898066e-7 + ], + "timestamp": 1.308838494883306 + }, + { + "x": 4.644532338221993, + "y": 7.370584890433222, + "heading": -2.492787474155635, + "angularVelocity": -0.0007199567323240461, + "velocityX": 3.6556278540182605, + "velocityY": 0.6055810920576199, + "moduleForcesX": [ + -1.0166114532307651e-9, + 3.733063203040434e-8, + 2.8062898670513328e-8, + 6.641014215414841e-8 + ], + "moduleForcesY": [ + -2.1576670992600522e-7, + -1.8668719980226116e-7, + -2.541139534096403e-7, + -2.2503444328589622e-7 + ], + "timestamp": 1.3906409008135128 + }, + { + "x": 4.943571482063242, + "y": 7.4201228793349525, + "heading": -2.492846368410551, + "angularVelocity": -0.0007199574908202425, + "velocityX": 3.655627856450527, + "velocityY": 0.60558107727333, + "moduleForcesX": [ + 5.170006516399061e-7, + 5.553595266840418e-7, + 5.460854264126991e-7, + 5.844443014568349e-7 + ], + "moduleForcesY": [ + -0.000003342874907899535, + -0.0000033137901331267416, + -0.0000033812337829436708, + -0.0000033521490081708774 + ], + "timestamp": 1.4724433067437195 + }, + { + "x": 5.242610646117307, + "y": 7.4696608499797295, + "heading": -2.492905262727423, + "angularVelocity": -0.000719958248198361, + "velocityX": 3.655627894215044, + "velocityY": 0.6055808492039377, + "moduleForcesX": [ + 0.000008517105719077667, + 0.000008555409761070612, + 0.000008546145364671363, + 0.000008584449406664307 + ], + "moduleForcesY": [ + -0.0000516356579026866, + -0.000051606618257092915, + -0.000051673961944679516, + -0.00005164492229908583 + ], + "timestamp": 1.5542457126739262 + }, + { + "x": 5.5416498369418346, + "y": 7.519198532525922, + "heading": -2.4929641571050025, + "angularVelocity": -0.0007199589903281, + "velocityX": 3.655628476722988, + "velocityY": 0.6055773327481002, + "moduleForcesX": [ + 0.00013186054495488035, + 0.00013189807949265153, + 0.00013188899773175296, + 0.0001319265322695241 + ], + "moduleForcesY": [ + -0.0007962040065681791, + -0.0007961755537913086, + -0.000796241541105944, + -0.0007962130883290733 + ], + "timestamp": 1.636048118604133 + }, + { + "x": 5.840689788714502, + "y": 7.568731780332111, + "heading": -2.4930230515239487, + "angularVelocity": -0.0007199594960188154, + "velocityX": 3.6556374574858914, + "velocityY": 0.6055231169021489, + "moduleForcesX": [ + 0.0020334340769035566, + 0.0020334596542576017, + 0.002033453463254187, + 0.0020334790406082316 + ], + "moduleForcesY": [ + -0.012275743266230177, + -0.012275723879879885, + -0.012275768843583206, + -0.012275749457232912 + ], + "timestamp": 1.7178505245343396 + }, + { + "x": 6.13974104317783, + "y": 7.618196649659188, + "heading": -2.4930819457408244, + "angularVelocity": -0.0007199570257876147, + "velocityX": 3.655775817191724, + "velocityY": 0.6046872200301471, + "moduleForcesX": [ + 0.03132801083671379, + 0.03132788588860738, + 0.031327916144162526, + 0.031327791196056116 + ], + "moduleForcesY": [ + -0.18926678400677058, + -0.18926687869893, + -0.18926665905984474, + -0.1892667537520042 + ], + "timestamp": 1.7996529304645463 + }, + { + "x": 6.438964639990969, + "y": 7.666607945276391, + "heading": -2.493140846849599, + "angularVelocity": -0.0007200412763564167, + "velocityX": 3.6578826805954936, + "velocityY": 0.5918077244194747, + "moduleForcesX": [ + 0.4770398224007296, + 0.47704408886545374, + 0.47704305336350017, + 0.4770473198282167 + ], + "moduleForcesY": [ + -2.9162210920332026, + -2.916217864276581, + -2.916225348956017, + -2.9162221211994117 + ], + "timestamp": 1.881455336394753 + }, + { + "x": 6.7400869789705276, + "y": 7.701294700323264, + "heading": -2.4932010186878983, + "angularVelocity": -0.0007355754102317422, + "velocityX": 3.6810939391212796, + "velocityY": 0.4240309855462845, + "moduleForcesX": [ + 5.254784560404537, + 5.255715903329803, + 5.25543696959438, + 5.256368308240926 + ], + "moduleForcesY": [ + -37.98856022681178, + -37.98802186640095, + -37.989200499013286, + -37.98866214312402 + ], + "timestamp": 1.9632577423249598 + }, + { + "x": 7.042051526481832, + "y": 7.703854164932228, + "heading": -2.496083506103539, + "angularVelocity": -0.035237196065060886, + "velocityX": 3.6913892776063815, + "velocityY": 0.03128838043669109, + "moduleForcesX": [ + -0.5131181103743951, + 2.8144249926569724, + 1.8764683611619357, + 5.146654665852247 + ], + "moduleForcesY": [ + -88.94598545278514, + -88.70624701966364, + -89.17904565592751, + -88.87348097115982 + ], + "timestamp": 2.0450601482551662 + }, + { + "x": 7.3159128076819595, + "y": 7.695470072126158, + "heading": -2.564557795212664, + "angularVelocity": -0.8370693787027762, + "velocityX": 3.3478390552326878, + "velocityY": -0.10249201171946311, + "moduleForcesX": [ + -95.16504284068242, + -89.8727533881857, + -71.77682575685434, + -54.336873900065136 + ], + "moduleForcesY": [ + -11.987099194062607, + 32.55720054677347, + -63.56562077075268, + -78.1686261193333 + ], + "timestamp": 2.1268625541853727 + }, + { + "x": 7.556311034412061, + "y": 7.6790347107736, + "heading": -2.649049475519964, + "angularVelocity": -1.032875247938158, + "velocityX": 2.9387673211916443, + "velocityY": -0.20091537300352555, + "moduleForcesX": [ + -94.9084342470495, + -95.8282615383345, + -89.32763449621226, + -90.4296285719666 + ], + "moduleForcesY": [ + -15.068782312431582, + -6.394953748196941, + -35.398885148440776, + -32.2788611774532 + ], + "timestamp": 2.2086649601155792 + }, + { + "x": 7.762744704458051, + "y": 7.655403771202389, + "heading": -2.7379426173904067, + "angularVelocity": -1.0866812639508867, + "velocityX": 2.523564703055092, + "velocityY": -0.28887829689307887, + "moduleForcesX": [ + -94.57661276830882, + -94.83247775739406, + -93.19246452145045, + -93.44511093002149 + ], + "moduleForcesY": [ + -17.406383668867363, + -15.889376348780091, + -23.71730489148478, + -22.65445841343478 + ], + "timestamp": 2.2904673660457857 + }, + { + "x": 7.935092302435521, + "y": 7.624985450298367, + "heading": -2.8260897872684283, + "angularVelocity": -1.077562070159109, + "velocityX": 2.1068769587213065, + "velocityY": -0.37185117632421305, + "moduleForcesX": [ + -94.24821923368096, + -94.22331177998737, + -94.47307334472593, + -94.44713200088614 + ], + "moduleForcesY": [ + -19.285904135245183, + -19.41394264633224, + -18.153319119476812, + -18.29490233989464 + ], + "timestamp": 2.372269771975992 + }, + { + "x": 8.073313135874166, + "y": 7.588017849593726, + "heading": -2.9105632356686155, + "angularVelocity": -1.0326523705481847, + "velocityX": 1.6896915370741834, + "velocityY": -0.45191336796574816, + "moduleForcesX": [ + -93.92962533071282, + -93.92006235182441, + -95.02590532426422, + -94.96688687854932 + ], + "moduleForcesY": [ + -20.886167520727035, + -20.953306957000848, + -15.137025278692608, + -15.535374995968011 + ], + "timestamp": 2.4540721779061987 + }, + { + "x": 8.177391969807777, + "y": 7.544656680913358, + "heading": -2.9894662981032174, + "angularVelocity": -0.9645567454596974, + "velocityX": 1.2723199028996386, + "velocityY": -0.5300720425079714, + "moduleForcesX": [ + -93.63054251216636, + -93.78893587985038, + -95.29968094257883, + -95.29197221759159 + ], + "moduleForcesY": [ + -22.255074584683925, + -21.60676410729548, + -13.412316192927724, + -13.513715183042004 + ], + "timestamp": 2.535874583836405 + }, + { + "x": 8.247323315092254, + "y": 7.49501173678298, + "heading": -3.0614700110183795, + "angularVelocity": -0.8802150999912234, + "velocityX": 0.8548813600752648, + "velocityY": -0.6068885582449909, + "moduleForcesX": [ + -93.35875637815658, + -93.75385753876255, + -95.4445391321016, + -95.51457728088783 + ], + "moduleForcesY": [ + -23.414893013893387, + -21.80867916490578, + -12.421603089211969, + -11.927109151127686 + ], + "timestamp": 2.6176769897666117 + }, + { + "x": 8.283105850219727, + "y": 7.439164638519287, + "heading": -3.125593998990179, + "angularVelocity": -0.7838887773877783, + "velocityX": 0.43742639841990505, + "velocityY": -0.6827072833806145, + "moduleForcesX": [ + -93.1187700450084, + -93.77083871108206, + -95.52263726576938, + -95.67435477806364 + ], + "moduleForcesY": [ + -24.385127065875785, + -21.771991689483382, + -11.87048635120808, + -10.640986001481616 + ], + "timestamp": 2.699479395696818 + }, + { + "x": 8.275898245132629, + "y": 7.361846362147694, + "heading": -3.191010868155049, + "angularVelocity": -0.6551331823095494, + "velocityX": -0.07218231700436661, + "velocityY": -0.774322727838524, + "moduleForcesX": [ + -92.9089458924044, + -93.83763069600508, + -95.56352646300107, + -95.80534842812062 + ], + "moduleForcesY": [ + -25.232613867087057, + -21.545751167591764, + -11.665466913153518, + -9.532272266434608 + ], + "timestamp": 2.799332175148509 + }, + { + "x": 8.217789777207303, + "y": 7.275476721098813, + "heading": -3.2436371761542473, + "angularVelocity": -0.5270389896802727, + "velocityX": -0.5819414168057278, + "velocityY": -0.8649698219168481, + "moduleForcesX": [ + -92.903588592184, + -93.96151379686765, + -95.52285840742927, + -95.83907171208575 + ], + "moduleForcesY": [ + -25.227520329041567, + -20.972125046118922, + -11.936712741166867, + -9.121257127428992 + ], + "timestamp": 2.8991849546002 + }, + { + "x": 8.108760004308806, + "y": 7.180190329323991, + "heading": -3.283555808706276, + "angularVelocity": -0.3997748762832392, + "velocityX": -1.091905232613487, + "velocityY": -0.9542687957197068, + "moduleForcesX": [ + -92.93731213024337, + -94.07473097235011, + -95.50023614869534, + -95.866646809436 + ], + "moduleForcesY": [ + -25.067964224128147, + -20.419767904766065, + -12.037931780772084, + -8.731683695063731 + ], + "timestamp": 2.9990377340518912 + }, + { + "x": 7.948779341315781, + "y": 7.076187580845669, + "heading": -3.310880239000986, + "angularVelocity": -0.2736471678063067, + "velocityX": -1.6021653534899039, + "velocityY": -1.0415608828486471, + "moduleForcesX": [ + -93.02012672348597, + -94.18592804778983, + -95.50167347150334, + -95.89104788722628 + ], + "moduleForcesY": [ + -24.705640250061386, + -19.841338914754612, + -11.909295005918898, + -8.312019503273916 + ], + "timestamp": 3.0988905135035822 + }, + { + "x": 7.737800987525542, + "y": 6.963798526731024, + "heading": -3.3257881664902467, + "angularVelocity": -0.1492990738062935, + "velocityX": -2.1128941435110864, + "velocityY": -1.1255475647607016, + "moduleForcesX": [ + -93.17981821761498, + -94.31443307151197, + -95.53616352094721, + -95.91610080841099 + ], + "moduleForcesY": [ + -24.00567642399376, + -19.119657574468487, + -11.429975968225625, + -7.760465240295746 + ], + "timestamp": 3.1987432929552733 + }, + { + "x": 7.475739881792017, + "y": 6.843669004629576, + "heading": -3.3286214140139636, + "angularVelocity": -0.028374247959601157, + "velocityX": -2.6244748301340293, + "velocityY": -1.2030664104890665, + "moduleForcesX": [ + -93.49694581572524, + -94.51204039840655, + -95.62179794832846, + -95.94781494882054 + ], + "moduleForcesY": [ + -22.550099126216203, + -17.904977932919422, + -10.263423866721734, + -6.798320249867356 + ], + "timestamp": 3.2985960724069643 + }, + { + "x": 7.162400592151543, + "y": 6.7176290311369, + "heading": -3.3203583608518663, + "angularVelocity": 0.08275236009960128, + "velocityX": -3.1380126981548124, + "velocityY": -1.262258020697683, + "moduleForcesX": [ + -94.28109580745485, + -94.97229266492786, + -95.79508980007127, + -95.98229486276374 + ], + "moduleForcesY": [ + -18.35760590231681, + -14.54231668075914, + -6.872120919312799, + -4.146480147937392 + ], + "timestamp": 3.3984488518586553 + }, + { + "x": 6.80601537289982, + "y": 6.618111915454223, + "heading": -3.320313068048379, + "angularVelocity": 0.00045359582113106104, + "velocityX": -3.5691066470153108, + "velocityY": -0.9966384088782563, + "moduleForcesX": [ + -78.95467888636652, + -76.95520120222969, + -82.73624014699872, + -81.21356460648425 + ], + "moduleForcesY": [ + 51.277036005904584, + 53.921241048671874, + 44.784632790420325, + 47.09943277699262 + ], + "timestamp": 3.4983016313103463 + }, + { + "x": 6.442926260897238, + "y": 6.546762452334811, + "heading": -3.3202777142315334, + "angularVelocity": 0.00035405941667094167, + "velocityX": -3.636244415351703, + "velocityY": -0.7145465890265993, + "moduleForcesX": [ + -12.455832844815848, + -12.449451390978085, + -12.4577244126878, + -12.451342678563787 + ], + "moduleForcesY": [ + 52.32794150283485, + 52.32799328195423, + 52.32413746080307, + 52.32418925677162 + ], + "timestamp": 3.5981544107620373 + }, + { + "x": 6.0795333305691415, + "y": 6.476976723504877, + "heading": -3.3202423729046426, + "angularVelocity": 0.00035393433297164096, + "velocityX": -3.6392870923962186, + "velocityY": -0.6988861912629167, + "moduleForcesX": [ + -0.5643979963447718, + -0.56439159116073, + -0.5643991552960914, + -0.5643927501120266 + ], + "moduleForcesY": [ + 2.904898348816943, + 2.9048971964131893, + 2.904891954019758, + 2.9048908016159896 + ], + "timestamp": 3.6980071902137284 + }, + { + "x": 5.716129009250863, + "y": 6.407250328199235, + "heading": -3.320207031543497, + "angularVelocity": 0.00035393467602626457, + "velocityX": -3.6394011532386092, + "velocityY": -0.698291987196345, + "moduleForcesX": [ + -0.021157483652564316, + -0.021157501205314427, + -0.02115748048361132, + -0.021157498036361425 + ], + "moduleForcesY": [ + 0.1102207002240024, + 0.11022070339292976, + 0.11022071777671147, + 0.11022072094563878 + ], + "timestamp": 3.7978599696654194 + }, + { + "x": 5.352724258498889, + "y": 6.3375261800976155, + "heading": -3.3201716901716383, + "angularVelocity": 0.0003539347833152721, + "velocityX": -3.63940547318192, + "velocityY": -0.6982694718699336, + "moduleForcesX": [ + -0.0008013171090822399, + -0.000801322598665709, + -0.000801316118201953, + -0.000801321607785422 + ], + "moduleForcesY": [ + 0.004176432795334672, + 0.004176433786214948, + 0.004176438284918122, + 0.004176439275798398 + ], + "timestamp": 3.8977127491171104 + }, + { + "x": 4.989319488930932, + "y": 6.26780211882884, + "heading": -3.3201363487909856, + "angularVelocity": 0.0003539348713868943, + "velocityX": -3.639405636864583, + "velocityY": -0.6982686187433157, + "moduleForcesX": [ + -0.00003036014590563864, + -0.000030364652235018398, + -0.000030359332668939323, + -0.000030363838998319076 + ], + "moduleForcesY": [ + 0.0001582463784475217, + 0.00015824719168422102, + 0.00015825088477690145, + 0.00015825169801360074 + ], + "timestamp": 3.9975655285688014 + }, + { + "x": 4.625914720026027, + "y": 6.198078059112632, + "heading": -3.320101007401615, + "angularVelocity": 0.00035393495868955816, + "velocityX": -3.6394056430662434, + "velocityY": -0.6982685864138914, + "moduleForcesX": [ + -0.000001148534287448773, + -0.0000011530013002516125, + -0.0000011477283092472209, + -0.0000011521953220500606 + ], + "moduleForcesY": [ + 0.000005994246259781661, + 0.000005995052237983213, + 0.0000059987132725845, + 0.000005999519250786051 + ], + "timestamp": 4.097418308020493 + }, + { + "x": 4.262509951038118, + "y": 6.128354000339291, + "heading": -3.3200656660035315, + "angularVelocity": 0.0003539350459510133, + "velocityX": -3.639405643318748, + "velocityY": -0.6982685850917263, + "moduleForcesX": [ + -4.5008107633990304e-8, + -4.947304034975265e-8, + -4.4202667796223006e-8, + -4.866760051198534e-8 + ], + "moduleForcesY": [ + 2.4261721596538975e-7, + 2.4342265580315705e-7, + 2.470821486811521e-7, + 2.4788758851891935e-7 + ], + "timestamp": 4.197271087472184 + }, + { + "x": 3.899105182546151, + "y": 6.058629942355329, + "heading": -3.3200303245967318, + "angularVelocity": 0.00035393513323821684, + "velocityX": -3.6394056438191997, + "velocityY": -0.6982685824772484, + "moduleForcesX": [ + -9.099983784292496e-8, + -9.546611644631589e-8, + -9.019431849061925e-8, + -9.466059709401017e-8 + ], + "moduleForcesY": [ + 4.823315062045864e-7, + 4.831370255568921e-7, + 4.867977848079773e-7, + 4.87603304160283e-7 + ], + "timestamp": 4.297123866923875 + }, + { + "x": 3.5357004118221393, + "y": 5.988905889397698, + "heading": -3.319994983181212, + "angularVelocity": 0.000353935220572087, + "velocityX": -3.6394056568219217, + "velocityY": -0.6982685147003905, + "moduleForcesX": [ + -0.0000024100828073008026, + -0.0000024145515024069127, + -0.0000024092770142660406, + -0.000002413745709372151 + ], + "moduleForcesY": [ + 0.000012569497256119221, + 0.000012570303049153985, + 0.000012573965951225331, + 0.000012574771744260094 + ], + "timestamp": 4.396976646375566 + }, + { + "x": 3.1722956084102734, + "y": 5.919182016036882, + "heading": -3.319959641753852, + "angularVelocity": 0.0003539353391446321, + "velocityX": -3.6394059999801684, + "velocityY": -0.69826672600501, + "moduleForcesX": [ + -0.00006365097504612675, + -0.00006365704219861527, + -0.00006364988124242055, + -0.00006365594839490907 + ], + "moduleForcesY": [ + 0.00033178693886780613, + 0.00033178803267151227, + 0.00033179300602029455, + 0.0003317940998240007 + ], + "timestamp": 4.496829425827257 + }, + { + "x": 2.8088900399053154, + "y": 5.849462882527497, + "heading": -3.3199240650931148, + "angularVelocity": 0.00035629114114364946, + "velocityX": -3.639413644585623, + "velocityY": -0.6982192486493579, + "moduleForcesX": [ + -0.0013686131704105154, + -0.0014891562687215552, + -0.0013468857334453688, + -0.0014674288317313169 + ], + "moduleForcesY": [ + 0.008735583117033041, + 0.008757310553014873, + 0.008856126213492287, + 0.008877853649459875 + ], + "timestamp": 4.596682205278948 + }, + { + "x": 2.4532247267194442, + "y": 5.781353045466849, + "heading": -3.299135360037311, + "angularVelocity": 0.20819355424606867, + "velocityX": -3.561896973670374, + "velocityY": -0.6821025813017123, + "moduleForcesX": [ + 18.868598944322862, + 8.32571363330943, + 20.348816615507218, + 9.972078331030568 + ], + "moduleForcesY": [ + -3.285533267638885, + -1.6202335960945649, + 7.437451110648959, + 9.42643335735913 + ], + "timestamp": 4.696534984730639 + }, + { + "x": 2.1481491038692218, + "y": 5.722895709329591, + "heading": -3.2630668401523923, + "angularVelocity": 0.36121698447193346, + "velocityX": -3.055254191420303, + "velocityY": -0.5854352373206007, + "moduleForcesX": [ + 95.62438307992944, + 95.16549067183493, + 93.35192749436273, + 91.77301060402911 + ], + "moduleForcesY": [ + 8.698058484871185, + 12.37287648011562, + 22.527375059298596, + 28.126162716809013 + ], + "timestamp": 4.79638776418233 + }, + { + "x": 1.893918919733568, + "y": 5.67416974242098, + "heading": -3.2266605845857983, + "angularVelocity": 0.3645993207835013, + "velocityX": -2.5460501436061524, + "velocityY": -0.4879780693566059, + "moduleForcesX": [ + 94.49456074129621, + 94.47644542180454, + 94.43142039606398, + 94.41277288887017 + ], + "moduleForcesY": [ + 17.86520147577287, + 17.957985013996876, + 18.196759690401734, + 18.290553923527458 + ], + "timestamp": 4.896240543634021 + }, + { + "x": 1.6905463426612732, + "y": 5.63518580098643, + "heading": -3.194492220115979, + "angularVelocity": 0.3221579273641397, + "velocityX": -2.0367242719846113, + "velocityY": -0.3904141835695623, + "moduleForcesX": [ + 93.95193830631428, + 94.19640950574573, + 94.79615129087857, + 94.96109021202487 + ], + "moduleForcesY": [ + 20.758019444351795, + 19.639392628256566, + 16.472251298678543, + 15.520018379563828 + ], + "timestamp": 4.996093323085712 + }, + { + "x": 1.5380260018243401, + "y": 5.605946840237555, + "heading": -3.168787387706304, + "angularVelocity": 0.25742730999325175, + "velocityX": -1.5274521085978272, + "velocityY": -0.2928207049112749, + "moduleForcesX": [ + 93.66310688465127, + 94.03395023535653, + 94.98702800360682, + 95.18165416889012 + ], + "moduleForcesY": [ + 22.131228901998117, + 20.52067691520685, + 15.485662425458505, + 14.274070597153358 + ], + "timestamp": 5.095946102537403 + }, + { + "x": 1.4363512552558055, + "y": 5.5864538973635876, + "heading": -3.150863748064947, + "angularVelocity": 0.17950065826642012, + "velocityX": -1.0182465405134766, + "velocityY": -0.19521682614201732, + "moduleForcesX": [ + 93.48733465079248, + 93.92658984979096, + 95.10458301157264, + 95.29781991554545 + ], + "moduleForcesY": [ + 22.923872857045062, + 21.07263381336957, + 14.842494589006758, + 13.580354171192587 + ], + "timestamp": 5.195798881989094 + }, + { + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "angularVelocity": 0.09284763554946453, + "velocityX": -0.5090991956375953, + "velocityY": -0.09760904249885234, + "moduleForcesX": [ + 93.36693167786208, + 93.85448481654781, + 95.18105538852447, + 95.3706555686842 + ], + "moduleForcesY": [ + 23.44951629201878, + 21.433762366300886, + 14.409999663578962, + 13.128974415173587 + ], + "timestamp": 5.295651661440785 + }, + { + "x": 1.3855162858963013, + "y": 5.576707363128662, + "heading": -3.141592653589793, + "angularVelocity": -5.523755381949148e-18, + "velocityX": -7.397578436541153e-17, + "velocityY": 5.726973296434314e-18, + "moduleForcesX": [ + 93.2742477353994, + 93.80974821349238, + 95.22986105500625, + 95.42354502309182 + ], + "moduleForcesY": [ + 23.843716595149832, + 21.65816851036705, + 14.131988322823306, + 12.7893133457974 + ], + "timestamp": 5.395504440892476 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 83dfdd1..794287e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { @@ -10,6 +10,8 @@ java { def ROBOT_MAIN_CLASS = "frc.robot.Main" + + // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { @@ -80,6 +82,15 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() +// Moves files into the deploy folder +task copyChoreoToDeploy(type: Copy) { + from ('Choreo/deploy/choreo') { + include('*.traj') + } + into ('src/main/deploy/choreo') + +} + // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. @@ -88,6 +99,7 @@ jar { from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE + dependsOn tasks.copyChoreoToDeploy } // Configure jar and deploy tasks diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..7e30be0 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "030000005e040000120b000009050000", + "useGamepad": true + } ] } diff --git a/simgui.json b/simgui.json index f8d2d18..45a6c3c 100644 --- a/simgui.json +++ b/simgui.json @@ -1,37 +1,11 @@ { - "HALProvider": { - "Other Devices": { - "navX-Sensor[0]": { - "header": { - "open": true - } - } - }, - "RoboRIO": { - "window": { - "visible": true - } - } - }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" - } - }, - "NetworkTables": { - "transitory": { - "LiveWindow": { - "open": true - } + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" } }, "NetworkTables Info": { - "Connections": { - "open": true - }, - "Server": { - "open": true - }, "visible": true } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6c5f2f..5da781f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,7 +6,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Units; +import frc.robot.helpers.NamedUnits.PercentOutput; +import frc.robot.helpers.NamedUnits.RevolutionsPerMinute; +import edu.wpi.first.units.*; public final class Constants { @@ -23,13 +26,13 @@ public static final class HingeConstants { public static final float topEncoderSoftLimit = 73; public static final float bottomEncoderSoftLimit = -0.01f; - + public static final double kHingeP = 0.27; //UNTUNED public static final double kHingeI = 0; //UNTUNED public static final double kHingeD = 0; //UNTUNED - public static final double kHingeG = .14; - public static final double kHingeV = 5.35; - public static final double kHingeA = 0.0; + public static final double kHingeG = .14; + public static final double kHingeV = 5.35; + public static final double kHingeA = 0.0; public static final double kHingeS = 0.0; public static final double kHingeMaxVelocityRadPerSecond = 0; @@ -49,8 +52,8 @@ public static final class HingeConstants { public static final int CANdleid = 20; public static final class ModuleConstants { - // these constants should be correct for the current 4ki module we are using - public static final double kWheelDiameterMeters = Units.inchesToMeters(4); + // these constants should be correct for the current 4ki module we are using + public static final double kWheelDiameterMeters = Units.Meters.convertFrom(4, Units.Inch); public static final double kDriveMotorGearRatio = 1/6.75; public static final double kTurningMotorGearRatio = 1/21.0; public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters; @@ -60,7 +63,7 @@ public static final class ModuleConstants { // We may need to tune this for the PID turning - + // we may need to tune it so this is not set in stone public static final double kPTurning = .01; public static final double kDTurning = 0; @@ -71,13 +74,13 @@ public static final class ModuleConstants { public static final class DriveConstants { - //Used for uh, idk. + //Used for uh, idk. public static final double kNavxUnitsToMetersConversion = 19.4; // we need to update this // no longer needs to be updated: I measured from center of axle to center of axle //thse seem to be based off of the base dimensions - public static final double kTrackWidth = Units.inchesToMeters(21); - public static final double kWheelBase = Units.inchesToMeters(21); + public static final double kTrackWidth = Units.Meters.convertFrom(21, Units.Inch); + public static final double kWheelBase = Units.Meters.convertFrom(21, Units.Inch); //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics //Ive changed it. It looked wrong and I fixed it based on the coordinate system at https://hhs-team670.github.io/MustangLib/frc/team670/robot/utils/math/Translation2d.html @@ -93,7 +96,7 @@ public static final class DriveConstants { public static final int kFrontLeftDriveMotorPort = 51; //motors updated public static final int kFrontLeftTurningMotorPort = 50; //motors updated public static final boolean kFrontLeftDriveMotorReversed = true; //re updated //updated - public static final boolean kFrontLeftTurningMotorReversed = true; //updated + public static final boolean kFrontLeftTurningMotorReversed = true; //updated public static final int kFrontLeftDriveAbsoluteEncoderPort = 33; //updated public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -.230225; //updated // they want this to be the negative of the reported values? @@ -106,7 +109,7 @@ public static final class DriveConstants { public static final int kFrontRightTurningMotorPort = 57; //motors updated public static final boolean kFrontRightDriveMotorReversed = false; //updated public static final boolean kFrontRightTurningMotorReversed = true; //updated - public static final int kFrontRightDriveAbsoluteEncoderPort = 30; //updated + public static final int kFrontRightDriveAbsoluteEncoderPort = 30; //updated public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -0.261475; //updated, in degrees public static final SensorDirectionValue kFrontRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; @@ -135,27 +138,27 @@ public static final class DriveConstants { public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = 0.397461; //updated, in degrees public static final SensorDirectionValue kBackRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive; - + //NOTE: these are not used in actual code they are just used to define max based on physical contraints - //If you want to ignore these then change the limit on the max speeds manually + //If you want to ignore these then change the limit on the max speeds manually //these seem to be mostly fine but we may need change some things - // also we need to change the physical dimensions of our base if we are going to use this + // also we need to change the physical dimensions of our base if we are going to use this public static final double kPhysicalMaxSpeedMetersPerSecond = 5676.0 / 60.0 * ModuleConstants.kDriveEncoderRot2Meter; //4.47332629073 m/s public static final double kPhysicalMaxAngularSpeedDegreesPerSecond = 360; //2 * Math.PI; //kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3); //These are the variables that determine the max speeds of our swerve drive - public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond * (1); // Divide by 4 for slow testing. + public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond * (1); // Divide by 4 for slow testing. public static final double kTeleDriveMaxAngularSpeedDegreesPerSecond = kPhysicalMaxAngularSpeedDegreesPerSecond * (0.75); - + public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 3; public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3; } - //these we can ignore for now since they are only for autonmous + //these we can ignore for now since they are only for autonmous public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; public static final double kMaxAngularSpeedDegreesPerSecond = DriveConstants.kPhysicalMaxAngularSpeedDegreesPerSecond / 10; - + public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularAccelerationDegreesPerSecondSquared = 45.0; public static final double kPXController = 1.5; @@ -186,24 +189,35 @@ public static final class PIDConstants { } public static final class ManipulatorConstants { + public static enum ArmTargets { + AMP, + SPEAKER, + SOURCE + } + public static final int kIntakeLeaderPort = 60; //20; Test bed value public static final boolean kInakeLeaderInverted = false; - public static final int kShooterLeaderPort = 54; //10; TEst bed value + public static final int kShooterLeaderPort = 54; //10; Test bed value public static final boolean kShooterLeaderInverted = true; - public static final double intakeSpeed = 0.3; - public static final double intakeSpeedWhenOuttaking = 1;//0.6; - public static final double outtakeSpeedSpeaker = 1; - public static final double outtakeSpeedAmp = 1; + public static final PercentOutput intakePercentOut = new PercentOutput(0.3); + public static final PercentOutput intakePercentOutWhenOuttaking = new PercentOutput(1);//0.6; + public static final PercentOutput outtakePercentOutSpeaker = new PercentOutput(1); + public static final PercentOutput outtakePercentOutAmp = new PercentOutput(1); + + public static final RevolutionsPerMinute shooterIntakingRPM = new RevolutionsPerMinute(1500); + public static final RevolutionsPerMinute shooterOuttakingRPM = new RevolutionsPerMinute(1000);//0.6; + public static final RevolutionsPerMinute shooterSpeakerRPM = new RevolutionsPerMinute(1500); + public static final RevolutionsPerMinute shooterAmpRPM = new RevolutionsPerMinute(1500); - public static final double outtakeSpeedSpeakerVoltage = 12; - public static final double outtakeSpeedAmpVoltage = 12; + public static final Measure outtakeSpeedSpeakerVoltage = Units.Volts.of(12); + public static final Measure outtakeSpeedAmpVoltage = Units.Volts.of(12); public static final int kIntakeSensorPort = 9; //0f test bed value - public static final int speakerShooterVelocityToReachBeforeFeedingNote = 3500;//3000; - public static final int passerShooterVelocityToReachBeforeFeedingNote = 5000;//3000 - public static final int ampShooterVelocityToReachBeforeFeedingNote = 2000; + public static final RevolutionsPerMinute speakerShooterVelocityToReachBeforeFeedingNote = new RevolutionsPerMinute(3500);//3000; + public static final RevolutionsPerMinute passerShooterVelocityToReachBeforeFeedingNote = new RevolutionsPerMinute(5000);//3000 + public static final RevolutionsPerMinute ampShooterVelocityToReachBeforeFeedingNote = new RevolutionsPerMinute(2000); //Shooter PID public static final double SHOOTER_KP = 0.0023237; @@ -216,14 +230,14 @@ public static final class ManipulatorConstants { public static final double SHOOTER_MAX_OUTPUT = 1; public static final double SHOOTER_MIN_OUTPUT = 0; - public static final double shooterIdleRPM = 1500; - + public static final RevolutionsPerMinute shooterIdleRPM = new RevolutionsPerMinute(1500.0); + } public static final class ClimbConstants { public static final int kLeftArmForwardPort = 5; public static final int kLeftArmBackwardPort = 4; - + public static final int kRightArmForwardPort = 3; public static final int kRightArmBackwardPort = 2; } @@ -235,7 +249,7 @@ public static final class GroundIntakeConstants{ public static final class SwitchboardConstants { public static final int diOnesPort = 8; - public static final int diTwosPort = 7; + public static final int diTwosPort = 7; public static final int diFoursPort = 6; public static final int diEightsPort = 5; } @@ -244,4 +258,4 @@ public static final class LEDConstants { public static final int CANdleid = 40; public static final int ledCount = 200; } -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5b9cf28..80ac45e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -27,7 +28,7 @@ public class Robot extends TimedRobot { public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + m_robotContainer = new RobotContainer(Autos.AMP_SIDE); } /** @@ -50,6 +51,7 @@ public void robotPeriodic() { @Override public void disabledInit() { OurRobotState.isEnabled = false; + } @Override @@ -59,7 +61,8 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { OurRobotState.isEnabled = true; - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + m_autonomousCommand = m_robotContainer.getAutonomousCommand(Autos.AMP_SIDE); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -82,9 +85,8 @@ public void teleopInit() { m_autonomousCommand.cancel(); } - if (RobotContainer.enableHinge) { - this.m_robotContainer.InitializeHinge(); - } + this.m_robotContainer.configureForTeleop(); + } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1533a90..c659cd9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,11 +1,15 @@ package frc.robot; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import java.util.function.BooleanSupplier; + import java.util.Optional; -import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -14,49 +18,41 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.ManipulatorSubsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.GroundIntakeCommand; -import frc.robot.commands.LedAnimationCommand; -import frc.robot.commands.AutoCommands.AutoShootCommand; -import frc.robot.commands.ClimbCommands.ClimbCommand; +import frc.robot.commands.AutoCommands.FollowTrajectoryCommand; import frc.robot.commands.ClimbCommands.ClimbCommandDown; import frc.robot.commands.ClimbCommands.ClimbCommandUp; import frc.robot.commands.DrivetrainCommands.DriveToPointCommand; import frc.robot.commands.DrivetrainCommands.SwerveCommand; import frc.robot.commands.HingeCommands.HingeInitializeCommand; import frc.robot.commands.ShooterCommands.ClimbWithArmCommand; -import frc.robot.commands.ShooterCommands.DefaultShootCommand; import frc.robot.commands.ShooterCommands.ClimbWithArmCommandDown; import frc.robot.commands.ShooterCommands.GoToAmpPosCommand; import frc.robot.commands.ShooterCommands.GoToInakePosCommand; -import frc.robot.commands.ShooterCommands.GoToInakePosCommandGroundIntakeTesting; import frc.robot.commands.ShooterCommands.GoToShootPosCommand; -import frc.robot.commands.ShooterCommands.GoToTrussPosCommand; import frc.robot.commands.ShooterCommands.IntakeCommand; import frc.robot.commands.ShooterCommands.OuttakeCommand; +import frc.robot.commands.ShooterCommands.RevUpShooterCommand; import frc.robot.commands.ShooterCommands.ShootCommand; +import frc.robot.commands.ShooterCommands.ShootCommandSequence; +import frc.robot.commands.ShooterCommands.SimulateNoteCommand; import frc.robot.commands.ShooterCommands.StopManipulatorCommand; -import frc.robot.commands.Unused.TestHingeCommand; import frc.robot.helpers.AutoSelector; import frc.robot.helpers.Crashboard; -import frc.robot.subsystems.AutoSwitchBoardSub; import frc.robot.subsystems.ClimbSubsystem; -import frc.robot.subsystems.GroundIntakeSubsystem; import frc.robot.subsystems.HingeSubsystem; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.ledSubsystem; +enum Autos { + DO_NOTHING, + JUST_SHOOT, + FORWARD_SIDE, + SOURCE_SIDE, + AMP_SIDE +} + public class RobotContainer { - //private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); - //private final TestHingeSubsystem testHingeSubsystem = new TestHingeSubsystem(); - private final HingeSubsystem hingeSubsystem = new HingeSubsystem(); - private final Joystick driverOne = new Joystick(0); - private final CommandXboxController operatorController = new CommandXboxController(1); - private final ManipulatorSubsystem manipulatorSubsystem = new ManipulatorSubsystem(); - private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); - private final GroundIntakeSubsystem groundIntakeSubsystem = new GroundIntakeSubsystem(); - private final AutoSwitchBoardSub switchBoard = new AutoSwitchBoardSub(false); - private final ledSubsystem LEDs = new ledSubsystem(); ////////////////////////////////////////////////////////////////////////////////////// @@ -74,92 +70,149 @@ public class RobotContainer { ////////////////////////////////////////////////////////////////////////////////////// - private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); - - public RobotContainer() { - - AutoSelector.setGyro(swerveSubsystem.getGyro()); - - if (enableDrivetrain) { - swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne)); - } - - if (enableHinge) { - hingeSubsystem.setDefaultCommand(new GoToShootPosCommand(hingeSubsystem)); - } - - if (enableManipulator) { - manipulatorSubsystem.setDefaultCommand(new DefaultShootCommand(manipulatorSubsystem)); + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(enableDrivetrain); + private final HingeSubsystem hingeSubsystem = new HingeSubsystem(); + //private final Joystick driverOne = new Joystick(0); + private final CommandXboxController controller = new CommandXboxController(0); + private final ManipulatorSubsystem manipulatorSubsystem = new ManipulatorSubsystem(enableManipulator); + private final ClimbSubsystem climbSubsystem = new ClimbSubsystem(); + private final ledSubsystem LEDs = new ledSubsystem(); + + public RobotContainer(Autos auto) { + + switch (auto) { + case JUST_SHOOT: + { + //just shoot speaker + break; + } + + + case FORWARD_SIDE: + { + // Tell the odeometry where we are + swerveSubsystem.initializePose(new Pose2d( + Units.Meters.of(1.348), + Units.Meters.of(5.512), + Rotation2d.fromDegrees(180) + )); + break; + } + + case SOURCE_SIDE: + { + // Tell the odeometry where we are + swerveSubsystem.initializePose(new Pose2d( + Units.Meters.of(0.672), + Units.Meters.of(4.385), + Rotation2d.fromDegrees(120) + )); + + // Starting source side (automatically detects red or blue) + break; + } + + + case AMP_SIDE: + { + // Tell the odeometry where we are + swerveSubsystem.initializePose(new Pose2d( + Units.Meters.of(0.738), + Units.Meters.of(6.661), + Rotation2d.fromDegrees(-120) + )); + + //shoot and taxi + break; + } + + case DO_NOTHING: + default: + { + break; + } } - if (enableClimb) { - // - } + AutoSelector.setGyro(swerveSubsystem.getGyro()); + } - if (enableAutoSwitchBoard) { - // + /** + * Helper function to smooth out controller input and adds a deadband. + * @param axis Value between [-1.0, 1.0] from a controller axis + * @return Smoothed value between [-1.0, 1.0] + */ + private double axisSmoother(double axis) { + double deadband = 0.1; + if (Math.abs(axis) < deadband) { + return 0.0; } - if (enableLEDs) - { - LEDs.setDefaultCommand(new LedAnimationCommand(LEDs)); - } + // x^2 + return axis * axis * Math.signum(axis); - configureButtonBindings(); } - private void configureButtonBindings() { + public void configureForTeleop() { if (enableDrivetrain) { - swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne)); + final boolean onLinux = System.getProperty("os.name").equals("Linux"); + swerveSubsystem.setDefaultCommand(new SwerveCommand( + swerveSubsystem, + () -> {return axisSmoother(-controller.getLeftY());}, // dx + () -> {return axisSmoother(-controller.getLeftX());}, // dy + () -> { + // On Linux, the RightX and RightTrigger axes are swapped from Windows + if (onLinux) { + return axisSmoother(controller.getRightTriggerAxis()); + } else { + return axisSmoother(controller.getRightX()); + } + }, //domega + (BooleanSupplier)controller.rightBumper(), // slow mode + (BooleanSupplier)controller.leftStick() // reorient + )); } if (enableHinge) { - hingeSubsystem.setDefaultCommand(new GoToShootPosCommand(hingeSubsystem)); - - - operatorController.y().whileTrue(new GoToInakePosCommand(hingeSubsystem)); - operatorController.a().whileTrue(new GoToAmpPosCommand(hingeSubsystem)); - - //operatorController.start().onTrue(new HingeInitializeCommand(hingeSubsystem)); - operatorController.b().whileTrue(new TestHingeCommand(hingeSubsystem, operatorController)); + // Reset the encoders first + new HingeInitializeCommand(hingeSubsystem).schedule(); - //operatorController.rightTrigger().whileTrue(new GoToInakePosCommand(hingeSubsystem)); + hingeSubsystem.setDefaultCommand(new GoToShootPosCommand(hingeSubsystem)); - operatorController.x().whileTrue(new GoToTrussPosCommand(hingeSubsystem)); - + controller.y().whileTrue(new GoToInakePosCommand(hingeSubsystem)); + controller.a().whileTrue(new GoToAmpPosCommand(hingeSubsystem)); + controller.x().whileTrue(new ClimbWithArmCommand(hingeSubsystem)); } if (enableManipulator) { // ========= MANIPULATOR BINDINGS ========= - Trigger shooTrigger = operatorController.leftTrigger(); - shooTrigger.onTrue(new ShootCommand(manipulatorSubsystem)); - shooTrigger.onFalse(new StopManipulatorCommand(manipulatorSubsystem)); - - Trigger intakeTrigger = operatorController.y(); - intakeTrigger.onTrue(new IntakeCommand(manipulatorSubsystem)); - intakeTrigger.onFalse(new StopManipulatorCommand(manipulatorSubsystem)); - - Trigger outtakeTrigger = operatorController.rightTrigger(); - outtakeTrigger.whileTrue(new OuttakeCommand(manipulatorSubsystem)); - + controller.leftTrigger().onTrue(new ShootCommand(manipulatorSubsystem)); + controller.leftTrigger().onFalse(new StopManipulatorCommand(manipulatorSubsystem)); + + controller.y().onTrue(new IntakeCommand(manipulatorSubsystem)); + controller.y().onFalse(new StopManipulatorCommand(manipulatorSubsystem)); + + controller.rightTrigger().whileTrue(new OuttakeCommand(manipulatorSubsystem)); + + controller.b().onTrue(new ShootCommandSequence( + swerveSubsystem, + manipulatorSubsystem, + hingeSubsystem + )); + + controller.rightBumper().onTrue(Commands.runOnce( + () -> { + System.out.println("Stop shooter"); + manipulatorSubsystem.stopAll(); + }, + manipulatorSubsystem) + ); } - // if (enableClimb && false) { - // Trigger newClimbTrigger = operatorController.back(); - - // newClimbTrigger.onTrue(new ClimbCommand(climbSubsystem)); - // newClimbTrigger.onFalse(new ClimbCommand(climbSubsystem)); - - // if (enableHinge) { - // newClimbTrigger.whileTrue(new ClimbWithArmCommand(hingeSubsystem)); - // } - // } - if (enableClimb) { - Trigger upClimbTrigger = operatorController.back(); - Trigger downClimbTrigger = operatorController.start(); + Trigger upClimbTrigger = controller.back(); + Trigger downClimbTrigger = controller.start(); upClimbTrigger.onTrue(new ClimbCommandUp(climbSubsystem)); upClimbTrigger.onTrue(new ClimbWithArmCommand(hingeSubsystem)); @@ -168,34 +221,15 @@ private void configureButtonBindings() { downClimbTrigger.onTrue(new SequentialCommandGroup(new WaitCommand(.75), new ClimbWithArmCommandDown(hingeSubsystem))); } - if (enableGroundIntake) { - // Trigger intakeTrigger = operatorController.rightTrigger(); - // intakeTrigger.whileTrue(new GroundIntakeCommand(groundIntakeSubsystem)); - } - if (enableLEDs) { LEDs.setColor(0, 0, 255); } - - // ========= OTHER BINDINGS ========= - - } - public SwerveSubsystem getSwerve() { - return swerveSubsystem; - } - - public void InitializeHinge() { - new HingeInitializeCommand(hingeSubsystem).schedule(); + // ========= OTHER BINDINGS ========= } - public Command getAutonomousCommand() { - - int selectedAuto = 0; - if (enableAutoSwitchBoard) selectedAuto = switchBoard.getSwitchCombo(); - System.out.println(selectedAuto); - - // ally multiplier is used to set the proper direction for + public Command getAutonomousCommand(Autos auto) { + // ally multiplier is used to set the proper direction for // red vs blue since they are mirrored int allyMultiplier = 1; Optional ally = DriverStation.getAlliance(); @@ -205,67 +239,56 @@ public Command getAutonomousCommand() { Crashboard.toDashboard("Alliance: ", ally.get().toString(), "Competition"); - if (!switchBoard.shouldTaxi()) { - //just shoot speaker - return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) - .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0)})) - .andThen(new AutoShootCommand(manipulatorSubsystem)); - } else if(switchBoard.isPositionForward()) { - // Shoot, delay and move at end of autonomous - return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) - .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0)})) - .andThen(new AutoShootCommand(manipulatorSubsystem)) - .andThen(new WaitCommand(9.0)) - .andThen(new DriveToPointCommand(swerveSubsystem, -1.5,0, 0.3)); - - - }else if(switchBoard.isPositionSourceSide()){ - - // Starting source side (automatically detects red or blue) - return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) - .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0)})) - .andThen(() -> swerveSubsystem.resetOdometry()) - .andThen(new AutoShootCommand(manipulatorSubsystem)) - .andThen(new WaitCommand(5.0)) - .andThen(new DriveToPointCommand(swerveSubsystem, -1.7, 1.0 * allyMultiplier, 0.3)) - .andThen(new DriveToPointCommand(swerveSubsystem, -1.7, -1.7 * allyMultiplier, 0.3)) - .andThen(new DriveToPointCommand(swerveSubsystem, -30 * allyMultiplier, 0.15)); - //.andThen(() -> swerveSubsystem.reorientMidMatch()); - - } else if (switchBoard.isPositionAmpSide()) { - //shoot and taxi - return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) - .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0)})) - .andThen(new AutoShootCommand(manipulatorSubsystem)) - .andThen(new WaitCommand(8.0)) - .andThen(new DriveToPointCommand(swerveSubsystem, -0.7, -.3 * allyMultiplier, 0.3)) - .andThen(new DriveToPointCommand(swerveSubsystem, -1.3, 2.1 * allyMultiplier, 0.3)) - .andThen(new DriveToPointCommand(swerveSubsystem, 30 * allyMultiplier, 0.15)); - //.andThen(() -> swerveSubsystem.reorientMidMatch()); + switch (auto) { + + case JUST_SHOOT: + { + //just shoot speaker + return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) + .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { + new Rotation2d(0), + new Rotation2d(0), + new Rotation2d(0), + new Rotation2d(0)})) + .andThen(new ShootCommand(manipulatorSubsystem)); + } + + + case FORWARD_SIDE: + { + return Commands.sequence( + new ShootCommand(manipulatorSubsystem), + new WaitCommand(1.0), + new DriveToPointCommand(swerveSubsystem, -1.5,0, 0.3) + ); + } + + case SOURCE_SIDE: + { + + // Starting source side (automatically detects red or blue) + return Commands.sequence( + new ShootCommand(manipulatorSubsystem), + new WaitCommand(1.0), + new DriveToPointCommand(swerveSubsystem, -1.7, 1.0 * allyMultiplier, 0.3), + new DriveToPointCommand(swerveSubsystem, -1.7, -1.7 * allyMultiplier, 0.3), + new DriveToPointCommand(swerveSubsystem, -30 * allyMultiplier, 0.15) + ); + } + + + case AMP_SIDE: + { + //shoot and taxi + return new FollowTrajectoryCommand("TestPath", swerveSubsystem, false); + } + + case DO_NOTHING: + default: + { + return Commands.idle(); + } } - else return Commands.runOnce(() -> swerveSubsystem.zeroHeading()) - .andThen( () -> swerveSubsystem.getKinematics().resetHeadings(new Rotation2d[] { - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0), - new Rotation2d(0)})) - .andThen(new AutoShootCommand(manipulatorSubsystem)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Animations/NightRider.java b/src/main/java/frc/robot/commands/Animations/NightRider.java index fc588f0..7788caa 100644 --- a/src/main/java/frc/robot/commands/Animations/NightRider.java +++ b/src/main/java/frc/robot/commands/Animations/NightRider.java @@ -1,10 +1,6 @@ package frc.robot.commands.Animations; -import com.revrobotics.jni.RevJNIWrapper; - import edu.wpi.first.wpilibj.util.Color; -import frc.robot.Constants; -import frc.robot.commands.Unused.ledCommand; import frc.robot.subsystems.ledSubsystem; public class NightRider implements CARAnimation { diff --git a/src/main/java/frc/robot/commands/AutoCommands/AutoShootCommand.java b/src/main/java/frc/robot/commands/AutoCommands/AutoShootCommand.java deleted file mode 100644 index 4474536..0000000 --- a/src/main/java/frc/robot/commands/AutoCommands/AutoShootCommand.java +++ /dev/null @@ -1,87 +0,0 @@ -package frc.robot.commands.AutoCommands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.OurRobotState; -import frc.robot.helpers.ArmPosEnum; -import frc.robot.subsystems.ManipulatorSubsystem; - -public class AutoShootCommand extends Command { - - private ManipulatorSubsystem subsystem; - private boolean hasReachedVelocity = false; - - private double outtakeSpeed; - private double intakeSpeed; - private int RPMsRequiredForOuttake; - private Timer timer; - - public AutoShootCommand(ManipulatorSubsystem subsystem) { - this.subsystem = subsystem; - timer = new Timer(); - timer.reset(); - - addRequirements(subsystem); - } - - @Override - public void initialize() { - if (OurRobotState.currentArmPosition == ArmPosEnum.AMP) { - - this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedAmp; - this.intakeSpeed = Constants.ManipulatorConstants.intakeSpeedWhenOuttaking; - this.RPMsRequiredForOuttake = Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote; - - } else if (OurRobotState.currentArmPosition == ArmPosEnum.SPEAKER) { - - this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedSpeaker; - this.intakeSpeed = Constants.ManipulatorConstants.intakeSpeedWhenOuttaking; - this.RPMsRequiredForOuttake = Constants.ManipulatorConstants.speakerShooterVelocityToReachBeforeFeedingNote; - - } else { - // do nothing, no shooting! - // Shooter, no shooting! - this.outtakeSpeed = 0; - this.intakeSpeed = 0; - this.RPMsRequiredForOuttake = 0; - } - - subsystem.spinShooter(this.outtakeSpeed); // test value, make sure to change once we g - } - - @Override - public void execute() { - if(subsystem.GetShooterVelocity() >= this.RPMsRequiredForOuttake){ - - hasReachedVelocity = true; - timer.start(); - - } - - if(hasReachedVelocity){ - - subsystem.spintake(this.intakeSpeed); - - } - } - - @Override - public boolean isFinished() { - if (timer.hasElapsed(2.5)) { - timer.stop(); - timer.reset(); - return true; - } - else { - return false; - } - } - - @Override - public void end(boolean interrupted) { - hasReachedVelocity = false; - subsystem.stopShooter(); - subsystem.stopIntake(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutoCommands/AutoTaxiCommand.java b/src/main/java/frc/robot/commands/AutoCommands/AutoTaxiCommand.java index ddd92d5..62ba520 100644 --- a/src/main/java/frc/robot/commands/AutoCommands/AutoTaxiCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommands/AutoTaxiCommand.java @@ -7,7 +7,7 @@ public class AutoTaxiCommand extends Command { private SwerveSubsystem sub; private double startingOrientation = 0; - + public AutoTaxiCommand(SwerveSubsystem system) { sub = system; addRequirements(system); @@ -15,7 +15,7 @@ public AutoTaxiCommand(SwerveSubsystem system) { @Override public void initialize() { - startingOrientation = sub.getHeading(); + startingOrientation = sub.getHeadingDegrees(); if (Math.abs(startingOrientation) < 15) { startingOrientation = 0;} } @@ -34,6 +34,6 @@ public void end(boolean interrupted) { sub.stopModules(); } - + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutoCommands/FollowTrajectoryCommand.java b/src/main/java/frc/robot/commands/AutoCommands/FollowTrajectoryCommand.java new file mode 100644 index 0000000..bbc4a92 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoCommands/FollowTrajectoryCommand.java @@ -0,0 +1,94 @@ +package frc.robot.commands.AutoCommands; + +import com.choreo.lib.ChoreoTrajectory; +import com.choreo.lib.ChoreoTrajectoryState; + +import com.choreo.lib.Choreo; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.SwerveSubsystem; + +/* + * WARNING!!!! + * + * It looks like Choreo 2025 has significantly rearchitected the API + * and .traj file format. They're moving to a more flexible AutoRoutine structure. + * This code only works for the 2024 version. + */ + +public class FollowTrajectoryCommand extends Command { + private final ChoreoTrajectory trajectory; + private final Timer timer; + private final SwerveSubsystem swerveSubsystem; + private final boolean isRedAlliance; + private boolean isFinished = false; + private final DoubleArrayPublisher commandedPosePublisher; + + public FollowTrajectoryCommand(String trajectoryName, SwerveSubsystem swerveSubsystem, boolean isRedAlliance) { + + ChoreoTrajectory loadedTrajectory; + try { + loadedTrajectory = Choreo.getTrajectory(trajectoryName); + } catch (Exception e) { + loadedTrajectory = new ChoreoTrajectory(); + } + + this.trajectory = loadedTrajectory; + + this.timer = new Timer(); + this.swerveSubsystem = swerveSubsystem; + this.isRedAlliance = isRedAlliance; + + // This prevents other commands from using the swerve subsytem while we're using it + addRequirements(swerveSubsystem); + + commandedPosePublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic("CommandedTrajectory").publish(); + Pose2d initialPose = this.trajectory.getInitialPose(); + commandedPosePublisher.set(new double[]{initialPose.getX(), initialPose.getY(), initialPose.getRotation().getRadians()}); + } + + @Override + public void initialize() { + timer.start(); + isFinished = false; + } + + + @Override + public void execute() { + /* The idea is to grab a point in the trajectory and interpret it + * It should give use our current pose which we can then calcluate a wheel state + * via ChassisSpeeds. + */ + double time = timer.get(); + try { + ChoreoTrajectoryState currentState = trajectory.sample(time, isRedAlliance); + ChassisSpeeds speed = currentState.getChassisSpeeds(); + swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(speed)); + + commandedPosePublisher.set(new double[]{currentState.x, currentState.y, currentState.heading}); + isFinished = currentState == trajectory.getFinalState(); + } catch (Exception e) { + isFinished = true; + e.printStackTrace(System.err); + } + + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + } + + @Override + public boolean isFinished() { + // This may compare two different objects ... + return isFinished; + } +} diff --git a/src/main/java/frc/robot/commands/DrivetrainCommands/DriveToPointCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommands/DriveToPointCommand.java index 6142076..34d458f 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommands/DriveToPointCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommands/DriveToPointCommand.java @@ -17,8 +17,8 @@ public class DriveToPointCommand extends Command { private SwerveSubsystem swerve; private Pose2d firstPos, secondPos; private Transform2d dPos; - private double baseMaxSpeed, rotationMaxSpeed; - private double calculatedXSpeed, calculatedYSpeed, calculatedRotSpeed; + private double baseMaxSpeed, rotationMaxSpeed; + private double calculatedXSpeed, calculatedYSpeed, calculatedRotSpeed; private double dx, dy, dTheta; private boolean stickyX, stickyY, stickyR; private Trigger stop; @@ -49,15 +49,15 @@ public DriveToPointCommand(SwerveSubsystem subsystem, double changeInHeadingDegr swerve = subsystem; dPos = new Transform2d( - new Translation2d(0, 0), + new Translation2d(0, 0), Rotation2d.fromDegrees(changeInHeadingDegrees) ); - + baseMaxSpeed = Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond * percentageOfMaxSpeed; rotationMaxSpeed = Constants.DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond * percentageOfMaxSpeed * percentageOfMaxSpeed; addRequirements(subsystem); - } + } public DriveToPointCommand(SwerveSubsystem subsystem, double percentageOfMaxSpeed, Trigger stopTrigger) { stickyX = false; @@ -81,13 +81,11 @@ public DriveToPointCommand(SwerveSubsystem subsystem, double percentageOfMaxSpee @Override public void initialize() { - + firstPos = swerve.getPose2d(); secondPos = firstPos.transformBy(dPos); - System.out.println("Starting Odometry Position: (X/Y/R), X: " + swerve.getPose2d().getX() + ", Y: " + swerve.getPose2d().getY() + ", R: " + swerve.getPose2d().getRotation().getDegrees()); - // Get the time it would take for each item to reach the destination if it is at the max speed @@ -105,25 +103,19 @@ public void initialize() { xSecMax = dx / baseMaxSpeed; ySecMax = dy / baseMaxSpeed; - tSecMax = dTheta / rotationMaxSpeed; + tSecMax = dTheta / rotationMaxSpeed; // Get the longest of these times. In other words, the operation should be performed as fast as the slowest movement can go. double bottleneckTime = Math.max(Math.max(Math.abs(xSecMax), Math.abs(ySecMax)), Math.abs(tSecMax)); - System.out.println("XSM: " + xSecMax + ", YSM: " + ySecMax + ", TSM: " + tSecMax); - System.out.println("BOTTLENECK: " + bottleneckTime); - calculatedXSpeed = dx / bottleneckTime; // literally meters / seconds - calculatedYSpeed = dy / bottleneckTime; + calculatedYSpeed = dy / bottleneckTime; calculatedRotSpeed = dTheta / bottleneckTime; // Right now, the calculated X and Y speeds are in meters/second, and not scalar values between 0 and 1. - Rotation2d currentHeading = Rotation2d.fromDegrees(-swerve.getHeading()); //inverted - - System.out.println("DX: " + dx + ", DY: " + dy + ", DTHETA: " + dTheta); - System.out.println("CXS: " + calculatedXSpeed + ", CYS: " + calculatedYSpeed + ", CRS: " + calculatedRotSpeed); + Rotation2d currentHeading = Rotation2d.fromDegrees(-swerve.getHeadingDegrees()); //inverted ChassisSpeeds speeds = ChassisSpeeds.fromRobotRelativeSpeeds(calculatedXSpeed, calculatedYSpeed, calculatedRotSpeed, currentHeading); //from Field swerve.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds)); @@ -133,17 +125,13 @@ public void initialize() { @Override public void execute() { //if (isFinished()) {end(true);} - ChassisSpeeds speeds = ChassisSpeeds.fromRobotRelativeSpeeds(calculatedXSpeed, calculatedYSpeed, calculatedRotSpeed, Rotation2d.fromDegrees(-swerve.getHeading())); //from Field + ChassisSpeeds speeds = ChassisSpeeds.fromRobotRelativeSpeeds(calculatedXSpeed, calculatedYSpeed, calculatedRotSpeed, Rotation2d.fromDegrees(-swerve.getHeadingDegrees())); //from Field swerve.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds)); } @Override public boolean isFinished() { - // System.out.println("X !!" + (swerve.getPose2d().getX() - secondPos.getX()) + "!!"); - // System.out.println("Y !!" + (swerve.getPose2d().getY() - secondPos.getY()) + "!!"); - // System.out.println("R !!" + (swerve.getPose2d().getRotation().getDegrees() - secondPos.getRotation().getDegrees()) + "!!"); - boolean xArrived = Math.abs(swerve.getPose2d().getX() - secondPos.getX()) <= 0.4; boolean yArrived = Math.abs(swerve.getPose2d().getY() - secondPos.getY()) <= 0.4; boolean tArrived = Math.abs(swerve.getPose2d().getRotation().getDegrees() - secondPos.getRotation().getDegrees()) <= 4; @@ -162,9 +150,6 @@ public boolean isFinished() { } return stickyX && stickyY && stickyR; - - // System.out.println("X !!" + (swerve.getPose2d().getX() - secondPos.getX()) + "!!"); - // return (Math.abs(secondPos.getX() - swerve.getPose2d().getX()) <= 0.2); } @Override diff --git a/src/main/java/frc/robot/commands/DrivetrainCommands/SwerveCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommands/SwerveCommand.java index dc5b8c8..83bd080 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommands/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommands/SwerveCommand.java @@ -1,22 +1,16 @@ package frc.robot.commands.DrivetrainCommands; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix6.hardware.CANcoder; -import com.kauailabs.navx.frc.AHRS; -import com.revrobotics.CANSparkMax; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.OIConstants; import frc.robot.helpers.Crashboard; import frc.robot.subsystems.SwerveSubsystem; @@ -25,23 +19,45 @@ public class SwerveCommand extends Command { private final double slow = 0.3; private final SwerveSubsystem swerveSubsystem; - private final Joystick controller; - private CANcoder FrontLeftCoder; - private CANSparkMax FrontLeftTurnSpark; + private final DoubleSupplier getDx; + private final DoubleSupplier getDy; + private final DoubleSupplier getDOmega; + private final BooleanSupplier activateSlowMode; + private final BooleanSupplier reorient; - private SlewRateLimiter driveLimiter, driveLimiterX, driveLimiterY, turnLimiter; + private SlewRateLimiter driveLimiterX, driveLimiterY, turnLimiter; public SwerveCommand(SwerveSubsystem swerveSubsystem, Joystick controller) { + this( + swerveSubsystem, + () -> {return controller.getRawAxis(1);}, + () -> {return controller.getRawAxis(0);}, + () -> {return controller.getRawAxis(3);}, + () -> {return controller.getRawButton(1);}, + () -> {return controller.getRawButtonPressed(2);} + ); + } + + public SwerveCommand( + SwerveSubsystem swerveSubsystem, + DoubleSupplier dx, + DoubleSupplier dy, + DoubleSupplier domega, + BooleanSupplier activateSlowMode, + BooleanSupplier reorient + ) { this.swerveSubsystem = swerveSubsystem; - this.controller = controller; + this.getDx = dx; + this.getDy = dy; + this.getDOmega = domega; + this.activateSlowMode = activateSlowMode; + this.reorient = reorient; + addRequirements(swerveSubsystem); - this.FrontLeftCoder = swerveSubsystem.frontLeft.getCANcoder(); - this.FrontLeftTurnSpark = swerveSubsystem.frontLeft.turningMotor; driveLimiterX = new SlewRateLimiter(16); driveLimiterY = new SlewRateLimiter(16); turnLimiter = new SlewRateLimiter(20); - //driveLimiter = new SlewRateLimiter(0.5); } @Override @@ -53,23 +69,16 @@ public void execute() { // reorient button! - if (controller.getRawButtonPressed(2)) { + if (reorient.getAsBoolean()) { swerveSubsystem.reorientMidMatch(); } // end reorient - boolean slowMode = controller.getRawButton(1); - - // :3 - - // - //System.out.println("ANGLE = " + swerveSubsystem.frontLeft.getAbsoluteEncoderDeg() + ", SPEED = " + FrontLeftTurnSpark.get()); + boolean slowMode = activateSlowMode.getAsBoolean(); - // \:3 - //double xSpeed = Math.abs(controller.getLeftY()) > OIConstants.kDeadband ? controller.getLeftY() : 0.0; - double xSpeed = controller.getRawAxis(1); + double xSpeed = getDx.getAsDouble(); if (Math.abs(xSpeed) <= Constants.OIConstants.kDeadband) { xSpeed = 0; } @@ -81,21 +90,21 @@ public void execute() { if (slowMode) xSpeed *= slow; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - double ySpeed = controller.getRawAxis(0); + + double ySpeed = getDy.getAsDouble(); if (Math.abs(ySpeed) <= Constants.OIConstants.kDeadband) { ySpeed = 0; } - + ySpeed = driveLimiterY.calculate(ySpeed); - - ySpeed = ySpeed * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + + ySpeed = ySpeed * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; if (slowMode) ySpeed *= slow; ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - double turningSpeed = controller.getRawAxis(3); // 4 is for thrust buttons. + + double turningSpeed = getDOmega.getAsDouble(); if (Math.abs(turningSpeed) <= Constants.OIConstants.kDeadband) { turningSpeed = 0; } @@ -107,18 +116,15 @@ public void execute() { if (slowMode) turningSpeed *= slow; Crashboard.toDashboard("kTeleDriveMaxAngularSpeedDegreesPerSecond", DriveConstants.kTeleDriveMaxAngularSpeedDegreesPerSecond, "navx"); - //turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; - //turningSpeed = 0; - //System.out.println("xSpeed: " + xSpeed + " ySpeed: " + ySpeed + " turningSpeed: " + turningSpeed); double turningSpeedRadiansPerSecond = Rotation2d.fromDegrees(turningSpeed).getRadians(); - Rotation2d currentHeading = Rotation2d.fromDegrees(-swerveSubsystem.getHeading()); //inverted + Rotation2d currentHeading = Rotation2d.fromDegrees(swerveSubsystem.getHeadingDegrees()); //inverted ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeedRadiansPerSecond, currentHeading); SwerveDriveKinematics.desaturateWheelSpeeds(swerveSubsystem.getModuleStates(), Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond); swerveSubsystem.setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)); Crashboard.toDashboard("turningSpeedRadiansPerSecond", turningSpeedRadiansPerSecond, "navx"); Crashboard.toDashboard("currentHeading", currentHeading.getRadians(), "navx"); - + //Crashboard.toDashboard("desired states", DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds)[0].toString(), "navx"); } diff --git a/src/main/java/frc/robot/commands/GroundIntakeCommand.java b/src/main/java/frc/robot/commands/GroundIntakeCommand.java deleted file mode 100644 index 34e2ad4..0000000 --- a/src/main/java/frc/robot/commands/GroundIntakeCommand.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.GroundIntakeSubsystem; - -public class GroundIntakeCommand extends Command { - - private GroundIntakeSubsystem sub; - - public GroundIntakeCommand(GroundIntakeSubsystem system) { - - sub = system; - - addRequirements(system); - } - - @Override - public void end(boolean interrupted) { - sub.spintake(0); - } - - @Override - public void execute() { - } - - @Override - public void initialize() { - sub.spintake(Constants.GroundIntakeConstants.intakeSpeed); - } - - @Override - public boolean isFinished() { - return false; - } - -} diff --git a/src/main/java/frc/robot/commands/HingeCommands/HingeInitializeCommand.java b/src/main/java/frc/robot/commands/HingeCommands/HingeInitializeCommand.java index 5fcdf5c..e21eeb2 100644 --- a/src/main/java/frc/robot/commands/HingeCommands/HingeInitializeCommand.java +++ b/src/main/java/frc/robot/commands/HingeCommands/HingeInitializeCommand.java @@ -4,7 +4,6 @@ import frc.robot.Constants; import frc.robot.OurRobotState; import frc.robot.subsystems.HingeSubsystem; -import frc.robot.subsystems.TestHingeSubsystem; public class HingeInitializeCommand extends Command { private HingeSubsystem subsystem; diff --git a/src/main/java/frc/robot/commands/LedAnimationCommand.java b/src/main/java/frc/robot/commands/LedAnimationCommand.java index 99b1ed2..fca9df6 100644 --- a/src/main/java/frc/robot/commands/LedAnimationCommand.java +++ b/src/main/java/frc/robot/commands/LedAnimationCommand.java @@ -29,7 +29,6 @@ public class LedAnimationCommand extends Command { public LedAnimationCommand(ledSubsystem subsystem) { this.subsystem = subsystem; sideLedCount = 39; - // System.out.println("This was called"); animations = new ArrayList(); animations.add(new NightRider(new Color(0, 0, 255), new Color(255, 64, 0), 6, 1, sideLedCount, initialOffset, false, subsystem)); diff --git a/src/main/java/frc/robot/commands/ShooterCommands/ClimbWithArmCommandDown.java b/src/main/java/frc/robot/commands/ShooterCommands/ClimbWithArmCommandDown.java index 46dc27d..6d15cd8 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/ClimbWithArmCommandDown.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/ClimbWithArmCommandDown.java @@ -31,9 +31,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - //System.out.println(":3333333333333"); - //return subsystem.isAtPosition(Constants.HingeConstants.kAmpAngle, 0 /*temp deadzone*/); - //return system.getController().atGoal();// || system.outputThresholdReached(12); return true; } diff --git a/src/main/java/frc/robot/commands/ShooterCommands/DefaultShootCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/DefaultShootCommand.java deleted file mode 100644 index c110a6b..0000000 --- a/src/main/java/frc/robot/commands/ShooterCommands/DefaultShootCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.commands.ShooterCommands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.ManipulatorSubsystem; - -public class DefaultShootCommand extends Command { - - private ManipulatorSubsystem subsystem; - - public DefaultShootCommand(ManipulatorSubsystem subsystem) { - this.subsystem = subsystem; - - addRequirements(subsystem); - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - if(subsystem.intakeTrigged()){ - - subsystem.setShooterSpeedByRPM(Constants.ManipulatorConstants.shooterIdleRPM); - - }else{ - - subsystem.stopShooter(); - - } - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShooterCommands/GoToShootPosCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/GoToShootPosCommand.java index 613f70b..939a138 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/GoToShootPosCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/GoToShootPosCommand.java @@ -23,14 +23,12 @@ public void initialize() { subsystem.setGoal(Constants.HingeConstants.kShootingAngle); } subsystem.enable(); - //System.out.println("I am initialize"); OurRobotState.currentArmPosition = ArmPosEnum.SPEAKER; } @Override public void execute() { subsystem.enable(); - //System.out.println("I am execute"); } @Override diff --git a/src/main/java/frc/robot/commands/ShooterCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/IntakeCommand.java index c858475..653a780 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/IntakeCommand.java @@ -7,7 +7,7 @@ public class IntakeCommand extends Command{ private ManipulatorSubsystem subsystem; - + public IntakeCommand(ManipulatorSubsystem subsystem) { this.subsystem = subsystem; @@ -16,13 +16,14 @@ public IntakeCommand(ManipulatorSubsystem subsystem) { @Override public void initialize() { - subsystem.spintake(Constants.ManipulatorConstants.intakeSpeed); // test value plz change. NO! I WON'T! + // test value plz change. + // NO! I WON'T! + subsystem.setShooterRPM(Constants.ManipulatorConstants.shooterIntakingRPM); } @Override public boolean isFinished() { - System.out.println(subsystem.intakeTrigged()); - return subsystem.intakeTrigged(); + return subsystem.isIntakeTriggered(); } @Override diff --git a/src/main/java/frc/robot/commands/ShooterCommands/OuttakeCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/OuttakeCommand.java index 0e8ea5a..6f8f8f2 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/OuttakeCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/OuttakeCommand.java @@ -4,36 +4,36 @@ import frc.robot.Constants; import frc.robot.subsystems.ManipulatorSubsystem; -public class OuttakeCommand extends Command { +public class OuttakeCommand extends Command { - private ManipulatorSubsystem mani; + private ManipulatorSubsystem subsystem; - public OuttakeCommand(ManipulatorSubsystem pulator) { - mani = pulator; - addRequirements(mani); + public OuttakeCommand(ManipulatorSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); } @Override - public void end(boolean interrupted) { - mani.stopAll(); + public void initialize() { + subsystem.setShooterRPM(Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote.times(-0.5)); + subsystem.setIntakePercentOut(Constants.ManipulatorConstants.intakePercentOutWhenOuttaking.times(-0.5)); } @Override public void execute() { - mani.spinShooter(-Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote * 0.5); - mani.spintake(-Constants.ManipulatorConstants.intakeSpeedWhenOuttaking * 0.5); + // We don't update anything, so ignore? } @Override - public void initialize() { - // + public void end(boolean interrupted) { + subsystem.stopAll(); } @Override public boolean isFinished() { - return !mani.intakeTrigged(); + return !subsystem.isIntakeTriggered(); } - - - + + + } diff --git a/src/main/java/frc/robot/commands/ShooterCommands/RevUpShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/RevUpShooterCommand.java new file mode 100644 index 0000000..6343f87 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterCommands/RevUpShooterCommand.java @@ -0,0 +1,48 @@ +package frc.robot.commands.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Constants.ManipulatorConstants.ArmTargets; +import frc.robot.subsystems.ManipulatorSubsystem; + +public class RevUpShooterCommand extends Command { + + private final ManipulatorSubsystem manipulatorSubsystem; + private final ArmTargets targetPos; + + public RevUpShooterCommand(ManipulatorSubsystem manipulatorSubsystem, ArmTargets targetPos) { + this.manipulatorSubsystem = manipulatorSubsystem; + this.targetPos = targetPos; + + addRequirements(manipulatorSubsystem); + } + + @Override + public void initialize() { + if (ArmTargets.SPEAKER == targetPos) { + manipulatorSubsystem.setShooterRPM(Constants.ManipulatorConstants.speakerShooterVelocityToReachBeforeFeedingNote); + } else { + manipulatorSubsystem.setShooterRPM(Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote); + } + } + + @Override + public void execute() { + // Nothing to do here. + } + + @Override + public void end(boolean interrupted) { + // Nothing to do here. + } + + @Override + public boolean isFinished() { + if (ArmTargets.SPEAKER == targetPos) { + return manipulatorSubsystem.getShooterRPM().isNear(Constants.ManipulatorConstants.speakerShooterVelocityToReachBeforeFeedingNote, 0.05); + } else { + return manipulatorSubsystem.getShooterRPM().isNear(Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote, 0.05); + } + } +} + diff --git a/src/main/java/frc/robot/commands/ShooterCommands/ShootCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/ShootCommand.java index ea48b56..ff0d729 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/ShootCommand.java @@ -1,86 +1,71 @@ package frc.robot.commands.ShooterCommands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; import frc.robot.OurRobotState; import frc.robot.helpers.ArmPosEnum; import frc.robot.helpers.Crashboard; +import frc.robot.helpers.NamedUnits.PercentOutput; +import frc.robot.helpers.NamedUnits.RevolutionsPerMinute; import frc.robot.subsystems.ManipulatorSubsystem; + public class ShootCommand extends Command { - private ManipulatorSubsystem subsystem; - private boolean hasReachedVelocity = false; + private final ManipulatorSubsystem subsystem; + private final Timer timer; + + private PercentOutput intakePercentOut; - private double outtakeSpeed; - private double intakeSpeed; - private int RPMsRequiredForOuttake; - public ShootCommand(ManipulatorSubsystem subsystem) { this.subsystem = subsystem; + timer = new Timer(); addRequirements(subsystem); } @Override public void initialize() { + // Determine the speed to feed the note into the shooter. + if (OurRobotState.currentArmPosition == ArmPosEnum.AMP) { - // this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedAmp; - this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedSpeakerVoltage; - this.intakeSpeed = Constants.ManipulatorConstants.intakeSpeedWhenOuttaking; - this.RPMsRequiredForOuttake = Constants.ManipulatorConstants.ampShooterVelocityToReachBeforeFeedingNote; + this.intakePercentOut = Constants.ManipulatorConstants.intakePercentOutWhenOuttaking; } else if (OurRobotState.currentArmPosition == ArmPosEnum.SPEAKER) { - //this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedSpeaker; - this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedSpeakerVoltage; - this.intakeSpeed = Constants.ManipulatorConstants.intakeSpeedWhenOuttaking; - this.RPMsRequiredForOuttake = Constants.ManipulatorConstants.speakerShooterVelocityToReachBeforeFeedingNote; + this.intakePercentOut = Constants.ManipulatorConstants.intakePercentOutWhenOuttaking; } else if (OurRobotState.currentArmPosition == ArmPosEnum.LONG_SHOT) { - this.outtakeSpeed = Constants.ManipulatorConstants.outtakeSpeedSpeakerVoltage; - this.intakeSpeed = Constants.ManipulatorConstants.intakeSpeedWhenOuttaking; - this.RPMsRequiredForOuttake = Constants.ManipulatorConstants.passerShooterVelocityToReachBeforeFeedingNote; + this.intakePercentOut = Constants.ManipulatorConstants.intakePercentOutWhenOuttaking; } else { // do nothing, no shooting! - // Shooter, no shooting! - this.outtakeSpeed = 0; - this.intakeSpeed = 0; - this.RPMsRequiredForOuttake = 0; + // Shooter, no shooting! + this.intakePercentOut = new PercentOutput(0); } - //subsystem.spinShooterVoltage(this.outtakeSpeed); // test value, make sure to change once we g - subsystem.setShooterSpeedByRPM(RPMsRequiredForOuttake); + // Kick the note out. + subsystem.setIntakePercentOut(intakePercentOut); + timer.start(); } @Override public void execute() { - - Crashboard.toDashboard("shooter flywheel RPMS: ", subsystem.GetShooterVelocity(), "shooter"); - - if(subsystem.GetShooterVelocity() >= this.RPMsRequiredForOuttake * 0.90){ - - hasReachedVelocity = true; - - } - - if(hasReachedVelocity){ - - subsystem.spintake(this.intakeSpeed); - - } + // Condition checking done in isFinished } @Override public boolean isFinished() { - return false; + // Give a little time for the motors to spin the note out. + return timer.hasElapsed(0.5); } @Override public void end(boolean interrupted) { - hasReachedVelocity = false; + timer.stop(); subsystem.stopShooter(); subsystem.stopIntake(); } diff --git a/src/main/java/frc/robot/commands/ShooterCommands/ShootCommandSequence.java b/src/main/java/frc/robot/commands/ShooterCommands/ShootCommandSequence.java new file mode 100644 index 0000000..fb3cd6b --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterCommands/ShootCommandSequence.java @@ -0,0 +1,27 @@ +package frc.robot.commands.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.subsystems.HingeSubsystem; +import frc.robot.subsystems.ManipulatorSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +public class ShootCommandSequence extends SequentialCommandGroup { + + public ShootCommandSequence( + SwerveSubsystem swerveSubsystem, + ManipulatorSubsystem manipulatorSubsystem, + HingeSubsystem hingeSubsystem + ) { + // Create the sequences + super( + new RevUpShooterCommand( + manipulatorSubsystem, + Constants.ManipulatorConstants.ArmTargets.SPEAKER + ), + new ShootCommand(manipulatorSubsystem), + new SimulateNoteCommand(swerveSubsystem, hingeSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/commands/ShooterCommands/SimulateNoteCommand.java b/src/main/java/frc/robot/commands/ShooterCommands/SimulateNoteCommand.java new file mode 100644 index 0000000..5baf1c8 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterCommands/SimulateNoteCommand.java @@ -0,0 +1,253 @@ +package frc.robot.commands.ShooterCommands; + +import java.util.ArrayList; +import java.util.Arrays; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.HingeSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +// Based off the example code: https://github.com/Mechanical-Advantage/AdvantageKit/blob/18a0219f60108e3dc1e8512d59fcba0e657770af/example_projects/kitbot_2024/src/main/java/frc/robot/util/NoteVisualizer.javahttps://github.com/Mechanical-Advantage/AdvantageKit/blob/18a0219f60108e3dc1e8512d59fcba0e657770af/example_projects/kitbot_2024/src/main/java/frc/robot/util/NoteVisualizer.java + +public class SimulateNoteCommand extends Command { + + private final StructArrayPublisher trajectoryPublisher; + private final StructPublisher notePublisher; + + private final SwerveSubsystem swerveSubsystem; + private final HingeSubsystem hingeSubsystem; + private final Timer timer; + private final double dt = 0.02; + + private Pose3d[] trajectory; + + public SimulateNoteCommand(SwerveSubsystem swerveSubsystem, HingeSubsystem hingeSubsystem) { + this.swerveSubsystem = swerveSubsystem; + this.hingeSubsystem = hingeSubsystem; + this.timer = new Timer(); + + trajectoryPublisher = NetworkTableInstance.getDefault().getStructArrayTopic("NoteTrajectory", Pose3d.struct).publish(); + trajectoryPublisher.set(new Pose3d[0]); + + notePublisher = NetworkTableInstance.getDefault().getStructTopic("Note", Pose3d.struct).publish(); + notePublisher.set(new Pose3d()); + + // We intentionally don't call addRequirement here, + // because we want this to run while those subsystems are executing other commands + } + + private boolean withinField(Pose3d pose) { + return pose.getX() > 0 && + pose.getY() > 0 && + pose.getX() < Units.Meters.convertFrom(52, Units.Feet) && + pose.getY() < Units.Meters.convertFrom(26, Units.Feet); + } + + private Pose3d[] caculateTrajectory(Pose3d initialPose, Measure> initialSpeed) { + /* + * Naive strategy: + * 1. Move into the trajectory plane + * 2. Use classial mechanics to find all the points. + * The rotations will follow the velocity vector + * 3. Convert the points in that plane back into the 3D field frame. + * + * x_x = v_0*t + * x_y = v_0*t -g/2 * t^2 + * v_x = v_0 + * v_y = v_0 - g*t + */ + double rz = initialPose.getRotation().getZ(); + double ry = initialPose.getRotation().getY(); + double rx = initialPose.getRotation().getX(); + + double v_x = initialSpeed.magnitude() * Math.cos(rz) * Math.cos(ry); + double v_y = initialSpeed.magnitude() * Math.sin(rz) * Math.cos(ry); + double v_z = initialSpeed.magnitude() * Math.sin(ry); + double x_0_x = 0.0; + double x_0_z = initialPose.getZ(); + + + // Project the launch vector into the launch plane + // the horizontal vector (b) is the unit vector in the xy plane + double b_x = Math.cos(rz); + double b_y = Math.sin(rz); + + double horizontalMag = v_x*b_x + v_y*b_y; + double verticalMag = v_z; + + double endTime = (v_z + Math.sqrt(v_z*v_z + 2*x_0_z*9.8))/9.8; + + ArrayList trajectory = new ArrayList((int)(endTime/0.02)); + for (double t = 0.0; t < endTime; t += dt) { + double x_t = x_0_x + horizontalMag*t; + double z_t = x_0_z + verticalMag*t - 4.9*t*t; + double v_x_t = horizontalMag; + double v_z_t = verticalMag - 9.8*t; + + // Put things back in Field coords + double roll = 0; + double pitch = -Math.atan2(v_z_t, v_x_t); + Pose3d pose = new Pose3d( + new Translation3d( + Units.Meters.of(initialPose.getX()).plus(Units.Meters.of(x_t * Math.cos(rz))), + Units.Meters.of(initialPose.getY()).plus(Units.Meters.of(x_t * Math.sin(rz))), + Units.Meters.of(z_t) + ), + new Rotation3d( + roll, + pitch, + initialPose.getRotation().getZ() + ) + ); + + trajectory.add(pose); + + // Our simple physics sim is to consider the note scored when it leaves the field + if (!withinField(pose)) { + break; + } + } + + return (Pose3d[])trajectory.toArray(new Pose3d[trajectory.size()]); + } + + private Pose3d[] calculateTrajectoryWAirResistance(Pose3d initialPose, Measure> initialSpeed) { + /* + * Naive strategy: + * 1. Move into the trajectory plane + * 2. Use classial mechanics to find all the points. + * The rotations will follow the velocity vector + * 3. Convert the points in that plane back into the 3D field frame. + * + */ + double rz = initialPose.getRotation().getZ(); + double ry = initialPose.getRotation().getY(); + double rx = initialPose.getRotation().getX(); + + double v_x = initialSpeed.magnitude() * Math.cos(rz) * Math.cos(ry); + double v_y = initialSpeed.magnitude() * Math.sin(rz) * Math.cos(ry); + double v_z = initialSpeed.magnitude() * Math.sin(ry); + double x_0_x = 0.0; + double x_0_z = initialPose.getZ(); + + + // Project the launch vector into the launch plane + // the horizontal vector (b) is the unit vector in the xy plane + double b_x = Math.cos(rz); + double b_y = Math.sin(rz); + + double horizontalMag = v_x*b_x + v_y*b_y; + double verticalMag = v_z; + + ArrayList trajectory = new ArrayList(25); + + // Initial conditions + double x_t = x_0_x; + double z_t = x_0_z; + double v_x_t = horizontalMag; + double v_z_t = verticalMag; + + double dragCoefficient = 0.42; // https://en.wikipedia.org/wiki/Drag_coefficient + double airDensity = 1.293; // https://www.earthdata.nasa.gov/topics/atmosphere/air-mass-density + double crossSectionalArea = 0.018; // 0.05m * 0.36m. Approximated as rectangle + + for (double t = 0.0; z_t > 0; t += dt) { + double v = Math.sqrt(v_x_t*v_x_t + v_z_t*v_z_t); + double forceDrag = -0.5 * dragCoefficient * airDensity * crossSectionalArea * v*v; + + v_x_t = v_x_t + forceDrag * (v_x_t / v) * dt; + v_z_t = v_z_t + (forceDrag * (v_z_t / v) * dt) + -9.8*dt; + + x_t = x_t + v_x_t*dt; + z_t = z_t + v_z_t*dt; + + // Put things back in Field coords + double roll = 0; + double pitch = -Math.atan2(v_z_t, v_x_t); + Pose3d pose = new Pose3d( + new Translation3d( + Units.Meters.of(initialPose.getX()).plus(Units.Meters.of(x_t * Math.cos(rz))), + Units.Meters.of(initialPose.getY()).plus(Units.Meters.of(x_t * Math.sin(rz))), + Units.Meters.of(z_t) + ), + new Rotation3d( + roll, + pitch, + initialPose.getRotation().getZ() + ) + ); + + trajectory.add(pose); + + // Our simple physics sim is to consider the note scored when it leaves the field + if (!withinField(pose)) { + break; + } + } + + return (Pose3d[])trajectory.toArray(new Pose3d[trajectory.size()]); + } + + @Override + public void initialize() { + // TODO: These use SPEAKER defaults for now, because + // the hinge isn't simulated/animated yet + Pose3d initialPose = new Pose3d( + new Translation3d( + swerveSubsystem.getPose2d().getX(), + swerveSubsystem.getPose2d().getY(), + 0.6 // This should be 1 of two values depending on angle of the hinge + ), + // We do a bad thing here. This rotation is in Robot coords, not Field coords. + new Rotation3d( + 0, + Units.Radians.convertFrom(55, Units.Degrees), //Units.Radians.convertFrom(hingeSubsystem.getAbsoluteDegrees(), Units.Degrees), + Units.Radians.convertFrom(swerveSubsystem.getHeadingDegrees(), Units.Degrees) + ) + ); + // TODO: This is a guess. It will vary depending on AMP or SPEAKER + Measure> initialVelocity = Units.MetersPerSecond.of(10.0); + + //trajectory = caculateTrajectory(initialPose, initialVelocity); + trajectory = calculateTrajectoryWAirResistance(initialPose, initialVelocity); + + trajectoryPublisher.set(Arrays.copyOfRange(trajectory, 0, 0)); + notePublisher.set(trajectory[0]); + + timer.start(); + } + + @Override + public void execute() { + // There may be an off by 1 here, but it looks close enough + int index = (int)(timer.get() / dt); + if (index < trajectory.length) { + trajectoryPublisher.set(Arrays.copyOfRange(trajectory, 0, index)); + notePublisher.set(trajectory[index]); + } + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + timer.reset(); + } + + @Override + public boolean isFinished() { + int index = (int)(timer.get() / dt); + return index > trajectory.length; + } + +} diff --git a/src/main/java/frc/robot/commands/Unused/ExampleCommand.java b/src/main/java/frc/robot/commands/Unused/ExampleCommand.java deleted file mode 100644 index 5f37738..0000000 --- a/src/main/java/frc/robot/commands/Unused/ExampleCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.Unused; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/Unused/LimeToPoint.java b/src/main/java/frc/robot/commands/Unused/LimeToPoint.java deleted file mode 100644 index 7084047..0000000 --- a/src/main/java/frc/robot/commands/Unused/LimeToPoint.java +++ /dev/null @@ -1 +0,0 @@ -package frc.robot.commands.Unused; diff --git a/src/main/java/frc/robot/commands/Unused/TestHingeCommand.java b/src/main/java/frc/robot/commands/Unused/TestHingeCommand.java deleted file mode 100644 index 122abd6..0000000 --- a/src/main/java/frc/robot/commands/Unused/TestHingeCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.commands.Unused; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.HingeSubsystem; -import frc.robot.subsystems.TestHingeSubsystem; - -public class TestHingeCommand extends Command { - HingeSubsystem subsystem; - double speed = 0; - CommandXboxController controller; - - public TestHingeCommand(HingeSubsystem subsystem, CommandXboxController contr) { - this.subsystem = subsystem; - if (Math.abs(speed) < 0.1) speed = 0; - controller = contr; - - addRequirements(subsystem); - } - - @Override - public void initialize() { - //subsystem.spinge(speed * .1); - - } - - @Override - public void execute() { - speed = controller.getLeftY(); - if (Math.abs(speed) < 0.1) speed = 0; - subsystem.spinge(speed * 0.25); - } - - @Override - public void end(boolean interrupted) { - subsystem.stopHinge(); - } - -} diff --git a/src/main/java/frc/robot/commands/Unused/ledCommand.java b/src/main/java/frc/robot/commands/Unused/ledCommand.java deleted file mode 100644 index 9c054f8..0000000 --- a/src/main/java/frc/robot/commands/Unused/ledCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.commands.Unused; - - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ledSubsystem; - - -public class ledCommand extends Command { - private ledSubsystem subsystem; - private final XboxController controller; - - public ledCommand(ledSubsystem subsystem, XboxController controller) { - this.subsystem = subsystem; - this.controller = controller; - } - - @Override - public void execute() { - if (controller.getAButtonPressed()){ - subsystem.setColor(100, 255, 70); - } - else { - subsystem.setColor(255, 255, 255); - } - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/helpers/AutoSelector.java b/src/main/java/frc/robot/helpers/AutoSelector.java index e01b001..f17043b 100644 --- a/src/main/java/frc/robot/helpers/AutoSelector.java +++ b/src/main/java/frc/robot/helpers/AutoSelector.java @@ -15,7 +15,6 @@ public static boolean isFacingSide() { return true; } else return false; } else { - System.out.println("Missing Gyro. :("); return false; } } @@ -28,7 +27,6 @@ public static boolean isfacingLeft() { } else return false; } else { - System.out.println("Missing Gyro. :("); return false; } } @@ -42,7 +40,6 @@ public static AutoLocation getLocation() { } else return AutoLocation.CENTER; } else { - System.out.println("Missing Gyro. :("); return AutoLocation.NONE; } } diff --git a/src/main/java/frc/robot/helpers/Crashboard.java b/src/main/java/frc/robot/helpers/Crashboard.java index 81bf80e..17df4eb 100644 --- a/src/main/java/frc/robot/helpers/Crashboard.java +++ b/src/main/java/frc/robot/helpers/Crashboard.java @@ -4,7 +4,6 @@ import java.util.Hashtable; import java.util.Map; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; diff --git a/src/main/java/frc/robot/helpers/NamedUnits/Degrees.java b/src/main/java/frc/robot/helpers/NamedUnits/Degrees.java new file mode 100644 index 0000000..1a98e34 --- /dev/null +++ b/src/main/java/frc/robot/helpers/NamedUnits/Degrees.java @@ -0,0 +1,76 @@ +package frc.robot.helpers.NamedUnits; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Dimensionless; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Units; + +public class Degrees implements Measure{ + private final Measure measure; + + public Degrees(double magnitude) { + measure = Units.Degrees.of(magnitude); + } + + // Prevent an extra allocation + private Degrees(Measure measure) { + this.measure = measure; + } + + @Override + public double magnitude() { + return measure.magnitude(); + } + + @Override + public double baseUnitMagnitude() { + return measure.baseUnitMagnitude(); + } + + @Override + public Angle unit() { + return measure.unit(); + } + + @Override + public Measure copy() { + return measure.copy(); + } + + @Override + public Degrees times(double multiplier) { + return new Degrees(measure.times(multiplier)); + } + + @Override + public > Measure times(Measure other) { + return measure.times(other); + } + + @Override + public Degrees divide(Measure divisor) { + return new Degrees(measure.divide(divisor)); + } + + @Override + public Degrees divide(double divisor) { + return new Degrees(measure.divide(divisor)); + } + + @Override + public Degrees plus(Measure other) { + return new Degrees(measure.plus(other)); + } + + @Override + public Degrees minus(Measure other) { + return new Degrees(measure.minus(other)); + } + + @Override + public Degrees negate() { + return new Degrees(measure.negate()); + } + +} diff --git a/src/main/java/frc/robot/helpers/NamedUnits/PercentOutput.java b/src/main/java/frc/robot/helpers/NamedUnits/PercentOutput.java new file mode 100644 index 0000000..5227cb8 --- /dev/null +++ b/src/main/java/frc/robot/helpers/NamedUnits/PercentOutput.java @@ -0,0 +1,101 @@ +package frc.robot.helpers.NamedUnits; + +import edu.wpi.first.units.Dimensionless; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Units; + +/** + * Value guarenteed to be between [-1.0, 1.0] + */ +public class PercentOutput implements Measure { + private final Measure measure; + + public PercentOutput(double value) { + if (value > 1.0) { + System.err.println(String.format("[WARNING]: Value of %d exceeds maximum of 1.0", value)); + // AnonymousBaseUnit is the only instance of Dimensionless I could find + // However, instead of being type Dimensionless, it is of type Unit, + // so we have to cast it back + this.measure = ((Dimensionless)Units.AnonymousBaseUnit).of(1.0); + } else if (value < -1.0) { + System.err.println(String.format("[WARNING]: Value of %d exceeds minimum of -1.0", value)); + + this.measure = ((Dimensionless)Units.AnonymousBaseUnit).of(-1.0); + } else { + this.measure = ((Dimensionless)Units.AnonymousBaseUnit).of(value); + } + } + + private PercentOutput(Measure measure) { + if (measure.magnitude() > 1.0) { + System.err.println(String.format("[WARNING]: Measure of %d exceeds maximum of 1.0", measure.magnitude())); + // AnonymousBaseUnit is the only instance of Dimensionless I could find + // However, instead of being type Dimensionless, it is of type Unit, + // so we have to cast it back + this.measure = ((Dimensionless)Units.AnonymousBaseUnit).of(1.0); + } else if (measure.magnitude() < -1.0) { + System.err.println(String.format("[WARNING]: Measure of %d exceeds minimum of -1.0", measure.magnitude())); + + this.measure = ((Dimensionless)Units.AnonymousBaseUnit).of(-1.0); + } else { + this.measure = measure; + } + } + + @Override + public double magnitude() { + return measure.magnitude(); + } + + @Override + public double baseUnitMagnitude() { + return measure.baseUnitMagnitude(); + } + + @Override + public Dimensionless unit() { + return measure.unit(); + } + + @Override + public Measure copy() { + return measure.copy(); + } + + @Override + public > Measure times(Measure other) { + return measure.times(other); + } + + @Override + public PercentOutput times(double multiplier) { + return new PercentOutput(measure.magnitude() * multiplier); + } + + @Override + public PercentOutput divide(Measure divisor) { + return new PercentOutput(measure.divide(divisor)); + } + + @Override + public PercentOutput divide(double divisor) { + return new PercentOutput(measure.magnitude() / divisor); + } + + @Override + public PercentOutput plus(Measure other) { + return new PercentOutput(measure.plus(other)); + } + + @Override + public PercentOutput minus(Measure other) { + return new PercentOutput(measure.minus(other)); + } + + @Override + public PercentOutput negate() { + return new PercentOutput(measure.negate()); + } + +} diff --git a/src/main/java/frc/robot/helpers/NamedUnits/Radians.java b/src/main/java/frc/robot/helpers/NamedUnits/Radians.java new file mode 100644 index 0000000..0b44f7e --- /dev/null +++ b/src/main/java/frc/robot/helpers/NamedUnits/Radians.java @@ -0,0 +1,76 @@ +package frc.robot.helpers.NamedUnits; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Dimensionless; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Units; + +public class Radians implements Measure{ + private final Measure measure; + + public Radians(double magnitude) { + measure = Units.Radians.of(magnitude); + } + + // Prevent an extra allocation + private Radians(Measure measure) { + this.measure = measure; + } + + @Override + public double magnitude() { + return measure.magnitude(); + } + + @Override + public double baseUnitMagnitude() { + return measure.baseUnitMagnitude(); + } + + @Override + public Angle unit() { + return measure.unit(); + } + + @Override + public Measure copy() { + return measure.copy(); + } + + @Override + public Radians times(double multiplier) { + return new Radians(measure.times(multiplier)); + } + + @Override + public > Measure times(Measure other) { + return measure.times(other); + } + + @Override + public Radians divide(Measure divisor) { + return new Radians(measure.divide(divisor)); + } + + @Override + public Radians divide(double divisor) { + return new Radians(measure.divide(divisor)); + } + + @Override + public Radians plus(Measure other) { + return new Radians(measure.plus(other)); + } + + @Override + public Radians minus(Measure other) { + return new Radians(measure.minus(other)); + } + + @Override + public Radians negate() { + return new Radians(measure.negate()); + } + +} diff --git a/src/main/java/frc/robot/helpers/NamedUnits/RevolutionsPerMinute.java b/src/main/java/frc/robot/helpers/NamedUnits/RevolutionsPerMinute.java new file mode 100644 index 0000000..6d329bf --- /dev/null +++ b/src/main/java/frc/robot/helpers/NamedUnits/RevolutionsPerMinute.java @@ -0,0 +1,77 @@ +package frc.robot.helpers.NamedUnits; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Dimensionless; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; + +public class RevolutionsPerMinute implements Measure>{ + // TODO: To save allocations/speed, maybe make this a MutableMeasure? + private Measure> measure; + + public RevolutionsPerMinute(double value) { + this.measure = Units.RPM.of(value); + } + + // Prevent an extra allocation + private RevolutionsPerMinute(Measure> measure) { + this.measure = measure; + } + + @Override + public double magnitude() { + return measure.magnitude(); + } + + @Override + public double baseUnitMagnitude() { + return measure.baseUnitMagnitude(); + } + + @Override + public Velocity unit() { + return measure.unit(); + } + + @Override + public Measure> copy() { + return measure.copy(); + } + + @Override + public RevolutionsPerMinute times(double multiplier) { + return new RevolutionsPerMinute(measure.times(multiplier)); + } + + @Override + public > Measure times(Measure other) { + return measure.times(other); + } + + @Override + public RevolutionsPerMinute divide(Measure divisor) { + return new RevolutionsPerMinute(measure.divide(divisor)); + } + + @Override + public RevolutionsPerMinute divide(double divisor) { + return new RevolutionsPerMinute(measure.divide(divisor)); + } + + @Override + public RevolutionsPerMinute plus(Measure> other) { + return new RevolutionsPerMinute(measure.plus(other)); + } + + @Override + public RevolutionsPerMinute minus(Measure> other) { + return new RevolutionsPerMinute(measure.minus(other)); + } + + @Override + public RevolutionsPerMinute negate() { + return new RevolutionsPerMinute(measure.negate()); + } +} diff --git a/src/main/java/frc/robot/helpers/TunablePIDFController.java b/src/main/java/frc/robot/helpers/TunablePIDFController.java new file mode 100644 index 0000000..9189e03 --- /dev/null +++ b/src/main/java/frc/robot/helpers/TunablePIDFController.java @@ -0,0 +1,227 @@ +package frc.robot.helpers; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class TunablePIDFController { + private final String controllerName; + private final PIDController pidController; + private SimpleMotorFeedforward ffController; + private double setpoint = 0.0; + private boolean isTuning = false; + + // TODO: Make this a struct + private final DoubleEntry KpEntry; + private final DoubleEntry KiEntry; + private final DoubleEntry KdEntry; + private final DoubleEntry KsEntry; + private final DoubleEntry KvEntry; + private final DoubleEntry KaEntry; + private final DoublePublisher setpointPublisher; + private final DoublePublisher errorPublisher; + private final DoublePublisher outputPIDPublisher; + private final DoublePublisher outputFFPublisher; + private final DoublePublisher outputPublisher; + + + public TunablePIDFController( + String controllerName, + double Kp, + double Ki, + double Kd, + double Ks, + double Kv, + double Ka + ) { + this.controllerName = controllerName; + pidController = new PIDController(Kp, Ki, Kd); + ffController = new SimpleMotorFeedforward(Ks, Kv, Ka); + setpoint = 0.0; + + KpEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kp").getEntry(Kp); + KiEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ki").getEntry(Ki); + KdEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kd").getEntry(Kd); + KsEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ks").getEntry(Ks); + KvEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kv").getEntry(Kv); + KaEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ka").getEntry(Ka); + setpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/setpoint").publish(); + errorPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/error").publish(); + outputPIDPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/PID Output").publish(); + outputFFPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/FF Output").publish(); + outputPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Output").publish(); + + KpEntry.set(Kp); + KiEntry.set(Ki); + KdEntry.set(Kd); + KsEntry.set(Ks); + KvEntry.set(Kv); + KaEntry.set(Ka); + setpointPublisher.set(0.0); + errorPublisher.set(0.0); + outputPIDPublisher.set(0.0); + outputFFPublisher.set(0.0); + outputPublisher.set(0.0); + + } + + public TunablePIDFController( + String controllerName, + double Kp, + double Ki, + double Kd + ) { + this.controllerName = controllerName; + pidController = new PIDController(Kp, Ki, Kd); + ffController = new SimpleMotorFeedforward(0, 0, 0); + setpoint = 0.0; + + KpEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kp").getEntry(Kp); + KiEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ki").getEntry(Ki); + KdEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kd").getEntry(Kd); + KsEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ks").getEntry(0); + KvEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Kv").getEntry(0); + KaEntry = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Ka").getEntry(0); + setpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/setpoint").publish(); + errorPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/error").publish(); + outputPIDPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/PID Output").publish(); + outputFFPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/FF Output").publish(); + outputPublisher = NetworkTableInstance.getDefault().getDoubleTopic(controllerName + "/Output").publish(); + + KpEntry.set(Kp); + KiEntry.set(Ki); + KdEntry.set(Kd); + KsEntry.set(0.0); + KvEntry.set(0.0); + KaEntry.set(0.0); + setpointPublisher.set(0.0); + errorPublisher.set(0.0); + outputPIDPublisher.set(0.0); + outputFFPublisher.set(0.0); + outputPublisher.set(0.0); + + } + + public void setTuningMode(boolean isTuning) { + this.isTuning = isTuning; + } + + public void setKp(double Kp) { + pidController.setP(Kp); + KpEntry.set(Kp); + } + + public void setKi(double Ki) { + pidController.setI(Ki); + KiEntry.set(Ki); + } + + public void setKd(double Kd) { + pidController.setD(Kd); + KdEntry.set(Kd); + } + + public void setPID(double Kp, double Ki, double Kd) { + KpEntry.set(Kp); + KiEntry.set(Ki); + KpEntry.set(Kd); + + pidController.setPID(Kp, Ki, Kd); + } + + /** + * This updates the controller with the current values in + * network tables + */ + public void updatePID() { + pidController.setPID( + KpEntry.get(), + KiEntry.get(), + KdEntry.get() + ); + } + + /** + * WARNING: This makes an allocation. DO NOT CALL IN A LOOP! + * @param Ks + * @param Kv + * @param Ka + */ + public void setFF(double Ks, double Kv, double Ka) { + // The FF values aren't supposed to be changed, + // but in case we want to, this mechanism allows for it + KsEntry.set(Ks); + KvEntry.set(Kv); + KaEntry.set(Ka); + + ffController = new SimpleMotorFeedforward(Ks, Kv, Ka); + } + + /** + * Updates the Ks, Kv, and Ka values from network tables + * + * WARNING: This makes an allocation. DO NOT CALL IN A LOOP! + */ + public void updateFF() { + ffController = new SimpleMotorFeedforward( + KsEntry.get(), + KvEntry.get(), + KaEntry.get() + ); + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + pidController.setSetpoint(setpoint); + setpointPublisher.set(pidController.getSetpoint()); + } + + public double getSetpoint() { + return setpoint; + } + + public double calculate(double measurement) { + if (measurement < 1e-6) { + measurement = 0; + } + + if (isTuning) { + updatePID(); + } + + double pidOut = pidController.calculate(measurement); + double ffOut = ffController.calculate(setpoint); + + errorPublisher.set(pidController.getPositionError()); + outputPIDPublisher.set(pidOut); + outputFFPublisher.set(ffOut); + outputPublisher.set(pidOut + ffOut); + + return pidOut + ffOut; + } + + public double calculate(double measurement, double setpoint) { + if (measurement < 1e-6) { + measurement = 0; + } + + if (isTuning) { + updatePID(); + } + + setSetpoint(setpoint); + + double pidOut = pidController.calculate(measurement); + double ffOut = ffController.calculate(setpoint); + + errorPublisher.set(pidController.getPositionError()); + outputPIDPublisher.set(pidOut); + outputFFPublisher.set(ffOut); + outputPublisher.set(pidOut + ffOut); + + return pidOut + ffOut; + } + +} diff --git a/src/main/java/frc/robot/subsystems/AutoSwitchBoardSub.java b/src/main/java/frc/robot/subsystems/AutoSwitchBoardSub.java index db85539..bf30687 100644 --- a/src/main/java/frc/robot/subsystems/AutoSwitchBoardSub.java +++ b/src/main/java/frc/robot/subsystems/AutoSwitchBoardSub.java @@ -57,7 +57,6 @@ public void periodic() { Crashboard.toDashboard("Amp Side Switch", isPositionAmpSide(), "Competition"); Crashboard.toDashboard("Forward Side Switch", isPositionForward(), "Competition"); Crashboard.toDashboard("Source Side Switch", isPositionSourceSide(), "Competition"); - //System.out.println(diOnes.get()); } } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index 064ece8..8139115 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -38,7 +38,6 @@ public void goDown() { @Override public void periodic() { - //System.out.println("First: " + leftArm.get() + ", Second: " + rightArm.get()); } } diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 6b375da..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} diff --git a/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java deleted file mode 100644 index 7eba578..0000000 --- a/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; -import frc.robot.Constants; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class GroundIntakeSubsystem extends SubsystemBase { - private CANSparkMax intakeMotor; - - public GroundIntakeSubsystem() { - - intakeMotor = new CANSparkMax(Constants.GroundIntakeConstants.intakeMotorId, CANSparkLowLevel.MotorType.kBrushless); - - intakeMotor.restoreFactoryDefaults(); - - } - - public void spintake(double speed) { - intakeMotor.set(speed); - } - -} diff --git a/src/main/java/frc/robot/subsystems/HingeSubsystem.java b/src/main/java/frc/robot/subsystems/HingeSubsystem.java index bf67393..7454c75 100644 --- a/src/main/java/frc/robot/subsystems/HingeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HingeSubsystem.java @@ -25,7 +25,7 @@ public class HingeSubsystem extends ProfiledPIDSubsystem { private final DutyCycleEncoder encoder; private double outVoltage = 0; - private RelativeEncoder hingeLeaderRelativeEncoder; + private RelativeEncoder hingeLeaderRelativeEncoder; private RelativeEncoder hingeFollowerRelativeEncoder; private final ArmFeedforward ff; @@ -37,11 +37,11 @@ public class HingeSubsystem extends ProfiledPIDSubsystem { public HingeSubsystem() { super( new ProfiledPIDController( - Constants.HingeConstants.kHingeP, - Constants.HingeConstants.kHingeI, - Constants.HingeConstants.kHingeD, + Constants.HingeConstants.kHingeP, + Constants.HingeConstants.kHingeI, + Constants.HingeConstants.kHingeD, new TrapezoidProfile.Constraints( - Constants.HingeConstants.kHingeMaxVelocityRadPerSecond, + Constants.HingeConstants.kHingeMaxVelocityRadPerSecond, Constants.HingeConstants.KHingeMaxAccelerationRadPerSecond)), Constants.HingeConstants.kShootingAngle); @@ -81,8 +81,6 @@ public HingeSubsystem() { protected void useOutput(double output, State setpoint) { //double feedforward = ff.calculate(setpoint.position, setpoint.velocity); outVoltage = output; - //System.out.println("Howdy! useOutput was successfully called. >:3c"); - System.out.println(output + " :3"); Crashboard.toDashboard("Motor Voltage", output, "Hinge"); hingeLeader.setVoltage(output); @@ -151,14 +149,14 @@ public boolean isAtPosition(double setpoint, double deadzone) { public double getAbsoluteDegrees() { //return ((.87 - encoder.getAbsolutePosition())/.26) * (72) + (60); - return parseRawAbsEncoderValue(encoder.getAbsolutePosition(), - Constants.HingeConstants.rawEncoderLow, - Constants.HingeConstants.rawEncoderHigh, - Constants.HingeConstants.degreesLow, + return parseRawAbsEncoderValue(encoder.getAbsolutePosition(), + Constants.HingeConstants.rawEncoderLow, + Constants.HingeConstants.rawEncoderHigh, + Constants.HingeConstants.degreesLow, Constants.HingeConstants.degreesHigh); } public double parseRawAbsEncoderValue(double rawAbsoluteEncoderValue, double rawEncoderLow, double rawEncoderHigh, double degreesLow, double degreesHigh) { - + return (rawAbsoluteEncoderValue-rawEncoderLow)/(rawEncoderHigh-rawEncoderLow)*(degreesHigh-degreesLow)+degreesLow; } @@ -167,7 +165,7 @@ public void SetRelativeEncoderSoftLimits(float lowerLimit, float upperLimit){ hingeLeader.setSoftLimit(SoftLimitDirection.kForward, upperLimit); hingeFollower.setSoftLimit(SoftLimitDirection.kForward, upperLimit); - + hingeLeader.enableSoftLimit(SoftLimitDirection.kForward, true); hingeFollower.enableSoftLimit(SoftLimitDirection.kForward, true); diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 89cc876..a518cb0 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -23,12 +23,7 @@ public void getEntry(){ //throws InterruptedException{ NetworkTableEntry ta = table.getEntry("ta"); double id = LimelightHelpers.getFiducialID(""); - //System.out.println(); - //System.out.println(LimelightHelpers.getTX("")); - //System.out.println(LimelightHelpers.getTY("")); - //System.out.println(LimelightHelpers.getTA("")); System.out.println(LimelightHelpers.getBotPose_TargetSpace("")[0]); - //System.out.println(id); Crashboard.toDashboard("tx", tx.getDouble(0.0), "Limelight"); Crashboard.toDashboard("ty", ty.getDouble(0.0), "Limelight"); diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index d15356f..90b5328 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -1,33 +1,57 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.OurRobotState; +import frc.robot.Robot; import frc.robot.helpers.Crashboard; +import frc.robot.helpers.TunablePIDFController; +import frc.robot.helpers.NamedUnits.PercentOutput; +import frc.robot.helpers.NamedUnits.RevolutionsPerMinute; public class ManipulatorSubsystem extends SubsystemBase { + private final boolean enabled; - public final CANSparkMax intakeLeader; - public final CANSparkMax shooterLeader; + private final CANSparkMax intakeLeader; + private final CANSparkMax shooterLeader; - public final DigitalInput intakeProximitySensor; + private final DigitalInput intakeProximitySensor; - public final RelativeEncoder shooterEncoder; + private final RelativeEncoder shooterEncoder; - private PIDController shooterOnboardPID; - private SimpleMotorFeedforward shooterFeedforward; + private final DCMotorSim shooterSim; + private final DCMotorSim intakeSim; + private final DoublePublisher shooterVelocityPublisher; + private final DoublePublisher shooterVoltagePublisher; - private double shooterVelocity; + private final DoublePublisher intakeVelocityPublisher; + + private final TunablePIDFController shooterOnboardPIDF; + private final MutableMeasure> shooterRPMSetpoint = MutableMeasure.zero(Units.RPM); + + public ManipulatorSubsystem(boolean enabled) { + this.enabled = enabled; - public ManipulatorSubsystem() { intakeLeader = new CANSparkMax(Constants.ManipulatorConstants.kIntakeLeaderPort, MotorType.kBrushless); intakeLeader.restoreFactoryDefaults(); intakeLeader.setIdleMode(IdleMode.kBrake); @@ -36,7 +60,7 @@ public ManipulatorSubsystem() { shooterLeader = new CANSparkMax(Constants.ManipulatorConstants.kShooterLeaderPort, MotorType.kBrushless); shooterLeader.restoreFactoryDefaults(); - shooterLeader.setIdleMode(IdleMode.kBrake); + shooterLeader.setIdleMode(IdleMode.kBrake); shooterLeader.setSmartCurrentLimit(35); shooterLeader.setInverted(Constants.ManipulatorConstants.kShooterLeaderInverted); @@ -44,71 +68,155 @@ public ManipulatorSubsystem() { shooterEncoder = this.shooterLeader.getEncoder(); - this.initPIDs(); - - } - - public void setShooterSpeedByRPM(double speed) { - speed = speed/60.0; - shooterLeader.setVoltage(shooterOnboardPID.calculate(speed) + shooterFeedforward.calculate(speed)); - shooterVelocity = shooterEncoder.getVelocity(); - } - - public double getShooterRPM() { - shooterVelocity = shooterEncoder.getVelocity(); - return shooterVelocity; + // TODO: These numbers are nonsense for this + shooterSim = new DCMotorSim( + DCMotor.getNEO(1), + 1.0, + 0.0002 + ); + + intakeSim = new DCMotorSim( + DCMotor.getNEO(1), + 1.0, + 0.000418 + ); + + shooterVelocityPublisher = NetworkTableInstance.getDefault().getTable("Shuffleboard/Manipulator").getDoubleTopic("ShooterVelocity").publish(); + shooterVoltagePublisher = NetworkTableInstance.getDefault().getTable("Shuffleboard/Manipulator").getDoubleTopic("ShooterVoltage").publish(); + intakeVelocityPublisher = NetworkTableInstance.getDefault().getTable("Shuffleboard/Manipulator").getDoubleTopic("IntakeVelocity").publish(); + + shooterVelocityPublisher.set(0.0); + shooterVoltagePublisher.set(0.0); + intakeVelocityPublisher.set(0.0); + + shooterOnboardPIDF = new TunablePIDFController( + "ShooterPID", + 0.003, + 0, + 0, + 0.001, //Constants.ManipulatorConstants.SHOOTER_KS, + 0.002, //Constants.ManipulatorConstants.SHOOTER_KV, + 0 //Constants.ManipulatorConstants.SHOOTER_KA + ); + // TODO + shooterOnboardPIDF.setTuningMode(true); + + shooterOnboardPIDF.setSetpoint(shooterRPMSetpoint.magnitude()); } - public void initPIDs() { - shooterOnboardPID = new PIDController(Constants.ManipulatorConstants.SHOOTER_KP, Constants.ManipulatorConstants.SHOOTER_KI, Constants.ManipulatorConstants.SHOOTER_KD); - shooterFeedforward = new SimpleMotorFeedforward(Constants.ManipulatorConstants.SHOOTER_KS, Constants.ManipulatorConstants.SHOOTER_KV, Constants.ManipulatorConstants.SHOOTER_KA); + @Override + public void simulationPeriodic() { + shooterSim.update(0.02); + intakeSim.update(0.02); } @Override public void periodic() { - Crashboard.toDashboard("Sensor value: ", intakeProximitySensor.get(), "Manipulator"); OurRobotState.hasNote = !intakeProximitySensor.get(); - - } + // Update our PID controller + if (enabled) { + if (Robot.isReal()) { + double shooterSpeed = shooterEncoder.getVelocity(); + double voltage = shooterOnboardPIDF.calculate(shooterSpeed); + shooterLeader.setVoltage(voltage); + + shooterVelocityPublisher.set(shooterSpeed); + shooterVoltagePublisher.set(voltage); + } else { + double shooterSpeed = shooterSim.getAngularVelocityRPM(); - public double GetShooterVelocity(){ + shooterSim.setInputVoltage(shooterOnboardPIDF.calculate(shooterSpeed)); - return shooterEncoder.getVelocity(); + shooterVelocityPublisher.set(shooterSpeed); + + intakeVelocityPublisher.set(intakeSim.getAngularVelocityRPM()); + } + } + } + // TODO: Make this a mutableRevolutionsPerMinute. We're going to + // run out of memory by creating this many objects + public RevolutionsPerMinute getShooterRPM() { + if (Robot.isReal()) { + return new RevolutionsPerMinute(shooterEncoder.getVelocity()); + } else { + return new RevolutionsPerMinute(shooterSim.getAngularVelocityRPM()); + } } - public void spintake(double speed) { - intakeLeader.set(speed); + public void setShooterRPM(RevolutionsPerMinute rpm) { + if (enabled) { + // Update the setpoint. The actual motor is controlled + // in the periodic loop so we get continous feedback + shooterRPMSetpoint.mut_replace(rpm); + shooterOnboardPIDF.setSetpoint(shooterRPMSetpoint.magnitude()); + } } - public void spintakeVoltage(double voltage) { - intakeLeader.setVoltage(voltage); + public void setShooterVoltage(Measure voltage) { + if (enabled) { + if (Robot.isReal()) { + shooterLeader.setVoltage(voltage.magnitude()); + } else { + shooterSim.setInputVoltage(voltage.magnitude()); + } + } } - public void spinShooterVoltage(double voltage) { - shooterLeader.setVoltage(voltage); + public boolean hasShooterReachedRPM(RevolutionsPerMinute rpm) { + return rpm.isNear(getShooterRPM(), 0.05); } - public void spinShooter(double speed) { - shooterLeader.set(speed); + /** + * @param value Should be [-1.0, 1.0] + */ + public void setIntakePercentOut(PercentOutput value) { + if (enabled) { + if (Robot.isReal()) { + intakeLeader.set(value.magnitude()); + } else { + intakeSim.setInputVoltage(value.magnitude() * RobotController.getBatteryVoltage()); + } + } } - public boolean intakeTrigged() { + public void setIntakeVoltage(Measure voltage) { + if (enabled) { + if (Robot.isReal()) { + intakeLeader.setVoltage(voltage.magnitude()); + } else { + intakeSim.setInputVoltage(voltage.magnitude()); + } + } + } + + public boolean isIntakeTriggered() { return !intakeProximitySensor.get(); } public void stopIntake(){ - intakeLeader.stopMotor(); + if (Robot.isReal()) { + intakeLeader.stopMotor(); + } else { + intakeSim.setInputVoltage(0); + } } public void stopShooter(){ - shooterLeader.stopMotor(); + shooterRPMSetpoint.mut_setMagnitude(0); + shooterOnboardPIDF.setSetpoint(shooterRPMSetpoint.magnitude()); + + if (Robot.isReal()) { + shooterLeader.stopMotor(); + } else { + shooterSim.setInputVoltage(0); + } } public void stopAll(){ - intakeLeader.stopMotor(); - shooterLeader.stopMotor(); - } + stopShooter(); + stopIntake(); + } } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index f12b67b..44471aa 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,6 +4,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.*; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVPhysicsSim; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -14,14 +15,29 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StructEntry; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructSubscriber; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.Constants.ModuleConstants; import frc.robot.helpers.Crashboard; -import com.ctre.phoenix.sensors.AbsoluteSensorRange; -import com.ctre.phoenix.sensors.CANCoder; //BAD CRINGE -import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; + +import java.nio.ByteBuffer; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -31,16 +47,71 @@ public class SwerveModule { public final CANSparkMax turningMotor; private final RelativeEncoder driveEncoder; - //private final RelativeEncoder turningEncoder; + private final CANcoder absoluteEncoder; private final PIDController turningPidController; - private final PIDController drivePidController; - - private final CANcoder absoluteEncoder; + private DCMotorSim driveMotorSim; + private DCMotorSim turningMotorSim; private final String ModuleName; + class SwerveModuleTuning { + private final DoubleEntry driveMomentSub; + private final DoubleEntry turningMomentSub; + private final DoubleEntry turningkPSub; + private final DoubleEntry turningkISub; + private final DoubleEntry turningkDSub; + + SwerveModuleTuning( + double driveMoment, + double turningMoment, + double turningkP, + double turningkI, + double turningkD, + String moduleName + ) { + NetworkTable table = NetworkTableInstance + .getDefault() + .getTable("/SwerveModuleTuning/" + moduleName); + + driveMomentSub = table.getDoubleTopic("DriveMoment").getEntry(driveMoment); + turningMomentSub = table.getDoubleTopic("TurningMoment").getEntry(turningMoment); + turningkPSub = table.getDoubleTopic("TurningMomementkP").getEntry(turningkP); + turningkISub = table.getDoubleTopic("TurningMomementkI").getEntry(turningkI); + turningkDSub = table.getDoubleTopic("TurningMomementkD").getEntry(turningkD); + + // Publish once so they show up in the table + driveMomentSub.set(driveMoment); + turningMomentSub.set(turningMoment); + turningkPSub.set(turningkP); + turningkISub.set(turningkI); + turningkDSub.set(turningkD); + } + + double getDriveMoment() { + return driveMomentSub.get(); + } + + double getTurningMoment() { + return turningMomentSub.get(); + } + + double getTurningkP() { + return turningkPSub.get(); + } + + double getTurningkI() { + return turningkISub.get(); + } + + double getTurningkD() { + return turningkDSub.get(); + } + } + + private final SwerveModuleTuning moduleTuning; + public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boolean driveMotorReversed, boolean turningMotorReversed, int absoluteEncoderId, double absoluteEncoderOffset, SensorDirectionValue positiveDirection) { @@ -65,11 +136,37 @@ public SwerveModule(String ModuleName, int driveMotorId, int turningMotorId, boo driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); //turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Deg); //turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2DegPerSec); - - turningPidController = new PIDController(ModuleConstants.kPTurning, 0, ModuleConstants.kDTurning); + + /* Tuning */ + moduleTuning = new SwerveModuleTuning( + 0.000418, + 0.000418, + 0.01, + 0.0, + 0.0, + this.ModuleName + ); + + turningPidController = new PIDController( + moduleTuning.getTurningkP(), + moduleTuning.getTurningkI(), + moduleTuning.getTurningkD() + ); turningPidController.enableContinuousInput(-180, 180); - this.drivePidController = new PIDController(ModuleConstants.kPModuleDriveController, 0, ModuleConstants.kDModuleDriveController); + /* ----------------- Simulation ---------------- */ + + // TODO: These numbers are bogus. Fix them! + driveMotorSim = new DCMotorSim( + DCMotor.getNEO(1), + 1.0, // This is a n:m where n>m represents a reduction + moduleTuning.getDriveMoment() // This is the moment of inertia of the motor. This number is made up + ); + turningMotorSim = new DCMotorSim( + DCMotor.getNEO(1), + 1.0 / ModuleConstants.kTurningMotorGearRatio, // This is a n:m where n>m represents a reduction + moduleTuning.getTurningMoment() // This is the moment of inertia of the motor. This number is made up + ); resetEncoders(); } @@ -83,21 +180,48 @@ private void configureMotor(CANSparkMax motor, Boolean inverted) { } /////Get position from encoders rather than gyro public SwerveModulePosition getPosition() { - return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAbsoluteEncoderDeg())); + if (Robot.isReal()) { + return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAbsoluteEncoderDeg())); + } else { + double speed = driveMotorSim.getAngularPositionRotations() * ModuleConstants.kDriveEncoderRot2Meter; + Rotation2d angle = Rotation2d.fromDegrees(getAbsoluteEncoderDeg()); + return new SwerveModulePosition( + speed, + angle + ); + } } + // What the heck are the units on this thing?!? public double getDriveVelocity() { - return driveEncoder.getVelocity(); + if (Robot.isReal()) { + return driveEncoder.getVelocity(); + } else { + return driveMotorSim.getAngularVelocityRPM() * ModuleConstants.kDriveEncoderRPM2MeterPerSec; + } } public double getAbsoluteEncoderDeg() { - return (absoluteEncoder.getAbsolutePosition().getValue() * 360);// - absoluteEncoderOffset; + if (Robot.isReal()) { + return (absoluteEncoder.getAbsolutePosition().getValue() * 360);// - absoluteEncoderOffset; + } else { + return turningMotorSim.getAngularPositionRad() * 180 / Math.PI; + } //should be in degrees??? + // No. This should absolutely be in radians. I won't change it because I don't know the + // codebase very well yet, but we almost always convert back this value to radians } public void resetEncoders() { driveEncoder.setPosition(0); + driveMotorSim.setState(0.0, 0.0); + //turningEncoder.setPosition(0); + } + + public void resetEncoders(Rotation2d rotation) { + driveEncoder.setPosition(rotation.getRotations()); + driveMotorSim.setState(rotation.getRadians(), 0.0); //turningEncoder.setPosition(0); } @@ -106,43 +230,57 @@ public SwerveModuleState getState() { } public void setDesiredState(SwerveModuleState state) { - - if (Math.abs(state.speedMetersPerSecond) < 0.001) { + // Update the parameters + turningPidController.setPID( + moduleTuning.getTurningkP(), + moduleTuning.getTurningkI(), + moduleTuning.getTurningkD() + ); + + + if (Math.abs(state.speedMetersPerSecond) < 0.01) { stop(); return; } state = SwerveModuleState.optimize(state, getState().angle); // This is the jank line that we thought broke things earlier. - state.speedMetersPerSecond *= state.angle.minus(Rotation2d.fromDegrees(getAbsoluteEncoderDeg())).getCos(); - + //state.speedMetersPerSecond *= state.angle.minus(Rotation2d.fromDegrees(getAbsoluteEncoderDeg())).getCos(); + // Calculate the drive output from the drive PID controller. ;} - double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond/* / 360*/, -1 ,1); // Halved for practice for David 😈 - + double driveSpeed = MathUtil.clamp(state.speedMetersPerSecond / Constants.DriveConstants.kTeleDriveMaxSpeedMetersPerSecond/* / 360*/, -1 ,1); + driveMotor.set(driveSpeed); + driveMotorSim.setInputVoltage(driveSpeed * RobotController.getBatteryVoltage()); + // Crashboard.toDashboard("driveSpeed", driveSpeed, "Swerve"); // Crashboard.toDashboard("speed in m/s", state.speedMetersPerSecond, "Swerve"); - - + + double turnSpeed = (turningPidController.calculate(getAbsoluteEncoderDeg(), state.angle.getDegrees())); - //System.out.println("Turn Speed Calculated " + this.ModuleName + ": " + turnSpeed); if (turnSpeed > 0) turnSpeed = Math.min(turnSpeed, .2); else turnSpeed = Math.max(turnSpeed, -.2); - - //System.out.println("Turn Speed Final " + this.ModuleName + ": " + turnSpeed); - // Crashboard.toDashboard(ModuleName + "Turn Speed Final", turnSpeed, "Swerve"); - // + // turningMotor.set(turnSpeed); - //System.out.println(ModuleName + "- DriveMotorCommand: " + driveSpeed + " - True Angle: " + getAbsoluteEncoderRad() + " AngleSetPoint: " + state.angle.getDegrees() + " AngleMotorCommand: " + turnSpeed); + turningMotorSim.setInputVoltage(turnSpeed * RobotController.getBatteryVoltage()); + + } + + public void updateSim(double dt) { + driveMotorSim.update(dt); + turningMotorSim.update(dt); } public void stop() { driveMotor.set(0); turningMotor.set(0); + + driveMotorSim.setInputVoltage(0.0); + turningMotorSim.setInputVoltage(0.0); } public CANcoder getCANcoder() { diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 804f696..c2349e8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,28 +1,35 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.Constants.DriveConstants; import frc.robot.helpers.AutoSelector; import frc.robot.helpers.Crashboard; public class SwerveSubsystem extends SubsystemBase { + public final boolean enabled; + public static final double MAX_VOLTAGE = 12.0; public final SwerveModule frontLeft; - public final SwerveModule frontRight; + public final SwerveModule frontRight; public final SwerveModule backLeft; public final SwerveModule backRight; public final StructArrayPublisher publisher; @@ -36,6 +43,11 @@ public class SwerveSubsystem extends SubsystemBase { private final StructArrayPublisher publisher_current; private final StructArrayPublisher publisher_desired; + /* ------------------- Simulation objects -------------------- */ + private final Field2d fieldSim; + private final AnalogGyro dummyGryo = new AnalogGyro(0); // The simulated version crashes without this + private final AnalogGyroSim gyroSim = new AnalogGyroSim(0); + SwerveModuleState[] states = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), @@ -47,7 +59,9 @@ public class SwerveSubsystem extends SubsystemBase { 30, 12, 30, 12, 30, 12, 30, 12 }; - public SwerveSubsystem() { + public SwerveSubsystem(boolean enabled) { + this.enabled = enabled; + frontLeft = new SwerveModule( "Front Left", DriveConstants.kFrontLeftDriveMotorPort, @@ -99,66 +113,64 @@ public SwerveSubsystem() { odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()}); -//MUST USE A / IN THE NAME OR DIE + // MUST USE A / IN THE NAME publisher_current = NetworkTableInstance.getDefault() - .getStructArrayTopic("/MyStatesExpected", SwerveModuleState.struct).publish(); + .getStructArrayTopic("/SwerveMeasured", SwerveModuleState.struct).publish(); publisher_desired = NetworkTableInstance.getDefault() - .getStructArrayTopic("/MyStatesDesired", SwerveModuleState.struct).publish(); + .getStructArrayTopic("/SwerveCommanded", SwerveModuleState.struct).publish(); publisher = NetworkTableInstance.getDefault().getStructArrayTopic("MyStates", SwerveModuleState.struct).publish(); - new Thread(() -> { - try { - Thread.sleep(1000); - zeroHeading(); - - } - catch (Exception e) { - } - }).start(); - } - + + fieldSim = new Field2d(); + SmartDashboard.putData("Field", fieldSim); + + } + public void zeroHeading() { gyro.reset(); - + gyroSim.resetData(); } - public double getHeading() { - return Math.IEEEremainder(gyro.getAngle(), 360); + // This really should return type Degrees, but we don't want + // to do an allocation each time. + public double getHeadingDegrees() { + if (Robot.isReal()) { + return Math.IEEEremainder(gyro.getAngle(), 360); + } else { + return Math.IEEEremainder(gyroSim.getAngle(), 360); + } } public Rotation2d getRotation2d() { - return Rotation2d.fromDegrees(getHeading()); + return Rotation2d.fromDegrees(getHeadingDegrees()); } + + @Override + public void simulationPeriodic() { + // Update our fake motors and gyro + double dt = 0.02; // 20ms + + frontLeft.updateSim(dt); + frontRight.updateSim(dt); + backLeft.updateSim(dt); + backRight.updateSim(dt); + + ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates()); + double dOmega = Units.Degrees.convertFrom(chassisSpeeds.omegaRadiansPerSecond * dt, Units.Radians); + + gyroSim.setAngle(getHeadingDegrees() + dOmega); + } + + // This runs in both simulation and real robot operations @Override public void periodic() { odometer.update(getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition() }); - // Crashboard.toDashboard("Robot Heading", getHeading(), "navx"); - // frontLeft.logIt(); - // frontRight.logIt(); - // backLeft.logIt(); - // backRight.logIt(); - // Crashboard.toDashboard("gyro angle", -gyro.getAngle(), "Odometry"); - // Crashboard.toDashboard("navx odometry pose x", odometer.getPoseMeters().getX(), "Odometry"); - // Crashboard.toDashboard("navx odometry pose y", odometer.getPoseMeters().getY(), "Odometry"); - - Crashboard.toDashboard("Robot Heading", getHeading(), "navx"); - frontLeft.logIt(); - frontRight.logIt(); - backLeft.logIt(); - backRight.logIt(); - Crashboard.toDashboard("PRE-MATCH ORIENTATION", (Math.abs(gyro.getAngle()) < 10), "Competition"); // Comp Orientation Check - Crashboard.toDashboard("DETERMINED POSITION", "" + AutoSelector.getLocation(), "Competition"); - Crashboard.toDashboard("gyro angle", gyro.getAngle(), "navx"); - publisher_current.set(getModuleStates(), 0); - //System.out.println(getModuleStates()[1].speedMetersPerSecond); - //System.out.println(getModuleStates()[1].angle); - //SmartDashboard.putNumber("Front Right Wheel Angle", frontRight.getAbsoluteEncoderDeg()); - //SmartDashboard.putNumber("Back Left Wheel Angle", backLeft.getAbsoluteEncoderDeg()); - //SmartDashboard.putNumber("Back Right Wheel Angle", backRight.getAbsoluteEncoderDeg()); - //SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); + + publisher_current.set(getModuleStates()); + fieldSim.setRobotPose(getPose2d()); } public void stopModules() { @@ -170,15 +182,15 @@ public void stopModules() { } public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); - frontLeft.setDesiredState(desiredStates[0]); - frontRight.setDesiredState(desiredStates[1]); - backLeft.setDesiredState(desiredStates[2]); - backRight.setDesiredState(desiredStates[3]); - publisher.set(desiredStates); - //System.out.println(desiredStates); - publisher_desired.set(desiredStates, 0); - System.out.println(desiredStates); + if (enabled) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + frontLeft.setDesiredState(desiredStates[0]); + frontRight.setDesiredState(desiredStates[1]); + backLeft.setDesiredState(desiredStates[2]); + backRight.setDesiredState(desiredStates[3]); + publisher.set(desiredStates); + publisher_desired.set(desiredStates); + } } public SwerveModuleState[] getModuleStates() { @@ -200,6 +212,14 @@ public void ResetRelativePositionEncoders() backRight.resetEncoders(); } + public void ResetRelativePositionEncoders(Rotation2d rotation) + { + frontLeft.resetEncoders(rotation); + frontRight.resetEncoders(rotation); + backLeft.resetEncoders(rotation); + backRight.resetEncoders(rotation); + } + public void resetOdometry() { odometer.resetPosition(gyro.getRotation2d(), new SwerveModulePosition[] { frontLeft.getPosition(), @@ -210,19 +230,27 @@ public void resetOdometry() { } public void reorientMidMatch() { - new Thread(() -> { - try { - zeroHeading(); - } - catch (Exception e) { - } - }).start(); + zeroHeading(); } public AHRS getGyro() { return gyro; } - - + public void initializePose(Pose2d initialPose) { + // Reset all the things + gyroSim.setAngle(initialPose.getRotation().getDegrees()); + odometer.resetPosition( + initialPose.getRotation(), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }, + initialPose); + ResetRelativePositionEncoders(initialPose.getRotation()); + fieldSim.setRobotPose(initialPose); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/TestHingeSubsystem.java b/src/main/java/frc/robot/subsystems/TestHingeSubsystem.java deleted file mode 100644 index 6d49ab7..0000000 --- a/src/main/java/frc/robot/subsystems/TestHingeSubsystem.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkBase.SoftLimitDirection; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; - -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.helpers.Crashboard; - -public class TestHingeSubsystem extends SubsystemBase { - private final CANSparkMax hingeLeader; - private CANSparkMax hingeFollower; - - private DutyCycleEncoder encoder; - private RelativeEncoder hingeLeaderRelativeEncoder; - private RelativeEncoder hingeFollowerRelativeEncoder; - - public TestHingeSubsystem() { - //left motor - hingeLeader = new CANSparkMax(Constants.HingeConstants.kHingeLeaderPort, MotorType.kBrushless); - hingeLeader.restoreFactoryDefaults(); - hingeLeader.setIdleMode(IdleMode.kBrake); - hingeLeader.setSmartCurrentLimit(35); - hingeLeader.setInverted(Constants.HingeConstants.kHingeLeaderInverted); - // hingeLeader.getEncoder().setInverted(true); - - // right motor - hingeFollower = new CANSparkMax(Constants.HingeConstants.kHingeFollowerPort, MotorType.kBrushless); - hingeFollower.restoreFactoryDefaults(); - hingeFollower.setIdleMode(IdleMode.kCoast); - hingeFollower.setSmartCurrentLimit(35); - hingeFollower.setInverted(Constants.HingeConstants.kHingeFollowerInverted); - - hingeLeaderRelativeEncoder = hingeLeader.getEncoder(); - hingeFollowerRelativeEncoder = hingeFollower.getEncoder(); - - //hingeFollower.follow(hingeLeader); - encoder = new DutyCycleEncoder(Constants.HingeConstants.kAbsoluteEncoderPort); - encoder.setDistancePerRotation(360.0); - encoder.setPositionOffset(Constants.HingeConstants.kAbsoluteEncoderOffset); - } - - - - @Override - public void periodic() { - // - Crashboard.toDashboard("Left Motor Encoder", hingeLeader.getEncoder().getPosition(), "Hinge"); - Crashboard.toDashboard("Right Motor Encoder", hingeLeader.getEncoder().getPosition(), "Hinge"); - Crashboard.toDashboard("AbsoluteEncoderPositon", getAbsoluteAngle(), "Hinge"); - Crashboard.toDashboard("AbsoluteEncoderAngle", getAbsoluteDegrees(), "Hinge"); - } - - public void spinge(double speed) { - hingeLeader.set(speed); - hingeFollower.set(speed); - } - - public void stopHinge(){ - hingeLeader.stopMotor(); - hingeFollower.stopMotor(); - } - - - public void resetRelativeEncoders() { - hingeLeaderRelativeEncoder.setPosition(0); - hingeFollowerRelativeEncoder.setPosition(0); - } - - public double getAbsoluteAngle() { - return (encoder.getAbsolutePosition()); - } - public double getAbsoluteDegrees() { - //return ((.87 - encoder.getAbsolutePosition())/.26) * (72) + (60); - return parseRawAbsEncoderValue(encoder.getAbsolutePosition(), - Constants.HingeConstants.rawEncoderLow, - Constants.HingeConstants.rawEncoderHigh, - Constants.HingeConstants.degreesLow, - Constants.HingeConstants.degreesHigh); - } - public double parseRawAbsEncoderValue(double rawAbsoluteEncoderValue, double rawEncoderLow, double rawEncoderHigh, double degreesLow, double degreesHigh) { - - return (rawAbsoluteEncoderValue-rawEncoderLow)/(rawEncoderHigh-rawEncoderLow)*(degreesHigh-degreesLow)+degreesLow; - - } - - public void SetRelativeEncoderSoftLimits(float lowerLimit, float upperLimit){ - - hingeLeader.setSoftLimit(SoftLimitDirection.kForward, upperLimit); - hingeFollower.setSoftLimit(SoftLimitDirection.kForward, upperLimit); - - hingeLeader.enableSoftLimit(SoftLimitDirection.kForward, true); - hingeFollower.enableSoftLimit(SoftLimitDirection.kForward, true); - - - hingeLeader.setSoftLimit(SoftLimitDirection.kReverse, lowerLimit); - hingeFollower.setSoftLimit(SoftLimitDirection.kReverse, lowerLimit); - - hingeLeader.enableSoftLimit(SoftLimitDirection.kReverse, true); - hingeFollower.enableSoftLimit(SoftLimitDirection.kReverse, true); - - } -} diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json new file mode 100644 index 0000000..2d7fbce --- /dev/null +++ b/vendordeps/ChoreoLib.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib.json", + "name": "ChoreoLib", + "version": "2024.2.3", + "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", + "frcYear": "2024", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "javaDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-java", + "version": "2024.2.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-cpp", + "version": "2024.2.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file