diff --git a/.AdvantageScope/2-7-24-copy-of-config.json b/.AdvantageScope/2-7-24-copy-of-config.json new file mode 100644 index 0000000..82a73a2 --- /dev/null +++ b/.AdvantageScope/2-7-24-copy-of-config.json @@ -0,0 +1,72 @@ +{ + "name": "4909", + "sourceUrl": "", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 0 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": -90 + }, + { + "axis": "x", + "degrees": 0 + } + ], + "zeroedPosition": [ + -0.506, + 0.0, + 0.234 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 127 + }, + { + "axis": "y", + "degrees": 0 + }, + { + "axis": "z", + "degrees": -90 + }, + { + "axis": "x", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0.41, + 0.0, + -0.931 + ] + } + ] +} diff --git a/AdvantageScope 1-26-2024.json b/.AdvantageScope/AdvantageScope 2-16-2024.json similarity index 70% rename from AdvantageScope 1-26-2024.json rename to .AdvantageScope/AdvantageScope 2-16-2024.json index 5a193b4..28bc860 100644 --- a/AdvantageScope 1-26-2024.json +++ b/.AdvantageScope/AdvantageScope 2-16-2024.json @@ -1,5 +1,5 @@ { - "version": "3.0.1", + "version": "3.0.2", "layout": [ { "type": 9, @@ -32,130 +32,6 @@ "configHidden": false, "title": "Swerve" }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/3/speed", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/2/speed", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/1/speed", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/speed", - "color": "#80588e", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Line Graph" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/AdvantageKit/RealOutputs/LoggedRobot/FullCycleMS", - "color": "#2b66a2", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Line Graph" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/speed", - "color": "#2b66a2", - "show": false - }, - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", - "color": "#e5b31b", - "show": false - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured/0/angle/value", - "color": "#80588e", - "show": true - } - ] - } - }, - "title": "Line Graph" - }, { "type": 5, "fields": [], @@ -210,6 +86,18 @@ "key": "NT:/AdvantageKit/RealOutputs/Vision/TagPoses", "sourceTypeIndex": 2, "sourceType": "Pose3d[]" + }, + { + "type": "Component (Robot)", + "key": "NT:/AdvantageKit/RealOutputs/Mechanism3d/ArmMeasured", + "sourceTypeIndex": 2, + "sourceType": "Pose3d[]" + }, + { + "type": "Component (Green Ghost)", + "key": "NT:/AdvantageKit/RealOutputs/Mechanism3d/ArmSetpoint", + "sourceTypeIndex": 2, + "sourceType": "Pose3d[]" } ], [ @@ -242,7 +130,7 @@ "options": { "field": "2024 Field", "alliance": "blue", - "robot": "2024 KitBot", + "robot": "4909", "unitDistance": "meters", "unitRotation": "radians", "cameraIndex": -1, @@ -251,6 +139,11 @@ "configHidden": false, "title": "3D Field" }, + { + "type": 3, + "field": "NT:/AdvantageKit/RealOutputs/Console", + "title": "Console" + }, { "type": 8, "fields": [], @@ -269,6 +162,192 @@ }, "configHidden": false, "title": "Joysticks" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Elbow Feed Forward", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Wrist Feed Forward", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" + }, + { + "type": 10, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/Mechanism2d/ArmSetpoint", + "sourceTypeIndex": 0, + "sourceType": "Mechanism2d" + }, + { + "key": "NT:/AdvantageKit/RealOutputs/Mechanism2d/ArmMeasured", + "sourceTypeIndex": 0, + "sourceType": "Mechanism2d" + }, + null + ], + "listFields": [], + "options": {}, + "configHidden": false, + "title": "Mechanism" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/LoggedRobot/FullCycleMS", + "color": "#2b66a2", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Elbow Feed Forward", + "color": "#2b66a2", + "show": false + }, + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Wrist Feed Forward", + "color": "#e5b31b", + "show": false + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Elbow Progress", + "color": "#af2437", + "show": true + }, + { + "key": "NT:/AdvantageKit/RealOutputs/Arm/Wrist Progress", + "color": "#80588e", + "show": true + }, + { + "key": "NT:/AdvantageKit/ArmInputs/ElbowVelocityRadPerSec", + "color": "#e48b32", + "show": false + } + ] + } + }, + "title": "Line Graph" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/ArmInputs/WristVelocityRadPerSec", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/AdvantageKit/ArmInputs/ElbowVelocityRadPerSec", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/AdvantageKit/ArmInputs/ElbowAppliedVolts", + "color": "#af2437", + "show": false + }, + { + "key": "NT:/AdvantageKit/ArmInputs/WristAppliedVolts", + "color": "#80588e", + "show": false + } + ] + } + }, + "title": "Line Graph" } ] } diff --git a/.OutlineViewer/outlineviewer-window.json b/.OutlineViewer/outlineviewer-window.json index 7616dcc..31ff8a8 100644 --- a/.OutlineViewer/outlineviewer-window.json +++ b/.OutlineViewer/outlineviewer-window.json @@ -29,6 +29,11 @@ "Collapsed": "0", "Pos": "0,0", "Size": "600,400" + }, + "Settings": { + "Collapsed": "0", + "Pos": "173,161", + "Size": "329,169" } } } diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..3f8bf66 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1,29 @@ +{ + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "4909" + }, + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "transitory": { + "SmartDashboard": { + "TunableNumbers": { + "Arm": { + "Elbow": { + "open": true + }, + "Wrist": { + "open": true + }, + "open": true + }, + "open": true + }, + "open": true + } + } +} diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index e054053..805b184 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,8 +1,12 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.8254998984, + "robotLength": 0.8254998984, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Amp Side", + "Source Side", + "4 Note" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/.vscode/launch.json b/.vscode/launch.json index b8c1920..b4b9950 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,6 +1,13 @@ { "version": "0.2.0", "configurations": [ + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "2024-Crescendo" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", diff --git a/.vscode/settings.json b/.vscode/settings.json index f6161e7..d9e9859 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,8 +25,7 @@ "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } - }, - null + } ], "java.test.defaultConfig": "WPIlibUnitTests", "spotlessGradle.format.enable": true, @@ -36,6 +35,6 @@ "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, "[java]": { - "editor.defaultFormatter": "redhat.java" + "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" } } diff --git a/ChoreoProject.chor b/ChoreoProject.chor new file mode 100644 index 0000000..c73ce5a --- /dev/null +++ b/ChoreoProject.chor @@ -0,0 +1,3070 @@ +{ + "version": "v0.3", + "robotConfiguration": { + "mass": 65.1974197627209, + "rotationalInertia": 15, + "motorMaxTorque": 1.1251759834368529, + "motorMaxVelocity": 3600, + "gearing": 6.593406593406593, + "wheelbase": 0.40004978397311663, + "trackWidth": 0.5270497153931537, + "bumperLength": 0.8254998984, + "bumperWidth": 0.8254998984, + "wheelRadius": 0.04914897345955433 + }, + "paths": { + "Centerline Auto": { + "waypoints": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 2.486186981201172, + "y": 7.724460124969482, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 5.0970048904418945, + "y": 6.844727993011475, + "heading": 0.2938692977225419, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 4.501265048980713, + "y": 6.1953558921813965, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.139572620391846, + "y": 6.3622941970825195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "angularVelocity": 5.825284855617402e-28, + "velocityX": -1.9685293298057674e-27, + "velocityY": 1.4418564963898336e-27, + "timestamp": 0 + }, + { + "x": 0.6784036347143663, + "y": 6.707198268292706, + "heading": 1.0175475056665193, + "angularVelocity": -0.5991822637957332, + "velocityX": 0.32672283914932904, + "velocityY": 0.23758411786628814, + "timestamp": 0.05393128412995569 + }, + { + "x": 0.7135162843776108, + "y": 6.732573985913541, + "heading": 0.9516130808872657, + "angularVelocity": -1.2225635981589924, + "velocityX": 0.6510627408506594, + "velocityY": 0.4705194402508243, + "timestamp": 0.10786256825991138 + }, + { + "x": 0.7660994692609764, + "y": 6.770017684654193, + "heading": 0.8504346226500515, + "angularVelocity": -1.8760624722639487, + "velocityX": 0.9750033905489506, + "velocityY": 0.6942853177837462, + "timestamp": 0.16179385238986707 + }, + { + "x": 0.836479083356807, + "y": 6.818804043384565, + "heading": 0.7130270962325688, + "angularVelocity": -2.5478259721459318, + "velocityX": 1.3049868036933845, + "velocityY": 0.9046022084846815, + "timestamp": 0.21572513651982275 + }, + { + "x": 0.9255037273969096, + "y": 6.878132431612943, + "heading": 0.5402736783143448, + "angularVelocity": -3.2032135096569965, + "velocityX": 1.6507050680563082, + "velocityY": 1.1000737176110378, + "timestamp": 0.26965642064977846 + }, + { + "x": 1.035016950774323, + "y": 6.947674803955847, + "heading": 0.3381434888609651, + "angularVelocity": -3.747920946334524, + "velocityX": 2.0306066347970617, + "velocityY": 1.289462571952319, + "timestamp": 0.32358770477973414 + }, + { + "x": 1.162461605941772, + "y": 7.018689321892079, + "heading": 0.18269313635392612, + "angularVelocity": -2.882378104190095, + "velocityX": 2.3630932810787844, + "velocityY": 1.3167592628633178, + "timestamp": 0.3775189889096898 + }, + { + "x": 1.3050246421848908, + "y": 7.094859887834467, + "heading": 0.07841153426675496, + "angularVelocity": -1.9336013182235503, + "velocityX": 2.6434200212921195, + "velocityY": 1.4123632910138557, + "timestamp": 0.4314502730396455 + }, + { + "x": 1.4591047302314346, + "y": 7.181935811218979, + "heading": 0.022297438159640273, + "angularVelocity": -1.0404739477721165, + "velocityX": 2.8569705048235896, + "velocityY": 1.6145716681748061, + "timestamp": 0.4853815571696012 + }, + { + "x": 1.6255168678692307, + "y": 7.279018436792272, + "heading": 0.014123453422294747, + "angularVelocity": -0.15156295402959544, + "velocityX": 3.0856327699670643, + "velocityY": 1.800117077489892, + "timestamp": 0.5393128412995569 + }, + { + "x": 1.793858101376193, + "y": 7.378107838800647, + "heading": 0.014120593150970575, + "angularVelocity": -0.00005303547598235602, + "velocityX": 3.121402284828195, + "velocityY": 1.8373269542331496, + "timestamp": 0.5932441254295127 + }, + { + "x": 1.9621993268656661, + "y": 7.477197254469766, + "heading": 0.014117732942591941, + "angularVelocity": -0.000053034308839039934, + "velocityX": 3.121402136167001, + "velocityY": 1.8373272075322298, + "timestamp": 0.6471754095594684 + }, + { + "x": 2.130540737170093, + "y": 7.576286356115685, + "heading": 0.014114872666943619, + "angularVelocity": -0.00005303555616123241, + "velocityX": 3.121405563026878, + "velocityY": 1.8373213848783807, + "timestamp": 0.7011066936894241 + }, + { + "x": 2.3040566274888983, + "y": 7.663067241706454, + "heading": 0.010006762068371105, + "angularVelocity": -0.0761730536338314, + "velocityX": 3.217351359568822, + "velocityY": 1.609101043870144, + "timestamp": 0.7550379778193799 + }, + { + "x": 2.486186981201172, + "y": 7.724460124969482, + "heading": -5.2714780410694486e-27, + "angularVelocity": -0.18554651960925458, + "velocityX": 3.377081718903682, + "velocityY": 1.1383538191876352, + "timestamp": 0.8089692619493356 + }, + { + "x": 2.8066356107472474, + "y": 7.755704422012626, + "heading": -3.618373256811644e-9, + "angularVelocity": -4.070535151890212e-8, + "velocityX": 3.6049277350997357, + "velocityY": 0.3514867051672969, + "timestamp": 0.8978610936568053 + }, + { + "x": 3.128175858643877, + "y": 7.739109235929212, + "heading": -3.6183723834730783e-9, + "angularVelocity": 9.824733603484871e-15, + "velocityX": 3.6172080349831512, + "velocityY": -0.18668966275806864, + "timestamp": 0.9867529253642751 + }, + { + "x": 3.449716088164231, + "y": 7.722513693799808, + "heading": -3.6183716236893314e-9, + "angularVelocity": 8.547284180686901e-15, + "velocityX": 3.6172078282568996, + "velocityY": -0.18669366814284583, + "timestamp": 1.0756447570717447 + }, + { + "x": 3.771256317684507, + "y": 7.7059181516688815, + "heading": -3.6183708487107216e-9, + "angularVelocity": 8.718220728132132e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997415, + "timestamp": 1.1645365887792143 + }, + { + "x": 4.092796547204784, + "y": 7.689322609537955, + "heading": -3.618370111441706e-9, + "angularVelocity": 8.29400184228923e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997418, + "timestamp": 1.253428420486684 + }, + { + "x": 4.414336776725059, + "y": 7.6727270674070285, + "heading": -3.6183692900088547e-9, + "angularVelocity": 9.240813644117634e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.3423202521941535 + }, + { + "x": 4.735877006245335, + "y": 7.656131525276102, + "heading": -3.6183684638446085e-9, + "angularVelocity": 9.294040074640308e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.4312120839016231 + }, + { + "x": 5.0574172357656115, + "y": 7.6395359831451755, + "heading": -3.618367662939708e-9, + "angularVelocity": 9.009881839671523e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.5201039156090927 + }, + { + "x": 5.378957465285888, + "y": 7.62294044101425, + "heading": -3.6183667762068362e-9, + "angularVelocity": 9.975414552450845e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997418, + "timestamp": 1.6089957473165624 + }, + { + "x": 5.700497694806164, + "y": 7.606344898883323, + "heading": -3.6183660187139743e-9, + "angularVelocity": 8.521512579724434e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997426, + "timestamp": 1.697887579024032 + }, + { + "x": 6.02203792432644, + "y": 7.589749356752397, + "heading": -3.618365173816459e-9, + "angularVelocity": 9.504782370286536e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.7867794107315016 + }, + { + "x": 6.343578153846717, + "y": 7.57315381462147, + "heading": -3.6183643942346165e-9, + "angularVelocity": 8.770005387165316e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.8756712424389712 + }, + { + "x": 6.665118383366993, + "y": 7.556558272490544, + "heading": -3.6183635769900644e-9, + "angularVelocity": 9.193696838193051e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997424, + "timestamp": 1.9645630741464408 + }, + { + "x": 6.986658612887269, + "y": 7.539962730359617, + "heading": -3.6183627012139706e-9, + "angularVelocity": 9.85215488559423e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.0534549058539104 + }, + { + "x": 7.3081988424075455, + "y": 7.523367188228691, + "heading": -3.618361818393418e-9, + "angularVelocity": 9.931402417986347e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.1423467375613803 + }, + { + "x": 7.629739071927805, + "y": 7.506771646097765, + "heading": -3.6183609804060354e-9, + "angularVelocity": 9.427045955569345e-15, + "velocityX": 3.617207828255831, + "velocityY": -0.1866936681599647, + "timestamp": 2.23123856926885 + }, + { + "x": 7.922054241421621, + "y": 7.491684486652487, + "heading": -2.171017261999522e-9, + "angularVelocity": 1.6282077786061535e-8, + "velocityX": 3.288436787485536, + "velocityY": -0.16972492472567544, + "timestamp": 2.32013040097632 + }, + { + "x": 8.14129063555876, + "y": 7.480369116190248, + "heading": -1.0855088008297502e-9, + "angularVelocity": 1.221156590340072e-8, + "velocityX": 2.466327782046558, + "velocityY": -0.12729370342458965, + "timestamp": 2.40902223268379 + }, + { + "x": 8.287448236582586, + "y": 7.472825535627514, + "heading": -3.6183629850910975e-10, + "angularVelocity": 8.141046127861815e-9, + "velocityX": 1.6442185768520245, + "velocityY": -0.08486247181359102, + "timestamp": 2.4979140643912596 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 6.2013561613280124e-27, + "angularVelocity": 4.070523596587155e-9, + "velocityX": 0.8221093050723073, + "velocityY": -0.042431236765954845, + "timestamp": 2.5868058960987295 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 4.2684027559613635e-27, + "angularVelocity": -4.179791912699714e-28, + "velocityX": 2.3088842991913657e-27, + "velocityY": -4.6090179027433225e-27, + "timestamp": 2.6756977278061993 + }, + { + "x": 8.313753662903038, + "y": 7.460040831603572, + "heading": 0.010953449074032317, + "angularVelocity": 0.1521110200792272, + "velocityX": -0.6495438868437788, + "velocityY": -0.12516272111059268, + "timestamp": 2.74770729731883 + }, + { + "x": 8.220205864520521, + "y": 7.442024067957108, + "heading": 0.03284727412772724, + "angularVelocity": 0.30404049353266294, + "velocityX": -1.299102313979371, + "velocityY": -0.2501995744232807, + "timestamp": 2.819716866831461 + }, + { + "x": 8.079881048506913, + "y": 7.415017178316717, + "heading": 0.06565325181712828, + "angularVelocity": 0.45557802819035925, + "velocityX": -1.9486967768776025, + "velocityY": -0.37504584214539577, + "timestamp": 2.8917264363440918 + }, + { + "x": 7.892775351387043, + "y": 7.379038650911545, + "heading": 0.10933893425739379, + "angularVelocity": 0.6066649576706988, + "velocityX": -2.598344892022307, + "velocityY": -0.4996353630313118, + "timestamp": 2.9637360058567226 + }, + { + "x": 7.65888403905923, + "y": 7.3341118085455905, + "heading": 0.1638822878842525, + "angularVelocity": 0.7574459060929595, + "velocityX": -3.248058749841399, + "velocityY": -0.623900999131414, + "timestamp": 3.0357455753693534 + }, + { + "x": 7.402696123218692, + "y": 7.2851734321166655, + "heading": 0.1638823285777561, + "angularVelocity": 5.651124407641853e-7, + "velocityX": -3.557692645219899, + "velocityY": -0.6796093458153674, + "timestamp": 3.1077551448819842 + }, + { + "x": 7.146508206567032, + "y": 7.2362350599332865, + "heading": 0.16388236927081118, + "angularVelocity": 5.651062127144245e-7, + "velocityX": -3.557692656483975, + "velocityY": -0.6796092868572866, + "timestamp": 3.179764714394615 + }, + { + "x": 6.8903202899153655, + "y": 7.187296687749944, + "heading": 0.16388240996386633, + "angularVelocity": 5.651062132417643e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567837, + "timestamp": 3.251774283907246 + }, + { + "x": 6.634132373263698, + "y": 7.138358315566601, + "heading": 0.16388245065692145, + "angularVelocity": 5.651062127692066e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567831, + "timestamp": 3.3237838534198767 + }, + { + "x": 6.377944456612031, + "y": 7.089419943383258, + "heading": 0.1638824913499765, + "angularVelocity": 5.651062124923317e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567826, + "timestamp": 3.3957934229325075 + }, + { + "x": 6.121756539960365, + "y": 7.040481571199916, + "heading": 0.1638825320430316, + "angularVelocity": 5.65106212535781e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567821, + "timestamp": 3.4678029924451383 + }, + { + "x": 5.865568623308698, + "y": 6.991543199016573, + "heading": 0.16388257273608667, + "angularVelocity": 5.651062121582895e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567813, + "timestamp": 3.539812561957769 + }, + { + "x": 5.6093807066570305, + "y": 6.94260482683323, + "heading": 0.16388261342914182, + "angularVelocity": 5.651062135141695e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567811, + "timestamp": 3.6118221314704 + }, + { + "x": 5.35319279000551, + "y": 6.893666454649124, + "heading": 0.16388265412219696, + "angularVelocity": 5.651062132389692e-7, + "velocityX": -3.5576926564820495, + "velocityY": -0.6796092868673679, + "timestamp": 3.683831700983031 + }, + { + "x": 5.0970048904418945, + "y": 6.844727993011475, + "heading": 0.16388269481525294, + "angularVelocity": 5.651062246743821e-7, + "velocityX": -3.5576924191815573, + "velocityY": -0.6796105291126032, + "timestamp": 3.7558412704956616 + }, + { + "x": 4.821698218246571, + "y": 6.778961174770714, + "heading": 0.16388272951436206, + "angularVelocity": 4.4401904271117674e-7, + "velocityX": -3.522897507544664, + "velocityY": -0.8415697237266404, + "timestamp": 3.833989065487064 + }, + { + "x": 4.546391565680663, + "y": 6.7131942743592505, + "heading": 0.163882764213471, + "angularVelocity": 4.440190404819063e-7, + "velocityX": -3.5228972563614294, + "velocityY": -0.8415707752048409, + "timestamp": 3.9121368604784665 + }, + { + "x": 4.271084913114876, + "y": 6.647427373947279, + "heading": 0.16388279891257995, + "angularVelocity": 4.440190403154282e-7, + "velocityX": -3.522897256359876, + "velocityY": -0.8415707752113447, + "timestamp": 3.990284655469869 + }, + { + "x": 3.995778260549088, + "y": 6.5816604735353135, + "heading": 0.1638828336116889, + "angularVelocity": 4.440190406386135e-7, + "velocityX": -3.5228972563598964, + "velocityY": -0.8415707752112599, + "timestamp": 4.068432450461271 + }, + { + "x": 3.7204716077277675, + "y": 6.515893574193041, + "heading": 0.16388286831080823, + "angularVelocity": 4.440191733622333e-7, + "velocityX": -3.522897259629757, + "velocityY": -0.8415707615231752, + "timestamp": 4.146580245452674 + }, + { + "x": 3.446523890940015, + "y": 6.450633718978252, + "heading": 0.1690942590558449, + "angularVelocity": 0.06668634406908087, + "velocityX": -3.50550795218074, + "velocityY": -0.8350824898126525, + "timestamp": 4.224728040444077 + }, + { + "x": 3.2268656747177786, + "y": 6.398260340243826, + "heading": 0.190922516728536, + "angularVelocity": 0.27932019931071, + "velocityX": -2.8108050425018627, + "velocityY": -0.670183704353886, + "timestamp": 4.30287583543548 + }, + { + "x": 3.062219316413325, + "y": 6.3589866212900406, + "heading": 0.21130201229843995, + "angularVelocity": 0.2607814535540765, + "velocityX": -2.1068586557377182, + "velocityY": -0.5025569686016857, + "timestamp": 4.381023630426883 + }, + { + "x": 2.9525091880643317, + "y": 6.3328115268917164, + "heading": 0.22637932021512078, + "angularVelocity": 0.1929332480633594, + "velocityX": -1.4038800245235723, + "velocityY": -0.334943479866623, + "timestamp": 4.459171425418286 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.10283289870283706, + "velocityX": -0.7016723976195487, + "velocityY": -0.1674338082530608, + "timestamp": 4.537319220409689 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 2.348167343500178e-28, + "velocityX": 2.328514438851991e-27, + "velocityY": -3.011007529624761e-27, + "timestamp": 4.615467015401092 + }, + { + "x": 2.931995611661183, + "y": 6.317030660120305, + "heading": 0.22827399101803314, + "angularVelocity": -0.1004895685231517, + "velocityX": 0.5615669396966905, + "velocityY": -0.04411767290505802, + "timestamp": 4.6765827468501096 + }, + { + "x": 3.0006366597167062, + "y": 6.311640001770152, + "heading": 0.21599961790492203, + "angularVelocity": -0.20083819373658737, + "velocityX": 1.1231322349922719, + "velocityY": -0.08820410428451562, + "timestamp": 4.737698478299127 + }, + { + "x": 3.1035985534895527, + "y": 6.303558029226905, + "heading": 0.1976138856084477, + "angularVelocity": -0.30083469281246417, + "velocityX": 1.684703616101463, + "velocityY": -0.13224046168846443, + "timestamp": 4.798814209748145 + }, + { + "x": 3.2408820865427965, + "y": 6.29278900601246, + "heading": 0.173149008298291, + "angularVelocity": -0.4003040907816843, + "velocityX": 2.2462879818065202, + "velocityY": -0.17620705764487352, + "timestamp": 4.859929941197163 + }, + { + "x": 3.412488399373154, + "y": 6.279338451783206, + "heading": 0.14264484283477577, + "angularVelocity": -0.4991213348884074, + "velocityX": 2.807891008774219, + "velocityY": -0.22008333877955472, + "timestamp": 4.921045672646181 + }, + { + "x": 3.618418876396286, + "y": 6.263213190116882, + "heading": 0.10614503413984216, + "angularVelocity": -0.597224443355985, + "velocityX": 3.3695166881037326, + "velocityY": -0.26384796981077935, + "timestamp": 4.982161404095199 + }, + { + "x": 3.839130420023726, + "y": 6.246248871896369, + "heading": 0.10614500290272542, + "angularVelocity": -5.111141764182067e-7, + "velocityX": 3.611370401605275, + "velocityY": -0.2775769481653736, + "timestamp": 5.0432771355442165 + }, + { + "x": 4.059841964366672, + "y": 6.229284562983422, + "heading": 0.10614497166605225, + "angularVelocity": -5.111069183870487e-7, + "velocityX": 3.6113704133126814, + "velocityY": -0.2775767958712516, + "timestamp": 5.104392866993234 + }, + { + "x": 4.280553508709572, + "y": 6.212320254069855, + "heading": 0.10614494042937911, + "angularVelocity": -5.111069180282712e-7, + "velocityX": 3.6113704133119002, + "velocityY": -0.2775767958814193, + "timestamp": 5.165508598442252 + }, + { + "x": 4.501265048980713, + "y": 6.1953558921813965, + "heading": 0.1061449091927048, + "angularVelocity": -5.111069369418327e-7, + "velocityX": 3.611370346688179, + "velocityY": -0.27757766267774153, + "timestamp": 5.22662432989127 + }, + { + "x": 4.810869637193169, + "y": 6.16135848513122, + "heading": 0.10614488355477533, + "angularVelocity": -2.98142570668873e-7, + "velocityX": 3.6003807524808256, + "velocityY": -0.39535463826432365, + "timestamp": 5.312616509468145 + }, + { + "x": 5.120474220145008, + "y": 6.12736103017416, + "heading": 0.1061448579168455, + "angularVelocity": -2.9814257488756455e-7, + "velocityX": 3.600380691305304, + "velocityY": -0.39535519537177727, + "timestamp": 5.3986086890450204 + }, + { + "x": 5.430078803096823, + "y": 6.093363575216878, + "heading": 0.1061448322789156, + "angularVelocity": -2.981425755600158e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435887, + "timestamp": 5.484600868621896 + }, + { + "x": 5.739683386048638, + "y": 6.059366120259595, + "heading": 0.10614480664098574, + "angularVelocity": -2.981425750224502e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.39535519537435865, + "timestamp": 5.570593048198771 + }, + { + "x": 6.049287969000453, + "y": 6.025368665302312, + "heading": 0.10614478100305595, + "angularVelocity": -2.981425744595309e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.3953551953743586, + "timestamp": 5.656585227775646 + }, + { + "x": 6.358892551952268, + "y": 5.991371210345029, + "heading": 0.1061447553651262, + "angularVelocity": -2.98142573863633e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 5.742577407352521 + }, + { + "x": 6.668497134904083, + "y": 5.9573737553877475, + "heading": 0.10614472972719659, + "angularVelocity": -2.981425722498942e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 5.8285695869293965 + }, + { + "x": 6.978101717855898, + "y": 5.923376300430465, + "heading": 0.10614470408926703, + "angularVelocity": -2.981425716615336e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435826, + "timestamp": 5.914561766506272 + }, + { + "x": 7.287706300807711, + "y": 5.889378845473171, + "heading": 0.10614467845133739, + "angularVelocity": -2.9814257255193557e-7, + "velocityX": 3.600380691305006, + "velocityY": -0.39535519537448677, + "timestamp": 6.000553946083147 + }, + { + "x": 7.597310883497341, + "y": 5.855381388128685, + "heading": 0.10614465281325862, + "angularVelocity": -2.981443068468836e-7, + "velocityX": 3.600380688256082, + "velocityY": -0.39535522313506416, + "timestamp": 6.086546125660022 + }, + { + "x": 7.868541995298217, + "y": 5.825436456932061, + "heading": 0.06366050775847867, + "angularVelocity": -0.4940466128876299, + "velocityX": 3.154136959145237, + "velocityY": -0.34822854059484143, + "timestamp": 6.172538305236897 + }, + { + "x": 8.071966668862895, + "y": 5.802984598737895, + "heading": 0.03182759417957115, + "angularVelocity": -0.3701838206164972, + "velocityX": 2.365618298845671, + "velocityY": -0.2610918609650363, + "timestamp": 6.258530484813773 + }, + { + "x": 8.207583377724607, + "y": 5.788018984348283, + "heading": 0.01061131016646268, + "angularVelocity": -0.24672341272780052, + "velocityX": 1.577081887318283, + "velocityY": -0.17403459783495048, + "timestamp": 6.344522664390648 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.762629528326585e-25, + "angularVelocity": -0.12339854878287405, + "velocityX": 0.7885391588323608, + "velocityY": -0.08701732658634945, + "timestamp": 6.430514843967523 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.890807484355009e-25, + "angularVelocity": 1.714109587590018e-25, + "velocityX": -1.5632941987910777e-25, + "velocityY": 4.01842027987716e-26, + "timestamp": 6.516507023544398 + }, + { + "x": 8.217699478068795, + "y": 5.791359796793642, + "heading": 0.01104341509549353, + "angularVelocity": 0.13833447495381113, + "velocityX": -0.7226755833441465, + "velocityY": 0.1355812541876774, + "timestamp": 6.596338281075688 + }, + { + "x": 8.10231615629365, + "y": 5.8130147597317485, + "heading": 0.03315104134696604, + "angularVelocity": 0.276929450131827, + "velocityX": -1.4453401505033163, + "velocityY": 0.2712591985616573, + "timestamp": 6.676169538606978 + }, + { + "x": 7.929243964127431, + "y": 5.845512314072997, + "heading": 0.06637865596420629, + "angularVelocity": 0.4162231141632274, + "velocityX": -2.167975270818996, + "velocityY": 0.40707807124934986, + "timestamp": 6.756000796138268 + }, + { + "x": 7.698485965371198, + "y": 5.888866692718938, + "heading": 0.11081243304487726, + "angularVelocity": 0.5565962312851562, + "velocityX": -2.8905720126704284, + "velocityY": 0.5430752312645023, + "timestamp": 6.8358320536695585 + }, + { + "x": 7.41422585651022, + "y": 5.941821981236076, + "heading": 0.11081246572396981, + "angularVelocity": 4.0935209556931575e-7, + "velocityX": -3.5607620079085063, + "velocityY": 0.6633402774142934, + "timestamp": 6.9156633112008485 + }, + { + "x": 7.129965619426801, + "y": 5.994776581882574, + "heading": 0.11081249814230922, + "angularVelocity": 4.060857915201533e-7, + "velocityX": -3.5607636140768695, + "velocityY": 0.6633316608565432, + "timestamp": 6.995494568732139 + }, + { + "x": 6.8457053823426035, + "y": 6.047731182524891, + "heading": 0.11081253056064869, + "angularVelocity": 4.060857924168534e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 7.075325826263429 + }, + { + "x": 6.561445145258405, + "y": 6.100685783167208, + "heading": 0.11081256297898824, + "angularVelocity": 4.060857934452354e-7, + "velocityX": -3.5607636140866235, + "velocityY": 0.6633316608041844, + "timestamp": 7.155157083794719 + }, + { + "x": 6.277184908174208, + "y": 6.153640383809526, + "heading": 0.11081259539732799, + "angularVelocity": 4.060857956958186e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 7.234988341326009 + }, + { + "x": 5.99292467109001, + "y": 6.206594984451843, + "heading": 0.11081262781566778, + "angularVelocity": 4.060857964387038e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041848, + "timestamp": 7.314819598857299 + }, + { + "x": 5.708664434005791, + "y": 6.259549585094043, + "heading": 0.11081266023400761, + "angularVelocity": 4.060857968841719e-7, + "velocityX": -3.5607636140868966, + "velocityY": 0.6633316608027189, + "timestamp": 7.394650856388589 + }, + { + "x": 5.424404193332451, + "y": 6.312504166469845, + "heading": 0.11081269265234713, + "angularVelocity": 4.060857929641026e-7, + "velocityX": -3.5607636590457346, + "velocityY": 0.6633314194636794, + "timestamp": 7.474482113919879 + }, + { + "x": 5.139572620391846, + "y": 6.3622941970825195, + "heading": 0.11081272507571231, + "angularVelocity": 4.061487466795785e-7, + "velocityX": -3.567920407980088, + "velocityY": 0.6236909219820286, + "timestamp": 7.554313371451169 + }, + { + "x": 4.8822045552292, + "y": 6.360012293211402, + "heading": 0.1108127534765096, + "angularVelocity": 3.996777164840354e-7, + "velocityX": -3.6218800325558864, + "velocityY": -0.03211269456367951, + "timestamp": 7.625372617712571 + }, + { + "x": 4.624881625896748, + "y": 6.3546795533312395, + "heading": 0.11081278178743512, + "angularVelocity": 3.98412972255652e-7, + "velocityX": -3.6212448466713205, + "velocityY": -0.07504638960770253, + "timestamp": 7.696431863973974 + }, + { + "x": 4.367558697083316, + "y": 6.349346788406661, + "heading": 0.11081281009836048, + "angularVelocity": 3.9841297029584103e-7, + "velocityX": -3.62124483936728, + "velocityY": -0.07504674205185301, + "timestamp": 7.767491110235376 + }, + { + "x": 4.110235768269888, + "y": 6.344014023481877, + "heading": 0.11081283840928584, + "angularVelocity": 3.9841296996573985e-7, + "velocityX": -3.6212448393672196, + "velocityY": -0.075046742054734, + "timestamp": 7.8385503564967784 + }, + { + "x": 3.852912839456459, + "y": 6.338681258557136, + "heading": 0.11081286672021123, + "angularVelocity": 3.984129707844367e-7, + "velocityX": -3.621244839367232, + "velocityY": -0.07504674205415067, + "timestamp": 7.909609602758181 + }, + { + "x": 3.5955899105380604, + "y": 6.333348498701343, + "heading": 0.110812895031426, + "angularVelocity": 3.98417042898143e-7, + "velocityX": -3.6212448408444415, + "velocityY": -0.07504667072002474, + "timestamp": 7.980668849019583 + }, + { + "x": 3.3629508998707167, + "y": 6.328797181710379, + "heading": 0.15195588309543634, + "angularVelocity": 0.5789955597426332, + "velocityX": -3.2738738856241922, + "velocityY": -0.06404960973301323, + "timestamp": 8.051728095280986 + }, + { + "x": 3.176840519639808, + "y": 6.325164863882251, + "heading": 0.18492044940341654, + "angularVelocity": 0.4639025607830811, + "velocityX": -2.6190874519872214, + "velocityY": -0.05111675143253586, + "timestamp": 8.122787341542388 + }, + { + "x": 3.0372579905429093, + "y": 6.322444909479725, + "heading": 0.2096671002374919, + "angularVelocity": 0.3482537760538718, + "velocityX": -1.9643119852893276, + "velocityY": -0.03827727629589979, + "timestamp": 8.19384658780379 + }, + { + "x": 2.944202841875484, + "y": 6.320632970128358, + "heading": 0.22616886731909797, + "angularVelocity": 0.2322254731059452, + "velocityX": -1.3095431427052717, + "velocityY": -0.025498994806420823, + "timestamp": 8.264905834065193 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.11605269709570704, + "velocityX": -0.6547748103081646, + "velocityY": -0.012750292274397987, + "timestamp": 8.335965080326595 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 6.477597733926451e-26, + "velocityX": -1.5355730051117952e-25, + "velocityY": -1.2183012914286086e-25, + "timestamp": 8.407024326587997 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 0.8089692619493356, + "isStopPoint": false, + "x": 2.486186981201172, + "y": 7.724460124969482, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 2.6756977278061993, + "isStopPoint": true, + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 3.7558412704956616, + "isStopPoint": false, + "x": 5.0970048904418945, + "y": 6.844727993011475, + "heading": 0.2938692977225419, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "timestamp": 4.615467015401092, + "isStopPoint": true, + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.22662432989127, + "isStopPoint": false, + "x": 4.501265048980713, + "y": 6.1953558921813965, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "timestamp": 6.516507023544398, + "isStopPoint": true, + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 7.554313371451169, + "isStopPoint": false, + "x": 5.139572620391846, + "y": 6.3622941970825195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "timestamp": 8.407024326587997, + "isStopPoint": true, + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": true + }, + "3PieceSourceSide": { + "waypoints": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.04647804366485621, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.783077239990234, + "y": 4.404496192932129, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": -2.3506527398385264e-24, + "velocityX": -4.584841665957037e-25, + "velocityY": 1.794170584470112e-24, + "timestamp": 0 + }, + { + "x": 0.692835797571452, + "y": 4.365429285724839, + "heading": -1.0363921320591865, + "angularVelocity": 0.19907376380794284, + "velocityX": 0.5901469714218079, + "velocityY": -0.33387419035865423, + "timestamp": 0.07664438434234237 + }, + { + "x": 0.7831334822061629, + "y": 4.314114094234643, + "heading": -1.005707809669664, + "angularVelocity": 0.4003466483919687, + "velocityX": 1.178138299492162, + "velocityY": -0.6695231742092023, + "timestamp": 0.15328876868468475 + }, + { + "x": 0.9181042106922528, + "y": 4.23677088276396, + "heading": -0.9591055566031375, + "angularVelocity": 0.608032192657082, + "velocityX": 1.7609995780411658, + "velocityY": -1.009117786441079, + "timestamp": 0.2299331530270271 + }, + { + "x": 1.0717422840106643, + "y": 4.1370476005177, + "heading": -0.8558256704459198, + "angularVelocity": 1.3475205919314872, + "velocityX": 2.004557471975592, + "velocityY": -1.3011166193315893, + "timestamp": 0.3065775373693695 + }, + { + "x": 1.203873817721518, + "y": 4.032707540283105, + "heading": -0.6892805911828493, + "angularVelocity": 2.172958667384875, + "velocityX": 1.7239558363554812, + "velocityY": -1.3613529696911204, + "timestamp": 0.3832219217117119 + }, + { + "x": 1.3594029637702276, + "y": 3.9301727362790966, + "heading": -0.5828610080840567, + "angularVelocity": 1.388485066609126, + "velocityX": 2.0292308090572955, + "velocityY": -1.3377993036779194, + "timestamp": 0.45986630605405426 + }, + { + "x": 1.5344193266927146, + "y": 3.825053132953821, + "heading": -0.5408815443461281, + "angularVelocity": 0.5477174107162478, + "velocityX": 2.2834857951334486, + "velocityY": -1.371523879110889, + "timestamp": 0.5365106903963967 + }, + { + "x": 1.7162004854447428, + "y": 3.7215786578067394, + "heading": -0.5174827165758584, + "angularVelocity": 0.30529083077705393, + "velocityX": 2.3717479148906513, + "velocityY": -1.3500594470809355, + "timestamp": 0.613155074738739 + }, + { + "x": 1.8995749541613056, + "y": 3.6205751589308552, + "heading": -0.4949062907307691, + "angularVelocity": 0.2945607305585351, + "velocityX": 2.3925362606801817, + "velocityY": -1.317819951749349, + "timestamp": 0.6897994590810813 + }, + { + "x": 2.0827414581868835, + "y": 3.51960952250048, + "heading": -0.4719606385920979, + "angularVelocity": 0.29937812581521006, + "velocityX": 2.389822889142672, + "velocityY": -1.3173259501883174, + "timestamp": 0.7664438434234236 + }, + { + "x": 2.265688686133792, + "y": 3.4186835470349224, + "heading": -0.4486127865936518, + "angularVelocity": 0.3046257361029851, + "velocityX": 2.3869619348724966, + "velocityY": -1.3168084828597342, + "timestamp": 0.843088227765766 + }, + { + "x": 2.4484040933032216, + "y": 3.3177997829423713, + "heading": -0.4248255225210326, + "angularVelocity": 0.3103588642107185, + "velocityX": 2.3839373065260236, + "velocityY": -1.3162577396660924, + "timestamp": 0.9197326121081083 + }, + { + "x": 2.6308732997732713, + "y": 3.216961383200288, + "heading": -0.4005559741960246, + "angularVelocity": 0.3166513572162684, + "velocityX": 2.380725059451553, + "velocityY": -1.3156658587232646, + "timestamp": 0.9963769964504506 + }, + { + "x": 2.813079628281676, + "y": 3.116172257902354, + "heading": -0.3757541558021446, + "angularVelocity": 0.32359602868097065, + "velocityX": 2.377295219628298, + "velocityY": -1.3150229617312381, + "timestamp": 1.073021380792793 + }, + { + "x": 2.9950035114027176, + "y": 3.015437295026463, + "heading": -0.3503610814650098, + "angularVelocity": 0.3313103047930201, + "velocityX": 2.3736100783125105, + "velocityY": -1.314316289970363, + "timestamp": 1.1496657651351354 + }, + { + "x": 3.176621688858468, + "y": 2.9147626672707037, + "heading": -0.3243062033321442, + "angularVelocity": 0.3399450378058761, + "velocityX": 2.3696214538631923, + "velocityY": -1.3135290813490341, + "timestamp": 1.2263101494774777 + }, + { + "x": 3.3579060875063353, + "y": 2.8141562687774018, + "heading": -0.2975038256949741, + "angularVelocity": 0.34969786589260987, + "velocityX": 2.3652665515341145, + "velocityY": -1.3126388757189247, + "timestamp": 1.30295453381982 + }, + { + "x": 3.538822210205962, + "y": 2.713628355793928, + "heading": -0.2698479287435623, + "angularVelocity": 0.3608339630974529, + "velocityX": 2.360461555689992, + "velocityY": -1.3116148540570647, + "timestamp": 1.3795989181621624 + }, + { + "x": 3.7193267349323875, + "y": 2.6131925205499895, + "heading": -0.24120442629231653, + "angularVelocity": 0.3737195190101045, + "velocityX": 2.3550913256759576, + "velocityY": -1.3104134909000977, + "timestamp": 1.4562433025045047 + }, + { + "x": 3.8993637811598734, + "y": 2.5128672365819, + "heading": -0.21139907897320392, + "angularVelocity": 0.38887842305553666, + "velocityX": 2.3489920073377766, + "velocityY": -1.3089710985213812, + "timestamp": 1.532887686846847 + }, + { + "x": 4.078858787892925, + "y": 2.4126784448405125, + "heading": -0.18019759590707388, + "angularVelocity": 0.4070941835316264, + "velocityX": 2.34191987153701, + "velocityY": -1.30719024754481, + "timestamp": 1.6095320711891894 + }, + { + "x": 4.257707770811751, + "y": 2.312664186467197, + "heading": -0.1472705722503286, + "angularVelocity": 0.42960777804245054, + "velocityX": 2.3334910242082882, + "velocityY": -1.3049130635140744, + "timestamp": 1.6861764555315317 + }, + { + "x": 4.435756686256742, + "y": 2.212883689420889, + "heading": -0.1121258503357887, + "angularVelocity": 0.4585426866704457, + "velocityX": 2.323052327613603, + "velocityY": -1.3018631162933536, + "timestamp": 1.762820839873874 + }, + { + "x": 4.612756399460092, + "y": 2.113437617489987, + "heading": -0.07396027426521577, + "angularVelocity": 0.4979565874011239, + "velocityX": 2.3093631023605954, + "velocityY": -1.2974997814153513, + "timestamp": 1.8394652242162164 + }, + { + "x": 4.788243072222853, + "y": 2.014522987095371, + "heading": -0.031264185221419126, + "angularVelocity": 0.5570674147904805, + "velocityX": 2.289622054747365, + "velocityY": -1.290565920039225, + "timestamp": 1.9161096085585587 + }, + { + "x": 4.961081026742291, + "y": 1.9166475782485084, + "heading": 0.019697120176888277, + "angularVelocity": 0.664905926710585, + "velocityX": 2.255063511860639, + "velocityY": -1.2770069155972317, + "timestamp": 1.992753992900901 + }, + { + "x": 5.123264328610966, + "y": 1.823526252461316, + "heading": 0.10546513381285838, + "angularVelocity": 1.1190384575714751, + "velocityX": 2.1160493787028227, + "velocityY": -1.214979108857517, + "timestamp": 2.0693983772432434 + }, + { + "x": 5.295119103926639, + "y": 1.7237778734191072, + "heading": 0.15590676979164073, + "angularVelocity": 0.6581256593239505, + "velocityX": 2.24223570702925, + "velocityY": -1.3014440640121712, + "timestamp": 2.1460427615855857 + }, + { + "x": 5.493802683128641, + "y": 1.6448426706793149, + "heading": 0.16075963684913044, + "angularVelocity": 0.06331666826122177, + "velocityX": 2.5922783633378295, + "velocityY": -1.029888926854938, + "timestamp": 2.222687145927928 + }, + { + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "angularVelocity": 0.000040076685010459537, + "velocityX": 2.7882659509385723, + "velocityY": -0.3502449569153927, + "timestamp": 2.2993315302702704 + }, + { + "x": 5.878932815955422, + "y": 1.6310716458031873, + "heading": 0.15207722794791115, + "angularVelocity": -0.13999820175733138, + "velocityX": 2.7631425083845143, + "velocityY": 0.21072366345901047, + "timestamp": 2.3613714739627154 + }, + { + "x": 6.041874898509352, + "y": 1.6781214038756682, + "heading": 0.13576601260266036, + "angularVelocity": -0.2629147348377915, + "velocityX": 2.6264060354679595, + "velocityY": 0.7583784779967588, + "timestamp": 2.4234114176551604 + }, + { + "x": 6.190063655455228, + "y": 1.7574804032751512, + "heading": 0.11619159565916007, + "angularVelocity": -0.3155131319998917, + "velocityX": 2.388602376567293, + "velocityY": 1.2791597586370276, + "timestamp": 2.4854513613476055 + }, + { + "x": 6.338186422130831, + "y": 1.840598011238034, + "heading": 0.10221625072518932, + "angularVelocity": -0.22526366244385518, + "velocityX": 2.387538702644562, + "velocityY": 1.3397434461727966, + "timestamp": 2.5474913050400505 + }, + { + "x": 6.486157900665009, + "y": 1.9236682945064507, + "heading": 0.08780760406345454, + "angularVelocity": -0.2322479003714719, + "velocityX": 2.3851001423812606, + "velocityY": 1.338980636091913, + "timestamp": 2.6095312487324955 + }, + { + "x": 6.633958405719426, + "y": 2.0066826604418155, + "heading": 0.07290105260244177, + "angularVelocity": -0.2402734524536335, + "velocityX": 2.382344281083177, + "velocityY": 1.3380793242962596, + "timestamp": 2.6715711924249406 + }, + { + "x": 6.781563534615612, + "y": 2.089630624589182, + "heading": 0.0574168439817537, + "angularVelocity": -0.24958450474179966, + "velocityX": 2.3791950816060985, + "velocityY": 1.33700901726427, + "timestamp": 2.7336111361173856 + }, + { + "x": 6.928941267710664, + "y": 2.172498140132396, + "heading": 0.04124990446967813, + "angularVelocity": -0.2605892035012319, + "velocityX": 2.3755297687834913, + "velocityY": 1.3357122945504047, + "timestamp": 2.7956510798098306 + }, + { + "x": 7.076047973733186, + "y": 2.255265656701969, + "heading": 0.024256449523087663, + "angularVelocity": -0.27391151466599145, + "velocityX": 2.3711611788653904, + "velocityY": 1.334100446317001, + "timestamp": 2.8576910235022757 + }, + { + "x": 7.222828783744423, + "y": 2.337909309225835, + "heading": 0.006256947367936294, + "angularVelocity": -0.2901276352599791, + "velocityX": 2.3659081758501124, + "velocityY": 1.3321039253929894, + "timestamp": 2.9197309671947207 + }, + { + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 6.712344331303172e-23, + "angularVelocity": -0.10085353073423609, + "velocityX": 2.608965463855139, + "velocityY": 0.9550610879656509, + "timestamp": 2.9817709108871657 + }, + { + "x": 7.581926675665867, + "y": 2.420671247090372, + "heading": -6.833387833600707e-9, + "angularVelocity": -9.667586454477017e-8, + "velocityX": 2.7904368043745778, + "velocityY": 0.33260950489354324, + "timestamp": 3.052454404288316 + }, + { + "x": 7.7805365921893905, + "y": 2.417576114646826, + "heading": -6.833380269760843e-9, + "angularVelocity": 1.0700998918582695e-13, + "velocityX": 2.8098486218890097, + "velocityY": -0.043788617322290876, + "timestamp": 3.1231378976894666 + }, + { + "x": 7.9655574312129644, + "y": 2.4146925691677037, + "heading": -4.100028695417927e-9, + "angularVelocity": 3.8670295465311964e-8, + "velocityX": 2.6175961334214803, + "velocityY": -0.0407951749463946, + "timestamp": 3.193821391090617 + }, + { + "x": 8.104323073360915, + "y": 2.412529909857503, + "heading": -2.050014465682545e-9, + "angularVelocity": 2.9002729365693992e-8, + "velocityX": 1.9631972822906998, + "velocityY": -0.03059638405147332, + "timestamp": 3.2645048844917675 + }, + { + "x": 8.196833505192958, + "y": 2.411088136925815, + "heading": -6.833381783003922e-10, + "angularVelocity": 1.933515480942414e-8, + "velocityX": 1.3087982410125008, + "velocityY": -0.020397590191323155, + "timestamp": 3.335188377892918 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.830282426459157e-24, + "angularVelocity": 9.667577894349971e-9, + "velocityX": 0.6543991363518575, + "velocityY": -0.010198795342763814, + "timestamp": 3.4058718712940683 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.851361909343925e-24, + "angularVelocity": 2.3995205522592844e-25, + "velocityX": -1.4269511777904615e-24, + "velocityY": -1.1273229249768833e-23, + "timestamp": 3.4765553646952188 + }, + { + "x": 8.183672767375366, + "y": 2.3891879079868126, + "heading": -0.016263075453039278, + "angularVelocity": -0.1934255260988222, + "velocityX": -0.7066659906617425, + "velocityY": -0.251897340619777, + "timestamp": 3.5606346265230004 + }, + { + "x": 8.064809267825755, + "y": 2.3467183743812834, + "heading": -0.04860613329336098, + "angularVelocity": -0.38467342763509904, + "velocityX": -1.413707696352949, + "velocityY": -0.5051130645333048, + "timestamp": 3.644713888350782 + }, + { + "x": 7.886434927132461, + "y": 2.2827439256274595, + "heading": -0.09665252446442359, + "angularVelocity": -0.5714416388368797, + "velocityX": -2.121502220828929, + "velocityY": -0.7608826167487317, + "timestamp": 3.728793150178564 + }, + { + "x": 7.663093479676549, + "y": 2.2056330475266677, + "heading": -0.09665343057468777, + "angularVelocity": -0.000010776857984756087, + "velocityX": -2.6563202697162027, + "velocityY": -0.9171212546886659, + "timestamp": 3.8128724120063455 + }, + { + "x": 7.439949308521917, + "y": 2.127952816871332, + "heading": -0.09665400996983407, + "angularVelocity": -0.000006891058909294993, + "velocityX": -2.653973956285386, + "velocityY": -0.9238928716387514, + "timestamp": 3.896951673834127 + }, + { + "x": 7.216805138002767, + "y": 2.0502725843903784, + "heading": -0.0966545893647738, + "angularVelocity": -0.0000068910564521426285, + "velocityX": -2.6539739487272445, + "velocityY": -0.9238928933518099, + "timestamp": 3.981030935661909 + }, + { + "x": 6.9936609674836125, + "y": 1.9725923519093191, + "heading": -0.09665516875951666, + "angularVelocity": -0.000006891054111144413, + "velocityX": -2.6539739487273097, + "velocityY": -0.923892893353071, + "timestamp": 4.06511019748969 + }, + { + "x": 6.77051679696445, + "y": 1.8949121194281597, + "heading": -0.09665574815406265, + "angularVelocity": -0.000006891051769457115, + "velocityX": -2.653973948727398, + "velocityY": -0.9238928933542627, + "timestamp": 4.149189459317472 + }, + { + "x": 6.547372626445282, + "y": 1.8172318869468993, + "heading": -0.09665632754841177, + "angularVelocity": -0.000006891049427961194, + "velocityX": -2.653973948727487, + "velocityY": -0.9238928933554545, + "timestamp": 4.2332687211452535 + }, + { + "x": 6.324228455926046, + "y": 1.7395516544657041, + "heading": -0.09665690694256396, + "angularVelocity": -0.000006891047085669869, + "velocityX": -2.653973948728258, + "velocityY": -0.9238928933546852, + "timestamp": 4.317347982973035 + }, + { + "x": 6.101084267512398, + "y": 1.6618714733923579, + "heading": -0.0966574863411208, + "angularVelocity": -0.000006891099472478973, + "velocityX": -2.65397416155617, + "velocityY": -0.9238922819334152, + "timestamp": 4.401427244800817 + }, + { + "x": 5.881706688567194, + "y": 1.6001619743925184, + "heading": -0.12280606064162562, + "angularVelocity": -0.3109990945694144, + "velocityX": -2.609175844033366, + "velocityY": -0.7339443479682077, + "timestamp": 4.485506506628599 + }, + { + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.19572183715761898, + "angularVelocity": -0.8672266493650448, + "velocityX": -2.5430419926070895, + "velocityY": -0.10199734429174848, + "timestamp": 4.56958576845638 + }, + { + "x": 5.472836165647801, + "y": 1.6405608510115604, + "heading": -0.2695567389374151, + "angularVelocity": -0.9372981580360532, + "velocityX": -2.4761083941768933, + "velocityY": 0.6217104735625232, + "timestamp": 4.648359956954205 + }, + { + "x": 5.3021481158717245, + "y": 1.712801388975876, + "heading": -0.4003704259742984, + "angularVelocity": -1.660616117175175, + "velocityX": -2.1668017536072774, + "velocityY": 0.9170584850431085, + "timestamp": 4.7271341454520295 + }, + { + "x": 5.1196004435735345, + "y": 1.8042197974618441, + "heading": -0.46428021863316127, + "angularVelocity": -0.8113037262278816, + "velocityX": -2.317353891918915, + "velocityY": 1.160512221442844, + "timestamp": 4.805908333949854 + }, + { + "x": 4.929096458765624, + "y": 1.904953408569058, + "heading": -0.48639976153317277, + "angularVelocity": -0.28079683614414136, + "velocityX": -2.4183554085507573, + "velocityY": 1.2787641869518616, + "timestamp": 4.884682522447679 + }, + { + "x": 4.737761672165951, + "y": 2.009314965020544, + "heading": -0.49933816069591924, + "angularVelocity": -0.1642466829487382, + "velocityX": -2.4289020331190754, + "velocityY": 1.3248191881325238, + "timestamp": 4.963456710945503 + }, + { + "x": 4.546911439579052, + "y": 2.114853649108329, + "heading": -0.5116829622012672, + "angularVelocity": -0.15671124946833295, + "velocityX": -2.4227508556583697, + "velocityY": 1.3397622508126334, + "timestamp": 5.042230899443328 + }, + { + "x": 4.356102309238819, + "y": 2.2203468351822107, + "heading": -0.524131682346679, + "angularVelocity": -0.15803044604839828, + "velocityX": -2.4222290826328545, + "velocityY": 1.3391846756605428, + "timestamp": 5.121005087941152 + }, + { + "x": 4.165335431709755, + "y": 2.3257928672028427, + "heading": -0.5366900663313149, + "angularVelocity": -0.15942257513681343, + "velocityX": -2.421692703750687, + "velocityY": 1.338586077894589, + "timestamp": 5.199779276438977 + }, + { + "x": 3.9746120593595244, + "y": 2.4311903935174577, + "heading": -0.5493635573249664, + "angularVelocity": -0.1608838026176728, + "velocityX": -2.421140426670299, + "velocityY": 1.3379703215543044, + "timestamp": 5.2785534649368016 + }, + { + "x": 3.783933515355731, + "y": 2.5365380161026203, + "heading": -0.5621578157899224, + "angularVelocity": -0.16241688691352676, + "velocityX": -2.420571352620895, + "velocityY": 1.337336817986659, + "timestamp": 5.357327653434626 + }, + { + "x": 3.593301216494676, + "y": 2.6418342523116722, + "heading": -0.5750788548667545, + "angularVelocity": -0.16402630510359092, + "velocityX": -2.4199842930317086, + "velocityY": 1.3366844929409736, + "timestamp": 5.436101841932451 + }, + { + "x": 3.4027166923911927, + "y": 2.7470775186047, + "heading": -0.5881331219292356, + "angularVelocity": -0.16571756956711148, + "velocityX": -2.41937781572635, + "velocityY": 1.3360120656264767, + "timestamp": 5.514876030430275 + }, + { + "x": 3.212181603664246, + "y": 2.8522661171153323, + "heading": -0.601327569493579, + "angularVelocity": -0.16749709284161973, + "velocityX": -2.4187502576711153, + "velocityY": 1.3353180847243658, + "timestamp": 5.5936502189281 + }, + { + "x": 3.0216977610811666, + "y": 2.9573982215901036, + "heading": -0.6146697280062193, + "angularVelocity": -0.1693722114701131, + "velocityX": -2.418099712805536, + "velocityY": 1.3346009204229918, + "timestamp": 5.6724244074259245 + }, + { + "x": 2.831267146879349, + "y": 3.0624718615565794, + "heading": -0.6281677862205469, + "angularVelocity": -0.17135128233914515, + "velocityX": -2.417424004400561, + "velocityY": 1.3338587419326686, + "timestamp": 5.751198595923749 + }, + { + "x": 2.640891939244419, + "y": 3.167484904027678, + "heading": -0.6418306831098634, + "angularVelocity": -0.17344382912549475, + "velocityX": -2.416720644988769, + "velocityY": 1.3330894862090363, + "timestamp": 5.829972784421574 + }, + { + "x": 2.4505746379296203, + "y": 3.272435032259823, + "heading": -0.655668504824747, + "angularVelocity": -0.17566441468661878, + "velocityX": -2.4159855524256404, + "velocityY": 1.3322908205527566, + "timestamp": 5.908746972919398 + }, + { + "x": 2.2618233887297388, + "y": 3.3772509809337916, + "heading": -0.674261275863946, + "angularVelocity": -0.2360261831159634, + "velocityX": -2.396105282697931, + "velocityY": 1.3305874763389332, + "timestamp": 5.987521161417223 + }, + { + "x": 2.116002537558778, + "y": 3.455078745186431, + "heading": -0.7196337030492912, + "angularVelocity": -0.5759808898139023, + "velocityX": -1.8511247649982117, + "velocityY": 0.9879856046347999, + "timestamp": 6.066295349915047 + }, + { + "x": 2.018778748901899, + "y": 3.5069532190081367, + "heading": -0.7498768931574519, + "angularVelocity": -0.3839225853655822, + "velocityX": -1.234208698443955, + "velocityY": 0.6585212086715196, + "timestamp": 6.145069538412872 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -0.19198048410191668, + "velocityX": -0.617100471546741, + "velocityY": 0.32925585007098457, + "timestamp": 6.2238437269106965 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -6.585493827087804e-24, + "velocityX": 2.2425722891132833e-25, + "velocityY": -1.917344763102387e-25, + "timestamp": 6.302617915408521 + }, + { + "x": 2.021866320736439, + "y": 3.5175409291347064, + "heading": -0.7649999986292967, + "angularVelocity": 1.7960508607271394e-8, + "velocityX": 0.6774210494904483, + "velocityY": -0.20112200466940705, + "timestamp": 6.37893553314589 + }, + { + "x": 2.1252646329115827, + "y": 3.486842627325834, + "heading": -0.7649999949328548, + "angularVelocity": 4.8434973827775214e-8, + "velocityX": 1.354841978047161, + "velocityY": -0.4022439735280295, + "timestamp": 6.45525315088326 + }, + { + "x": 2.264271046839328, + "y": 3.4455725064565113, + "heading": -0.7629134147129198, + "angularVelocity": 0.027340740995291143, + "velocityX": 1.8214197199669733, + "velocityY": -0.5407679392109075, + "timestamp": 6.531570768620629 + }, + { + "x": 2.403277460756995, + "y": 3.404302385553228, + "heading": -0.7557796152547765, + "angularVelocity": 0.09347513286765456, + "velocityX": 1.8214197198349207, + "velocityY": -0.5407679396558968, + "timestamp": 6.607888386357998 + }, + { + "x": 2.5422838746752925, + "y": 3.3630322646520687, + "heading": -0.7445970322006652, + "angularVelocity": 0.1465268883600871, + "velocityX": 1.8214197198431827, + "velocityY": -0.5407679396280685, + "timestamp": 6.684206004095367 + }, + { + "x": 2.6812902885942673, + "y": 3.321762143753191, + "heading": -0.7301562502874436, + "angularVelocity": 0.1892195058147188, + "velocityX": 1.821419719852059, + "velocityY": -0.540767939598171, + "timestamp": 6.7605236218327365 + }, + { + "x": 2.8202967025138705, + "y": 3.28049202285643, + "heading": -0.7130778427136069, + "angularVelocity": 0.2237806692631405, + "velocityX": 1.8214197198602944, + "velocityY": -0.5407679395704321, + "timestamp": 6.836841239570106 + }, + { + "x": 2.9593031164340085, + "y": 3.2392219019614696, + "heading": -0.6938417314556549, + "angularVelocity": 0.25205335056643224, + "velocityX": 1.8214197198673001, + "velocityY": -0.5407679395468353, + "timestamp": 6.913158857307475 + }, + { + "x": 3.0983095303545722, + "y": 3.1979517810679434, + "heading": -0.6728120786629418, + "angularVelocity": 0.27555436629432156, + "velocityX": 1.8214197198728768, + "velocityY": -0.5407679395280517, + "timestamp": 6.989476475044844 + }, + { + "x": 3.2373159442754544, + "y": 3.1566816601754897, + "heading": -0.6502538625008166, + "angularVelocity": 0.2955833375165695, + "velocityX": 1.8214197198770503, + "velocityY": -0.540767939513994, + "timestamp": 7.065794092782213 + }, + { + "x": 3.376322358196558, + "y": 3.1154115392837802, + "heading": -0.6263485003282439, + "angularVelocity": 0.31323517270727913, + "velocityX": 1.8214197198799456, + "velocityY": -0.5407679395042426, + "timestamp": 7.142111710519583 + }, + { + "x": 3.515328772117798, + "y": 3.074141418392532, + "heading": -0.6012000869662939, + "angularVelocity": 0.3295230394702948, + "velocityX": 1.821419719881741, + "velocityY": -0.5407679394981955, + "timestamp": 7.218429328256952 + }, + { + "x": 3.6543351862262687, + "y": 3.032871298131916, + "heading": -0.5748442396833396, + "angularVelocity": 0.3453442083799368, + "velocityX": 1.821419722335047, + "velocityY": -0.5407679312349462, + "timestamp": 7.294746945994321 + }, + { + "x": 3.7934386683270223, + "y": 2.9919295436941926, + "heading": -0.5472452836552583, + "angularVelocity": 0.3616328293036799, + "velocityX": 1.8226916172809426, + "velocityY": -0.5364653097351076, + "timestamp": 7.37106456373169 + }, + { + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": -0.5196460913813736, + "angularVelocity": 0.361635924864182, + "velocityX": 1.8926815517726128, + "velocityY": 0.16660126698243774, + "timestamp": 7.4473821814690595 + }, + { + "x": 4.075926460541626, + "y": 3.0786892532968806, + "heading": -0.4898305952486719, + "angularVelocity": 0.3616359432061461, + "velocityX": 1.674339217752807, + "velocityY": 0.8981024049221815, + "timestamp": 7.529828345056582 + }, + { + "x": 4.180404466119937, + "y": 3.195406219293214, + "heading": -0.4598310277403017, + "angularVelocity": 0.3638685683236616, + "velocityX": 1.2672270125389096, + "velocityY": 1.4156749194573495, + "timestamp": 7.612274508644104 + }, + { + "x": 4.2848822354372995, + "y": 3.3121233967757027, + "heading": -0.4287713690600015, + "angularVelocity": 0.37672654892338603, + "velocityX": 1.2672241468998562, + "velocityY": 1.4156774845998201, + "timestamp": 7.694720672231626 + }, + { + "x": 4.389360004752337, + "y": 3.428840574260275, + "heading": -0.39697164385324546, + "angularVelocity": 0.38570290991161005, + "velocityX": 1.2672241468716323, + "velocityY": 1.415677484625084, + "timestamp": 7.777166835819148 + }, + { + "x": 4.493837774065624, + "y": 3.545557751746413, + "heading": -0.36470801458445545, + "angularVelocity": 0.3913296612587668, + "velocityX": 1.2672241468504064, + "velocityY": 1.415677484644084, + "timestamp": 7.859612999406671 + }, + { + "x": 4.598315543377444, + "y": 3.662274929233865, + "heading": -0.3322260766024941, + "angularVelocity": 0.3939775553956429, + "velocityX": 1.267224146832605, + "velocityY": 1.4156774846600186, + "timestamp": 7.942059162994193 + }, + { + "x": 4.7027933126881845, + "y": 3.778992106722283, + "heading": -0.29975459263588233, + "angularVelocity": 0.3938507573143906, + "velocityX": 1.2672241468195173, + "velocityY": 1.415677484671734, + "timestamp": 8.024505326581716 + }, + { + "x": 4.807271113642882, + "y": 3.8957092558850426, + "heading": -0.26751670065443317, + "angularVelocity": 0.3910174904284827, + "velocityX": 1.2672245306330898, + "velocityY": 1.4156771411062206, + "timestamp": 8.106951490169239 + }, + { + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": -0.23561923052699993, + "angularVelocity": 0.386888470481369, + "velocityX": 1.4685230274935492, + "velocityY": 1.205586850387367, + "timestamp": 8.189397653756762 + }, + { + "x": 5.0976675546813945, + "y": 4.035971247104969, + "heading": -0.20015091281875375, + "angularVelocity": 0.38688845257830395, + "velocityX": 1.846968413139047, + "velocityY": 0.4457661652272064, + "timestamp": 8.281073477813743 + }, + { + "x": 5.270991671667468, + "y": 4.053258165584531, + "heading": -0.16565527924639437, + "angularVelocity": 0.37627841284435726, + "velocityX": 1.890619678295383, + "velocityY": 0.18856572774100455, + "timestamp": 8.372749301870725 + }, + { + "x": 5.444315791533781, + "y": 4.0705450551858755, + "heading": -0.13259854065392815, + "angularVelocity": 0.3605829446585594, + "velocityX": 1.8906197097130382, + "velocityY": 0.1885654127373893, + "timestamp": 8.464425125927706 + }, + { + "x": 5.617639911400379, + "y": 4.087831944784366, + "heading": -0.10138296753519387, + "angularVelocity": 0.34049950943808543, + "velocityX": 1.8906197097161441, + "velocityY": 0.18856541270624588, + "timestamp": 8.556100949984687 + }, + { + "x": 5.790964031267238, + "y": 4.105118834380232, + "heading": -0.07255368127548635, + "angularVelocity": 0.3144698894856807, + "velocityX": 1.8906197097189994, + "velocityY": 0.1885654126776188, + "timestamp": 8.647776774041668 + }, + { + "x": 5.9642881511343315, + "y": 4.122405723973758, + "heading": -0.046846868579348507, + "angularVelocity": 0.2804099440672579, + "velocityX": 1.890619709721544, + "velocityY": 0.18856541265210713, + "timestamp": 8.73945259809865 + }, + { + "x": 6.137612271001621, + "y": 4.139692613565316, + "heading": -0.02524481611998314, + "angularVelocity": 0.23563521442620056, + "velocityX": 1.890619709723685, + "velocityY": 0.18856541263063997, + "timestamp": 8.831128422155631 + }, + { + "x": 6.310936390869065, + "y": 4.156979503155326, + "heading": -0.009053820761373118, + "angularVelocity": 0.17661139700851253, + "velocityX": 1.8906197097253696, + "velocityY": 0.18856541261374912, + "timestamp": 8.922804246212612 + }, + { + "x": 6.484260512045991, + "y": 4.174266379616022, + "heading": -2.2378824393561327e-8, + "angularVelocity": 0.09875884373748682, + "velocityX": 1.890619724009207, + "velocityY": 0.188565269399175, + "timestamp": 9.014480070269594 + }, + { + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": 1.903926994866919e-23, + "angularVelocity": 2.4410824362649644e-7, + "velocityX": 1.8989734472164426, + "velocityY": 0.06244684149438171, + "timestamp": 9.106155894326575 + }, + { + "x": 6.892610562082988, + "y": 4.180747179242153, + "heading": 1.815110943876254e-8, + "angularVelocity": 2.0923543279309324e-7, + "velocityX": 2.7004141208851267, + "velocityY": 0.008713967176889024, + "timestamp": 9.192905586749424 + }, + { + "x": 7.135847606603004, + "y": 4.164439410363542, + "heading": 1.8151086995989633e-8, + "angularVelocity": -2.587072333894115e-13, + "velocityX": 2.803895180796635, + "velocityY": -0.18798647491591486, + "timestamp": 9.279655279172273 + }, + { + "x": 7.379084647879251, + "y": 4.14813159310281, + "heading": 1.8151064626797785e-8, + "angularVelocity": -2.5785903361398285e-13, + "velocityX": 2.803895143404351, + "velocityY": -0.18798703263687086, + "timestamp": 9.366404971595122 + }, + { + "x": 7.6223216891554895, + "y": 4.131823775841945, + "heading": 1.8151042187582598e-8, + "angularVelocity": -2.586662220352974e-13, + "velocityX": 2.8038951434042487, + "velocityY": -0.18798703263839134, + "timestamp": 9.45315466401797 + }, + { + "x": 7.865558730431713, + "y": 4.115515958581081, + "heading": 1.815101972750476e-8, + "angularVelocity": -2.589067144937878e-13, + "velocityX": 2.803895143404087, + "velocityY": -0.18798703263838049, + "timestamp": 9.53990435644082 + }, + { + "x": 8.07413296336415, + "y": 4.101532107856125, + "heading": 9.075510567318239e-9, + "angularVelocity": -1.0461719121664748e-7, + "velocityX": 2.404322448957763, + "velocityY": -0.16119769804825285, + "timestamp": 9.626654048863669 + }, + { + "x": 8.213182462520122, + "y": 4.092209539999878, + "heading": 3.025170279496359e-9, + "angularVelocity": -6.974480391619583e-8, + "velocityX": 1.6028817540723412, + "velocityY": -0.10746514017369478, + "timestamp": 9.713403741286518 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -2.730279460976851e-23, + "angularVelocity": -3.487240352104816e-8, + "velocityX": 0.8014409030577104, + "velocityY": -0.05373257183146045, + "timestamp": 9.800153433709367 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -1.3454200564607368e-23, + "angularVelocity": 4.663099853298001e-24, + "velocityX": 1.7036719365270032e-27, + "velocityY": 2.800838691430789e-26, + "timestamp": 9.886903126132216 + }, + { + "x": 8.214708333565108, + "y": 4.098060716631318, + "heading": 1.6847207046591105e-8, + "angularVelocity": 1.954345432230813e-7, + "velocityX": -0.7888150344560528, + "velocityY": 0.12194887565072522, + "timestamp": 9.973106960770398 + }, + { + "x": 8.078710576708064, + "y": 4.119085637322864, + "heading": 5.0541618776805506e-8, + "angularVelocity": 3.9086905903476216e-7, + "velocityX": -1.577630014115494, + "velocityY": 0.24389774283002824, + "timestamp": 10.05931079540858 + }, + { + "x": 7.8747139579553656, + "y": 4.15062301580424, + "heading": 1.0108322622636101e-7, + "angularVelocity": 5.863034708570761e-7, + "velocityX": -2.3664448293851432, + "velocityY": 0.3658465845950713, + "timestamp": 10.145514630046762 + }, + { + "x": 7.635308863013457, + "y": 4.1876344581296925, + "heading": 1.0108335901717796e-7, + "angularVelocity": 1.540428189705246e-12, + "velocityX": -2.7771977423829006, + "velocityY": 0.42934798064143237, + "timestamp": 10.231718464684944 + }, + { + "x": 7.395903768071533, + "y": 4.224645900455147, + "heading": 1.0108349186566566e-7, + "angularVelocity": 1.5410971948056658e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.317922299323126 + }, + { + "x": 7.156498673129608, + "y": 4.261657342780601, + "heading": 1.0108362465293872e-7, + "angularVelocity": 1.5403870787070826e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.404126133961308 + }, + { + "x": 6.917093578187684, + "y": 4.298668785106054, + "heading": 1.0108375751155647e-7, + "angularVelocity": 1.5412147075799493e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414625, + "timestamp": 10.49032996859949 + }, + { + "x": 6.677688483245759, + "y": 4.335680227431509, + "heading": 1.0108389035423936e-7, + "angularVelocity": 1.5410298558530182e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414627, + "timestamp": 10.576533803237671 + }, + { + "x": 6.438283388303835, + "y": 4.372691669756963, + "heading": 1.0108402320108348e-7, + "angularVelocity": 1.5410781280848912e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.4293479806414625, + "timestamp": 10.662737637875853 + }, + { + "x": 6.1988782933619095, + "y": 4.409703112082417, + "heading": 1.0108415606074574e-7, + "angularVelocity": 1.5412268238689798e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.748941472514035 + }, + { + "x": 5.959473198419985, + "y": 4.446714554407871, + "heading": 1.0108428894851187e-7, + "angularVelocity": 1.5415528401754331e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.42934798064146257, + "timestamp": 10.835145307152217 + }, + { + "x": 5.72006810347805, + "y": 4.483725996733255, + "heading": 1.0108442171706879e-7, + "angularVelocity": 1.5401699643641334e-12, + "velocityX": -2.7771977423832217, + "velocityY": 0.4293479806406499, + "timestamp": 10.9213491417904 + }, + { + "x": 5.480663004845917, + "y": 4.520737415188933, + "heading": 1.0108455459674728e-7, + "angularVelocity": 1.5414590205195092e-12, + "velocityX": -2.7771977851910266, + "velocityY": 0.4293477037422273, + "timestamp": 11.007552976428581 + }, + { + "x": 5.240152409397331, + "y": 4.549708085687679, + "heading": 1.0108468832096651e-7, + "angularVelocity": 1.5512560408121339e-12, + "velocityX": -2.790022003755009, + "velocityY": 0.3360717144468437, + "timestamp": 11.093756811066763 + }, + { + "x": 5.001198008888612, + "y": 4.509890511709968, + "heading": 1.0108464169686405e-7, + "angularVelocity": -5.408587989447552e-13, + "velocityX": -2.7719695012602092, + "velocityY": -0.46190026400605494, + "timestamp": 11.179960645704945 + }, + { + "x": 4.783077239990234, + "y": 4.404496192932129, + "heading": 4.371418464571693e-25, + "angularVelocity": -0.0000011726234931559648, + "velocityX": -2.5302907905881877, + "velocityY": -1.2226175229930707, + "timestamp": 11.266164480343127 + }, + { + "x": 4.627041500035927, + "y": 4.275874365802291, + "heading": -0.00461711894400582, + "angularVelocity": -0.06368383973540494, + "velocityX": -2.152198194752189, + "velocityY": -1.774078581199584, + "timestamp": 11.338665117289471 + }, + { + "x": 4.5068214168723975, + "y": 4.114451023957695, + "heading": -0.01240880783073842, + "angularVelocity": -0.10747062667185843, + "velocityX": -1.6581934756311134, + "velocityY": -2.226509292105373, + "timestamp": 11.411165754235816 + }, + { + "x": 4.429140519397286, + "y": 3.930818027867112, + "heading": -0.027772737926692166, + "angularVelocity": -0.21191441541850167, + "velocityX": -1.0714512416297768, + "velocityY": -2.53284665935394, + "timestamp": 11.48366639118216 + }, + { + "x": 4.383091867034577, + "y": 3.7471675526312227, + "heading": -0.08613270246137088, + "angularVelocity": -0.8049579561331118, + "velocityX": -0.635148245618701, + "velocityY": -2.5330877488952703, + "timestamp": 11.556167028128504 + }, + { + "x": 4.346524489213487, + "y": 3.5742818254238546, + "heading": -0.20843940130739744, + "angularVelocity": -1.6869741287451163, + "velocityX": -0.5043731939645176, + "velocityY": -2.3846097701915894, + "timestamp": 11.628667665074849 + }, + { + "x": 4.289423143132127, + "y": 3.4062373324265396, + "heading": -0.32755846162196556, + "angularVelocity": -1.643007086995987, + "velocityX": -0.7875978541211613, + "velocityY": -2.317834712565055, + "timestamp": 11.701168302021193 + }, + { + "x": 4.203799594769533, + "y": 3.2403627849834162, + "heading": -0.40165980770192133, + "angularVelocity": -1.022078552700102, + "velocityX": -1.1810040845014969, + "velocityY": -2.28790469200817, + "timestamp": 11.773668938967537 + }, + { + "x": 4.079421660594405, + "y": 3.103563620218193, + "heading": -0.4716628621936354, + "angularVelocity": -0.9655508894841919, + "velocityX": -1.7155426409174284, + "velocityY": -1.8868684542242327, + "timestamp": 11.846169575913882 + }, + { + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": -0.5385444180262977, + "angularVelocity": -0.9224961138225419, + "velocityX": -2.134381940831591, + "velocityY": -1.3643944230321734, + "timestamp": 11.918670212860226 + }, + { + "x": 3.7636444729485263, + "y": 2.9505820220343484, + "heading": -0.5968694156691691, + "angularVelocity": -0.8693980375808563, + "velocityX": -2.40037547092122, + "velocityY": -0.8058553732360577, + "timestamp": 11.98575685801204 + }, + { + "x": 3.5921162225925003, + "y": 2.93585850684271, + "heading": -0.6464965554196345, + "angularVelocity": -0.7397469293353471, + "velocityX": -2.556816635678569, + "velocityY": -0.21947013684048228, + "timestamp": 12.052843503163855 + }, + { + "x": 3.416479572667743, + "y": 2.9596548551854616, + "heading": -0.6807775343446694, + "angularVelocity": -0.5109955766525223, + "velocityX": -2.6180568357129337, + "velocityY": 0.3547106624410992, + "timestamp": 12.11993014831567 + }, + { + "x": 3.2390028785000764, + "y": 3.015476958858974, + "heading": -0.6885344713310354, + "angularVelocity": -0.115625650512295, + "velocityX": -2.645484712583867, + "velocityY": 0.8320896587866112, + "timestamp": 12.187016793467484 + }, + { + "x": 3.0657580475568027, + "y": 3.089829788896707, + "heading": -0.6885346535011025, + "angularVelocity": -0.00000271544458244769, + "velocityX": -2.5824041513959415, + "velocityY": 1.1083104523929412, + "timestamp": 12.254103438619298 + }, + { + "x": 2.8925132745209385, + "y": 3.1641827538610285, + "heading": -0.6885348356707467, + "angularVelocity": -0.000002715438277887047, + "velocityX": -2.582403288222506, + "velocityY": 1.1083124636216772, + "timestamp": 12.321190083771112 + }, + { + "x": 2.7192685014854168, + "y": 3.2385357188261574, + "heading": -0.6885350178403685, + "angularVelocity": -0.000002715437944094969, + "velocityX": -2.5824032882174053, + "velocityY": 1.108312463633708, + "timestamp": 12.388276728922927 + }, + { + "x": 2.5460237284499403, + "y": 3.312888683791401, + "heading": -0.6885352000099678, + "angularVelocity": -0.0000027154376105306044, + "velocityX": -2.5824032882167316, + "velocityY": 1.1083124636354227, + "timestamp": 12.455363374074741 + }, + { + "x": 2.3727789637362293, + "y": 3.3872416681385586, + "heading": -0.6885353821903477, + "angularVelocity": -0.0000027155983032171972, + "velocityX": -2.5824031641716974, + "velocityY": 1.108312752544088, + "timestamp": 12.522450019226556 + }, + { + "x": 2.2223047239417384, + "y": 3.4507934701253355, + "heading": -0.7191097564620745, + "angularVelocity": -0.45574457036177557, + "velocityX": -2.2429835245744076, + "velocityY": 0.9473092870117661, + "timestamp": 12.58953666437837 + }, + { + "x": 2.109444085282166, + "y": 3.4984470664225062, + "heading": -0.7420547518086502, + "angularVelocity": -0.34202031260695603, + "velocityX": -1.6823115599859222, + "velocityY": 0.7103291003646417, + "timestamp": 12.656623309530184 + }, + { + "x": 2.0342014899368133, + "y": 3.5302130152316624, + "heading": -0.7573535418351998, + "angularVelocity": -0.22804523898800838, + "velocityX": -1.1215733798445486, + "velocityY": 0.47350629528829474, + "timestamp": 12.723709954681999 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": -0.11397884254752216, + "velocityX": -0.5607948167249468, + "velocityY": 0.23675814542593593, + "timestamp": 12.790796599833813 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": 6.11903975445096e-25, + "velocityX": -4.7665538572669345e-25, + "velocityY": 2.8118183260637723e-25, + "timestamp": 12.857883244985628 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "timestamp": 2.2993315302702704, + "isStopPoint": false, + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 2.9817709108871657, + "isStopPoint": false, + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 3.4765553646952188, + "isStopPoint": true, + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 4.56958576845638, + "isStopPoint": false, + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.04647804366485621, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "timestamp": 6.302617915408521, + "isStopPoint": true, + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 7.4473821814690595, + "isStopPoint": false, + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 8.189397653756762, + "isStopPoint": false, + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "timestamp": 9.106155894326575, + "isStopPoint": false, + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 9.886903126132216, + "isStopPoint": true, + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 11.266164480343127, + "isStopPoint": false, + "x": 4.783077239990234, + "y": 4.404496192932129, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 11.918670212860226, + "isStopPoint": false, + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 14 + }, + { + "timestamp": 12.857883244985628, + "isStopPoint": true, + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 9 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5, + 8 + ], + "type": "MaxVelocity", + "velocity": 1.9 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + } + }, + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": true +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index bca596a..6c233b5 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/deploy/apriltags/2024-wpi-custom.json b/src/main/deploy/apriltags/2024-wpi-custom.json new file mode 100644 index 0000000..bbf6862 --- /dev/null +++ b/src/main/deploy/apriltags/2024-wpi-custom.json @@ -0,0 +1,116 @@ +{ + "tags": [ + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/choreo/2PieceSourceSide.1.traj b/src/main/deploy/choreo/2PieceSourceSide.1.traj new file mode 100644 index 0000000..c9746fc --- /dev/null +++ b/src/main/deploy/choreo/2PieceSourceSide.1.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": -9.608358932437547e-22, + "velocityX": -5.5744455071802725e-22, + "velocityY": 6.012447358293193e-22, + "timestamp": 0 + }, + { + "x": 0.6928354443574093, + "y": 4.365429179748156, + "heading": -1.036391352044434, + "angularVelocity": 0.19908395199277742, + "velocityX": 0.5901423959101106, + "velocityY": -0.33387559171653325, + "timestamp": 0.07664438006072226 + }, + { + "x": 0.7831322431143182, + "y": 4.3141136547419725, + "heading": -1.0057051864407323, + "angularVelocity": 0.4003707196716923, + "velocityX": 1.1781268070192532, + "velocityY": -0.6695275630845758, + "timestamp": 0.15328876012144452 + }, + { + "x": 0.9181010090515465, + "y": 4.236769551252555, + "heading": -0.9590991419103531, + "angularVelocity": 0.6080816948803682, + "velocityX": 1.7609740705097754, + "velocityY": -1.0091294812240699, + "timestamp": 0.22993314018216676 + }, + { + "x": 1.0717378394911299, + "y": 4.137047861123805, + "heading": -0.8558129213022901, + "angularVelocity": 1.3476033145056867, + "velocityX": 2.004541367780169, + "velocityY": -1.3010959192278055, + "timestamp": 0.30657752024288903 + }, + { + "x": 1.2038674832955552, + "y": 4.032707483948123, + "heading": -0.6892611362287111, + "angularVelocity": 2.1730462813010782, + "velocityX": 1.723931274540212, + "velocityY": -1.3613571809572687, + "timestamp": 0.3832219003036113 + }, + { + "x": 1.3593994671434348, + "y": 3.930173585943927, + "heading": -0.5828481506767698, + "angularVelocity": 1.3883990641927644, + "velocityX": 2.0292679479520617, + "velocityY": -1.337787557586892, + "timestamp": 0.4598662803643336 + }, + { + "x": 1.5344212392742493, + "y": 3.825045969669984, + "heading": -0.5409009895716792, + "angularVelocity": 0.547295980107838, + "velocityX": 2.283556498103993, + "velocityY": -1.3716285028420194, + "timestamp": 0.5365106604250558 + }, + { + "x": 1.71620093794988, + "y": 3.7215668910578086, + "heading": -0.5175059363043579, + "angularVelocity": 0.30524160086866525, + "velocityX": 2.3717289973722004, + "velocityY": -1.3501195851567043, + "timestamp": 0.6131550404857781 + }, + { + "x": 1.8995763969111468, + "y": 3.620563279311395, + "heading": -0.4949325103664128, + "angularVelocity": 0.29452160641212227, + "velocityX": 2.3925493143265815, + "velocityY": -1.317821498019721, + "timestamp": 0.6897994205465003 + }, + { + "x": 2.0827436178189283, + "y": 3.519597469891814, + "heading": -0.47198902860852304, + "angularVelocity": 0.299349824993203, + "velocityX": 2.389832376002851, + "velocityY": -1.317328280815764, + "timestamp": 0.7664438006072226 + }, + { + "x": 2.26569149707491, + "y": 3.4186714157782796, + "heading": -0.4486429208265262, + "angularVelocity": 0.30460299585567374, + "velocityX": 2.386970566022445, + "velocityY": -1.3168095825626738, + "timestamp": 0.8430881806679449 + }, + { + "x": 2.448407519348329, + "y": 3.3177876585647876, + "heading": -0.4248570996145935, + "angularVelocity": 0.31034005615399535, + "velocityX": 2.383945465129479, + "velocityY": -1.3162577234438597, + "timestamp": 0.9197325607286672 + }, + { + "x": 2.630877314948694, + "y": 3.216949340893998, + "heading": -0.4005887556404844, + "angularVelocity": 0.3166356614129087, + "velocityX": 2.380732878989976, + "velocityY": -1.315664861414486, + "timestamp": 0.9963769407893894 + }, + { + "x": 2.813084210940684, + "y": 3.1161603652063548, + "heading": -0.3757879402454182, + "angularVelocity": 0.32358296036079226, + "velocityX": 2.3773027565443785, + "velocityY": -1.315021083186951, + "timestamp": 1.0730213208501116 + }, + { + "x": 2.995008641657437, + "y": 3.015425613984386, + "heading": -0.3503956914244872, + "angularVelocity": 0.33129955256750454, + "velocityX": 2.3736173555402456, + "velocityY": -1.3143136018865376, + "timestamp": 1.1496657009108338 + }, + { + "x": 3.17662734722959, + "y": 2.9147512559646014, + "heading": -0.3243414773656405, + "angularVelocity": 0.3399363924426679, + "velocityX": 2.369628476716278, + "velocityY": -1.3135256354089386, + "timestamp": 1.226310080971556 + }, + { + "x": 3.357912254126645, + "y": 2.8141451824248387, + "heading": -0.2975396133974428, + "angularVelocity": 0.3496911834496373, + "velocityX": 2.3652733149309855, + "velocityY": -1.3126347092905821, + "timestamp": 1.302954461032278 + }, + { + "x": 3.5388288643370807, + "y": 2.7136176475439253, + "heading": -0.2698840874526075, + "angularVelocity": 0.36082914263152605, + "velocityX": 2.360468048239194, + "velocityY": -1.3116099941218018, + "timestamp": 1.3795988410930002 + }, + { + "x": 3.719333854734169, + "y": 2.6131822420433046, + "heading": -0.24124081910323272, + "angularVelocity": 0.37371648549680814, + "velocityX": 2.3550975329708663, + "velocityY": -1.310407957126798, + "timestamp": 1.4562432211537224 + }, + { + "x": 3.8993713437949977, + "y": 2.5128574382631568, + "heading": -0.21143557422637974, + "angularVelocity": 0.38887710818770393, + "velocityX": 2.3489979163272277, + "velocityY": -1.30896490650279, + "timestamp": 1.5328876012144446 + }, + { + "x": 4.078866770289129, + "y": 2.412669175875395, + "heading": -0.1802340691832318, + "angularVelocity": 0.40709449301342465, + "velocityX": 2.341925479101333, + "velocityY": -1.3071834139487721, + "timestamp": 1.6095319812751667 + }, + { + "x": 4.257716152023489, + "y": 2.312655493774915, + "heading": -0.14730691408099778, + "angularVelocity": 0.42960951704674416, + "velocityX": 2.333496358019524, + "velocityY": -1.3049056176231448, + "timestamp": 1.6861763613358889 + }, + { + "x": 4.435765454390052, + "y": 2.2128756142037065, + "heading": -0.11216198987129261, + "angularVelocity": 0.4585453516860762, + "velocityX": 2.3230575056579674, + "velocityY": -1.3018551326549586, + "timestamp": 1.762820741396611 + }, + { + "x": 4.612765574615127, + "y": 2.1134301828129587, + "heading": -0.07399625871834163, + "angularVelocity": 0.4979586386205198, + "velocityX": 2.309368541892369, + "velocityY": -1.2974914965971616, + "timestamp": 1.8394651214573332 + }, + { + "x": 4.788252812057872, + "y": 2.0145161377885903, + "heading": -0.03130054809700164, + "angularVelocity": 0.5570625085298347, + "velocityX": 2.289629550186371, + "velocityY": -1.29055835465043, + "timestamp": 1.9161095015180554 + }, + { + "x": 4.961093908416901, + "y": 1.9166397715885357, + "heading": 0.01965125716102525, + "angularVelocity": 0.6647820129494142, + "velocityX": 2.255104630268974, + "velocityY": -1.2770194777818196, + "timestamp": 1.9927538815787775 + }, + { + "x": 5.123267857704269, + "y": 1.8235248300121272, + "heading": 0.1054534053975583, + "angularVelocity": 1.119483883470066, + "velocityX": 2.115927471249473, + "velocityY": -1.2148958802020104, + "timestamp": 2.0693982616395 + }, + { + "x": 5.295117773108933, + "y": 1.723779183372333, + "heading": 0.15591188430453226, + "angularVelocity": 0.6583454503382731, + "velocityX": 2.242172423712138, + "velocityY": -1.3014084863204511, + "timestamp": 2.1460426417002223 + }, + { + "x": 5.493802177107404, + "y": 1.644843167367094, + "heading": 0.16076148201528526, + "angularVelocity": 0.06327401574532741, + "velocityX": 2.5922892694945285, + "velocityY": -1.0298995952828023, + "timestamp": 2.2226870217609447 + }, + { + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "angularVelocity": 0.000016002304321454365, + "velocityX": 2.788272708897006, + "velocityY": -0.35025145690136233, + "timestamp": 2.299331401821667 + }, + { + "x": 5.878932542660695, + "y": 1.6310713138935253, + "heading": 0.15207614318358995, + "angularVelocity": -0.14001568544582638, + "velocityX": 2.7631380786635638, + "velocityY": 0.21071831164963453, + "timestamp": 2.361371346065989 + }, + { + "x": 6.041874382995733, + "y": 1.6781208308561997, + "heading": 0.13576389170853745, + "angularVelocity": -0.2629314335102006, + "velocityX": 2.626402107863764, + "velocityY": 0.7583745848865838, + "timestamp": 2.4234112903103107 + }, + { + "x": 6.190062507577151, + "y": 1.7574806196630754, + "heading": 0.1161889234588019, + "angularVelocity": -0.3155220155041827, + "velocityX": 2.388592162459606, + "velocityY": 1.2791724714378518, + "timestamp": 2.4854512345546325 + }, + { + "x": 6.338186561266622, + "y": 1.840599179088551, + "heading": 0.10221849097307459, + "angularVelocity": -0.2251844784178048, + "velocityX": 2.3875594263292355, + "velocityY": 1.3397587705453586, + "timestamp": 2.5474911787989543 + }, + { + "x": 6.486158965288044, + "y": 1.923670012271326, + "heading": 0.0878131418422515, + "angularVelocity": -0.2321947465667089, + "velocityX": 2.385115038767372, + "velocityY": 1.3389894880567688, + "timestamp": 2.609531123043276 + }, + { + "x": 6.633960340204339, + "y": 2.0066848372490824, + "heading": 0.07290958069680836, + "angularVelocity": -0.24022525047332188, + "velocityX": 2.3823582808880834, + "velocityY": 1.338086711535915, + "timestamp": 2.671571067287598 + }, + { + "x": 6.781566267214599, + "y": 2.089633159840961, + "heading": 0.05742800134595927, + "angularVelocity": -0.2495421222475715, + "velocityX": 2.3792079249615, + "velocityY": 1.3370147830116785, + "timestamp": 2.7336110115319197 + }, + { + "x": 6.928944704154211, + "y": 2.172500919861751, + "heading": 0.041263254214314184, + "angularVelocity": -0.26055386297553834, + "velocityX": 2.3755410926743434, + "velocityY": 1.3357162233164679, + "timestamp": 2.7956509557762415 + }, + { + "x": 7.0760519843153995, + "y": 2.2552685458346695, + "heading": 0.024271433663850765, + "angularVelocity": -0.27388516797415663, + "velocityX": 2.371170412111595, + "velocityY": 1.3341021978834797, + "timestamp": 2.8576909000205633 + }, + { + "x": 7.222828595738127, + "y": 2.3379092854906895, + "heading": 0.0062563830373599805, + "angularVelocity": -0.290378253009787, + "velocityX": 2.365840479235448, + "velocityY": 1.3320569620528595, + "timestamp": 2.919730844264885 + }, + { + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 5.891733337285271e-23, + "angularVelocity": -0.10084443359138874, + "velocityX": 2.608968471054304, + "velocityY": 0.9550614620483442, + "timestamp": 2.981770788509207 + }, + { + "x": 7.581926700062977, + "y": 2.4206710216508216, + "heading": -2.7266129355478664e-9, + "angularVelocity": -3.8574963800368557e-8, + "velocityX": 2.790437419391855, + "velocityY": 0.33260634763633495, + "timestamp": 3.052454275074705 + }, + { + "x": 7.780536613286139, + "y": 2.4175758734140427, + "heading": -2.7266116803811135e-9, + "angularVelocity": 1.77575670611303e-14, + "velocityX": 2.8098488469314913, + "velocityY": -0.0437888449929639, + "timestamp": 3.1231377616402027 + }, + { + "x": 7.965557451678622, + "y": 2.414692424306247, + "heading": -1.6359670992111923e-9, + "angularVelocity": 1.5429977129938083e-8, + "velocityX": 2.617596377635332, + "velocityY": -0.040793815470937494, + "timestamp": 3.1938212482057007 + }, + { + "x": 8.104323085612952, + "y": 2.4125298373952777, + "heading": -8.179835707958311e-10, + "angularVelocity": 1.1572484156640823e-8, + "velocityX": 1.9631973559445737, + "velocityY": -0.03059536273674731, + "timestamp": 3.2645047347711986 + }, + { + "x": 8.196833509725684, + "y": 2.4110881127647406, + "heading": -2.7266119351817786e-10, + "angularVelocity": 7.714989791460502e-9, + "velocityX": 1.3087982583740818, + "velocityY": -0.020396908819728356, + "timestamp": 3.3351882213366966 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 8.681753022528805e-26, + "angularVelocity": 3.857494964761243e-9, + "velocityX": 0.6543991355103502, + "velocityY": -0.010198454508433217, + "timestamp": 3.4058717079021945 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 8.728420230679324e-26, + "angularVelocity": 1.0134949222687695e-26, + "velocityX": -1.3642535441280806e-24, + "velocityY": -9.978597912861742e-24, + "timestamp": 3.4765551944676925 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2PieceSourceSide.2.traj b/src/main/deploy/choreo/2PieceSourceSide.2.traj new file mode 100644 index 0000000..a23cc17 --- /dev/null +++ b/src/main/deploy/choreo/2PieceSourceSide.2.traj @@ -0,0 +1,329 @@ +{ + "samples": [ + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 8.728420230679324e-26, + "angularVelocity": 1.0134949222687695e-26, + "velocityX": -1.3642535441280806e-24, + "velocityY": -9.978597912861742e-24, + "timestamp": 0 + }, + { + "x": 8.18367291539843, + "y": 2.389187806239919, + "heading": -0.01626333469089185, + "angularVelocity": -0.19342866418868207, + "velocityX": -0.7066644304729924, + "velocityY": -0.2518986221601445, + "timestamp": 0.08407923799250172 + }, + { + "x": 8.064809767858126, + "y": 2.3467179961156654, + "heading": -0.048607026534506215, + "angularVelocity": -0.3846810772297782, + "velocityX": -1.4137039104815106, + "velocityY": -0.505116496512981, + "timestamp": 0.168158475985003 + }, + { + "x": 7.886436148912839, + "y": 2.282742871071392, + "heading": -0.09665476605625407, + "angularVelocity": -0.5714578374988715, + "velocityX": -2.121494238104243, + "velocityY": -0.7608908759375165, + "timestamp": 0.2522377139775043 + }, + { + "x": 7.663093979787681, + "y": 2.2056337349216286, + "heading": -0.09665512745000811, + "angularVelocity": -0.000004298252013772573, + "velocityX": -2.6563296059495225, + "velocityY": -0.9171007967108408, + "timestamp": 0.33631695197000555 + }, + { + "x": 7.439949744198702, + "y": 2.127953535989356, + "heading": -0.09665535866484175, + "angularVelocity": -0.0000027499634769757167, + "velocityX": -2.6539754750022797, + "velocityY": -0.9238927562497667, + "timestamp": 0.4203961899625068 + }, + { + "x": 7.216805508864092, + "y": 2.0502733363263643, + "heading": -0.0966555898796425, + "angularVelocity": -0.0000027499630855554996, + "velocityX": -2.6539754719769384, + "velocityY": -0.9238927649406101, + "timestamp": 0.5044754279550081 + }, + { + "x": 6.993661273529481, + "y": 1.9725931366633558, + "heading": -0.09665582109441187, + "angularVelocity": -0.0000027499627124784167, + "velocityX": -2.6539754719769486, + "velocityY": -0.9238927649408113, + "timestamp": 0.5885546659475094 + }, + { + "x": 6.7705170381948685, + "y": 1.894912937000331, + "heading": -0.09665605230914993, + "angularVelocity": -0.000002749962340016734, + "velocityX": -2.6539754719769633, + "velocityY": -0.923892764941001, + "timestamp": 0.6726339039400107 + }, + { + "x": 6.547372802860255, + "y": 1.8172327373372903, + "heading": -0.09665628352385662, + "angularVelocity": -0.000002749961966980963, + "velocityX": -2.653975471976977, + "velocityY": -0.9238927649411908, + "timestamp": 0.7567131419325119 + }, + { + "x": 6.324228567525631, + "y": 1.73955253767426, + "heading": -0.09665651473853194, + "angularVelocity": -0.000002749961593809523, + "velocityX": -2.6539754719771, + "velocityY": -0.9238927649410678, + "timestamp": 0.8407923799250132 + }, + { + "x": 6.101084325049948, + "y": 1.661872358525338, + "heading": -0.0966567459539087, + "angularVelocity": -0.0000027499699365368005, + "velocityX": -2.65397555690958, + "velocityY": -0.9238925209556449, + "timestamp": 0.9248716179175145 + }, + { + "x": 5.881706570976293, + "y": 1.600162551700404, + "heading": -0.12280434503917524, + "angularVelocity": -0.31098758396928483, + "velocityX": -2.6091786665956676, + "velocityY": -0.7339482171619789, + "timestamp": 1.0089508559100158 + }, + { + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.19572019487189304, + "angularVelocity": -0.8672277672071758, + "velocityX": -2.543041314951311, + "velocityY": -0.10200423944249568, + "timestamp": 1.093030093902517 + }, + { + "x": 5.472836105360247, + "y": 1.6405603677433935, + "heading": -0.26955534724389113, + "angularVelocity": -0.9373012553007992, + "velocityX": -2.476108937905111, + "velocityY": 0.6217042830704372, + "timestamp": 1.171804289450034 + }, + { + "x": 5.302149571829305, + "y": 1.7128008557862997, + "heading": -0.40037410366922566, + "angularVelocity": -1.6606803219770656, + "velocityX": -2.166782311702363, + "velocityY": 0.9170577692453934, + "timestamp": 1.250578484997551 + }, + { + "x": 5.119601106892005, + "y": 1.8042196789980731, + "heading": -0.4642807567510961, + "angularVelocity": -0.8112637982233885, + "velocityX": -2.3173637467003596, + "velocityY": 1.160517382327688, + "timestamp": 1.329352680545068 + }, + { + "x": 4.9290964037273115, + "y": 1.9049531149336705, + "heading": -0.4863983419405073, + "angularVelocity": -0.2807719588335222, + "velocityX": -2.4183643113153757, + "velocityY": 1.2787618487939325, + "timestamp": 1.408126876092585 + }, + { + "x": 4.737760376090184, + "y": 2.0093145223316466, + "heading": -0.4993330246766849, + "angularVelocity": -0.16419949002684986, + "velocityX": -2.4289175701161345, + "velocityY": 1.3248171774096373, + "timestamp": 1.4869010716401019 + }, + { + "x": 4.546909916253962, + "y": 2.1148536600688166, + "heading": -0.5116764197734127, + "angularVelocity": -0.1566933817722359, + "velocityX": -2.4227535236599187, + "velocityY": 1.3397678897718286, + "timestamp": 1.5656752671876188 + }, + { + "x": 4.356100812945195, + "y": 2.22034699353671, + "heading": -0.5241251158097335, + "angularVelocity": -0.15803012585271836, + "velocityX": -2.422228522710459, + "velocityY": 1.3391864269087888, + "timestamp": 1.6444494627351358 + }, + { + "x": 4.165334183903592, + "y": 2.3257931232255338, + "heading": -0.5366842727158639, + "angularVelocity": -0.15943237273117897, + "velocityX": -2.42168933260045, + "velocityY": 1.338587197951371, + "timestamp": 1.7232236582826528 + }, + { + "x": 3.9746112406101144, + "y": 2.4311907250831752, + "heading": -0.5493591358398626, + "angularVelocity": -0.16090120674546474, + "velocityX": -2.421134763330369, + "velocityY": 1.337971160798023, + "timestamp": 1.8019978538301697 + }, + { + "x": 3.783933281721757, + "y": 2.536538405516183, + "heading": -0.5621552659145432, + "angularVelocity": -0.16244063155125335, + "velocityX": -2.420563708243014, + "velocityY": 1.337337432655366, + "timestamp": 1.8807720493776867 + }, + { + "x": 3.593301708908867, + "y": 2.6418346807737962, + "heading": -0.575078618641915, + "angularVelocity": -0.16405566108988592, + "velocityX": -2.4199748596340855, + "velocityY": 1.3366848690203137, + "timestamp": 1.9595462449252037 + }, + { + "x": 3.402718042259555, + "y": 2.747077964346348, + "heading": -0.5881356065830585, + "angularVelocity": -0.1657520949644925, + "velocityX": -2.4193667142478334, + "velocityY": 1.3360121654186723, + "timestamp": 2.0383204404727207 + }, + { + "x": 3.2121839365027345, + "y": 2.8522665545853982, + "heading": -0.6013331613423861, + "angularVelocity": -0.16753652217707185, + "velocityX": -2.4187375628849153, + "velocityY": 1.3353178602198477, + "timestamp": 2.1170946360202376 + }, + { + "x": 3.021701199078829, + "y": 2.95739862098606, + "heading": -0.614678801952211, + "angularVelocity": -0.1694163998383799, + "velocityX": -2.4180854669471756, + "velocityY": 1.3346003176540937, + "timestamp": 2.1958688315677546 + }, + { + "x": 2.831271810849958, + "y": 3.0624721884566903, + "heading": -0.6281807128614904, + "angularVelocity": -0.17140017508823685, + "velocityX": -2.4174082249307642, + "velocityY": 1.333857702263045, + "timestamp": 2.2746430271152716 + }, + { + "x": 2.6408979502222807, + "y": 3.167485119026869, + "heading": -0.6418478345096594, + "angularVelocity": -0.17349744485711852, + "velocityX": -2.416703329110398, + "velocityY": 1.3330879463800378, + "timestamp": 2.3534172226627885 + }, + { + "x": 2.4505820600594026, + "y": 3.272435090456081, + "heading": -0.6556900847735371, + "angularVelocity": -0.17572061723598228, + "velocityX": -2.4159674223277707, + "velocityY": 1.3322887107860868, + "timestamp": 2.4321914182103055 + }, + { + "x": 2.2618232105010683, + "y": 3.377251285216961, + "heading": -0.6742598798823499, + "angularVelocity": -0.2357344937608601, + "velocityX": -2.3962015511091392, + "velocityY": 1.3305904812148108, + "timestamp": 2.5109656137578225 + }, + { + "x": 2.116002465177744, + "y": 3.455078881481672, + "heading": -0.7196330985470573, + "angularVelocity": -0.5759908857125439, + "velocityX": -1.8511232556524801, + "velocityY": 0.9879833836927662, + "timestamp": 2.5897398093053394 + }, + { + "x": 2.018778727407275, + "y": 3.506953261839289, + "heading": -0.7498767066928216, + "angularVelocity": -0.38392785779096056, + "velocityX": -1.2342079420134902, + "velocityY": 0.6585199632578456, + "timestamp": 2.6685140048528564 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -0.19198283399868005, + "velocityX": -0.6171001434571658, + "velocityY": 0.32925527688449013, + "timestamp": 2.7472882004003734 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": 5.701667392522477e-25, + "velocityX": -1.884546108540493e-26, + "velocityY": 1.8442293688731203e-27, + "timestamp": 2.8260623959478903 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2PieceSourceSide.3.traj b/src/main/deploy/choreo/2PieceSourceSide.3.traj new file mode 100644 index 0000000..ac482b0 --- /dev/null +++ b/src/main/deploy/choreo/2PieceSourceSide.3.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": 5.701667392522477e-25, + "velocityX": -1.884546108540493e-26, + "velocityY": 1.8442293688731203e-27, + "timestamp": 0 + }, + { + "x": 2.0038609226281077, + "y": 3.52162417456583, + "heading": -0.7649999977283855, + "angularVelocity": 3.667263913113266e-8, + "velocityX": 0.5439475719559151, + "velocityY": -0.18187528490808497, + "timestamp": 0.061943033356638644 + }, + { + "x": 2.071248446997458, + "y": 3.4990923611553195, + "heading": -0.764999992959822, + "angularVelocity": 7.698304705023737e-8, + "velocityX": 1.0878951306979854, + "velocityY": -0.36375056547170626, + "timestamp": 0.12388606671327729 + }, + { + "x": 2.1723297315052195, + "y": 3.4652946417123274, + "heading": -0.7649999852436408, + "angularVelocity": 1.2456899230649337e-7, + "velocityX": 1.6318426630123661, + "velocityY": -0.5456258373464002, + "timestamp": 0.18582910006991593 + }, + { + "x": 2.3071047712403585, + "y": 3.4202310178515085, + "heading": -0.7649999732278352, + "angularVelocity": 1.939815494742613e-7, + "velocityX": 2.1757901160436854, + "velocityY": -0.7275010831543144, + "timestamp": 0.24777213342655457 + }, + { + "x": 2.4539701954241457, + "y": 3.3711248317579967, + "heading": -0.7568529781297836, + "angularVelocity": 0.13152399320106095, + "velocityX": 2.370975656587645, + "velocityY": -0.7927636641683562, + "timestamp": 0.3097151667831932 + }, + { + "x": 2.600835619548289, + "y": 3.3220186454861014, + "heading": -0.7421829922911787, + "angularVelocity": 0.23683027846153368, + "velocityX": 2.3709756556247594, + "velocityY": -0.7927636670481597, + "timestamp": 0.37165820013983186 + }, + { + "x": 2.7477010436813765, + "y": 3.2729124592409553, + "heading": -0.7236922295459556, + "angularVelocity": 0.2985123869985814, + "velocityX": 2.370975655769149, + "velocityY": -0.7927636666163211, + "timestamp": 0.4336012334964705 + }, + { + "x": 2.8945664678191285, + "y": 3.223806273009761, + "heading": -0.7029849463910878, + "angularVelocity": 0.3342955943995355, + "velocityX": 2.3709756558444592, + "velocityY": -0.7927636663910833, + "timestamp": 0.49554426685310915 + }, + { + "x": 3.0414318919586276, + "y": 3.1747000867837922, + "heading": -0.6809396437515444, + "angularVelocity": 0.35589640101441095, + "velocityX": 2.370975655872664, + "velocityY": -0.7927636663067286, + "timestamp": 0.5574873002097478 + }, + { + "x": 3.1882973160983084, + "y": 3.1255939005583664, + "heading": -0.6579622312792146, + "angularVelocity": 0.3709442567986083, + "velocityX": 2.370975655875593, + "velocityY": -0.7927636662979668, + "timestamp": 0.6194303335663864 + }, + { + "x": 3.335162740237555, + "y": 3.0764877143316434, + "heading": -0.6341135338832845, + "angularVelocity": 0.38501016342904104, + "velocityX": 2.3709756558685937, + "velocityY": -0.7927636663189009, + "timestamp": 0.6813733669230251 + }, + { + "x": 3.4820281699266116, + "y": 3.02738154470314, + "heading": -0.609122805852069, + "angularVelocity": 0.4034469524172422, + "velocityX": 2.37097574546397, + "velocityY": -0.792763398359474, + "timestamp": 0.7433164002796637 + }, + { + "x": 3.6308650910444418, + "y": 2.9846212682684787, + "heading": -0.5823864010108707, + "angularVelocity": 0.43162892406741965, + "velocityX": 2.4028032379508626, + "velocityY": -0.6903161520758537, + "timestamp": 0.8052594336363024 + }, + { + "x": 3.7855303944927208, + "y": 2.9769067251940045, + "heading": -0.5556499700120278, + "angularVelocity": 0.4316293463529163, + "velocityX": 2.496895858454154, + "velocityY": -0.12454254589144183, + "timestamp": 0.867202466992941 + }, + { + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": -0.5289135345304437, + "angularVelocity": 0.4316294187216871, + "velocityX": 2.4595699103703104, + "velocityY": 0.4477893445840132, + "timestamp": 0.9291455003495797 + }, + { + "x": 4.0824073189453145, + "y": 3.0682416154915506, + "heading": -0.5016521854897775, + "angularVelocity": 0.43162944872904263, + "velocityX": 2.288246497119936, + "velocityY": 1.0069397723044053, + "timestamp": 0.9923046502197721 + }, + { + "x": 4.208202033139849, + "y": 3.1636744602160114, + "heading": -0.47439083212785377, + "angularVelocity": 0.4316295171475982, + "velocityX": 1.9917100602695579, + "velocityY": 1.5109900136496308, + "timestamp": 1.0554638000899645 + }, + { + "x": 4.3083845304850525, + "y": 3.2857203747505457, + "heading": -0.44712944555921064, + "angularVelocity": 0.43163004291020496, + "velocityX": 1.5861913523393378, + "velocityY": 1.9323552452078465, + "timestamp": 1.118622949960157 + }, + { + "x": 4.405336824547438, + "y": 3.4103477822806245, + "heading": -0.4176900619419933, + "angularVelocity": 0.46611431087534605, + "velocityX": 1.5350474834073353, + "velocityY": 1.9732280720405366, + "timestamp": 1.1817820998303494 + }, + { + "x": 4.502289110835403, + "y": 3.534975195858712, + "heading": -0.3872228793179382, + "angularVelocity": 0.48238747175465096, + "velocityX": 1.535047360314808, + "velocityY": 1.9732281677987837, + "timestamp": 1.2449412497005419 + }, + { + "x": 4.599241397101022, + "y": 3.6596026094541845, + "heading": -0.3565807251916121, + "angularVelocity": 0.4851577988193852, + "velocityX": 1.5350473599609986, + "velocityY": 1.9732281680740251, + "timestamp": 1.3081003995707343 + }, + { + "x": 4.696193700299505, + "y": 3.7842300098769526, + "heading": -0.32648642480057394, + "angularVelocity": 0.47648362039212855, + "velocityX": 1.5350476280593386, + "velocityY": 1.9732279595103561, + "timestamp": 1.3712595494409268 + }, + { + "x": 4.799852119504964, + "y": 3.9033378188276986, + "heading": -0.2975971211067133, + "angularVelocity": 0.4574048851708002, + "velocityX": 1.6412256880990796, + "velocityY": 1.885836164602317, + "timestamp": 1.4344186993111192 + }, + { + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": -0.2687078272474491, + "angularVelocity": 0.4574047294594519, + "velocityX": 2.034433398150223, + "velocityY": 1.4529557147610683, + "timestamp": 1.4975778491813116 + }, + { + "x": 5.091718476098542, + "y": 4.056742420888441, + "heading": -0.23676016499896843, + "angularVelocity": 0.45740470036593445, + "velocityX": 2.3390663850588584, + "velocityY": 0.8824784700263478, + "timestamp": 1.5674233540983398 + }, + { + "x": 5.265436131272645, + "y": 4.074409944270163, + "heading": -0.20481252500040537, + "angularVelocity": 0.4574043818068857, + "velocityX": 2.487170153332956, + "velocityY": 0.2529514734371195, + "timestamp": 1.637268859015368 + }, + { + "x": 5.439550422671689, + "y": 4.0876076196519335, + "heading": -0.1739815304617893, + "angularVelocity": 0.44141701853599924, + "velocityX": 2.4928489185650484, + "velocityY": 0.18895525771414254, + "timestamp": 1.707114363932396 + }, + { + "x": 5.613664714661083, + "y": 4.100805287245334, + "heading": -0.14381526883234147, + "angularVelocity": 0.43189982899090373, + "velocityX": 2.4928489270172745, + "velocityY": 0.18895514620558412, + "timestamp": 1.7769598688494241 + }, + { + "x": 5.787779006651462, + "y": 4.114002954825723, + "heading": -0.11419727190637251, + "angularVelocity": 0.42405015127534945, + "velocityX": 2.4928489270313947, + "velocityY": 0.18895514601929783, + "timestamp": 1.8468053737664523 + }, + { + "x": 5.9618932986427895, + "y": 4.127200622393612, + "heading": -0.08529666229520746, + "angularVelocity": 0.413779092090422, + "velocityX": 2.4928489270449585, + "velocityY": 0.18895514584035356, + "timestamp": 1.9166508786834804 + }, + { + "x": 6.136007590635063, + "y": 4.140398289949022, + "heading": -0.05764746698018865, + "angularVelocity": 0.39586220112323917, + "velocityX": 2.492848927058503, + "velocityY": 0.18895514566166938, + "timestamp": 1.9864963836005085 + }, + { + "x": 6.31012188262831, + "y": 4.1535959574915955, + "heading": -0.03239841025240522, + "angularVelocity": 0.3614986641986141, + "velocityX": 2.492848927072435, + "velocityY": 0.18895514547786693, + "timestamp": 2.0563418885175366 + }, + { + "x": 6.484236174622575, + "y": 4.166793625020717, + "heading": -0.011804071567896217, + "angularVelocity": 0.2948556060833649, + "velocityX": 2.492848927087033, + "velocityY": 0.18895514528529114, + "timestamp": 2.1261873934345648 + }, + { + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": -2.7503715712588036e-25, + "angularVelocity": 0.16900259482580393, + "velocityX": 2.4928489423355984, + "velocityY": 0.1889544683618032, + "timestamp": 2.196032898351593 + }, + { + "x": 6.90055242137025, + "y": 4.166207447816762, + "heading": 1.0089498558185064e-14, + "angularVelocity": 1.1687603682470004e-13, + "velocityX": 2.8056502802672902, + "velocityY": -0.1596705336116014, + "timestamp": 2.2823593929228583 + }, + { + "x": 7.142754372257995, + "y": 4.152423601152713, + "heading": 1.0071066789817695e-14, + "angularVelocity": -2.1351229956858005e-16, + "velocityX": 2.805650247825106, + "velocityY": -0.15967110366876425, + "timestamp": 2.3686858874941237 + }, + { + "x": 7.384956323145737, + "y": 4.138639754488611, + "heading": 1.012050696342135e-14, + "angularVelocity": 5.727114699487227e-16, + "velocityX": 2.8056502478250716, + "velocityY": -0.15967110366937468, + "timestamp": 2.455012382065389 + }, + { + "x": 7.627158274033478, + "y": 4.124855907824508, + "heading": 1.0143494124917898e-14, + "angularVelocity": 2.662816509401409e-16, + "velocityX": 2.8056502478250716, + "velocityY": -0.15967110366937465, + "timestamp": 2.5413388766366545 + }, + { + "x": 7.869360224921218, + "y": 4.111072061160406, + "heading": 1.0132250593717563e-14, + "angularVelocity": -1.30244269252452e-16, + "velocityX": 2.8056502478250454, + "velocityY": -0.15967110366937318, + "timestamp": 2.62766537120792 + }, + { + "x": 8.076033716141364, + "y": 4.099310158739423, + "heading": 5.065708579525022e-15, + "angularVelocity": -5.869046391096223e-14, + "velocityX": 2.39409108694351, + "velocityY": -0.13624904473877847, + "timestamp": 2.7139918657791853 + }, + { + "x": 8.21381604770127, + "y": 4.091468890226584, + "heading": 1.6859122476292957e-15, + "angularVelocity": -3.915132137223281e-14, + "velocityX": 1.5960607718892357, + "velocityY": -0.0908326991821247, + "timestamp": 2.8003183603504507 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -1.9541619269839356e-27, + "angularVelocity": -1.952948809060791e-14, + "velocityX": 0.7980303960718105, + "velocityY": -0.045416350167406465, + "timestamp": 2.886644854921716 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": 4.106650917468526e-28, + "angularVelocity": 1.7218306624303264e-27, + "velocityX": 3.309441192247122e-27, + "velocityY": -1.6370040698633354e-26, + "timestamp": 2.9729713494929815 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2PieceSourceSide.4.traj b/src/main/deploy/choreo/2PieceSourceSide.4.traj new file mode 100644 index 0000000..adc7aa3 --- /dev/null +++ b/src/main/deploy/choreo/2PieceSourceSide.4.traj @@ -0,0 +1,365 @@ +{ + "samples": [ + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": 4.106650917468526e-28, + "angularVelocity": 1.7218306624303264e-27, + "velocityX": 3.309441192247122e-27, + "velocityY": -1.6370040698633354e-26, + "timestamp": 0 + }, + { + "x": 8.214743893146172, + "y": 4.097431952557445, + "heading": 7.777167054938412e-9, + "angularVelocity": 9.030267112433999e-8, + "velocityX": -0.7891394643225377, + "velocityY": 0.1147621236115998, + "timestamp": 0.0861233334308551 + }, + { + "x": 8.078817252614842, + "y": 4.117199345557055, + "heading": 2.3331500454738663e-8, + "angularVelocity": 1.8060533400379965e-7, + "velocityX": -1.5782789067315892, + "velocityY": 0.22952424403638874, + "timestamp": 0.1722466668617102 + }, + { + "x": 7.8749272984232634, + "y": 4.146850434095866, + "heading": 4.666299775490785e-8, + "angularVelocity": 2.709079684996286e-7, + "velocityX": -2.367418283400189, + "velocityY": 0.34428635490074616, + "timestamp": 0.2583700002925653 + }, + { + "x": 7.635423742070671, + "y": 4.181680699191212, + "heading": 4.6663022241482475e-8, + "angularVelocity": 2.843198660161906e-13, + "velocityX": -2.7809369053844195, + "velocityY": 0.4044230954356897, + "timestamp": 0.3444933337234204 + }, + { + "x": 7.395920185718074, + "y": 4.2165109642865595, + "heading": 4.666304684050159e-8, + "angularVelocity": 2.8562548767333325e-13, + "velocityX": -2.780936905384451, + "velocityY": 0.4044230954356944, + "timestamp": 0.4306166671542755 + }, + { + "x": 7.156416629365477, + "y": 4.251341229381906, + "heading": 4.666307138108751e-8, + "angularVelocity": 2.849470049921651e-13, + "velocityX": -2.7809369053844515, + "velocityY": 0.40442309543569427, + "timestamp": 0.5167400005851306 + }, + { + "x": 6.91691307301288, + "y": 4.286171494477253, + "heading": 4.666309588467985e-8, + "angularVelocity": 2.8451746308260675e-13, + "velocityX": -2.7809369053844515, + "velocityY": 0.4044230954356943, + "timestamp": 0.6028633340159857 + }, + { + "x": 6.677409516660284, + "y": 4.3210017595726, + "heading": 4.666312037830467e-8, + "angularVelocity": 2.8440172767152884e-13, + "velocityX": -2.780936905384451, + "velocityY": 0.40442309543569427, + "timestamp": 0.6889866674468408 + }, + { + "x": 6.437905960307688, + "y": 4.355832024667947, + "heading": 4.6663144966570676e-8, + "angularVelocity": 2.855006305543674e-13, + "velocityX": -2.7809369053844515, + "velocityY": 0.4044230954356942, + "timestamp": 0.7751100008776959 + }, + { + "x": 6.198402403955091, + "y": 4.390662289763294, + "heading": 4.666316954652242e-8, + "angularVelocity": 2.8540409165670356e-13, + "velocityX": -2.780936905384451, + "velocityY": 0.40442309543569427, + "timestamp": 0.861233334308551 + }, + { + "x": 5.958898847602494, + "y": 4.425492554858642, + "heading": 4.666319411639766e-8, + "angularVelocity": 2.8528709076579723e-13, + "velocityX": -2.7809369053844515, + "velocityY": 0.4044230954356943, + "timestamp": 0.9473566677394061 + }, + { + "x": 5.7193952912498975, + "y": 4.46032281995398, + "heading": 4.666321861149493e-8, + "angularVelocity": 2.8441882463303997e-13, + "velocityX": -2.780936905384466, + "velocityY": 0.40442309543559324, + "timestamp": 1.0334800011702612 + }, + { + "x": 5.479891733819148, + "y": 4.4951530776356154, + "heading": 4.6663243116068356e-8, + "angularVelocity": 2.845288547509047e-13, + "velocityX": -2.7809369179031624, + "velocityY": 0.4044230093531968, + "timestamp": 1.1196033346011163 + }, + { + "x": 5.239559654001658, + "y": 4.5237116535808255, + "heading": 4.666326769971955e-8, + "angularVelocity": 2.8544704688890543e-13, + "velocityX": -2.7905571027443106, + "velocityY": 0.33160091240707185, + "timestamp": 1.2057266680319714 + }, + { + "x": 5.000881742056918, + "y": 4.483612344217189, + "heading": 4.6663260799944615e-8, + "angularVelocity": -8.011504743438211e-14, + "velocityX": -2.771350137490483, + "velocityY": -0.465603312902775, + "timestamp": 1.2918500014628265 + }, + { + "x": 4.783077239990234, + "y": 4.3780837059021, + "heading": 2.1991195110083723e-26, + "angularVelocity": -5.418190279108096e-7, + "velocityX": -2.5289836492632825, + "velocityY": -1.225319946537063, + "timestamp": 1.3779733348936816 + }, + { + "x": 4.62885889818481, + "y": 4.25151091916486, + "heading": -0.005196955217048624, + "angularVelocity": -0.07257686989661408, + "velocityX": -2.1537003998353113, + "velocityY": -1.7676228275635255, + "timestamp": 1.4495795535753597 + }, + { + "x": 4.509627007017704, + "y": 4.09311912822229, + "heading": -0.014574726409382607, + "angularVelocity": -0.13096308344422491, + "velocityX": -1.665105257088721, + "velocityY": -2.2119837335175307, + "timestamp": 1.5211857722570379 + }, + { + "x": 4.431627486858581, + "y": 3.913385971662455, + "heading": -0.03321756171123277, + "angularVelocity": -0.26035218232547713, + "velocityX": -1.089284165469857, + "velocityY": -2.510021613609162, + "timestamp": 1.592791990938716 + }, + { + "x": 4.384157066656816, + "y": 3.7334659930943066, + "heading": -0.09426978760930255, + "angularVelocity": -0.8526106673705912, + "velocityX": -0.6629371174142339, + "velocityY": -2.5126306329339134, + "timestamp": 1.6643982096203942 + }, + { + "x": 4.346637809417128, + "y": 3.5635880977422283, + "heading": -0.21737045179893905, + "angularVelocity": -1.719133707323312, + "velocityX": -0.5239664645116665, + "velocityY": -2.37239025436133, + "timestamp": 1.7360044283020724 + }, + { + "x": 4.289169567952475, + "y": 3.3974991339726657, + "heading": -0.33311481037206947, + "angularVelocity": -1.6164009314284105, + "velocityX": -0.8025593659696832, + "velocityY": -2.319476811195739, + "timestamp": 1.8076106469837505 + }, + { + "x": 4.202451160987308, + "y": 3.235028424104835, + "heading": -0.40652514792222744, + "angularVelocity": -1.0251950026365775, + "velocityX": -1.2110457521946352, + "velocityY": -2.2689469275020184, + "timestamp": 1.8792168656654287 + }, + { + "x": 4.0782345443237835, + "y": 3.1012570317174037, + "heading": -0.4755728465694835, + "angularVelocity": -0.9642695832634941, + "velocityX": -1.7347182821610028, + "velocityY": -1.8681532812409178, + "timestamp": 1.9508230843471068 + }, + { + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": -0.5414144650624281, + "angularVelocity": -0.9194958162173116, + "velocityX": -2.144463661865391, + "velocityY": -1.3492246622401947, + "timestamp": 2.022429303028785 + }, + { + "x": 3.763366908865081, + "y": 2.951711995611183, + "heading": -0.599396946347794, + "angularVelocity": -0.8651307180525442, + "velocityX": -2.4068449633835187, + "velocityY": -0.7897771271518286, + "timestamp": 2.089450945048256 + }, + { + "x": 3.5917870336334814, + "y": 2.938067741208574, + "heading": -0.6485687079792504, + "angularVelocity": -0.7336699034793993, + "velocityX": -2.560066719668727, + "velocityY": -0.2035798287162928, + "timestamp": 2.156472587067727 + }, + { + "x": 3.4162652567574, + "y": 2.9628199438049303, + "heading": -0.6822561889676714, + "angularVelocity": -0.5026358646753835, + "velocityX": -2.618882074316952, + "velocityY": 0.36931656477716224, + "timestamp": 2.2234942290871977 + }, + { + "x": 3.2383426806882314, + "y": 3.0185148167713165, + "heading": -0.6882275124716133, + "angularVelocity": -0.08909545221537823, + "velocityX": -2.654703327284604, + "velocityY": 0.8309983355854799, + "timestamp": 2.2905158711066687 + }, + { + "x": 3.065127895889447, + "y": 3.092473757436008, + "heading": -0.6882275861864502, + "angularVelocity": -0.0000010998661715070204, + "velocityX": -2.5844604754455514, + "velocityY": 1.1035083360566535, + "timestamp": 2.3575375131261396 + }, + { + "x": 2.8919131336789268, + "y": 3.166432751003332, + "heading": -0.6882276599012388, + "angularVelocity": -0.0000010998654520040694, + "velocityX": -2.5844601384161447, + "velocityY": 1.1035091253932068, + "timestamp": 2.4245591551456105 + }, + { + "x": 2.71869837146846, + "y": 3.240391744570782, + "heading": -0.6882277336160237, + "angularVelocity": -0.0000010998653968205764, + "velocityX": -2.5844601384153485, + "velocityY": 1.1035091253950962, + "timestamp": 2.4915807971650814 + }, + { + "x": 2.545483609258002, + "y": 3.314350738138256, + "heading": -0.6882278073308049, + "angularVelocity": -0.0000010998653414295304, + "velocityX": -2.5844601384152073, + "velocityY": 1.103509125395451, + "timestamp": 2.5586024391845523 + }, + { + "x": 2.37226885132054, + "y": 3.3883097417118213, + "heading": -0.6882278810474789, + "angularVelocity": -0.0000010998935829984454, + "velocityX": -2.5844600746597552, + "velocityY": 1.1035092746918727, + "timestamp": 2.6256240812040232 + }, + { + "x": 2.221998746240238, + "y": 3.4514344392389487, + "heading": -0.718925382734092, + "angularVelocity": -0.4580237183340691, + "velocityX": -2.242113152593967, + "velocityY": 0.9418554309485306, + "timestamp": 2.692645723223494 + }, + { + "x": 2.1092911265732077, + "y": 3.498767580374514, + "heading": -0.7419626163050886, + "angularVelocity": -0.34372827756597907, + "velocityX": -1.6816600768194587, + "velocityY": 0.706236667878329, + "timestamp": 2.759667365242965 + }, + { + "x": 2.0341505090924006, + "y": 3.530319851474966, + "heading": -0.7573228472825582, + "angularVelocity": -0.22918314912378704, + "velocityX": -1.1211396082921554, + "velocityY": 0.470777351161958, + "timestamp": 2.826689007262436 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": -0.11454736837410578, + "velocityX": -0.5605780595061581, + "velocityY": 0.23539371716832447, + "timestamp": 2.893710649281907 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": 1.329207005367143e-24, + "velocityX": -6.317194891717379e-25, + "velocityY": 1.0973166640246365e-24, + "timestamp": 2.960732291301378 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.1.traj b/src/main/deploy/choreo/3PieceSourceSide.1.traj new file mode 100644 index 0000000..836a57b --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSide.1.traj @@ -0,0 +1,446 @@ +{ + "samples": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": -2.3506527398385264e-24, + "velocityX": -4.584841665957037e-25, + "velocityY": 1.794170584470112e-24, + "timestamp": 0 + }, + { + "x": 0.692835797571452, + "y": 4.365429285724839, + "heading": -1.0363921320591865, + "angularVelocity": 0.19907376380794284, + "velocityX": 0.5901469714218079, + "velocityY": -0.33387419035865423, + "timestamp": 0.07664438434234237 + }, + { + "x": 0.7831334822061629, + "y": 4.314114094234643, + "heading": -1.005707809669664, + "angularVelocity": 0.4003466483919687, + "velocityX": 1.178138299492162, + "velocityY": -0.6695231742092023, + "timestamp": 0.15328876868468475 + }, + { + "x": 0.9181042106922528, + "y": 4.23677088276396, + "heading": -0.9591055566031375, + "angularVelocity": 0.608032192657082, + "velocityX": 1.7609995780411658, + "velocityY": -1.009117786441079, + "timestamp": 0.2299331530270271 + }, + { + "x": 1.0717422840106643, + "y": 4.1370476005177, + "heading": -0.8558256704459198, + "angularVelocity": 1.3475205919314872, + "velocityX": 2.004557471975592, + "velocityY": -1.3011166193315893, + "timestamp": 0.3065775373693695 + }, + { + "x": 1.203873817721518, + "y": 4.032707540283105, + "heading": -0.6892805911828493, + "angularVelocity": 2.172958667384875, + "velocityX": 1.7239558363554812, + "velocityY": -1.3613529696911204, + "timestamp": 0.3832219217117119 + }, + { + "x": 1.3594029637702276, + "y": 3.9301727362790966, + "heading": -0.5828610080840567, + "angularVelocity": 1.388485066609126, + "velocityX": 2.0292308090572955, + "velocityY": -1.3377993036779194, + "timestamp": 0.45986630605405426 + }, + { + "x": 1.5344193266927146, + "y": 3.825053132953821, + "heading": -0.5408815443461281, + "angularVelocity": 0.5477174107162478, + "velocityX": 2.2834857951334486, + "velocityY": -1.371523879110889, + "timestamp": 0.5365106903963967 + }, + { + "x": 1.7162004854447428, + "y": 3.7215786578067394, + "heading": -0.5174827165758584, + "angularVelocity": 0.30529083077705393, + "velocityX": 2.3717479148906513, + "velocityY": -1.3500594470809355, + "timestamp": 0.613155074738739 + }, + { + "x": 1.8995749541613056, + "y": 3.6205751589308552, + "heading": -0.4949062907307691, + "angularVelocity": 0.2945607305585351, + "velocityX": 2.3925362606801817, + "velocityY": -1.317819951749349, + "timestamp": 0.6897994590810813 + }, + { + "x": 2.0827414581868835, + "y": 3.51960952250048, + "heading": -0.4719606385920979, + "angularVelocity": 0.29937812581521006, + "velocityX": 2.389822889142672, + "velocityY": -1.3173259501883174, + "timestamp": 0.7664438434234236 + }, + { + "x": 2.265688686133792, + "y": 3.4186835470349224, + "heading": -0.4486127865936518, + "angularVelocity": 0.3046257361029851, + "velocityX": 2.3869619348724966, + "velocityY": -1.3168084828597342, + "timestamp": 0.843088227765766 + }, + { + "x": 2.4484040933032216, + "y": 3.3177997829423713, + "heading": -0.4248255225210326, + "angularVelocity": 0.3103588642107185, + "velocityX": 2.3839373065260236, + "velocityY": -1.3162577396660924, + "timestamp": 0.9197326121081083 + }, + { + "x": 2.6308732997732713, + "y": 3.216961383200288, + "heading": -0.4005559741960246, + "angularVelocity": 0.3166513572162684, + "velocityX": 2.380725059451553, + "velocityY": -1.3156658587232646, + "timestamp": 0.9963769964504506 + }, + { + "x": 2.813079628281676, + "y": 3.116172257902354, + "heading": -0.3757541558021446, + "angularVelocity": 0.32359602868097065, + "velocityX": 2.377295219628298, + "velocityY": -1.3150229617312381, + "timestamp": 1.073021380792793 + }, + { + "x": 2.9950035114027176, + "y": 3.015437295026463, + "heading": -0.3503610814650098, + "angularVelocity": 0.3313103047930201, + "velocityX": 2.3736100783125105, + "velocityY": -1.314316289970363, + "timestamp": 1.1496657651351354 + }, + { + "x": 3.176621688858468, + "y": 2.9147626672707037, + "heading": -0.3243062033321442, + "angularVelocity": 0.3399450378058761, + "velocityX": 2.3696214538631923, + "velocityY": -1.3135290813490341, + "timestamp": 1.2263101494774777 + }, + { + "x": 3.3579060875063353, + "y": 2.8141562687774018, + "heading": -0.2975038256949741, + "angularVelocity": 0.34969786589260987, + "velocityX": 2.3652665515341145, + "velocityY": -1.3126388757189247, + "timestamp": 1.30295453381982 + }, + { + "x": 3.538822210205962, + "y": 2.713628355793928, + "heading": -0.2698479287435623, + "angularVelocity": 0.3608339630974529, + "velocityX": 2.360461555689992, + "velocityY": -1.3116148540570647, + "timestamp": 1.3795989181621624 + }, + { + "x": 3.7193267349323875, + "y": 2.6131925205499895, + "heading": -0.24120442629231653, + "angularVelocity": 0.3737195190101045, + "velocityX": 2.3550913256759576, + "velocityY": -1.3104134909000977, + "timestamp": 1.4562433025045047 + }, + { + "x": 3.8993637811598734, + "y": 2.5128672365819, + "heading": -0.21139907897320392, + "angularVelocity": 0.38887842305553666, + "velocityX": 2.3489920073377766, + "velocityY": -1.3089710985213812, + "timestamp": 1.532887686846847 + }, + { + "x": 4.078858787892925, + "y": 2.4126784448405125, + "heading": -0.18019759590707388, + "angularVelocity": 0.4070941835316264, + "velocityX": 2.34191987153701, + "velocityY": -1.30719024754481, + "timestamp": 1.6095320711891894 + }, + { + "x": 4.257707770811751, + "y": 2.312664186467197, + "heading": -0.1472705722503286, + "angularVelocity": 0.42960777804245054, + "velocityX": 2.3334910242082882, + "velocityY": -1.3049130635140744, + "timestamp": 1.6861764555315317 + }, + { + "x": 4.435756686256742, + "y": 2.212883689420889, + "heading": -0.1121258503357887, + "angularVelocity": 0.4585426866704457, + "velocityX": 2.323052327613603, + "velocityY": -1.3018631162933536, + "timestamp": 1.762820839873874 + }, + { + "x": 4.612756399460092, + "y": 2.113437617489987, + "heading": -0.07396027426521577, + "angularVelocity": 0.4979565874011239, + "velocityX": 2.3093631023605954, + "velocityY": -1.2974997814153513, + "timestamp": 1.8394652242162164 + }, + { + "x": 4.788243072222853, + "y": 2.014522987095371, + "heading": -0.031264185221419126, + "angularVelocity": 0.5570674147904805, + "velocityX": 2.289622054747365, + "velocityY": -1.290565920039225, + "timestamp": 1.9161096085585587 + }, + { + "x": 4.961081026742291, + "y": 1.9166475782485084, + "heading": 0.019697120176888277, + "angularVelocity": 0.664905926710585, + "velocityX": 2.255063511860639, + "velocityY": -1.2770069155972317, + "timestamp": 1.992753992900901 + }, + { + "x": 5.123264328610966, + "y": 1.823526252461316, + "heading": 0.10546513381285838, + "angularVelocity": 1.1190384575714751, + "velocityX": 2.1160493787028227, + "velocityY": -1.214979108857517, + "timestamp": 2.0693983772432434 + }, + { + "x": 5.295119103926639, + "y": 1.7237778734191072, + "heading": 0.15590676979164073, + "angularVelocity": 0.6581256593239505, + "velocityX": 2.24223570702925, + "velocityY": -1.3014440640121712, + "timestamp": 2.1460427615855857 + }, + { + "x": 5.493802683128641, + "y": 1.6448426706793149, + "heading": 0.16075963684913044, + "angularVelocity": 0.06331666826122177, + "velocityX": 2.5922783633378295, + "velocityY": -1.029888926854938, + "timestamp": 2.222687145927928 + }, + { + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "angularVelocity": 0.000040076685010459537, + "velocityX": 2.7882659509385723, + "velocityY": -0.3502449569153927, + "timestamp": 2.2993315302702704 + }, + { + "x": 5.878932815955422, + "y": 1.6310716458031873, + "heading": 0.15207722794791115, + "angularVelocity": -0.13999820175733138, + "velocityX": 2.7631425083845143, + "velocityY": 0.21072366345901047, + "timestamp": 2.3613714739627154 + }, + { + "x": 6.041874898509352, + "y": 1.6781214038756682, + "heading": 0.13576601260266036, + "angularVelocity": -0.2629147348377915, + "velocityX": 2.6264060354679595, + "velocityY": 0.7583784779967588, + "timestamp": 2.4234114176551604 + }, + { + "x": 6.190063655455228, + "y": 1.7574804032751512, + "heading": 0.11619159565916007, + "angularVelocity": -0.3155131319998917, + "velocityX": 2.388602376567293, + "velocityY": 1.2791597586370276, + "timestamp": 2.4854513613476055 + }, + { + "x": 6.338186422130831, + "y": 1.840598011238034, + "heading": 0.10221625072518932, + "angularVelocity": -0.22526366244385518, + "velocityX": 2.387538702644562, + "velocityY": 1.3397434461727966, + "timestamp": 2.5474913050400505 + }, + { + "x": 6.486157900665009, + "y": 1.9236682945064507, + "heading": 0.08780760406345454, + "angularVelocity": -0.2322479003714719, + "velocityX": 2.3851001423812606, + "velocityY": 1.338980636091913, + "timestamp": 2.6095312487324955 + }, + { + "x": 6.633958405719426, + "y": 2.0066826604418155, + "heading": 0.07290105260244177, + "angularVelocity": -0.2402734524536335, + "velocityX": 2.382344281083177, + "velocityY": 1.3380793242962596, + "timestamp": 2.6715711924249406 + }, + { + "x": 6.781563534615612, + "y": 2.089630624589182, + "heading": 0.0574168439817537, + "angularVelocity": -0.24958450474179966, + "velocityX": 2.3791950816060985, + "velocityY": 1.33700901726427, + "timestamp": 2.7336111361173856 + }, + { + "x": 6.928941267710664, + "y": 2.172498140132396, + "heading": 0.04124990446967813, + "angularVelocity": -0.2605892035012319, + "velocityX": 2.3755297687834913, + "velocityY": 1.3357122945504047, + "timestamp": 2.7956510798098306 + }, + { + "x": 7.076047973733186, + "y": 2.255265656701969, + "heading": 0.024256449523087663, + "angularVelocity": -0.27391151466599145, + "velocityX": 2.3711611788653904, + "velocityY": 1.334100446317001, + "timestamp": 2.8576910235022757 + }, + { + "x": 7.222828783744423, + "y": 2.337909309225835, + "heading": 0.006256947367936294, + "angularVelocity": -0.2901276352599791, + "velocityX": 2.3659081758501124, + "velocityY": 1.3321039253929894, + "timestamp": 2.9197309671947207 + }, + { + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 6.712344331303172e-23, + "angularVelocity": -0.10085353073423609, + "velocityX": 2.608965463855139, + "velocityY": 0.9550610879656509, + "timestamp": 2.9817709108871657 + }, + { + "x": 7.581926675665867, + "y": 2.420671247090372, + "heading": -6.833387833600707e-9, + "angularVelocity": -9.667586454477017e-8, + "velocityX": 2.7904368043745778, + "velocityY": 0.33260950489354324, + "timestamp": 3.052454404288316 + }, + { + "x": 7.7805365921893905, + "y": 2.417576114646826, + "heading": -6.833380269760843e-9, + "angularVelocity": 1.0700998918582695e-13, + "velocityX": 2.8098486218890097, + "velocityY": -0.043788617322290876, + "timestamp": 3.1231378976894666 + }, + { + "x": 7.9655574312129644, + "y": 2.4146925691677037, + "heading": -4.100028695417927e-9, + "angularVelocity": 3.8670295465311964e-8, + "velocityX": 2.6175961334214803, + "velocityY": -0.0407951749463946, + "timestamp": 3.193821391090617 + }, + { + "x": 8.104323073360915, + "y": 2.412529909857503, + "heading": -2.050014465682545e-9, + "angularVelocity": 2.9002729365693992e-8, + "velocityX": 1.9631972822906998, + "velocityY": -0.03059638405147332, + "timestamp": 3.2645048844917675 + }, + { + "x": 8.196833505192958, + "y": 2.411088136925815, + "heading": -6.833381783003922e-10, + "angularVelocity": 1.933515480942414e-8, + "velocityX": 1.3087982410125008, + "velocityY": -0.020397590191323155, + "timestamp": 3.335188377892918 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.830282426459157e-24, + "angularVelocity": 9.667577894349971e-9, + "velocityX": 0.6543991363518575, + "velocityY": -0.010198795342763814, + "timestamp": 3.4058718712940683 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.851361909343925e-24, + "angularVelocity": 2.3995205522592844e-25, + "velocityX": -1.4269511777904615e-24, + "velocityY": -1.1273229249768833e-23, + "timestamp": 3.4765553646952188 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.2.traj b/src/main/deploy/choreo/3PieceSourceSide.2.traj new file mode 100644 index 0000000..208c091 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSide.2.traj @@ -0,0 +1,329 @@ +{ + "samples": [ + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.851361909343925e-24, + "angularVelocity": 2.3995205522592844e-25, + "velocityX": -1.4269511777904615e-24, + "velocityY": -1.1273229249768833e-23, + "timestamp": 0 + }, + { + "x": 8.183672767375366, + "y": 2.3891879079868126, + "heading": -0.016263075453039278, + "angularVelocity": -0.1934255260988222, + "velocityX": -0.7066659906617425, + "velocityY": -0.251897340619777, + "timestamp": 0.08407926182778169 + }, + { + "x": 8.064809267825755, + "y": 2.3467183743812834, + "heading": -0.04860613329336098, + "angularVelocity": -0.38467342763509904, + "velocityX": -1.413707696352949, + "velocityY": -0.5051130645333048, + "timestamp": 0.16815852365556339 + }, + { + "x": 7.886434927132461, + "y": 2.2827439256274595, + "heading": -0.09665252446442359, + "angularVelocity": -0.5714416388368797, + "velocityX": -2.121502220828929, + "velocityY": -0.7608826167487317, + "timestamp": 0.2522377854833451 + }, + { + "x": 7.663093479676549, + "y": 2.2056330475266677, + "heading": -0.09665343057468777, + "angularVelocity": -0.000010776857984756087, + "velocityX": -2.6563202697162027, + "velocityY": -0.9171212546886659, + "timestamp": 0.33631704731112677 + }, + { + "x": 7.439949308521917, + "y": 2.127952816871332, + "heading": -0.09665400996983407, + "angularVelocity": -0.000006891058909294993, + "velocityX": -2.653973956285386, + "velocityY": -0.9238928716387514, + "timestamp": 0.42039630913890846 + }, + { + "x": 7.216805138002767, + "y": 2.0502725843903784, + "heading": -0.0966545893647738, + "angularVelocity": -0.0000068910564521426285, + "velocityX": -2.6539739487272445, + "velocityY": -0.9238928933518099, + "timestamp": 0.5044755709666902 + }, + { + "x": 6.9936609674836125, + "y": 1.9725923519093191, + "heading": -0.09665516875951666, + "angularVelocity": -0.000006891054111144413, + "velocityX": -2.6539739487273097, + "velocityY": -0.923892893353071, + "timestamp": 0.5885548327944714 + }, + { + "x": 6.77051679696445, + "y": 1.8949121194281597, + "heading": -0.09665574815406265, + "angularVelocity": -0.000006891051769457115, + "velocityX": -2.653973948727398, + "velocityY": -0.9238928933542627, + "timestamp": 0.6726340946222531 + }, + { + "x": 6.547372626445282, + "y": 1.8172318869468993, + "heading": -0.09665632754841177, + "angularVelocity": -0.000006891049427961194, + "velocityX": -2.653973948727487, + "velocityY": -0.9238928933554545, + "timestamp": 0.7567133564500348 + }, + { + "x": 6.324228455926046, + "y": 1.7395516544657041, + "heading": -0.09665690694256396, + "angularVelocity": -0.000006891047085669869, + "velocityX": -2.653973948728258, + "velocityY": -0.9238928933546852, + "timestamp": 0.8407926182778165 + }, + { + "x": 6.101084267512398, + "y": 1.6618714733923579, + "heading": -0.0966574863411208, + "angularVelocity": -0.000006891099472478973, + "velocityX": -2.65397416155617, + "velocityY": -0.9238922819334152, + "timestamp": 0.9248718801055982 + }, + { + "x": 5.881706688567194, + "y": 1.6001619743925184, + "heading": -0.12280606064162562, + "angularVelocity": -0.3109990945694144, + "velocityX": -2.609175844033366, + "velocityY": -0.7339443479682077, + "timestamp": 1.0089511419333799 + }, + { + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.19572183715761898, + "angularVelocity": -0.8672266493650448, + "velocityX": -2.5430419926070895, + "velocityY": -0.10199734429174848, + "timestamp": 1.0930304037611616 + }, + { + "x": 5.472836165647801, + "y": 1.6405608510115604, + "heading": -0.2695567389374151, + "angularVelocity": -0.9372981580360532, + "velocityX": -2.4761083941768933, + "velocityY": 0.6217104735625232, + "timestamp": 1.1718045922589861 + }, + { + "x": 5.3021481158717245, + "y": 1.712801388975876, + "heading": -0.4003704259742984, + "angularVelocity": -1.660616117175175, + "velocityX": -2.1668017536072774, + "velocityY": 0.9170584850431085, + "timestamp": 1.2505787807568107 + }, + { + "x": 5.1196004435735345, + "y": 1.8042197974618441, + "heading": -0.46428021863316127, + "angularVelocity": -0.8113037262278816, + "velocityX": -2.317353891918915, + "velocityY": 1.160512221442844, + "timestamp": 1.3293529692546353 + }, + { + "x": 4.929096458765624, + "y": 1.904953408569058, + "heading": -0.48639976153317277, + "angularVelocity": -0.28079683614414136, + "velocityX": -2.4183554085507573, + "velocityY": 1.2787641869518616, + "timestamp": 1.40812715775246 + }, + { + "x": 4.737761672165951, + "y": 2.009314965020544, + "heading": -0.49933816069591924, + "angularVelocity": -0.1642466829487382, + "velocityX": -2.4289020331190754, + "velocityY": 1.3248191881325238, + "timestamp": 1.4869013462502845 + }, + { + "x": 4.546911439579052, + "y": 2.114853649108329, + "heading": -0.5116829622012672, + "angularVelocity": -0.15671124946833295, + "velocityX": -2.4227508556583697, + "velocityY": 1.3397622508126334, + "timestamp": 1.565675534748109 + }, + { + "x": 4.356102309238819, + "y": 2.2203468351822107, + "heading": -0.524131682346679, + "angularVelocity": -0.15803044604839828, + "velocityX": -2.4222290826328545, + "velocityY": 1.3391846756605428, + "timestamp": 1.6444497232459336 + }, + { + "x": 4.165335431709755, + "y": 2.3257928672028427, + "heading": -0.5366900663313149, + "angularVelocity": -0.15942257513681343, + "velocityX": -2.421692703750687, + "velocityY": 1.338586077894589, + "timestamp": 1.7232239117437582 + }, + { + "x": 3.9746120593595244, + "y": 2.4311903935174577, + "heading": -0.5493635573249664, + "angularVelocity": -0.1608838026176728, + "velocityX": -2.421140426670299, + "velocityY": 1.3379703215543044, + "timestamp": 1.8019981002415828 + }, + { + "x": 3.783933515355731, + "y": 2.5365380161026203, + "heading": -0.5621578157899224, + "angularVelocity": -0.16241688691352676, + "velocityX": -2.420571352620895, + "velocityY": 1.337336817986659, + "timestamp": 1.8807722887394074 + }, + { + "x": 3.593301216494676, + "y": 2.6418342523116722, + "heading": -0.5750788548667545, + "angularVelocity": -0.16402630510359092, + "velocityX": -2.4199842930317086, + "velocityY": 1.3366844929409736, + "timestamp": 1.959546477237232 + }, + { + "x": 3.4027166923911927, + "y": 2.7470775186047, + "heading": -0.5881331219292356, + "angularVelocity": -0.16571756956711148, + "velocityX": -2.41937781572635, + "velocityY": 1.3360120656264767, + "timestamp": 2.0383206657350565 + }, + { + "x": 3.212181603664246, + "y": 2.8522661171153323, + "heading": -0.601327569493579, + "angularVelocity": -0.16749709284161973, + "velocityX": -2.4187502576711153, + "velocityY": 1.3353180847243658, + "timestamp": 2.117094854232881 + }, + { + "x": 3.0216977610811666, + "y": 2.9573982215901036, + "heading": -0.6146697280062193, + "angularVelocity": -0.1693722114701131, + "velocityX": -2.418099712805536, + "velocityY": 1.3346009204229918, + "timestamp": 2.1958690427307057 + }, + { + "x": 2.831267146879349, + "y": 3.0624718615565794, + "heading": -0.6281677862205469, + "angularVelocity": -0.17135128233914515, + "velocityX": -2.417424004400561, + "velocityY": 1.3338587419326686, + "timestamp": 2.2746432312285303 + }, + { + "x": 2.640891939244419, + "y": 3.167484904027678, + "heading": -0.6418306831098634, + "angularVelocity": -0.17344382912549475, + "velocityX": -2.416720644988769, + "velocityY": 1.3330894862090363, + "timestamp": 2.353417419726355 + }, + { + "x": 2.4505746379296203, + "y": 3.272435032259823, + "heading": -0.655668504824747, + "angularVelocity": -0.17566441468661878, + "velocityX": -2.4159855524256404, + "velocityY": 1.3322908205527566, + "timestamp": 2.4321916082241795 + }, + { + "x": 2.2618233887297388, + "y": 3.3772509809337916, + "heading": -0.674261275863946, + "angularVelocity": -0.2360261831159634, + "velocityX": -2.396105282697931, + "velocityY": 1.3305874763389332, + "timestamp": 2.510965796722004 + }, + { + "x": 2.116002537558778, + "y": 3.455078745186431, + "heading": -0.7196337030492912, + "angularVelocity": -0.5759808898139023, + "velocityX": -1.8511247649982117, + "velocityY": 0.9879856046347999, + "timestamp": 2.5897399852198286 + }, + { + "x": 2.018778748901899, + "y": 3.5069532190081367, + "heading": -0.7498768931574519, + "angularVelocity": -0.3839225853655822, + "velocityX": -1.234208698443955, + "velocityY": 0.6585212086715196, + "timestamp": 2.668514173717653 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -0.19198048410191668, + "velocityX": -0.617100471546741, + "velocityY": 0.32925585007098457, + "timestamp": 2.747288362215478 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -6.585493827087804e-24, + "velocityX": 2.2425722891132833e-25, + "velocityY": -1.917344763102387e-25, + "timestamp": 2.8260625507133024 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.3.traj b/src/main/deploy/choreo/3PieceSourceSide.3.traj new file mode 100644 index 0000000..c6431f7 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSide.3.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -6.585493827087804e-24, + "velocityX": 2.2425722891132833e-25, + "velocityY": -1.917344763102387e-25, + "timestamp": 0 + }, + { + "x": 2.021866320736439, + "y": 3.5175409291347064, + "heading": -0.7649999986292967, + "angularVelocity": 1.7960508607271394e-8, + "velocityX": 0.6774210494904483, + "velocityY": -0.20112200466940705, + "timestamp": 0.07631761773736923 + }, + { + "x": 2.1252646329115827, + "y": 3.486842627325834, + "heading": -0.7649999949328548, + "angularVelocity": 4.8434973827775214e-8, + "velocityX": 1.354841978047161, + "velocityY": -0.4022439735280295, + "timestamp": 0.15263523547473845 + }, + { + "x": 2.264271046839328, + "y": 3.4455725064565113, + "heading": -0.7629134147129198, + "angularVelocity": 0.027340740995291143, + "velocityX": 1.8214197199669733, + "velocityY": -0.5407679392109075, + "timestamp": 0.22895285321210768 + }, + { + "x": 2.403277460756995, + "y": 3.404302385553228, + "heading": -0.7557796152547765, + "angularVelocity": 0.09347513286765456, + "velocityX": 1.8214197198349207, + "velocityY": -0.5407679396558968, + "timestamp": 0.3052704709494769 + }, + { + "x": 2.5422838746752925, + "y": 3.3630322646520687, + "heading": -0.7445970322006652, + "angularVelocity": 0.1465268883600871, + "velocityX": 1.8214197198431827, + "velocityY": -0.5407679396280685, + "timestamp": 0.3815880886868461 + }, + { + "x": 2.6812902885942673, + "y": 3.321762143753191, + "heading": -0.7301562502874436, + "angularVelocity": 0.1892195058147188, + "velocityX": 1.821419719852059, + "velocityY": -0.540767939598171, + "timestamp": 0.45790570642421535 + }, + { + "x": 2.8202967025138705, + "y": 3.28049202285643, + "heading": -0.7130778427136069, + "angularVelocity": 0.2237806692631405, + "velocityX": 1.8214197198602944, + "velocityY": -0.5407679395704321, + "timestamp": 0.5342233241615846 + }, + { + "x": 2.9593031164340085, + "y": 3.2392219019614696, + "heading": -0.6938417314556549, + "angularVelocity": 0.25205335056643224, + "velocityX": 1.8214197198673001, + "velocityY": -0.5407679395468353, + "timestamp": 0.6105409418989538 + }, + { + "x": 3.0983095303545722, + "y": 3.1979517810679434, + "heading": -0.6728120786629418, + "angularVelocity": 0.27555436629432156, + "velocityX": 1.8214197198728768, + "velocityY": -0.5407679395280517, + "timestamp": 0.686858559636323 + }, + { + "x": 3.2373159442754544, + "y": 3.1566816601754897, + "heading": -0.6502538625008166, + "angularVelocity": 0.2955833375165695, + "velocityX": 1.8214197198770503, + "velocityY": -0.540767939513994, + "timestamp": 0.7631761773736923 + }, + { + "x": 3.376322358196558, + "y": 3.1154115392837802, + "heading": -0.6263485003282439, + "angularVelocity": 0.31323517270727913, + "velocityX": 1.8214197198799456, + "velocityY": -0.5407679395042426, + "timestamp": 0.8394937951110615 + }, + { + "x": 3.515328772117798, + "y": 3.074141418392532, + "heading": -0.6012000869662939, + "angularVelocity": 0.3295230394702948, + "velocityX": 1.821419719881741, + "velocityY": -0.5407679394981955, + "timestamp": 0.9158114128484307 + }, + { + "x": 3.6543351862262687, + "y": 3.032871298131916, + "heading": -0.5748442396833396, + "angularVelocity": 0.3453442083799368, + "velocityX": 1.821419722335047, + "velocityY": -0.5407679312349462, + "timestamp": 0.9921290305857999 + }, + { + "x": 3.7934386683270223, + "y": 2.9919295436941926, + "heading": -0.5472452836552583, + "angularVelocity": 0.3616328293036799, + "velocityX": 1.8226916172809426, + "velocityY": -0.5364653097351076, + "timestamp": 1.0684466483231692 + }, + { + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": -0.5196460913813736, + "angularVelocity": 0.361635924864182, + "velocityX": 1.8926815517726128, + "velocityY": 0.16660126698243774, + "timestamp": 1.1447642660605384 + }, + { + "x": 4.075926460541626, + "y": 3.0786892532968806, + "heading": -0.4898305952486719, + "angularVelocity": 0.3616359432061461, + "velocityX": 1.674339217752807, + "velocityY": 0.8981024049221815, + "timestamp": 1.2272104296480606 + }, + { + "x": 4.180404466119937, + "y": 3.195406219293214, + "heading": -0.4598310277403017, + "angularVelocity": 0.3638685683236616, + "velocityX": 1.2672270125389096, + "velocityY": 1.4156749194573495, + "timestamp": 1.3096565932355828 + }, + { + "x": 4.2848822354372995, + "y": 3.3121233967757027, + "heading": -0.4287713690600015, + "angularVelocity": 0.37672654892338603, + "velocityX": 1.2672241468998562, + "velocityY": 1.4156774845998201, + "timestamp": 1.392102756823105 + }, + { + "x": 4.389360004752337, + "y": 3.428840574260275, + "heading": -0.39697164385324546, + "angularVelocity": 0.38570290991161005, + "velocityX": 1.2672241468716323, + "velocityY": 1.415677484625084, + "timestamp": 1.4745489204106272 + }, + { + "x": 4.493837774065624, + "y": 3.545557751746413, + "heading": -0.36470801458445545, + "angularVelocity": 0.3913296612587668, + "velocityX": 1.2672241468504064, + "velocityY": 1.415677484644084, + "timestamp": 1.5569950839981495 + }, + { + "x": 4.598315543377444, + "y": 3.662274929233865, + "heading": -0.3322260766024941, + "angularVelocity": 0.3939775553956429, + "velocityX": 1.267224146832605, + "velocityY": 1.4156774846600186, + "timestamp": 1.6394412475856717 + }, + { + "x": 4.7027933126881845, + "y": 3.778992106722283, + "heading": -0.29975459263588233, + "angularVelocity": 0.3938507573143906, + "velocityX": 1.2672241468195173, + "velocityY": 1.415677484671734, + "timestamp": 1.7218874111731948 + }, + { + "x": 4.807271113642882, + "y": 3.8957092558850426, + "heading": -0.26751670065443317, + "angularVelocity": 0.3910174904284827, + "velocityX": 1.2672245306330898, + "velocityY": 1.4156771411062206, + "timestamp": 1.8043335747607179 + }, + { + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": -0.23561923052699993, + "angularVelocity": 0.386888470481369, + "velocityX": 1.4685230274935492, + "velocityY": 1.205586850387367, + "timestamp": 1.886779738348241 + }, + { + "x": 5.0976675546813945, + "y": 4.035971247104969, + "heading": -0.20015091281875375, + "angularVelocity": 0.38688845257830395, + "velocityX": 1.846968413139047, + "velocityY": 0.4457661652272064, + "timestamp": 1.9784555624052222 + }, + { + "x": 5.270991671667468, + "y": 4.053258165584531, + "heading": -0.16565527924639437, + "angularVelocity": 0.37627841284435726, + "velocityX": 1.890619678295383, + "velocityY": 0.18856572774100455, + "timestamp": 2.0701313864622035 + }, + { + "x": 5.444315791533781, + "y": 4.0705450551858755, + "heading": -0.13259854065392815, + "angularVelocity": 0.3605829446585594, + "velocityX": 1.8906197097130382, + "velocityY": 0.1885654127373893, + "timestamp": 2.161807210519185 + }, + { + "x": 5.617639911400379, + "y": 4.087831944784366, + "heading": -0.10138296753519387, + "angularVelocity": 0.34049950943808543, + "velocityX": 1.8906197097161441, + "velocityY": 0.18856541270624588, + "timestamp": 2.253483034576166 + }, + { + "x": 5.790964031267238, + "y": 4.105118834380232, + "heading": -0.07255368127548635, + "angularVelocity": 0.3144698894856807, + "velocityX": 1.8906197097189994, + "velocityY": 0.1885654126776188, + "timestamp": 2.3451588586331473 + }, + { + "x": 5.9642881511343315, + "y": 4.122405723973758, + "heading": -0.046846868579348507, + "angularVelocity": 0.2804099440672579, + "velocityX": 1.890619709721544, + "velocityY": 0.18856541265210713, + "timestamp": 2.4368346826901286 + }, + { + "x": 6.137612271001621, + "y": 4.139692613565316, + "heading": -0.02524481611998314, + "angularVelocity": 0.23563521442620056, + "velocityX": 1.890619709723685, + "velocityY": 0.18856541263063997, + "timestamp": 2.52851050674711 + }, + { + "x": 6.310936390869065, + "y": 4.156979503155326, + "heading": -0.009053820761373118, + "angularVelocity": 0.17661139700851253, + "velocityX": 1.8906197097253696, + "velocityY": 0.18856541261374912, + "timestamp": 2.620186330804091 + }, + { + "x": 6.484260512045991, + "y": 4.174266379616022, + "heading": -2.2378824393561327e-8, + "angularVelocity": 0.09875884373748682, + "velocityX": 1.890619724009207, + "velocityY": 0.188565269399175, + "timestamp": 2.7118621548610724 + }, + { + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": 1.903926994866919e-23, + "angularVelocity": 2.4410824362649644e-7, + "velocityX": 1.8989734472164426, + "velocityY": 0.06244684149438171, + "timestamp": 2.8035379789180537 + }, + { + "x": 6.892610562082988, + "y": 4.180747179242153, + "heading": 1.815110943876254e-8, + "angularVelocity": 2.0923543279309324e-7, + "velocityX": 2.7004141208851267, + "velocityY": 0.008713967176889024, + "timestamp": 2.8902876713409027 + }, + { + "x": 7.135847606603004, + "y": 4.164439410363542, + "heading": 1.8151086995989633e-8, + "angularVelocity": -2.587072333894115e-13, + "velocityX": 2.803895180796635, + "velocityY": -0.18798647491591486, + "timestamp": 2.9770373637637517 + }, + { + "x": 7.379084647879251, + "y": 4.14813159310281, + "heading": 1.8151064626797785e-8, + "angularVelocity": -2.5785903361398285e-13, + "velocityX": 2.803895143404351, + "velocityY": -0.18798703263687086, + "timestamp": 3.0637870561866007 + }, + { + "x": 7.6223216891554895, + "y": 4.131823775841945, + "heading": 1.8151042187582598e-8, + "angularVelocity": -2.586662220352974e-13, + "velocityX": 2.8038951434042487, + "velocityY": -0.18798703263839134, + "timestamp": 3.1505367486094498 + }, + { + "x": 7.865558730431713, + "y": 4.115515958581081, + "heading": 1.815101972750476e-8, + "angularVelocity": -2.589067144937878e-13, + "velocityX": 2.803895143404087, + "velocityY": -0.18798703263838049, + "timestamp": 3.237286441032299 + }, + { + "x": 8.07413296336415, + "y": 4.101532107856125, + "heading": 9.075510567318239e-9, + "angularVelocity": -1.0461719121664748e-7, + "velocityX": 2.404322448957763, + "velocityY": -0.16119769804825285, + "timestamp": 3.324036133455148 + }, + { + "x": 8.213182462520122, + "y": 4.092209539999878, + "heading": 3.025170279496359e-9, + "angularVelocity": -6.974480391619583e-8, + "velocityX": 1.6028817540723412, + "velocityY": -0.10746514017369478, + "timestamp": 3.410785825877997 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -2.730279460976851e-23, + "angularVelocity": -3.487240352104816e-8, + "velocityX": 0.8014409030577104, + "velocityY": -0.05373257183146045, + "timestamp": 3.497535518300846 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -1.3454200564607368e-23, + "angularVelocity": 4.663099853298001e-24, + "velocityX": 1.7036719365270032e-27, + "velocityY": 2.800838691430789e-26, + "timestamp": 3.584285210723695 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.4.traj b/src/main/deploy/choreo/3PieceSourceSide.4.traj new file mode 100644 index 0000000..726d6c5 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSide.4.traj @@ -0,0 +1,365 @@ +{ + "samples": [ + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -1.3454200564607368e-23, + "angularVelocity": 4.663099853298001e-24, + "velocityX": 1.7036719365270032e-27, + "velocityY": 2.800838691430789e-26, + "timestamp": 0 + }, + { + "x": 8.214708333565108, + "y": 4.098060716631318, + "heading": 1.6847207046591105e-8, + "angularVelocity": 1.954345432230813e-7, + "velocityX": -0.7888150344560528, + "velocityY": 0.12194887565072522, + "timestamp": 0.08620383463818193 + }, + { + "x": 8.078710576708064, + "y": 4.119085637322864, + "heading": 5.0541618776805506e-8, + "angularVelocity": 3.9086905903476216e-7, + "velocityX": -1.577630014115494, + "velocityY": 0.24389774283002824, + "timestamp": 0.17240766927636386 + }, + { + "x": 7.8747139579553656, + "y": 4.15062301580424, + "heading": 1.0108322622636101e-7, + "angularVelocity": 5.863034708570761e-7, + "velocityX": -2.3664448293851432, + "velocityY": 0.3658465845950713, + "timestamp": 0.2586115039145458 + }, + { + "x": 7.635308863013457, + "y": 4.1876344581296925, + "heading": 1.0108335901717796e-7, + "angularVelocity": 1.540428189705246e-12, + "velocityX": -2.7771977423829006, + "velocityY": 0.42934798064143237, + "timestamp": 0.3448153385527277 + }, + { + "x": 7.395903768071533, + "y": 4.224645900455147, + "heading": 1.0108349186566566e-7, + "angularVelocity": 1.5410971948056658e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 0.43101917319090965 + }, + { + "x": 7.156498673129608, + "y": 4.261657342780601, + "heading": 1.0108362465293872e-7, + "angularVelocity": 1.5403870787070826e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 0.5172230078290916 + }, + { + "x": 6.917093578187684, + "y": 4.298668785106054, + "heading": 1.0108375751155647e-7, + "angularVelocity": 1.5412147075799493e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414625, + "timestamp": 0.6034268424672735 + }, + { + "x": 6.677688483245759, + "y": 4.335680227431509, + "heading": 1.0108389035423936e-7, + "angularVelocity": 1.5410298558530182e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414627, + "timestamp": 0.6896306771054554 + }, + { + "x": 6.438283388303835, + "y": 4.372691669756963, + "heading": 1.0108402320108348e-7, + "angularVelocity": 1.5410781280848912e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.4293479806414625, + "timestamp": 0.7758345117436374 + }, + { + "x": 6.1988782933619095, + "y": 4.409703112082417, + "heading": 1.0108415606074574e-7, + "angularVelocity": 1.5412268238689798e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 0.8620383463818193 + }, + { + "x": 5.959473198419985, + "y": 4.446714554407871, + "heading": 1.0108428894851187e-7, + "angularVelocity": 1.5415528401754331e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.42934798064146257, + "timestamp": 0.9482421810200012 + }, + { + "x": 5.72006810347805, + "y": 4.483725996733255, + "heading": 1.0108442171706879e-7, + "angularVelocity": 1.5401699643641334e-12, + "velocityX": -2.7771977423832217, + "velocityY": 0.4293479806406499, + "timestamp": 1.0344460156581832 + }, + { + "x": 5.480663004845917, + "y": 4.520737415188933, + "heading": 1.0108455459674728e-7, + "angularVelocity": 1.5414590205195092e-12, + "velocityX": -2.7771977851910266, + "velocityY": 0.4293477037422273, + "timestamp": 1.120649850296365 + }, + { + "x": 5.240152409397331, + "y": 4.549708085687679, + "heading": 1.0108468832096651e-7, + "angularVelocity": 1.5512560408121339e-12, + "velocityX": -2.790022003755009, + "velocityY": 0.3360717144468437, + "timestamp": 1.206853684934547 + }, + { + "x": 5.001198008888612, + "y": 4.509890511709968, + "heading": 1.0108464169686405e-7, + "angularVelocity": -5.408587989447552e-13, + "velocityX": -2.7719695012602092, + "velocityY": -0.46190026400605494, + "timestamp": 1.293057519572729 + }, + { + "x": 4.783077239990234, + "y": 4.404496192932129, + "heading": 4.371418464571693e-25, + "angularVelocity": -0.0000011726234931559648, + "velocityX": -2.5302907905881877, + "velocityY": -1.2226175229930707, + "timestamp": 1.379261354210911 + }, + { + "x": 4.627041500035927, + "y": 4.275874365802291, + "heading": -0.00461711894400582, + "angularVelocity": -0.06368383973540494, + "velocityX": -2.152198194752189, + "velocityY": -1.774078581199584, + "timestamp": 1.4517619911572552 + }, + { + "x": 4.5068214168723975, + "y": 4.114451023957695, + "heading": -0.01240880783073842, + "angularVelocity": -0.10747062667185843, + "velocityX": -1.6581934756311134, + "velocityY": -2.226509292105373, + "timestamp": 1.5242626281035996 + }, + { + "x": 4.429140519397286, + "y": 3.930818027867112, + "heading": -0.027772737926692166, + "angularVelocity": -0.21191441541850167, + "velocityX": -1.0714512416297768, + "velocityY": -2.53284665935394, + "timestamp": 1.596763265049944 + }, + { + "x": 4.383091867034577, + "y": 3.7471675526312227, + "heading": -0.08613270246137088, + "angularVelocity": -0.8049579561331118, + "velocityX": -0.635148245618701, + "velocityY": -2.5330877488952703, + "timestamp": 1.6692639019962883 + }, + { + "x": 4.346524489213487, + "y": 3.5742818254238546, + "heading": -0.20843940130739744, + "angularVelocity": -1.6869741287451163, + "velocityX": -0.5043731939645176, + "velocityY": -2.3846097701915894, + "timestamp": 1.7417645389426326 + }, + { + "x": 4.289423143132127, + "y": 3.4062373324265396, + "heading": -0.32755846162196556, + "angularVelocity": -1.643007086995987, + "velocityX": -0.7875978541211613, + "velocityY": -2.317834712565055, + "timestamp": 1.814265175888977 + }, + { + "x": 4.203799594769533, + "y": 3.2403627849834162, + "heading": -0.40165980770192133, + "angularVelocity": -1.022078552700102, + "velocityX": -1.1810040845014969, + "velocityY": -2.28790469200817, + "timestamp": 1.8867658128353213 + }, + { + "x": 4.079421660594405, + "y": 3.103563620218193, + "heading": -0.4716628621936354, + "angularVelocity": -0.9655508894841919, + "velocityX": -1.7155426409174284, + "velocityY": -1.8868684542242327, + "timestamp": 1.9592664497816656 + }, + { + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": -0.5385444180262977, + "angularVelocity": -0.9224961138225419, + "velocityX": -2.134381940831591, + "velocityY": -1.3643944230321734, + "timestamp": 2.03176708672801 + }, + { + "x": 3.7636444729485263, + "y": 2.9505820220343484, + "heading": -0.5968694156691691, + "angularVelocity": -0.8693980375808563, + "velocityX": -2.40037547092122, + "velocityY": -0.8058553732360577, + "timestamp": 2.0988537318798244 + }, + { + "x": 3.5921162225925003, + "y": 2.93585850684271, + "heading": -0.6464965554196345, + "angularVelocity": -0.7397469293353471, + "velocityX": -2.556816635678569, + "velocityY": -0.21947013684048228, + "timestamp": 2.1659403770316388 + }, + { + "x": 3.416479572667743, + "y": 2.9596548551854616, + "heading": -0.6807775343446694, + "angularVelocity": -0.5109955766525223, + "velocityX": -2.6180568357129337, + "velocityY": 0.3547106624410992, + "timestamp": 2.233027022183453 + }, + { + "x": 3.2390028785000764, + "y": 3.015476958858974, + "heading": -0.6885344713310354, + "angularVelocity": -0.115625650512295, + "velocityX": -2.645484712583867, + "velocityY": 0.8320896587866112, + "timestamp": 2.3001136673352676 + }, + { + "x": 3.0657580475568027, + "y": 3.089829788896707, + "heading": -0.6885346535011025, + "angularVelocity": -0.00000271544458244769, + "velocityX": -2.5824041513959415, + "velocityY": 1.1083104523929412, + "timestamp": 2.367200312487082 + }, + { + "x": 2.8925132745209385, + "y": 3.1641827538610285, + "heading": -0.6885348356707467, + "angularVelocity": -0.000002715438277887047, + "velocityX": -2.582403288222506, + "velocityY": 1.1083124636216772, + "timestamp": 2.4342869576388964 + }, + { + "x": 2.7192685014854168, + "y": 3.2385357188261574, + "heading": -0.6885350178403685, + "angularVelocity": -0.000002715437944094969, + "velocityX": -2.5824032882174053, + "velocityY": 1.108312463633708, + "timestamp": 2.5013736027907107 + }, + { + "x": 2.5460237284499403, + "y": 3.312888683791401, + "heading": -0.6885352000099678, + "angularVelocity": -0.0000027154376105306044, + "velocityX": -2.5824032882167316, + "velocityY": 1.1083124636354227, + "timestamp": 2.568460247942525 + }, + { + "x": 2.3727789637362293, + "y": 3.3872416681385586, + "heading": -0.6885353821903477, + "angularVelocity": -0.0000027155983032171972, + "velocityX": -2.5824031641716974, + "velocityY": 1.108312752544088, + "timestamp": 2.6355468930943395 + }, + { + "x": 2.2223047239417384, + "y": 3.4507934701253355, + "heading": -0.7191097564620745, + "angularVelocity": -0.45574457036177557, + "velocityX": -2.2429835245744076, + "velocityY": 0.9473092870117661, + "timestamp": 2.702633538246154 + }, + { + "x": 2.109444085282166, + "y": 3.4984470664225062, + "heading": -0.7420547518086502, + "angularVelocity": -0.34202031260695603, + "velocityX": -1.6823115599859222, + "velocityY": 0.7103291003646417, + "timestamp": 2.7697201833979683 + }, + { + "x": 2.0342014899368133, + "y": 3.5302130152316624, + "heading": -0.7573535418351998, + "angularVelocity": -0.22804523898800838, + "velocityX": -1.1215733798445486, + "velocityY": 0.47350629528829474, + "timestamp": 2.8368068285497827 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": -0.11397884254752216, + "velocityX": -0.5607948167249468, + "velocityY": 0.23675814542593593, + "timestamp": 2.903893473701597 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": 6.11903975445096e-25, + "velocityX": -4.7665538572669345e-25, + "velocityY": 2.8118183260637723e-25, + "timestamp": 2.9709801188534115 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.traj b/src/main/deploy/choreo/3PieceSourceSide.traj new file mode 100644 index 0000000..60ce6a0 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSide.traj @@ -0,0 +1,1499 @@ +{ + "samples": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": -2.3506527398385264e-24, + "velocityX": -4.584841665957037e-25, + "velocityY": 1.794170584470112e-24, + "timestamp": 0 + }, + { + "x": 0.692835797571452, + "y": 4.365429285724839, + "heading": -1.0363921320591865, + "angularVelocity": 0.19907376380794284, + "velocityX": 0.5901469714218079, + "velocityY": -0.33387419035865423, + "timestamp": 0.07664438434234237 + }, + { + "x": 0.7831334822061629, + "y": 4.314114094234643, + "heading": -1.005707809669664, + "angularVelocity": 0.4003466483919687, + "velocityX": 1.178138299492162, + "velocityY": -0.6695231742092023, + "timestamp": 0.15328876868468475 + }, + { + "x": 0.9181042106922528, + "y": 4.23677088276396, + "heading": -0.9591055566031375, + "angularVelocity": 0.608032192657082, + "velocityX": 1.7609995780411658, + "velocityY": -1.009117786441079, + "timestamp": 0.2299331530270271 + }, + { + "x": 1.0717422840106643, + "y": 4.1370476005177, + "heading": -0.8558256704459198, + "angularVelocity": 1.3475205919314872, + "velocityX": 2.004557471975592, + "velocityY": -1.3011166193315893, + "timestamp": 0.3065775373693695 + }, + { + "x": 1.203873817721518, + "y": 4.032707540283105, + "heading": -0.6892805911828493, + "angularVelocity": 2.172958667384875, + "velocityX": 1.7239558363554812, + "velocityY": -1.3613529696911204, + "timestamp": 0.3832219217117119 + }, + { + "x": 1.3594029637702276, + "y": 3.9301727362790966, + "heading": -0.5828610080840567, + "angularVelocity": 1.388485066609126, + "velocityX": 2.0292308090572955, + "velocityY": -1.3377993036779194, + "timestamp": 0.45986630605405426 + }, + { + "x": 1.5344193266927146, + "y": 3.825053132953821, + "heading": -0.5408815443461281, + "angularVelocity": 0.5477174107162478, + "velocityX": 2.2834857951334486, + "velocityY": -1.371523879110889, + "timestamp": 0.5365106903963967 + }, + { + "x": 1.7162004854447428, + "y": 3.7215786578067394, + "heading": -0.5174827165758584, + "angularVelocity": 0.30529083077705393, + "velocityX": 2.3717479148906513, + "velocityY": -1.3500594470809355, + "timestamp": 0.613155074738739 + }, + { + "x": 1.8995749541613056, + "y": 3.6205751589308552, + "heading": -0.4949062907307691, + "angularVelocity": 0.2945607305585351, + "velocityX": 2.3925362606801817, + "velocityY": -1.317819951749349, + "timestamp": 0.6897994590810813 + }, + { + "x": 2.0827414581868835, + "y": 3.51960952250048, + "heading": -0.4719606385920979, + "angularVelocity": 0.29937812581521006, + "velocityX": 2.389822889142672, + "velocityY": -1.3173259501883174, + "timestamp": 0.7664438434234236 + }, + { + "x": 2.265688686133792, + "y": 3.4186835470349224, + "heading": -0.4486127865936518, + "angularVelocity": 0.3046257361029851, + "velocityX": 2.3869619348724966, + "velocityY": -1.3168084828597342, + "timestamp": 0.843088227765766 + }, + { + "x": 2.4484040933032216, + "y": 3.3177997829423713, + "heading": -0.4248255225210326, + "angularVelocity": 0.3103588642107185, + "velocityX": 2.3839373065260236, + "velocityY": -1.3162577396660924, + "timestamp": 0.9197326121081083 + }, + { + "x": 2.6308732997732713, + "y": 3.216961383200288, + "heading": -0.4005559741960246, + "angularVelocity": 0.3166513572162684, + "velocityX": 2.380725059451553, + "velocityY": -1.3156658587232646, + "timestamp": 0.9963769964504506 + }, + { + "x": 2.813079628281676, + "y": 3.116172257902354, + "heading": -0.3757541558021446, + "angularVelocity": 0.32359602868097065, + "velocityX": 2.377295219628298, + "velocityY": -1.3150229617312381, + "timestamp": 1.073021380792793 + }, + { + "x": 2.9950035114027176, + "y": 3.015437295026463, + "heading": -0.3503610814650098, + "angularVelocity": 0.3313103047930201, + "velocityX": 2.3736100783125105, + "velocityY": -1.314316289970363, + "timestamp": 1.1496657651351354 + }, + { + "x": 3.176621688858468, + "y": 2.9147626672707037, + "heading": -0.3243062033321442, + "angularVelocity": 0.3399450378058761, + "velocityX": 2.3696214538631923, + "velocityY": -1.3135290813490341, + "timestamp": 1.2263101494774777 + }, + { + "x": 3.3579060875063353, + "y": 2.8141562687774018, + "heading": -0.2975038256949741, + "angularVelocity": 0.34969786589260987, + "velocityX": 2.3652665515341145, + "velocityY": -1.3126388757189247, + "timestamp": 1.30295453381982 + }, + { + "x": 3.538822210205962, + "y": 2.713628355793928, + "heading": -0.2698479287435623, + "angularVelocity": 0.3608339630974529, + "velocityX": 2.360461555689992, + "velocityY": -1.3116148540570647, + "timestamp": 1.3795989181621624 + }, + { + "x": 3.7193267349323875, + "y": 2.6131925205499895, + "heading": -0.24120442629231653, + "angularVelocity": 0.3737195190101045, + "velocityX": 2.3550913256759576, + "velocityY": -1.3104134909000977, + "timestamp": 1.4562433025045047 + }, + { + "x": 3.8993637811598734, + "y": 2.5128672365819, + "heading": -0.21139907897320392, + "angularVelocity": 0.38887842305553666, + "velocityX": 2.3489920073377766, + "velocityY": -1.3089710985213812, + "timestamp": 1.532887686846847 + }, + { + "x": 4.078858787892925, + "y": 2.4126784448405125, + "heading": -0.18019759590707388, + "angularVelocity": 0.4070941835316264, + "velocityX": 2.34191987153701, + "velocityY": -1.30719024754481, + "timestamp": 1.6095320711891894 + }, + { + "x": 4.257707770811751, + "y": 2.312664186467197, + "heading": -0.1472705722503286, + "angularVelocity": 0.42960777804245054, + "velocityX": 2.3334910242082882, + "velocityY": -1.3049130635140744, + "timestamp": 1.6861764555315317 + }, + { + "x": 4.435756686256742, + "y": 2.212883689420889, + "heading": -0.1121258503357887, + "angularVelocity": 0.4585426866704457, + "velocityX": 2.323052327613603, + "velocityY": -1.3018631162933536, + "timestamp": 1.762820839873874 + }, + { + "x": 4.612756399460092, + "y": 2.113437617489987, + "heading": -0.07396027426521577, + "angularVelocity": 0.4979565874011239, + "velocityX": 2.3093631023605954, + "velocityY": -1.2974997814153513, + "timestamp": 1.8394652242162164 + }, + { + "x": 4.788243072222853, + "y": 2.014522987095371, + "heading": -0.031264185221419126, + "angularVelocity": 0.5570674147904805, + "velocityX": 2.289622054747365, + "velocityY": -1.290565920039225, + "timestamp": 1.9161096085585587 + }, + { + "x": 4.961081026742291, + "y": 1.9166475782485084, + "heading": 0.019697120176888277, + "angularVelocity": 0.664905926710585, + "velocityX": 2.255063511860639, + "velocityY": -1.2770069155972317, + "timestamp": 1.992753992900901 + }, + { + "x": 5.123264328610966, + "y": 1.823526252461316, + "heading": 0.10546513381285838, + "angularVelocity": 1.1190384575714751, + "velocityX": 2.1160493787028227, + "velocityY": -1.214979108857517, + "timestamp": 2.0693983772432434 + }, + { + "x": 5.295119103926639, + "y": 1.7237778734191072, + "heading": 0.15590676979164073, + "angularVelocity": 0.6581256593239505, + "velocityX": 2.24223570702925, + "velocityY": -1.3014440640121712, + "timestamp": 2.1460427615855857 + }, + { + "x": 5.493802683128641, + "y": 1.6448426706793149, + "heading": 0.16075963684913044, + "angularVelocity": 0.06331666826122177, + "velocityX": 2.5922783633378295, + "velocityY": -1.029888926854938, + "timestamp": 2.222687145927928 + }, + { + "x": 5.707507610321045, + "y": 1.6179983615875244, + "heading": 0.16076270850197952, + "angularVelocity": 0.000040076685010459537, + "velocityX": 2.7882659509385723, + "velocityY": -0.3502449569153927, + "timestamp": 2.2993315302702704 + }, + { + "x": 5.878932815955422, + "y": 1.6310716458031873, + "heading": 0.15207722794791115, + "angularVelocity": -0.13999820175733138, + "velocityX": 2.7631425083845143, + "velocityY": 0.21072366345901047, + "timestamp": 2.3613714739627154 + }, + { + "x": 6.041874898509352, + "y": 1.6781214038756682, + "heading": 0.13576601260266036, + "angularVelocity": -0.2629147348377915, + "velocityX": 2.6264060354679595, + "velocityY": 0.7583784779967588, + "timestamp": 2.4234114176551604 + }, + { + "x": 6.190063655455228, + "y": 1.7574804032751512, + "heading": 0.11619159565916007, + "angularVelocity": -0.3155131319998917, + "velocityX": 2.388602376567293, + "velocityY": 1.2791597586370276, + "timestamp": 2.4854513613476055 + }, + { + "x": 6.338186422130831, + "y": 1.840598011238034, + "heading": 0.10221625072518932, + "angularVelocity": -0.22526366244385518, + "velocityX": 2.387538702644562, + "velocityY": 1.3397434461727966, + "timestamp": 2.5474913050400505 + }, + { + "x": 6.486157900665009, + "y": 1.9236682945064507, + "heading": 0.08780760406345454, + "angularVelocity": -0.2322479003714719, + "velocityX": 2.3851001423812606, + "velocityY": 1.338980636091913, + "timestamp": 2.6095312487324955 + }, + { + "x": 6.633958405719426, + "y": 2.0066826604418155, + "heading": 0.07290105260244177, + "angularVelocity": -0.2402734524536335, + "velocityX": 2.382344281083177, + "velocityY": 1.3380793242962596, + "timestamp": 2.6715711924249406 + }, + { + "x": 6.781563534615612, + "y": 2.089630624589182, + "heading": 0.0574168439817537, + "angularVelocity": -0.24958450474179966, + "velocityX": 2.3791950816060985, + "velocityY": 1.33700901726427, + "timestamp": 2.7336111361173856 + }, + { + "x": 6.928941267710664, + "y": 2.172498140132396, + "heading": 0.04124990446967813, + "angularVelocity": -0.2605892035012319, + "velocityX": 2.3755297687834913, + "velocityY": 1.3357122945504047, + "timestamp": 2.7956510798098306 + }, + { + "x": 7.076047973733186, + "y": 2.255265656701969, + "heading": 0.024256449523087663, + "angularVelocity": -0.27391151466599145, + "velocityX": 2.3711611788653904, + "velocityY": 1.334100446317001, + "timestamp": 2.8576910235022757 + }, + { + "x": 7.222828783744423, + "y": 2.337909309225835, + "heading": 0.006256947367936294, + "angularVelocity": -0.2901276352599791, + "velocityX": 2.3659081758501124, + "velocityY": 1.3321039253929894, + "timestamp": 2.9197309671947207 + }, + { + "x": 7.384688854217529, + "y": 2.3971612453460693, + "heading": 6.712344331303172e-23, + "angularVelocity": -0.10085353073423609, + "velocityX": 2.608965463855139, + "velocityY": 0.9550610879656509, + "timestamp": 2.9817709108871657 + }, + { + "x": 7.581926675665867, + "y": 2.420671247090372, + "heading": -6.833387833600707e-9, + "angularVelocity": -9.667586454477017e-8, + "velocityX": 2.7904368043745778, + "velocityY": 0.33260950489354324, + "timestamp": 3.052454404288316 + }, + { + "x": 7.7805365921893905, + "y": 2.417576114646826, + "heading": -6.833380269760843e-9, + "angularVelocity": 1.0700998918582695e-13, + "velocityX": 2.8098486218890097, + "velocityY": -0.043788617322290876, + "timestamp": 3.1231378976894666 + }, + { + "x": 7.9655574312129644, + "y": 2.4146925691677037, + "heading": -4.100028695417927e-9, + "angularVelocity": 3.8670295465311964e-8, + "velocityX": 2.6175961334214803, + "velocityY": -0.0407951749463946, + "timestamp": 3.193821391090617 + }, + { + "x": 8.104323073360915, + "y": 2.412529909857503, + "heading": -2.050014465682545e-9, + "angularVelocity": 2.9002729365693992e-8, + "velocityX": 1.9631972822906998, + "velocityY": -0.03059638405147332, + "timestamp": 3.2645048844917675 + }, + { + "x": 8.196833505192958, + "y": 2.411088136925815, + "heading": -6.833381783003922e-10, + "angularVelocity": 1.933515480942414e-8, + "velocityX": 1.3087982410125008, + "velocityY": -0.020397590191323155, + "timestamp": 3.335188377892918 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.830282426459157e-24, + "angularVelocity": 9.667577894349971e-9, + "velocityX": 0.6543991363518575, + "velocityY": -0.010198795342763814, + "timestamp": 3.4058718712940683 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 1.851361909343925e-24, + "angularVelocity": 2.3995205522592844e-25, + "velocityX": -1.4269511777904615e-24, + "velocityY": -1.1273229249768833e-23, + "timestamp": 3.4765553646952188 + }, + { + "x": 8.183672767375366, + "y": 2.3891879079868126, + "heading": -0.016263075453039278, + "angularVelocity": -0.1934255260988222, + "velocityX": -0.7066659906617425, + "velocityY": -0.251897340619777, + "timestamp": 3.5606346265230004 + }, + { + "x": 8.064809267825755, + "y": 2.3467183743812834, + "heading": -0.04860613329336098, + "angularVelocity": -0.38467342763509904, + "velocityX": -1.413707696352949, + "velocityY": -0.5051130645333048, + "timestamp": 3.644713888350782 + }, + { + "x": 7.886434927132461, + "y": 2.2827439256274595, + "heading": -0.09665252446442359, + "angularVelocity": -0.5714416388368797, + "velocityX": -2.121502220828929, + "velocityY": -0.7608826167487317, + "timestamp": 3.728793150178564 + }, + { + "x": 7.663093479676549, + "y": 2.2056330475266677, + "heading": -0.09665343057468777, + "angularVelocity": -0.000010776857984756087, + "velocityX": -2.6563202697162027, + "velocityY": -0.9171212546886659, + "timestamp": 3.8128724120063455 + }, + { + "x": 7.439949308521917, + "y": 2.127952816871332, + "heading": -0.09665400996983407, + "angularVelocity": -0.000006891058909294993, + "velocityX": -2.653973956285386, + "velocityY": -0.9238928716387514, + "timestamp": 3.896951673834127 + }, + { + "x": 7.216805138002767, + "y": 2.0502725843903784, + "heading": -0.0966545893647738, + "angularVelocity": -0.0000068910564521426285, + "velocityX": -2.6539739487272445, + "velocityY": -0.9238928933518099, + "timestamp": 3.981030935661909 + }, + { + "x": 6.9936609674836125, + "y": 1.9725923519093191, + "heading": -0.09665516875951666, + "angularVelocity": -0.000006891054111144413, + "velocityX": -2.6539739487273097, + "velocityY": -0.923892893353071, + "timestamp": 4.06511019748969 + }, + { + "x": 6.77051679696445, + "y": 1.8949121194281597, + "heading": -0.09665574815406265, + "angularVelocity": -0.000006891051769457115, + "velocityX": -2.653973948727398, + "velocityY": -0.9238928933542627, + "timestamp": 4.149189459317472 + }, + { + "x": 6.547372626445282, + "y": 1.8172318869468993, + "heading": -0.09665632754841177, + "angularVelocity": -0.000006891049427961194, + "velocityX": -2.653973948727487, + "velocityY": -0.9238928933554545, + "timestamp": 4.2332687211452535 + }, + { + "x": 6.324228455926046, + "y": 1.7395516544657041, + "heading": -0.09665690694256396, + "angularVelocity": -0.000006891047085669869, + "velocityX": -2.653973948728258, + "velocityY": -0.9238928933546852, + "timestamp": 4.317347982973035 + }, + { + "x": 6.101084267512398, + "y": 1.6618714733923579, + "heading": -0.0966574863411208, + "angularVelocity": -0.000006891099472478973, + "velocityX": -2.65397416155617, + "velocityY": -0.9238922819334152, + "timestamp": 4.401427244800817 + }, + { + "x": 5.881706688567194, + "y": 1.6001619743925184, + "heading": -0.12280606064162562, + "angularVelocity": -0.3109990945694144, + "velocityX": -2.609175844033366, + "velocityY": -0.7339443479682077, + "timestamp": 4.485506506628599 + }, + { + "x": 5.667889595031738, + "y": 1.5915861129760742, + "heading": -0.19572183715761898, + "angularVelocity": -0.8672266493650448, + "velocityX": -2.5430419926070895, + "velocityY": -0.10199734429174848, + "timestamp": 4.56958576845638 + }, + { + "x": 5.472836165647801, + "y": 1.6405608510115604, + "heading": -0.2695567389374151, + "angularVelocity": -0.9372981580360532, + "velocityX": -2.4761083941768933, + "velocityY": 0.6217104735625232, + "timestamp": 4.648359956954205 + }, + { + "x": 5.3021481158717245, + "y": 1.712801388975876, + "heading": -0.4003704259742984, + "angularVelocity": -1.660616117175175, + "velocityX": -2.1668017536072774, + "velocityY": 0.9170584850431085, + "timestamp": 4.7271341454520295 + }, + { + "x": 5.1196004435735345, + "y": 1.8042197974618441, + "heading": -0.46428021863316127, + "angularVelocity": -0.8113037262278816, + "velocityX": -2.317353891918915, + "velocityY": 1.160512221442844, + "timestamp": 4.805908333949854 + }, + { + "x": 4.929096458765624, + "y": 1.904953408569058, + "heading": -0.48639976153317277, + "angularVelocity": -0.28079683614414136, + "velocityX": -2.4183554085507573, + "velocityY": 1.2787641869518616, + "timestamp": 4.884682522447679 + }, + { + "x": 4.737761672165951, + "y": 2.009314965020544, + "heading": -0.49933816069591924, + "angularVelocity": -0.1642466829487382, + "velocityX": -2.4289020331190754, + "velocityY": 1.3248191881325238, + "timestamp": 4.963456710945503 + }, + { + "x": 4.546911439579052, + "y": 2.114853649108329, + "heading": -0.5116829622012672, + "angularVelocity": -0.15671124946833295, + "velocityX": -2.4227508556583697, + "velocityY": 1.3397622508126334, + "timestamp": 5.042230899443328 + }, + { + "x": 4.356102309238819, + "y": 2.2203468351822107, + "heading": -0.524131682346679, + "angularVelocity": -0.15803044604839828, + "velocityX": -2.4222290826328545, + "velocityY": 1.3391846756605428, + "timestamp": 5.121005087941152 + }, + { + "x": 4.165335431709755, + "y": 2.3257928672028427, + "heading": -0.5366900663313149, + "angularVelocity": -0.15942257513681343, + "velocityX": -2.421692703750687, + "velocityY": 1.338586077894589, + "timestamp": 5.199779276438977 + }, + { + "x": 3.9746120593595244, + "y": 2.4311903935174577, + "heading": -0.5493635573249664, + "angularVelocity": -0.1608838026176728, + "velocityX": -2.421140426670299, + "velocityY": 1.3379703215543044, + "timestamp": 5.2785534649368016 + }, + { + "x": 3.783933515355731, + "y": 2.5365380161026203, + "heading": -0.5621578157899224, + "angularVelocity": -0.16241688691352676, + "velocityX": -2.420571352620895, + "velocityY": 1.337336817986659, + "timestamp": 5.357327653434626 + }, + { + "x": 3.593301216494676, + "y": 2.6418342523116722, + "heading": -0.5750788548667545, + "angularVelocity": -0.16402630510359092, + "velocityX": -2.4199842930317086, + "velocityY": 1.3366844929409736, + "timestamp": 5.436101841932451 + }, + { + "x": 3.4027166923911927, + "y": 2.7470775186047, + "heading": -0.5881331219292356, + "angularVelocity": -0.16571756956711148, + "velocityX": -2.41937781572635, + "velocityY": 1.3360120656264767, + "timestamp": 5.514876030430275 + }, + { + "x": 3.212181603664246, + "y": 2.8522661171153323, + "heading": -0.601327569493579, + "angularVelocity": -0.16749709284161973, + "velocityX": -2.4187502576711153, + "velocityY": 1.3353180847243658, + "timestamp": 5.5936502189281 + }, + { + "x": 3.0216977610811666, + "y": 2.9573982215901036, + "heading": -0.6146697280062193, + "angularVelocity": -0.1693722114701131, + "velocityX": -2.418099712805536, + "velocityY": 1.3346009204229918, + "timestamp": 5.6724244074259245 + }, + { + "x": 2.831267146879349, + "y": 3.0624718615565794, + "heading": -0.6281677862205469, + "angularVelocity": -0.17135128233914515, + "velocityX": -2.417424004400561, + "velocityY": 1.3338587419326686, + "timestamp": 5.751198595923749 + }, + { + "x": 2.640891939244419, + "y": 3.167484904027678, + "heading": -0.6418306831098634, + "angularVelocity": -0.17344382912549475, + "velocityX": -2.416720644988769, + "velocityY": 1.3330894862090363, + "timestamp": 5.829972784421574 + }, + { + "x": 2.4505746379296203, + "y": 3.272435032259823, + "heading": -0.655668504824747, + "angularVelocity": -0.17566441468661878, + "velocityX": -2.4159855524256404, + "velocityY": 1.3322908205527566, + "timestamp": 5.908746972919398 + }, + { + "x": 2.2618233887297388, + "y": 3.3772509809337916, + "heading": -0.674261275863946, + "angularVelocity": -0.2360261831159634, + "velocityX": -2.396105282697931, + "velocityY": 1.3305874763389332, + "timestamp": 5.987521161417223 + }, + { + "x": 2.116002537558778, + "y": 3.455078745186431, + "heading": -0.7196337030492912, + "angularVelocity": -0.5759808898139023, + "velocityX": -1.8511247649982117, + "velocityY": 0.9879856046347999, + "timestamp": 6.066295349915047 + }, + { + "x": 2.018778748901899, + "y": 3.5069532190081367, + "heading": -0.7498768931574519, + "angularVelocity": -0.3839225853655822, + "velocityX": -1.234208698443955, + "velocityY": 0.6585212086715196, + "timestamp": 6.145069538412872 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -0.19198048410191668, + "velocityX": -0.617100471546741, + "velocityY": 0.32925585007098457, + "timestamp": 6.2238437269106965 + }, + { + "x": 1.9701671600341797, + "y": 3.5328900814056396, + "heading": -0.765, + "angularVelocity": -6.585493827087804e-24, + "velocityX": 2.2425722891132833e-25, + "velocityY": -1.917344763102387e-25, + "timestamp": 6.302617915408521 + }, + { + "x": 2.021866320736439, + "y": 3.5175409291347064, + "heading": -0.7649999986292967, + "angularVelocity": 1.7960508607271394e-8, + "velocityX": 0.6774210494904483, + "velocityY": -0.20112200466940705, + "timestamp": 6.37893553314589 + }, + { + "x": 2.1252646329115827, + "y": 3.486842627325834, + "heading": -0.7649999949328548, + "angularVelocity": 4.8434973827775214e-8, + "velocityX": 1.354841978047161, + "velocityY": -0.4022439735280295, + "timestamp": 6.45525315088326 + }, + { + "x": 2.264271046839328, + "y": 3.4455725064565113, + "heading": -0.7629134147129198, + "angularVelocity": 0.027340740995291143, + "velocityX": 1.8214197199669733, + "velocityY": -0.5407679392109075, + "timestamp": 6.531570768620629 + }, + { + "x": 2.403277460756995, + "y": 3.404302385553228, + "heading": -0.7557796152547765, + "angularVelocity": 0.09347513286765456, + "velocityX": 1.8214197198349207, + "velocityY": -0.5407679396558968, + "timestamp": 6.607888386357998 + }, + { + "x": 2.5422838746752925, + "y": 3.3630322646520687, + "heading": -0.7445970322006652, + "angularVelocity": 0.1465268883600871, + "velocityX": 1.8214197198431827, + "velocityY": -0.5407679396280685, + "timestamp": 6.684206004095367 + }, + { + "x": 2.6812902885942673, + "y": 3.321762143753191, + "heading": -0.7301562502874436, + "angularVelocity": 0.1892195058147188, + "velocityX": 1.821419719852059, + "velocityY": -0.540767939598171, + "timestamp": 6.7605236218327365 + }, + { + "x": 2.8202967025138705, + "y": 3.28049202285643, + "heading": -0.7130778427136069, + "angularVelocity": 0.2237806692631405, + "velocityX": 1.8214197198602944, + "velocityY": -0.5407679395704321, + "timestamp": 6.836841239570106 + }, + { + "x": 2.9593031164340085, + "y": 3.2392219019614696, + "heading": -0.6938417314556549, + "angularVelocity": 0.25205335056643224, + "velocityX": 1.8214197198673001, + "velocityY": -0.5407679395468353, + "timestamp": 6.913158857307475 + }, + { + "x": 3.0983095303545722, + "y": 3.1979517810679434, + "heading": -0.6728120786629418, + "angularVelocity": 0.27555436629432156, + "velocityX": 1.8214197198728768, + "velocityY": -0.5407679395280517, + "timestamp": 6.989476475044844 + }, + { + "x": 3.2373159442754544, + "y": 3.1566816601754897, + "heading": -0.6502538625008166, + "angularVelocity": 0.2955833375165695, + "velocityX": 1.8214197198770503, + "velocityY": -0.540767939513994, + "timestamp": 7.065794092782213 + }, + { + "x": 3.376322358196558, + "y": 3.1154115392837802, + "heading": -0.6263485003282439, + "angularVelocity": 0.31323517270727913, + "velocityX": 1.8214197198799456, + "velocityY": -0.5407679395042426, + "timestamp": 7.142111710519583 + }, + { + "x": 3.515328772117798, + "y": 3.074141418392532, + "heading": -0.6012000869662939, + "angularVelocity": 0.3295230394702948, + "velocityX": 1.821419719881741, + "velocityY": -0.5407679394981955, + "timestamp": 7.218429328256952 + }, + { + "x": 3.6543351862262687, + "y": 3.032871298131916, + "heading": -0.5748442396833396, + "angularVelocity": 0.3453442083799368, + "velocityX": 1.821419722335047, + "velocityY": -0.5407679312349462, + "timestamp": 7.294746945994321 + }, + { + "x": 3.7934386683270223, + "y": 2.9919295436941926, + "heading": -0.5472452836552583, + "angularVelocity": 0.3616328293036799, + "velocityX": 1.8226916172809426, + "velocityY": -0.5364653097351076, + "timestamp": 7.37106456373169 + }, + { + "x": 3.9378836154937744, + "y": 3.0046441555023193, + "heading": -0.5196460913813736, + "angularVelocity": 0.361635924864182, + "velocityX": 1.8926815517726128, + "velocityY": 0.16660126698243774, + "timestamp": 7.4473821814690595 + }, + { + "x": 4.075926460541626, + "y": 3.0786892532968806, + "heading": -0.4898305952486719, + "angularVelocity": 0.3616359432061461, + "velocityX": 1.674339217752807, + "velocityY": 0.8981024049221815, + "timestamp": 7.529828345056582 + }, + { + "x": 4.180404466119937, + "y": 3.195406219293214, + "heading": -0.4598310277403017, + "angularVelocity": 0.3638685683236616, + "velocityX": 1.2672270125389096, + "velocityY": 1.4156749194573495, + "timestamp": 7.612274508644104 + }, + { + "x": 4.2848822354372995, + "y": 3.3121233967757027, + "heading": -0.4287713690600015, + "angularVelocity": 0.37672654892338603, + "velocityX": 1.2672241468998562, + "velocityY": 1.4156774845998201, + "timestamp": 7.694720672231626 + }, + { + "x": 4.389360004752337, + "y": 3.428840574260275, + "heading": -0.39697164385324546, + "angularVelocity": 0.38570290991161005, + "velocityX": 1.2672241468716323, + "velocityY": 1.415677484625084, + "timestamp": 7.777166835819148 + }, + { + "x": 4.493837774065624, + "y": 3.545557751746413, + "heading": -0.36470801458445545, + "angularVelocity": 0.3913296612587668, + "velocityX": 1.2672241468504064, + "velocityY": 1.415677484644084, + "timestamp": 7.859612999406671 + }, + { + "x": 4.598315543377444, + "y": 3.662274929233865, + "heading": -0.3322260766024941, + "angularVelocity": 0.3939775553956429, + "velocityX": 1.267224146832605, + "velocityY": 1.4156774846600186, + "timestamp": 7.942059162994193 + }, + { + "x": 4.7027933126881845, + "y": 3.778992106722283, + "heading": -0.29975459263588233, + "angularVelocity": 0.3938507573143906, + "velocityX": 1.2672241468195173, + "velocityY": 1.415677484671734, + "timestamp": 8.024505326581716 + }, + { + "x": 4.807271113642882, + "y": 3.8957092558850426, + "heading": -0.26751670065443317, + "angularVelocity": 0.3910174904284827, + "velocityX": 1.2672245306330898, + "velocityY": 1.4156771411062206, + "timestamp": 8.106951490169239 + }, + { + "x": 4.928345203399658, + "y": 3.995105266571045, + "heading": -0.23561923052699993, + "angularVelocity": 0.386888470481369, + "velocityX": 1.4685230274935492, + "velocityY": 1.205586850387367, + "timestamp": 8.189397653756762 + }, + { + "x": 5.0976675546813945, + "y": 4.035971247104969, + "heading": -0.20015091281875375, + "angularVelocity": 0.38688845257830395, + "velocityX": 1.846968413139047, + "velocityY": 0.4457661652272064, + "timestamp": 8.281073477813743 + }, + { + "x": 5.270991671667468, + "y": 4.053258165584531, + "heading": -0.16565527924639437, + "angularVelocity": 0.37627841284435726, + "velocityX": 1.890619678295383, + "velocityY": 0.18856572774100455, + "timestamp": 8.372749301870725 + }, + { + "x": 5.444315791533781, + "y": 4.0705450551858755, + "heading": -0.13259854065392815, + "angularVelocity": 0.3605829446585594, + "velocityX": 1.8906197097130382, + "velocityY": 0.1885654127373893, + "timestamp": 8.464425125927706 + }, + { + "x": 5.617639911400379, + "y": 4.087831944784366, + "heading": -0.10138296753519387, + "angularVelocity": 0.34049950943808543, + "velocityX": 1.8906197097161441, + "velocityY": 0.18856541270624588, + "timestamp": 8.556100949984687 + }, + { + "x": 5.790964031267238, + "y": 4.105118834380232, + "heading": -0.07255368127548635, + "angularVelocity": 0.3144698894856807, + "velocityX": 1.8906197097189994, + "velocityY": 0.1885654126776188, + "timestamp": 8.647776774041668 + }, + { + "x": 5.9642881511343315, + "y": 4.122405723973758, + "heading": -0.046846868579348507, + "angularVelocity": 0.2804099440672579, + "velocityX": 1.890619709721544, + "velocityY": 0.18856541265210713, + "timestamp": 8.73945259809865 + }, + { + "x": 6.137612271001621, + "y": 4.139692613565316, + "heading": -0.02524481611998314, + "angularVelocity": 0.23563521442620056, + "velocityX": 1.890619709723685, + "velocityY": 0.18856541263063997, + "timestamp": 8.831128422155631 + }, + { + "x": 6.310936390869065, + "y": 4.156979503155326, + "heading": -0.009053820761373118, + "angularVelocity": 0.17661139700851253, + "velocityX": 1.8906197097253696, + "velocityY": 0.18856541261374912, + "timestamp": 8.922804246212612 + }, + { + "x": 6.484260512045991, + "y": 4.174266379616022, + "heading": -2.2378824393561327e-8, + "angularVelocity": 0.09875884373748682, + "velocityX": 1.890619724009207, + "velocityY": 0.188565269399175, + "timestamp": 9.014480070269594 + }, + { + "x": 6.658350467681885, + "y": 4.179991245269775, + "heading": 1.903926994866919e-23, + "angularVelocity": 2.4410824362649644e-7, + "velocityX": 1.8989734472164426, + "velocityY": 0.06244684149438171, + "timestamp": 9.106155894326575 + }, + { + "x": 6.892610562082988, + "y": 4.180747179242153, + "heading": 1.815110943876254e-8, + "angularVelocity": 2.0923543279309324e-7, + "velocityX": 2.7004141208851267, + "velocityY": 0.008713967176889024, + "timestamp": 9.192905586749424 + }, + { + "x": 7.135847606603004, + "y": 4.164439410363542, + "heading": 1.8151086995989633e-8, + "angularVelocity": -2.587072333894115e-13, + "velocityX": 2.803895180796635, + "velocityY": -0.18798647491591486, + "timestamp": 9.279655279172273 + }, + { + "x": 7.379084647879251, + "y": 4.14813159310281, + "heading": 1.8151064626797785e-8, + "angularVelocity": -2.5785903361398285e-13, + "velocityX": 2.803895143404351, + "velocityY": -0.18798703263687086, + "timestamp": 9.366404971595122 + }, + { + "x": 7.6223216891554895, + "y": 4.131823775841945, + "heading": 1.8151042187582598e-8, + "angularVelocity": -2.586662220352974e-13, + "velocityX": 2.8038951434042487, + "velocityY": -0.18798703263839134, + "timestamp": 9.45315466401797 + }, + { + "x": 7.865558730431713, + "y": 4.115515958581081, + "heading": 1.815101972750476e-8, + "angularVelocity": -2.589067144937878e-13, + "velocityX": 2.803895143404087, + "velocityY": -0.18798703263838049, + "timestamp": 9.53990435644082 + }, + { + "x": 8.07413296336415, + "y": 4.101532107856125, + "heading": 9.075510567318239e-9, + "angularVelocity": -1.0461719121664748e-7, + "velocityX": 2.404322448957763, + "velocityY": -0.16119769804825285, + "timestamp": 9.626654048863669 + }, + { + "x": 8.213182462520122, + "y": 4.092209539999878, + "heading": 3.025170279496359e-9, + "angularVelocity": -6.974480391619583e-8, + "velocityX": 1.6028817540723412, + "velocityY": -0.10746514017369478, + "timestamp": 9.713403741286518 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -2.730279460976851e-23, + "angularVelocity": -3.487240352104816e-8, + "velocityX": 0.8014409030577104, + "velocityY": -0.05373257183146045, + "timestamp": 9.800153433709367 + }, + { + "x": 8.282707214355469, + "y": 4.08754825592041, + "heading": -1.3454200564607368e-23, + "angularVelocity": 4.663099853298001e-24, + "velocityX": 1.7036719365270032e-27, + "velocityY": 2.800838691430789e-26, + "timestamp": 9.886903126132216 + }, + { + "x": 8.214708333565108, + "y": 4.098060716631318, + "heading": 1.6847207046591105e-8, + "angularVelocity": 1.954345432230813e-7, + "velocityX": -0.7888150344560528, + "velocityY": 0.12194887565072522, + "timestamp": 9.973106960770398 + }, + { + "x": 8.078710576708064, + "y": 4.119085637322864, + "heading": 5.0541618776805506e-8, + "angularVelocity": 3.9086905903476216e-7, + "velocityX": -1.577630014115494, + "velocityY": 0.24389774283002824, + "timestamp": 10.05931079540858 + }, + { + "x": 7.8747139579553656, + "y": 4.15062301580424, + "heading": 1.0108322622636101e-7, + "angularVelocity": 5.863034708570761e-7, + "velocityX": -2.3664448293851432, + "velocityY": 0.3658465845950713, + "timestamp": 10.145514630046762 + }, + { + "x": 7.635308863013457, + "y": 4.1876344581296925, + "heading": 1.0108335901717796e-7, + "angularVelocity": 1.540428189705246e-12, + "velocityX": -2.7771977423829006, + "velocityY": 0.42934798064143237, + "timestamp": 10.231718464684944 + }, + { + "x": 7.395903768071533, + "y": 4.224645900455147, + "heading": 1.0108349186566566e-7, + "angularVelocity": 1.5410971948056658e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.317922299323126 + }, + { + "x": 7.156498673129608, + "y": 4.261657342780601, + "heading": 1.0108362465293872e-7, + "angularVelocity": 1.5403870787070826e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.404126133961308 + }, + { + "x": 6.917093578187684, + "y": 4.298668785106054, + "heading": 1.0108375751155647e-7, + "angularVelocity": 1.5412147075799493e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414625, + "timestamp": 10.49032996859949 + }, + { + "x": 6.677688483245759, + "y": 4.335680227431509, + "heading": 1.0108389035423936e-7, + "angularVelocity": 1.5410298558530182e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.4293479806414627, + "timestamp": 10.576533803237671 + }, + { + "x": 6.438283388303835, + "y": 4.372691669756963, + "heading": 1.0108402320108348e-7, + "angularVelocity": 1.5410781280848912e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.4293479806414625, + "timestamp": 10.662737637875853 + }, + { + "x": 6.1988782933619095, + "y": 4.409703112082417, + "heading": 1.0108415606074574e-7, + "angularVelocity": 1.5412268238689798e-12, + "velocityX": -2.7771977423830965, + "velocityY": 0.42934798064146257, + "timestamp": 10.748941472514035 + }, + { + "x": 5.959473198419985, + "y": 4.446714554407871, + "heading": 1.0108428894851187e-7, + "angularVelocity": 1.5415528401754331e-12, + "velocityX": -2.777197742383096, + "velocityY": 0.42934798064146257, + "timestamp": 10.835145307152217 + }, + { + "x": 5.72006810347805, + "y": 4.483725996733255, + "heading": 1.0108442171706879e-7, + "angularVelocity": 1.5401699643641334e-12, + "velocityX": -2.7771977423832217, + "velocityY": 0.4293479806406499, + "timestamp": 10.9213491417904 + }, + { + "x": 5.480663004845917, + "y": 4.520737415188933, + "heading": 1.0108455459674728e-7, + "angularVelocity": 1.5414590205195092e-12, + "velocityX": -2.7771977851910266, + "velocityY": 0.4293477037422273, + "timestamp": 11.007552976428581 + }, + { + "x": 5.240152409397331, + "y": 4.549708085687679, + "heading": 1.0108468832096651e-7, + "angularVelocity": 1.5512560408121339e-12, + "velocityX": -2.790022003755009, + "velocityY": 0.3360717144468437, + "timestamp": 11.093756811066763 + }, + { + "x": 5.001198008888612, + "y": 4.509890511709968, + "heading": 1.0108464169686405e-7, + "angularVelocity": -5.408587989447552e-13, + "velocityX": -2.7719695012602092, + "velocityY": -0.46190026400605494, + "timestamp": 11.179960645704945 + }, + { + "x": 4.783077239990234, + "y": 4.404496192932129, + "heading": 4.371418464571693e-25, + "angularVelocity": -0.0000011726234931559648, + "velocityX": -2.5302907905881877, + "velocityY": -1.2226175229930707, + "timestamp": 11.266164480343127 + }, + { + "x": 4.627041500035927, + "y": 4.275874365802291, + "heading": -0.00461711894400582, + "angularVelocity": -0.06368383973540494, + "velocityX": -2.152198194752189, + "velocityY": -1.774078581199584, + "timestamp": 11.338665117289471 + }, + { + "x": 4.5068214168723975, + "y": 4.114451023957695, + "heading": -0.01240880783073842, + "angularVelocity": -0.10747062667185843, + "velocityX": -1.6581934756311134, + "velocityY": -2.226509292105373, + "timestamp": 11.411165754235816 + }, + { + "x": 4.429140519397286, + "y": 3.930818027867112, + "heading": -0.027772737926692166, + "angularVelocity": -0.21191441541850167, + "velocityX": -1.0714512416297768, + "velocityY": -2.53284665935394, + "timestamp": 11.48366639118216 + }, + { + "x": 4.383091867034577, + "y": 3.7471675526312227, + "heading": -0.08613270246137088, + "angularVelocity": -0.8049579561331118, + "velocityX": -0.635148245618701, + "velocityY": -2.5330877488952703, + "timestamp": 11.556167028128504 + }, + { + "x": 4.346524489213487, + "y": 3.5742818254238546, + "heading": -0.20843940130739744, + "angularVelocity": -1.6869741287451163, + "velocityX": -0.5043731939645176, + "velocityY": -2.3846097701915894, + "timestamp": 11.628667665074849 + }, + { + "x": 4.289423143132127, + "y": 3.4062373324265396, + "heading": -0.32755846162196556, + "angularVelocity": -1.643007086995987, + "velocityX": -0.7875978541211613, + "velocityY": -2.317834712565055, + "timestamp": 11.701168302021193 + }, + { + "x": 4.203799594769533, + "y": 3.2403627849834162, + "heading": -0.40165980770192133, + "angularVelocity": -1.022078552700102, + "velocityX": -1.1810040845014969, + "velocityY": -2.28790469200817, + "timestamp": 11.773668938967537 + }, + { + "x": 4.079421660594405, + "y": 3.103563620218193, + "heading": -0.4716628621936354, + "angularVelocity": -0.9655508894841919, + "velocityX": -1.7155426409174284, + "velocityY": -1.8868684542242327, + "timestamp": 11.846169575913882 + }, + { + "x": 3.924677610397339, + "y": 3.0046441555023193, + "heading": -0.5385444180262977, + "angularVelocity": -0.9224961138225419, + "velocityX": -2.134381940831591, + "velocityY": -1.3643944230321734, + "timestamp": 11.918670212860226 + }, + { + "x": 3.7636444729485263, + "y": 2.9505820220343484, + "heading": -0.5968694156691691, + "angularVelocity": -0.8693980375808563, + "velocityX": -2.40037547092122, + "velocityY": -0.8058553732360577, + "timestamp": 11.98575685801204 + }, + { + "x": 3.5921162225925003, + "y": 2.93585850684271, + "heading": -0.6464965554196345, + "angularVelocity": -0.7397469293353471, + "velocityX": -2.556816635678569, + "velocityY": -0.21947013684048228, + "timestamp": 12.052843503163855 + }, + { + "x": 3.416479572667743, + "y": 2.9596548551854616, + "heading": -0.6807775343446694, + "angularVelocity": -0.5109955766525223, + "velocityX": -2.6180568357129337, + "velocityY": 0.3547106624410992, + "timestamp": 12.11993014831567 + }, + { + "x": 3.2390028785000764, + "y": 3.015476958858974, + "heading": -0.6885344713310354, + "angularVelocity": -0.115625650512295, + "velocityX": -2.645484712583867, + "velocityY": 0.8320896587866112, + "timestamp": 12.187016793467484 + }, + { + "x": 3.0657580475568027, + "y": 3.089829788896707, + "heading": -0.6885346535011025, + "angularVelocity": -0.00000271544458244769, + "velocityX": -2.5824041513959415, + "velocityY": 1.1083104523929412, + "timestamp": 12.254103438619298 + }, + { + "x": 2.8925132745209385, + "y": 3.1641827538610285, + "heading": -0.6885348356707467, + "angularVelocity": -0.000002715438277887047, + "velocityX": -2.582403288222506, + "velocityY": 1.1083124636216772, + "timestamp": 12.321190083771112 + }, + { + "x": 2.7192685014854168, + "y": 3.2385357188261574, + "heading": -0.6885350178403685, + "angularVelocity": -0.000002715437944094969, + "velocityX": -2.5824032882174053, + "velocityY": 1.108312463633708, + "timestamp": 12.388276728922927 + }, + { + "x": 2.5460237284499403, + "y": 3.312888683791401, + "heading": -0.6885352000099678, + "angularVelocity": -0.0000027154376105306044, + "velocityX": -2.5824032882167316, + "velocityY": 1.1083124636354227, + "timestamp": 12.455363374074741 + }, + { + "x": 2.3727789637362293, + "y": 3.3872416681385586, + "heading": -0.6885353821903477, + "angularVelocity": -0.0000027155983032171972, + "velocityX": -2.5824031641716974, + "velocityY": 1.108312752544088, + "timestamp": 12.522450019226556 + }, + { + "x": 2.2223047239417384, + "y": 3.4507934701253355, + "heading": -0.7191097564620745, + "angularVelocity": -0.45574457036177557, + "velocityX": -2.2429835245744076, + "velocityY": 0.9473092870117661, + "timestamp": 12.58953666437837 + }, + { + "x": 2.109444085282166, + "y": 3.4984470664225062, + "heading": -0.7420547518086502, + "angularVelocity": -0.34202031260695603, + "velocityX": -1.6823115599859222, + "velocityY": 0.7103291003646417, + "timestamp": 12.656623309530184 + }, + { + "x": 2.0342014899368133, + "y": 3.5302130152316624, + "heading": -0.7573535418351998, + "angularVelocity": -0.22804523898800838, + "velocityX": -1.1215733798445486, + "velocityY": 0.47350629528829474, + "timestamp": 12.723709954681999 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": -0.11397884254752216, + "velocityX": -0.5607948167249468, + "velocityY": 0.23675814542593593, + "timestamp": 12.790796599833813 + }, + { + "x": 1.996579647064209, + "y": 3.5460963249206543, + "heading": -0.765, + "angularVelocity": 6.11903975445096e-25, + "velocityX": -4.7665538572669345e-25, + "velocityY": 2.8118183260637723e-25, + "timestamp": 12.857883244985628 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.1.traj b/src/main/deploy/choreo/Centerline Auto.1.traj new file mode 100644 index 0000000..d4736c5 --- /dev/null +++ b/src/main/deploy/choreo/Centerline Auto.1.traj @@ -0,0 +1,338 @@ +{ + "samples": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "angularVelocity": 5.825284855617402e-28, + "velocityX": -1.9685293298057674e-27, + "velocityY": 1.4418564963898336e-27, + "timestamp": 0 + }, + { + "x": 0.6784036347143663, + "y": 6.707198268292706, + "heading": 1.0175475056665193, + "angularVelocity": -0.5991822637957332, + "velocityX": 0.32672283914932904, + "velocityY": 0.23758411786628814, + "timestamp": 0.05393128412995569 + }, + { + "x": 0.7135162843776108, + "y": 6.732573985913541, + "heading": 0.9516130808872657, + "angularVelocity": -1.2225635981589924, + "velocityX": 0.6510627408506594, + "velocityY": 0.4705194402508243, + "timestamp": 0.10786256825991138 + }, + { + "x": 0.7660994692609764, + "y": 6.770017684654193, + "heading": 0.8504346226500515, + "angularVelocity": -1.8760624722639487, + "velocityX": 0.9750033905489506, + "velocityY": 0.6942853177837462, + "timestamp": 0.16179385238986707 + }, + { + "x": 0.836479083356807, + "y": 6.818804043384565, + "heading": 0.7130270962325688, + "angularVelocity": -2.5478259721459318, + "velocityX": 1.3049868036933845, + "velocityY": 0.9046022084846815, + "timestamp": 0.21572513651982275 + }, + { + "x": 0.9255037273969096, + "y": 6.878132431612943, + "heading": 0.5402736783143448, + "angularVelocity": -3.2032135096569965, + "velocityX": 1.6507050680563082, + "velocityY": 1.1000737176110378, + "timestamp": 0.26965642064977846 + }, + { + "x": 1.035016950774323, + "y": 6.947674803955847, + "heading": 0.3381434888609651, + "angularVelocity": -3.747920946334524, + "velocityX": 2.0306066347970617, + "velocityY": 1.289462571952319, + "timestamp": 0.32358770477973414 + }, + { + "x": 1.162461605941772, + "y": 7.018689321892079, + "heading": 0.18269313635392612, + "angularVelocity": -2.882378104190095, + "velocityX": 2.3630932810787844, + "velocityY": 1.3167592628633178, + "timestamp": 0.3775189889096898 + }, + { + "x": 1.3050246421848908, + "y": 7.094859887834467, + "heading": 0.07841153426675496, + "angularVelocity": -1.9336013182235503, + "velocityX": 2.6434200212921195, + "velocityY": 1.4123632910138557, + "timestamp": 0.4314502730396455 + }, + { + "x": 1.4591047302314346, + "y": 7.181935811218979, + "heading": 0.022297438159640273, + "angularVelocity": -1.0404739477721165, + "velocityX": 2.8569705048235896, + "velocityY": 1.6145716681748061, + "timestamp": 0.4853815571696012 + }, + { + "x": 1.6255168678692307, + "y": 7.279018436792272, + "heading": 0.014123453422294747, + "angularVelocity": -0.15156295402959544, + "velocityX": 3.0856327699670643, + "velocityY": 1.800117077489892, + "timestamp": 0.5393128412995569 + }, + { + "x": 1.793858101376193, + "y": 7.378107838800647, + "heading": 0.014120593150970575, + "angularVelocity": -0.00005303547598235602, + "velocityX": 3.121402284828195, + "velocityY": 1.8373269542331496, + "timestamp": 0.5932441254295127 + }, + { + "x": 1.9621993268656661, + "y": 7.477197254469766, + "heading": 0.014117732942591941, + "angularVelocity": -0.000053034308839039934, + "velocityX": 3.121402136167001, + "velocityY": 1.8373272075322298, + "timestamp": 0.6471754095594684 + }, + { + "x": 2.130540737170093, + "y": 7.576286356115685, + "heading": 0.014114872666943619, + "angularVelocity": -0.00005303555616123241, + "velocityX": 3.121405563026878, + "velocityY": 1.8373213848783807, + "timestamp": 0.7011066936894241 + }, + { + "x": 2.3040566274888983, + "y": 7.663067241706454, + "heading": 0.010006762068371105, + "angularVelocity": -0.0761730536338314, + "velocityX": 3.217351359568822, + "velocityY": 1.609101043870144, + "timestamp": 0.7550379778193799 + }, + { + "x": 2.486186981201172, + "y": 7.724460124969482, + "heading": -5.2714780410694486e-27, + "angularVelocity": -0.18554651960925458, + "velocityX": 3.377081718903682, + "velocityY": 1.1383538191876352, + "timestamp": 0.8089692619493356 + }, + { + "x": 2.8066356107472474, + "y": 7.755704422012626, + "heading": -3.618373256811644e-9, + "angularVelocity": -4.070535151890212e-8, + "velocityX": 3.6049277350997357, + "velocityY": 0.3514867051672969, + "timestamp": 0.8978610936568053 + }, + { + "x": 3.128175858643877, + "y": 7.739109235929212, + "heading": -3.6183723834730783e-9, + "angularVelocity": 9.824733603484871e-15, + "velocityX": 3.6172080349831512, + "velocityY": -0.18668966275806864, + "timestamp": 0.9867529253642751 + }, + { + "x": 3.449716088164231, + "y": 7.722513693799808, + "heading": -3.6183716236893314e-9, + "angularVelocity": 8.547284180686901e-15, + "velocityX": 3.6172078282568996, + "velocityY": -0.18669366814284583, + "timestamp": 1.0756447570717447 + }, + { + "x": 3.771256317684507, + "y": 7.7059181516688815, + "heading": -3.6183708487107216e-9, + "angularVelocity": 8.718220728132132e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997415, + "timestamp": 1.1645365887792143 + }, + { + "x": 4.092796547204784, + "y": 7.689322609537955, + "heading": -3.618370111441706e-9, + "angularVelocity": 8.29400184228923e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997418, + "timestamp": 1.253428420486684 + }, + { + "x": 4.414336776725059, + "y": 7.6727270674070285, + "heading": -3.6183692900088547e-9, + "angularVelocity": 9.240813644117634e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.3423202521941535 + }, + { + "x": 4.735877006245335, + "y": 7.656131525276102, + "heading": -3.6183684638446085e-9, + "angularVelocity": 9.294040074640308e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.4312120839016231 + }, + { + "x": 5.0574172357656115, + "y": 7.6395359831451755, + "heading": -3.618367662939708e-9, + "angularVelocity": 9.009881839671523e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.5201039156090927 + }, + { + "x": 5.378957465285888, + "y": 7.62294044101425, + "heading": -3.6183667762068362e-9, + "angularVelocity": 9.975414552450845e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997418, + "timestamp": 1.6089957473165624 + }, + { + "x": 5.700497694806164, + "y": 7.606344898883323, + "heading": -3.6183660187139743e-9, + "angularVelocity": 8.521512579724434e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997426, + "timestamp": 1.697887579024032 + }, + { + "x": 6.02203792432644, + "y": 7.589749356752397, + "heading": -3.618365173816459e-9, + "angularVelocity": 9.504782370286536e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.7867794107315016 + }, + { + "x": 6.343578153846717, + "y": 7.57315381462147, + "heading": -3.6183643942346165e-9, + "angularVelocity": 8.770005387165316e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.8756712424389712 + }, + { + "x": 6.665118383366993, + "y": 7.556558272490544, + "heading": -3.6183635769900644e-9, + "angularVelocity": 9.193696838193051e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997424, + "timestamp": 1.9645630741464408 + }, + { + "x": 6.986658612887269, + "y": 7.539962730359617, + "heading": -3.6183627012139706e-9, + "angularVelocity": 9.85215488559423e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.0534549058539104 + }, + { + "x": 7.3081988424075455, + "y": 7.523367188228691, + "heading": -3.618361818393418e-9, + "angularVelocity": 9.931402417986347e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.1423467375613803 + }, + { + "x": 7.629739071927805, + "y": 7.506771646097765, + "heading": -3.6183609804060354e-9, + "angularVelocity": 9.427045955569345e-15, + "velocityX": 3.617207828255831, + "velocityY": -0.1866936681599647, + "timestamp": 2.23123856926885 + }, + { + "x": 7.922054241421621, + "y": 7.491684486652487, + "heading": -2.171017261999522e-9, + "angularVelocity": 1.6282077786061535e-8, + "velocityX": 3.288436787485536, + "velocityY": -0.16972492472567544, + "timestamp": 2.32013040097632 + }, + { + "x": 8.14129063555876, + "y": 7.480369116190248, + "heading": -1.0855088008297502e-9, + "angularVelocity": 1.221156590340072e-8, + "velocityX": 2.466327782046558, + "velocityY": -0.12729370342458965, + "timestamp": 2.40902223268379 + }, + { + "x": 8.287448236582586, + "y": 7.472825535627514, + "heading": -3.6183629850910975e-10, + "angularVelocity": 8.141046127861815e-9, + "velocityX": 1.6442185768520245, + "velocityY": -0.08486247181359102, + "timestamp": 2.4979140643912596 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 6.2013561613280124e-27, + "angularVelocity": 4.070523596587155e-9, + "velocityX": 0.8221093050723073, + "velocityY": -0.042431236765954845, + "timestamp": 2.5868058960987295 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 4.2684027559613635e-27, + "angularVelocity": -4.179791912699714e-28, + "velocityX": 2.3088842991913657e-27, + "velocityY": -4.6090179027433225e-27, + "timestamp": 2.6756977278061993 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.2.traj b/src/main/deploy/choreo/Centerline Auto.2.traj new file mode 100644 index 0000000..7a0d96d --- /dev/null +++ b/src/main/deploy/choreo/Centerline Auto.2.traj @@ -0,0 +1,248 @@ +{ + "samples": [ + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 4.2684027559613635e-27, + "angularVelocity": -4.179791912699714e-28, + "velocityX": 2.3088842991913657e-27, + "velocityY": -4.6090179027433225e-27, + "timestamp": 0 + }, + { + "x": 8.313753662903038, + "y": 7.460040831603572, + "heading": 0.010953449074032317, + "angularVelocity": 0.1521110200792272, + "velocityX": -0.6495438868437788, + "velocityY": -0.12516272111059268, + "timestamp": 0.07200956951263082 + }, + { + "x": 8.220205864520521, + "y": 7.442024067957108, + "heading": 0.03284727412772724, + "angularVelocity": 0.30404049353266294, + "velocityX": -1.299102313979371, + "velocityY": -0.2501995744232807, + "timestamp": 0.14401913902526164 + }, + { + "x": 8.079881048506913, + "y": 7.415017178316717, + "heading": 0.06565325181712828, + "angularVelocity": 0.45557802819035925, + "velocityX": -1.9486967768776025, + "velocityY": -0.37504584214539577, + "timestamp": 0.21602870853789247 + }, + { + "x": 7.892775351387043, + "y": 7.379038650911545, + "heading": 0.10933893425739379, + "angularVelocity": 0.6066649576706988, + "velocityX": -2.598344892022307, + "velocityY": -0.4996353630313118, + "timestamp": 0.2880382780505233 + }, + { + "x": 7.65888403905923, + "y": 7.3341118085455905, + "heading": 0.1638822878842525, + "angularVelocity": 0.7574459060929595, + "velocityX": -3.248058749841399, + "velocityY": -0.623900999131414, + "timestamp": 0.3600478475631541 + }, + { + "x": 7.402696123218692, + "y": 7.2851734321166655, + "heading": 0.1638823285777561, + "angularVelocity": 5.651124407641853e-7, + "velocityX": -3.557692645219899, + "velocityY": -0.6796093458153674, + "timestamp": 0.43205741707578493 + }, + { + "x": 7.146508206567032, + "y": 7.2362350599332865, + "heading": 0.16388236927081118, + "angularVelocity": 5.651062127144245e-7, + "velocityX": -3.557692656483975, + "velocityY": -0.6796092868572866, + "timestamp": 0.5040669865884158 + }, + { + "x": 6.8903202899153655, + "y": 7.187296687749944, + "heading": 0.16388240996386633, + "angularVelocity": 5.651062132417643e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567837, + "timestamp": 0.5760765561010466 + }, + { + "x": 6.634132373263698, + "y": 7.138358315566601, + "heading": 0.16388245065692145, + "angularVelocity": 5.651062127692066e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567831, + "timestamp": 0.6480861256136774 + }, + { + "x": 6.377944456612031, + "y": 7.089419943383258, + "heading": 0.1638824913499765, + "angularVelocity": 5.651062124923317e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567826, + "timestamp": 0.7200956951263082 + }, + { + "x": 6.121756539960365, + "y": 7.040481571199916, + "heading": 0.1638825320430316, + "angularVelocity": 5.65106212535781e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567821, + "timestamp": 0.792105264638939 + }, + { + "x": 5.865568623308698, + "y": 6.991543199016573, + "heading": 0.16388257273608667, + "angularVelocity": 5.651062121582895e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567813, + "timestamp": 0.8641148341515699 + }, + { + "x": 5.6093807066570305, + "y": 6.94260482683323, + "heading": 0.16388261342914182, + "angularVelocity": 5.651062135141695e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567811, + "timestamp": 0.9361244036642007 + }, + { + "x": 5.35319279000551, + "y": 6.893666454649124, + "heading": 0.16388265412219696, + "angularVelocity": 5.651062132389692e-7, + "velocityX": -3.5576926564820495, + "velocityY": -0.6796092868673679, + "timestamp": 1.0081339731768315 + }, + { + "x": 5.0970048904418945, + "y": 6.844727993011475, + "heading": 0.16388269481525294, + "angularVelocity": 5.651062246743821e-7, + "velocityX": -3.5576924191815573, + "velocityY": -0.6796105291126032, + "timestamp": 1.0801435426894623 + }, + { + "x": 4.821698218246571, + "y": 6.778961174770714, + "heading": 0.16388272951436206, + "angularVelocity": 4.4401904271117674e-7, + "velocityX": -3.522897507544664, + "velocityY": -0.8415697237266404, + "timestamp": 1.1582913376808648 + }, + { + "x": 4.546391565680663, + "y": 6.7131942743592505, + "heading": 0.163882764213471, + "angularVelocity": 4.440190404819063e-7, + "velocityX": -3.5228972563614294, + "velocityY": -0.8415707752048409, + "timestamp": 1.2364391326722672 + }, + { + "x": 4.271084913114876, + "y": 6.647427373947279, + "heading": 0.16388279891257995, + "angularVelocity": 4.440190403154282e-7, + "velocityX": -3.522897256359876, + "velocityY": -0.8415707752113447, + "timestamp": 1.3145869276636697 + }, + { + "x": 3.995778260549088, + "y": 6.5816604735353135, + "heading": 0.1638828336116889, + "angularVelocity": 4.440190406386135e-7, + "velocityX": -3.5228972563598964, + "velocityY": -0.8415707752112599, + "timestamp": 1.3927347226550721 + }, + { + "x": 3.7204716077277675, + "y": 6.515893574193041, + "heading": 0.16388286831080823, + "angularVelocity": 4.440191733622333e-7, + "velocityX": -3.522897259629757, + "velocityY": -0.8415707615231752, + "timestamp": 1.470882517646475 + }, + { + "x": 3.446523890940015, + "y": 6.450633718978252, + "heading": 0.1690942590558449, + "angularVelocity": 0.06668634406908087, + "velocityX": -3.50550795218074, + "velocityY": -0.8350824898126525, + "timestamp": 1.549030312637878 + }, + { + "x": 3.2268656747177786, + "y": 6.398260340243826, + "heading": 0.190922516728536, + "angularVelocity": 0.27932019931071, + "velocityX": -2.8108050425018627, + "velocityY": -0.670183704353886, + "timestamp": 1.6271781076292808 + }, + { + "x": 3.062219316413325, + "y": 6.3589866212900406, + "heading": 0.21130201229843995, + "angularVelocity": 0.2607814535540765, + "velocityX": -2.1068586557377182, + "velocityY": -0.5025569686016857, + "timestamp": 1.7053259026206837 + }, + { + "x": 2.9525091880643317, + "y": 6.3328115268917164, + "heading": 0.22637932021512078, + "angularVelocity": 0.1929332480633594, + "velocityX": -1.4038800245235723, + "velocityY": -0.334943479866623, + "timestamp": 1.7834736976120866 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.10283289870283706, + "velocityX": -0.7016723976195487, + "velocityY": -0.1674338082530608, + "timestamp": 1.8616214926034895 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 2.348167343500178e-28, + "velocityX": 2.328514438851991e-27, + "velocityY": -3.011007529624761e-27, + "timestamp": 1.9397692875948924 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.3.traj b/src/main/deploy/choreo/Centerline Auto.3.traj new file mode 100644 index 0000000..fdb2b6d --- /dev/null +++ b/src/main/deploy/choreo/Centerline Auto.3.traj @@ -0,0 +1,239 @@ +{ + "samples": [ + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 2.348167343500178e-28, + "velocityX": 2.328514438851991e-27, + "velocityY": -3.011007529624761e-27, + "timestamp": 0 + }, + { + "x": 2.931995611661183, + "y": 6.317030660120305, + "heading": 0.22827399101803314, + "angularVelocity": -0.1004895685231517, + "velocityX": 0.5615669396966905, + "velocityY": -0.04411767290505802, + "timestamp": 0.06111573144901783 + }, + { + "x": 3.0006366597167062, + "y": 6.311640001770152, + "heading": 0.21599961790492203, + "angularVelocity": -0.20083819373658737, + "velocityX": 1.1231322349922719, + "velocityY": -0.08820410428451562, + "timestamp": 0.12223146289803566 + }, + { + "x": 3.1035985534895527, + "y": 6.303558029226905, + "heading": 0.1976138856084477, + "angularVelocity": -0.30083469281246417, + "velocityX": 1.684703616101463, + "velocityY": -0.13224046168846443, + "timestamp": 0.1833471943470535 + }, + { + "x": 3.2408820865427965, + "y": 6.29278900601246, + "heading": 0.173149008298291, + "angularVelocity": -0.4003040907816843, + "velocityX": 2.2462879818065202, + "velocityY": -0.17620705764487352, + "timestamp": 0.24446292579607132 + }, + { + "x": 3.412488399373154, + "y": 6.279338451783206, + "heading": 0.14264484283477577, + "angularVelocity": -0.4991213348884074, + "velocityX": 2.807891008774219, + "velocityY": -0.22008333877955472, + "timestamp": 0.30557865724508915 + }, + { + "x": 3.618418876396286, + "y": 6.263213190116882, + "heading": 0.10614503413984216, + "angularVelocity": -0.597224443355985, + "velocityX": 3.3695166881037326, + "velocityY": -0.26384796981077935, + "timestamp": 0.366694388694107 + }, + { + "x": 3.839130420023726, + "y": 6.246248871896369, + "heading": 0.10614500290272542, + "angularVelocity": -5.111141764182067e-7, + "velocityX": 3.611370401605275, + "velocityY": -0.2775769481653736, + "timestamp": 0.4278101201431248 + }, + { + "x": 4.059841964366672, + "y": 6.229284562983422, + "heading": 0.10614497166605225, + "angularVelocity": -5.111069183870487e-7, + "velocityX": 3.6113704133126814, + "velocityY": -0.2775767958712516, + "timestamp": 0.48892585159214264 + }, + { + "x": 4.280553508709572, + "y": 6.212320254069855, + "heading": 0.10614494042937911, + "angularVelocity": -5.111069180282712e-7, + "velocityX": 3.6113704133119002, + "velocityY": -0.2775767958814193, + "timestamp": 0.5500415830411605 + }, + { + "x": 4.501265048980713, + "y": 6.1953558921813965, + "heading": 0.1061449091927048, + "angularVelocity": -5.111069369418327e-7, + "velocityX": 3.611370346688179, + "velocityY": -0.27757766267774153, + "timestamp": 0.6111573144901783 + }, + { + "x": 4.810869637193169, + "y": 6.16135848513122, + "heading": 0.10614488355477533, + "angularVelocity": -2.98142570668873e-7, + "velocityX": 3.6003807524808256, + "velocityY": -0.39535463826432365, + "timestamp": 0.6971494940670535 + }, + { + "x": 5.120474220145008, + "y": 6.12736103017416, + "heading": 0.1061448579168455, + "angularVelocity": -2.9814257488756455e-7, + "velocityX": 3.600380691305304, + "velocityY": -0.39535519537177727, + "timestamp": 0.7831416736439287 + }, + { + "x": 5.430078803096823, + "y": 6.093363575216878, + "heading": 0.1061448322789156, + "angularVelocity": -2.981425755600158e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435887, + "timestamp": 0.8691338532208039 + }, + { + "x": 5.739683386048638, + "y": 6.059366120259595, + "heading": 0.10614480664098574, + "angularVelocity": -2.981425750224502e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.39535519537435865, + "timestamp": 0.9551260327976792 + }, + { + "x": 6.049287969000453, + "y": 6.025368665302312, + "heading": 0.10614478100305595, + "angularVelocity": -2.981425744595309e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.3953551953743586, + "timestamp": 1.0411182123745544 + }, + { + "x": 6.358892551952268, + "y": 5.991371210345029, + "heading": 0.1061447553651262, + "angularVelocity": -2.98142573863633e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 1.1271103919514296 + }, + { + "x": 6.668497134904083, + "y": 5.9573737553877475, + "heading": 0.10614472972719659, + "angularVelocity": -2.981425722498942e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 1.2131025715283048 + }, + { + "x": 6.978101717855898, + "y": 5.923376300430465, + "heading": 0.10614470408926703, + "angularVelocity": -2.981425716615336e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435826, + "timestamp": 1.29909475110518 + }, + { + "x": 7.287706300807711, + "y": 5.889378845473171, + "heading": 0.10614467845133739, + "angularVelocity": -2.9814257255193557e-7, + "velocityX": 3.600380691305006, + "velocityY": -0.39535519537448677, + "timestamp": 1.3850869306820552 + }, + { + "x": 7.597310883497341, + "y": 5.855381388128685, + "heading": 0.10614465281325862, + "angularVelocity": -2.981443068468836e-7, + "velocityX": 3.600380688256082, + "velocityY": -0.39535522313506416, + "timestamp": 1.4710791102589305 + }, + { + "x": 7.868541995298217, + "y": 5.825436456932061, + "heading": 0.06366050775847867, + "angularVelocity": -0.4940466128876299, + "velocityX": 3.154136959145237, + "velocityY": -0.34822854059484143, + "timestamp": 1.5570712898358057 + }, + { + "x": 8.071966668862895, + "y": 5.802984598737895, + "heading": 0.03182759417957115, + "angularVelocity": -0.3701838206164972, + "velocityX": 2.365618298845671, + "velocityY": -0.2610918609650363, + "timestamp": 1.6430634694126809 + }, + { + "x": 8.207583377724607, + "y": 5.788018984348283, + "heading": 0.01061131016646268, + "angularVelocity": -0.24672341272780052, + "velocityX": 1.577081887318283, + "velocityY": -0.17403459783495048, + "timestamp": 1.729055648989556 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.762629528326585e-25, + "angularVelocity": -0.12339854878287405, + "velocityX": 0.7885391588323608, + "velocityY": -0.08701732658634945, + "timestamp": 1.8150478285664313 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.890807484355009e-25, + "angularVelocity": 1.714109587590018e-25, + "velocityX": -1.5632941987910777e-25, + "velocityY": 4.01842027987716e-26, + "timestamp": 1.9010400081433065 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.4.traj b/src/main/deploy/choreo/Centerline Auto.4.traj new file mode 100644 index 0000000..3ab4d9f --- /dev/null +++ b/src/main/deploy/choreo/Centerline Auto.4.traj @@ -0,0 +1,239 @@ +{ + "samples": [ + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.890807484355009e-25, + "angularVelocity": 1.714109587590018e-25, + "velocityX": -1.5632941987910777e-25, + "velocityY": 4.01842027987716e-26, + "timestamp": 0 + }, + { + "x": 8.217699478068795, + "y": 5.791359796793642, + "heading": 0.01104341509549353, + "angularVelocity": 0.13833447495381113, + "velocityX": -0.7226755833441465, + "velocityY": 0.1355812541876774, + "timestamp": 0.07983125753129006 + }, + { + "x": 8.10231615629365, + "y": 5.8130147597317485, + "heading": 0.03315104134696604, + "angularVelocity": 0.276929450131827, + "velocityX": -1.4453401505033163, + "velocityY": 0.2712591985616573, + "timestamp": 0.1596625150625801 + }, + { + "x": 7.929243964127431, + "y": 5.845512314072997, + "heading": 0.06637865596420629, + "angularVelocity": 0.4162231141632274, + "velocityX": -2.167975270818996, + "velocityY": 0.40707807124934986, + "timestamp": 0.23949377259387017 + }, + { + "x": 7.698485965371198, + "y": 5.888866692718938, + "heading": 0.11081243304487726, + "angularVelocity": 0.5565962312851562, + "velocityX": -2.8905720126704284, + "velocityY": 0.5430752312645023, + "timestamp": 0.3193250301251602 + }, + { + "x": 7.41422585651022, + "y": 5.941821981236076, + "heading": 0.11081246572396981, + "angularVelocity": 4.0935209556931575e-7, + "velocityX": -3.5607620079085063, + "velocityY": 0.6633402774142934, + "timestamp": 0.3991562876564503 + }, + { + "x": 7.129965619426801, + "y": 5.994776581882574, + "heading": 0.11081249814230922, + "angularVelocity": 4.060857915201533e-7, + "velocityX": -3.5607636140768695, + "velocityY": 0.6633316608565432, + "timestamp": 0.47898754518774034 + }, + { + "x": 6.8457053823426035, + "y": 6.047731182524891, + "heading": 0.11081253056064869, + "angularVelocity": 4.060857924168534e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 0.5588188027190304 + }, + { + "x": 6.561445145258405, + "y": 6.100685783167208, + "heading": 0.11081256297898824, + "angularVelocity": 4.060857934452354e-7, + "velocityX": -3.5607636140866235, + "velocityY": 0.6633316608041844, + "timestamp": 0.6386500602503205 + }, + { + "x": 6.277184908174208, + "y": 6.153640383809526, + "heading": 0.11081259539732799, + "angularVelocity": 4.060857956958186e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 0.7184813177816105 + }, + { + "x": 5.99292467109001, + "y": 6.206594984451843, + "heading": 0.11081262781566778, + "angularVelocity": 4.060857964387038e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041848, + "timestamp": 0.7983125753129006 + }, + { + "x": 5.708664434005791, + "y": 6.259549585094043, + "heading": 0.11081266023400761, + "angularVelocity": 4.060857968841719e-7, + "velocityX": -3.5607636140868966, + "velocityY": 0.6633316608027189, + "timestamp": 0.8781438328441906 + }, + { + "x": 5.424404193332451, + "y": 6.312504166469845, + "heading": 0.11081269265234713, + "angularVelocity": 4.060857929641026e-7, + "velocityX": -3.5607636590457346, + "velocityY": 0.6633314194636794, + "timestamp": 0.9579750903754807 + }, + { + "x": 5.139572620391846, + "y": 6.3622941970825195, + "heading": 0.11081272507571231, + "angularVelocity": 4.061487466795785e-7, + "velocityX": -3.567920407980088, + "velocityY": 0.6236909219820286, + "timestamp": 1.0378063479067707 + }, + { + "x": 4.8822045552292, + "y": 6.360012293211402, + "heading": 0.1108127534765096, + "angularVelocity": 3.996777164840354e-7, + "velocityX": -3.6218800325558864, + "velocityY": -0.03211269456367951, + "timestamp": 1.108865594168173 + }, + { + "x": 4.624881625896748, + "y": 6.3546795533312395, + "heading": 0.11081278178743512, + "angularVelocity": 3.98412972255652e-7, + "velocityX": -3.6212448466713205, + "velocityY": -0.07504638960770253, + "timestamp": 1.1799248404295755 + }, + { + "x": 4.367558697083316, + "y": 6.349346788406661, + "heading": 0.11081281009836048, + "angularVelocity": 3.9841297029584103e-7, + "velocityX": -3.62124483936728, + "velocityY": -0.07504674205185301, + "timestamp": 1.2509840866909778 + }, + { + "x": 4.110235768269888, + "y": 6.344014023481877, + "heading": 0.11081283840928584, + "angularVelocity": 3.9841296996573985e-7, + "velocityX": -3.6212448393672196, + "velocityY": -0.075046742054734, + "timestamp": 1.3220433329523802 + }, + { + "x": 3.852912839456459, + "y": 6.338681258557136, + "heading": 0.11081286672021123, + "angularVelocity": 3.984129707844367e-7, + "velocityX": -3.621244839367232, + "velocityY": -0.07504674205415067, + "timestamp": 1.3931025792137826 + }, + { + "x": 3.5955899105380604, + "y": 6.333348498701343, + "heading": 0.110812895031426, + "angularVelocity": 3.98417042898143e-7, + "velocityX": -3.6212448408444415, + "velocityY": -0.07504667072002474, + "timestamp": 1.464161825475185 + }, + { + "x": 3.3629508998707167, + "y": 6.328797181710379, + "heading": 0.15195588309543634, + "angularVelocity": 0.5789955597426332, + "velocityX": -3.2738738856241922, + "velocityY": -0.06404960973301323, + "timestamp": 1.5352210717365873 + }, + { + "x": 3.176840519639808, + "y": 6.325164863882251, + "heading": 0.18492044940341654, + "angularVelocity": 0.4639025607830811, + "velocityX": -2.6190874519872214, + "velocityY": -0.05111675143253586, + "timestamp": 1.6062803179979896 + }, + { + "x": 3.0372579905429093, + "y": 6.322444909479725, + "heading": 0.2096671002374919, + "angularVelocity": 0.3482537760538718, + "velocityX": -1.9643119852893276, + "velocityY": -0.03827727629589979, + "timestamp": 1.677339564259392 + }, + { + "x": 2.944202841875484, + "y": 6.320632970128358, + "heading": 0.22616886731909797, + "angularVelocity": 0.2322254731059452, + "velocityX": -1.3095431427052717, + "velocityY": -0.025498994806420823, + "timestamp": 1.7483988105207944 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.11605269709570704, + "velocityX": -0.6547748103081646, + "velocityY": -0.012750292274397987, + "timestamp": 1.8194580567821967 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 6.477597733926451e-26, + "velocityX": -1.5355730051117952e-25, + "velocityY": -1.2183012914286086e-25, + "timestamp": 1.890517303043599 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.traj b/src/main/deploy/choreo/Centerline Auto.traj new file mode 100644 index 0000000..92fc558 --- /dev/null +++ b/src/main/deploy/choreo/Centerline Auto.traj @@ -0,0 +1,1022 @@ +{ + "samples": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "angularVelocity": 5.825284855617402e-28, + "velocityX": -1.9685293298057674e-27, + "velocityY": 1.4418564963898336e-27, + "timestamp": 0 + }, + { + "x": 0.6784036347143663, + "y": 6.707198268292706, + "heading": 1.0175475056665193, + "angularVelocity": -0.5991822637957332, + "velocityX": 0.32672283914932904, + "velocityY": 0.23758411786628814, + "timestamp": 0.05393128412995569 + }, + { + "x": 0.7135162843776108, + "y": 6.732573985913541, + "heading": 0.9516130808872657, + "angularVelocity": -1.2225635981589924, + "velocityX": 0.6510627408506594, + "velocityY": 0.4705194402508243, + "timestamp": 0.10786256825991138 + }, + { + "x": 0.7660994692609764, + "y": 6.770017684654193, + "heading": 0.8504346226500515, + "angularVelocity": -1.8760624722639487, + "velocityX": 0.9750033905489506, + "velocityY": 0.6942853177837462, + "timestamp": 0.16179385238986707 + }, + { + "x": 0.836479083356807, + "y": 6.818804043384565, + "heading": 0.7130270962325688, + "angularVelocity": -2.5478259721459318, + "velocityX": 1.3049868036933845, + "velocityY": 0.9046022084846815, + "timestamp": 0.21572513651982275 + }, + { + "x": 0.9255037273969096, + "y": 6.878132431612943, + "heading": 0.5402736783143448, + "angularVelocity": -3.2032135096569965, + "velocityX": 1.6507050680563082, + "velocityY": 1.1000737176110378, + "timestamp": 0.26965642064977846 + }, + { + "x": 1.035016950774323, + "y": 6.947674803955847, + "heading": 0.3381434888609651, + "angularVelocity": -3.747920946334524, + "velocityX": 2.0306066347970617, + "velocityY": 1.289462571952319, + "timestamp": 0.32358770477973414 + }, + { + "x": 1.162461605941772, + "y": 7.018689321892079, + "heading": 0.18269313635392612, + "angularVelocity": -2.882378104190095, + "velocityX": 2.3630932810787844, + "velocityY": 1.3167592628633178, + "timestamp": 0.3775189889096898 + }, + { + "x": 1.3050246421848908, + "y": 7.094859887834467, + "heading": 0.07841153426675496, + "angularVelocity": -1.9336013182235503, + "velocityX": 2.6434200212921195, + "velocityY": 1.4123632910138557, + "timestamp": 0.4314502730396455 + }, + { + "x": 1.4591047302314346, + "y": 7.181935811218979, + "heading": 0.022297438159640273, + "angularVelocity": -1.0404739477721165, + "velocityX": 2.8569705048235896, + "velocityY": 1.6145716681748061, + "timestamp": 0.4853815571696012 + }, + { + "x": 1.6255168678692307, + "y": 7.279018436792272, + "heading": 0.014123453422294747, + "angularVelocity": -0.15156295402959544, + "velocityX": 3.0856327699670643, + "velocityY": 1.800117077489892, + "timestamp": 0.5393128412995569 + }, + { + "x": 1.793858101376193, + "y": 7.378107838800647, + "heading": 0.014120593150970575, + "angularVelocity": -0.00005303547598235602, + "velocityX": 3.121402284828195, + "velocityY": 1.8373269542331496, + "timestamp": 0.5932441254295127 + }, + { + "x": 1.9621993268656661, + "y": 7.477197254469766, + "heading": 0.014117732942591941, + "angularVelocity": -0.000053034308839039934, + "velocityX": 3.121402136167001, + "velocityY": 1.8373272075322298, + "timestamp": 0.6471754095594684 + }, + { + "x": 2.130540737170093, + "y": 7.576286356115685, + "heading": 0.014114872666943619, + "angularVelocity": -0.00005303555616123241, + "velocityX": 3.121405563026878, + "velocityY": 1.8373213848783807, + "timestamp": 0.7011066936894241 + }, + { + "x": 2.3040566274888983, + "y": 7.663067241706454, + "heading": 0.010006762068371105, + "angularVelocity": -0.0761730536338314, + "velocityX": 3.217351359568822, + "velocityY": 1.609101043870144, + "timestamp": 0.7550379778193799 + }, + { + "x": 2.486186981201172, + "y": 7.724460124969482, + "heading": -5.2714780410694486e-27, + "angularVelocity": -0.18554651960925458, + "velocityX": 3.377081718903682, + "velocityY": 1.1383538191876352, + "timestamp": 0.8089692619493356 + }, + { + "x": 2.8066356107472474, + "y": 7.755704422012626, + "heading": -3.618373256811644e-9, + "angularVelocity": -4.070535151890212e-8, + "velocityX": 3.6049277350997357, + "velocityY": 0.3514867051672969, + "timestamp": 0.8978610936568053 + }, + { + "x": 3.128175858643877, + "y": 7.739109235929212, + "heading": -3.6183723834730783e-9, + "angularVelocity": 9.824733603484871e-15, + "velocityX": 3.6172080349831512, + "velocityY": -0.18668966275806864, + "timestamp": 0.9867529253642751 + }, + { + "x": 3.449716088164231, + "y": 7.722513693799808, + "heading": -3.6183716236893314e-9, + "angularVelocity": 8.547284180686901e-15, + "velocityX": 3.6172078282568996, + "velocityY": -0.18669366814284583, + "timestamp": 1.0756447570717447 + }, + { + "x": 3.771256317684507, + "y": 7.7059181516688815, + "heading": -3.6183708487107216e-9, + "angularVelocity": 8.718220728132132e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997415, + "timestamp": 1.1645365887792143 + }, + { + "x": 4.092796547204784, + "y": 7.689322609537955, + "heading": -3.618370111441706e-9, + "angularVelocity": 8.29400184228923e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997418, + "timestamp": 1.253428420486684 + }, + { + "x": 4.414336776725059, + "y": 7.6727270674070285, + "heading": -3.6183692900088547e-9, + "angularVelocity": 9.240813644117634e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.3423202521941535 + }, + { + "x": 4.735877006245335, + "y": 7.656131525276102, + "heading": -3.6183684638446085e-9, + "angularVelocity": 9.294040074640308e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.4312120839016231 + }, + { + "x": 5.0574172357656115, + "y": 7.6395359831451755, + "heading": -3.618367662939708e-9, + "angularVelocity": 9.009881839671523e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.1866936681599742, + "timestamp": 1.5201039156090927 + }, + { + "x": 5.378957465285888, + "y": 7.62294044101425, + "heading": -3.6183667762068362e-9, + "angularVelocity": 9.975414552450845e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.18669366815997418, + "timestamp": 1.6089957473165624 + }, + { + "x": 5.700497694806164, + "y": 7.606344898883323, + "heading": -3.6183660187139743e-9, + "angularVelocity": 8.521512579724434e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997426, + "timestamp": 1.697887579024032 + }, + { + "x": 6.02203792432644, + "y": 7.589749356752397, + "heading": -3.618365173816459e-9, + "angularVelocity": 9.504782370286536e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.7867794107315016 + }, + { + "x": 6.343578153846717, + "y": 7.57315381462147, + "heading": -3.6183643942346165e-9, + "angularVelocity": 8.770005387165316e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 1.8756712424389712 + }, + { + "x": 6.665118383366993, + "y": 7.556558272490544, + "heading": -3.6183635769900644e-9, + "angularVelocity": 9.193696838193051e-15, + "velocityX": 3.6172078282560163, + "velocityY": -0.18669366815997424, + "timestamp": 1.9645630741464408 + }, + { + "x": 6.986658612887269, + "y": 7.539962730359617, + "heading": -3.6183627012139706e-9, + "angularVelocity": 9.85215488559423e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.0534549058539104 + }, + { + "x": 7.3081988424075455, + "y": 7.523367188228691, + "heading": -3.618361818393418e-9, + "angularVelocity": 9.931402417986347e-15, + "velocityX": 3.617207828256016, + "velocityY": -0.1866936681599742, + "timestamp": 2.1423467375613803 + }, + { + "x": 7.629739071927805, + "y": 7.506771646097765, + "heading": -3.6183609804060354e-9, + "angularVelocity": 9.427045955569345e-15, + "velocityX": 3.617207828255831, + "velocityY": -0.1866936681599647, + "timestamp": 2.23123856926885 + }, + { + "x": 7.922054241421621, + "y": 7.491684486652487, + "heading": -2.171017261999522e-9, + "angularVelocity": 1.6282077786061535e-8, + "velocityX": 3.288436787485536, + "velocityY": -0.16972492472567544, + "timestamp": 2.32013040097632 + }, + { + "x": 8.14129063555876, + "y": 7.480369116190248, + "heading": -1.0855088008297502e-9, + "angularVelocity": 1.221156590340072e-8, + "velocityX": 2.466327782046558, + "velocityY": -0.12729370342458965, + "timestamp": 2.40902223268379 + }, + { + "x": 8.287448236582586, + "y": 7.472825535627514, + "heading": -3.6183629850910975e-10, + "angularVelocity": 8.141046127861815e-9, + "velocityX": 1.6442185768520245, + "velocityY": -0.08486247181359102, + "timestamp": 2.4979140643912596 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 6.2013561613280124e-27, + "angularVelocity": 4.070523596587155e-9, + "velocityX": 0.8221093050723073, + "velocityY": -0.042431236765954845, + "timestamp": 2.5868058960987295 + }, + { + "x": 8.360527038574219, + "y": 7.469053745269775, + "heading": 4.2684027559613635e-27, + "angularVelocity": -4.179791912699714e-28, + "velocityX": 2.3088842991913657e-27, + "velocityY": -4.6090179027433225e-27, + "timestamp": 2.6756977278061993 + }, + { + "x": 8.313753662903038, + "y": 7.460040831603572, + "heading": 0.010953449074032317, + "angularVelocity": 0.1521110200792272, + "velocityX": -0.6495438868437788, + "velocityY": -0.12516272111059268, + "timestamp": 2.74770729731883 + }, + { + "x": 8.220205864520521, + "y": 7.442024067957108, + "heading": 0.03284727412772724, + "angularVelocity": 0.30404049353266294, + "velocityX": -1.299102313979371, + "velocityY": -0.2501995744232807, + "timestamp": 2.819716866831461 + }, + { + "x": 8.079881048506913, + "y": 7.415017178316717, + "heading": 0.06565325181712828, + "angularVelocity": 0.45557802819035925, + "velocityX": -1.9486967768776025, + "velocityY": -0.37504584214539577, + "timestamp": 2.8917264363440918 + }, + { + "x": 7.892775351387043, + "y": 7.379038650911545, + "heading": 0.10933893425739379, + "angularVelocity": 0.6066649576706988, + "velocityX": -2.598344892022307, + "velocityY": -0.4996353630313118, + "timestamp": 2.9637360058567226 + }, + { + "x": 7.65888403905923, + "y": 7.3341118085455905, + "heading": 0.1638822878842525, + "angularVelocity": 0.7574459060929595, + "velocityX": -3.248058749841399, + "velocityY": -0.623900999131414, + "timestamp": 3.0357455753693534 + }, + { + "x": 7.402696123218692, + "y": 7.2851734321166655, + "heading": 0.1638823285777561, + "angularVelocity": 5.651124407641853e-7, + "velocityX": -3.557692645219899, + "velocityY": -0.6796093458153674, + "timestamp": 3.1077551448819842 + }, + { + "x": 7.146508206567032, + "y": 7.2362350599332865, + "heading": 0.16388236927081118, + "angularVelocity": 5.651062127144245e-7, + "velocityX": -3.557692656483975, + "velocityY": -0.6796092868572866, + "timestamp": 3.179764714394615 + }, + { + "x": 6.8903202899153655, + "y": 7.187296687749944, + "heading": 0.16388240996386633, + "angularVelocity": 5.651062132417643e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567837, + "timestamp": 3.251774283907246 + }, + { + "x": 6.634132373263698, + "y": 7.138358315566601, + "heading": 0.16388245065692145, + "angularVelocity": 5.651062127692066e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567831, + "timestamp": 3.3237838534198767 + }, + { + "x": 6.377944456612031, + "y": 7.089419943383258, + "heading": 0.1638824913499765, + "angularVelocity": 5.651062124923317e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567826, + "timestamp": 3.3957934229325075 + }, + { + "x": 6.121756539960365, + "y": 7.040481571199916, + "heading": 0.1638825320430316, + "angularVelocity": 5.65106212535781e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567821, + "timestamp": 3.4678029924451383 + }, + { + "x": 5.865568623308698, + "y": 6.991543199016573, + "heading": 0.16388257273608667, + "angularVelocity": 5.651062121582895e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567813, + "timestamp": 3.539812561957769 + }, + { + "x": 5.6093807066570305, + "y": 6.94260482683323, + "heading": 0.16388261342914182, + "angularVelocity": 5.651062135141695e-7, + "velocityX": -3.5576926564840714, + "velocityY": -0.6796092868567811, + "timestamp": 3.6118221314704 + }, + { + "x": 5.35319279000551, + "y": 6.893666454649124, + "heading": 0.16388265412219696, + "angularVelocity": 5.651062132389692e-7, + "velocityX": -3.5576926564820495, + "velocityY": -0.6796092868673679, + "timestamp": 3.683831700983031 + }, + { + "x": 5.0970048904418945, + "y": 6.844727993011475, + "heading": 0.16388269481525294, + "angularVelocity": 5.651062246743821e-7, + "velocityX": -3.5576924191815573, + "velocityY": -0.6796105291126032, + "timestamp": 3.7558412704956616 + }, + { + "x": 4.821698218246571, + "y": 6.778961174770714, + "heading": 0.16388272951436206, + "angularVelocity": 4.4401904271117674e-7, + "velocityX": -3.522897507544664, + "velocityY": -0.8415697237266404, + "timestamp": 3.833989065487064 + }, + { + "x": 4.546391565680663, + "y": 6.7131942743592505, + "heading": 0.163882764213471, + "angularVelocity": 4.440190404819063e-7, + "velocityX": -3.5228972563614294, + "velocityY": -0.8415707752048409, + "timestamp": 3.9121368604784665 + }, + { + "x": 4.271084913114876, + "y": 6.647427373947279, + "heading": 0.16388279891257995, + "angularVelocity": 4.440190403154282e-7, + "velocityX": -3.522897256359876, + "velocityY": -0.8415707752113447, + "timestamp": 3.990284655469869 + }, + { + "x": 3.995778260549088, + "y": 6.5816604735353135, + "heading": 0.1638828336116889, + "angularVelocity": 4.440190406386135e-7, + "velocityX": -3.5228972563598964, + "velocityY": -0.8415707752112599, + "timestamp": 4.068432450461271 + }, + { + "x": 3.7204716077277675, + "y": 6.515893574193041, + "heading": 0.16388286831080823, + "angularVelocity": 4.440191733622333e-7, + "velocityX": -3.522897259629757, + "velocityY": -0.8415707615231752, + "timestamp": 4.146580245452674 + }, + { + "x": 3.446523890940015, + "y": 6.450633718978252, + "heading": 0.1690942590558449, + "angularVelocity": 0.06668634406908087, + "velocityX": -3.50550795218074, + "velocityY": -0.8350824898126525, + "timestamp": 4.224728040444077 + }, + { + "x": 3.2268656747177786, + "y": 6.398260340243826, + "heading": 0.190922516728536, + "angularVelocity": 0.27932019931071, + "velocityX": -2.8108050425018627, + "velocityY": -0.670183704353886, + "timestamp": 4.30287583543548 + }, + { + "x": 3.062219316413325, + "y": 6.3589866212900406, + "heading": 0.21130201229843995, + "angularVelocity": 0.2607814535540765, + "velocityX": -2.1068586557377182, + "velocityY": -0.5025569686016857, + "timestamp": 4.381023630426883 + }, + { + "x": 2.9525091880643317, + "y": 6.3328115268917164, + "heading": 0.22637932021512078, + "angularVelocity": 0.1929332480633594, + "velocityX": -1.4038800245235723, + "velocityY": -0.334943479866623, + "timestamp": 4.459171425418286 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.10283289870283706, + "velocityX": -0.7016723976195487, + "velocityY": -0.1674338082530608, + "timestamp": 4.537319220409689 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 2.348167343500178e-28, + "velocityX": 2.328514438851991e-27, + "velocityY": -3.011007529624761e-27, + "timestamp": 4.615467015401092 + }, + { + "x": 2.931995611661183, + "y": 6.317030660120305, + "heading": 0.22827399101803314, + "angularVelocity": -0.1004895685231517, + "velocityX": 0.5615669396966905, + "velocityY": -0.04411767290505802, + "timestamp": 4.6765827468501096 + }, + { + "x": 3.0006366597167062, + "y": 6.311640001770152, + "heading": 0.21599961790492203, + "angularVelocity": -0.20083819373658737, + "velocityX": 1.1231322349922719, + "velocityY": -0.08820410428451562, + "timestamp": 4.737698478299127 + }, + { + "x": 3.1035985534895527, + "y": 6.303558029226905, + "heading": 0.1976138856084477, + "angularVelocity": -0.30083469281246417, + "velocityX": 1.684703616101463, + "velocityY": -0.13224046168846443, + "timestamp": 4.798814209748145 + }, + { + "x": 3.2408820865427965, + "y": 6.29278900601246, + "heading": 0.173149008298291, + "angularVelocity": -0.4003040907816843, + "velocityX": 2.2462879818065202, + "velocityY": -0.17620705764487352, + "timestamp": 4.859929941197163 + }, + { + "x": 3.412488399373154, + "y": 6.279338451783206, + "heading": 0.14264484283477577, + "angularVelocity": -0.4991213348884074, + "velocityX": 2.807891008774219, + "velocityY": -0.22008333877955472, + "timestamp": 4.921045672646181 + }, + { + "x": 3.618418876396286, + "y": 6.263213190116882, + "heading": 0.10614503413984216, + "angularVelocity": -0.597224443355985, + "velocityX": 3.3695166881037326, + "velocityY": -0.26384796981077935, + "timestamp": 4.982161404095199 + }, + { + "x": 3.839130420023726, + "y": 6.246248871896369, + "heading": 0.10614500290272542, + "angularVelocity": -5.111141764182067e-7, + "velocityX": 3.611370401605275, + "velocityY": -0.2775769481653736, + "timestamp": 5.0432771355442165 + }, + { + "x": 4.059841964366672, + "y": 6.229284562983422, + "heading": 0.10614497166605225, + "angularVelocity": -5.111069183870487e-7, + "velocityX": 3.6113704133126814, + "velocityY": -0.2775767958712516, + "timestamp": 5.104392866993234 + }, + { + "x": 4.280553508709572, + "y": 6.212320254069855, + "heading": 0.10614494042937911, + "angularVelocity": -5.111069180282712e-7, + "velocityX": 3.6113704133119002, + "velocityY": -0.2775767958814193, + "timestamp": 5.165508598442252 + }, + { + "x": 4.501265048980713, + "y": 6.1953558921813965, + "heading": 0.1061449091927048, + "angularVelocity": -5.111069369418327e-7, + "velocityX": 3.611370346688179, + "velocityY": -0.27757766267774153, + "timestamp": 5.22662432989127 + }, + { + "x": 4.810869637193169, + "y": 6.16135848513122, + "heading": 0.10614488355477533, + "angularVelocity": -2.98142570668873e-7, + "velocityX": 3.6003807524808256, + "velocityY": -0.39535463826432365, + "timestamp": 5.312616509468145 + }, + { + "x": 5.120474220145008, + "y": 6.12736103017416, + "heading": 0.1061448579168455, + "angularVelocity": -2.9814257488756455e-7, + "velocityX": 3.600380691305304, + "velocityY": -0.39535519537177727, + "timestamp": 5.3986086890450204 + }, + { + "x": 5.430078803096823, + "y": 6.093363575216878, + "heading": 0.1061448322789156, + "angularVelocity": -2.981425755600158e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435887, + "timestamp": 5.484600868621896 + }, + { + "x": 5.739683386048638, + "y": 6.059366120259595, + "heading": 0.10614480664098574, + "angularVelocity": -2.981425750224502e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.39535519537435865, + "timestamp": 5.570593048198771 + }, + { + "x": 6.049287969000453, + "y": 6.025368665302312, + "heading": 0.10614478100305595, + "angularVelocity": -2.981425744595309e-7, + "velocityX": 3.60038069130502, + "velocityY": -0.3953551953743586, + "timestamp": 5.656585227775646 + }, + { + "x": 6.358892551952268, + "y": 5.991371210345029, + "heading": 0.1061447553651262, + "angularVelocity": -2.98142573863633e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 5.742577407352521 + }, + { + "x": 6.668497134904083, + "y": 5.9573737553877475, + "heading": 0.10614472972719659, + "angularVelocity": -2.981425722498942e-7, + "velocityX": 3.6003806913050203, + "velocityY": -0.3953551953743583, + "timestamp": 5.8285695869293965 + }, + { + "x": 6.978101717855898, + "y": 5.923376300430465, + "heading": 0.10614470408926703, + "angularVelocity": -2.981425716615336e-7, + "velocityX": 3.6003806913050207, + "velocityY": -0.39535519537435826, + "timestamp": 5.914561766506272 + }, + { + "x": 7.287706300807711, + "y": 5.889378845473171, + "heading": 0.10614467845133739, + "angularVelocity": -2.9814257255193557e-7, + "velocityX": 3.600380691305006, + "velocityY": -0.39535519537448677, + "timestamp": 6.000553946083147 + }, + { + "x": 7.597310883497341, + "y": 5.855381388128685, + "heading": 0.10614465281325862, + "angularVelocity": -2.981443068468836e-7, + "velocityX": 3.600380688256082, + "velocityY": -0.39535522313506416, + "timestamp": 6.086546125660022 + }, + { + "x": 7.868541995298217, + "y": 5.825436456932061, + "heading": 0.06366050775847867, + "angularVelocity": -0.4940466128876299, + "velocityX": 3.154136959145237, + "velocityY": -0.34822854059484143, + "timestamp": 6.172538305236897 + }, + { + "x": 8.071966668862895, + "y": 5.802984598737895, + "heading": 0.03182759417957115, + "angularVelocity": -0.3701838206164972, + "velocityX": 2.365618298845671, + "velocityY": -0.2610918609650363, + "timestamp": 6.258530484813773 + }, + { + "x": 8.207583377724607, + "y": 5.788018984348283, + "heading": 0.01061131016646268, + "angularVelocity": -0.24672341272780052, + "velocityX": 1.577081887318283, + "velocityY": -0.17403459783495048, + "timestamp": 6.344522664390648 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.762629528326585e-25, + "angularVelocity": -0.12339854878287405, + "velocityX": 0.7885391588323608, + "velocityY": -0.08701732658634945, + "timestamp": 6.430514843967523 + }, + { + "x": 8.275391578674316, + "y": 5.78053617477417, + "heading": 6.890807484355009e-25, + "angularVelocity": 1.714109587590018e-25, + "velocityX": -1.5632941987910777e-25, + "velocityY": 4.01842027987716e-26, + "timestamp": 6.516507023544398 + }, + { + "x": 8.217699478068795, + "y": 5.791359796793642, + "heading": 0.01104341509549353, + "angularVelocity": 0.13833447495381113, + "velocityX": -0.7226755833441465, + "velocityY": 0.1355812541876774, + "timestamp": 6.596338281075688 + }, + { + "x": 8.10231615629365, + "y": 5.8130147597317485, + "heading": 0.03315104134696604, + "angularVelocity": 0.276929450131827, + "velocityX": -1.4453401505033163, + "velocityY": 0.2712591985616573, + "timestamp": 6.676169538606978 + }, + { + "x": 7.929243964127431, + "y": 5.845512314072997, + "heading": 0.06637865596420629, + "angularVelocity": 0.4162231141632274, + "velocityX": -2.167975270818996, + "velocityY": 0.40707807124934986, + "timestamp": 6.756000796138268 + }, + { + "x": 7.698485965371198, + "y": 5.888866692718938, + "heading": 0.11081243304487726, + "angularVelocity": 0.5565962312851562, + "velocityX": -2.8905720126704284, + "velocityY": 0.5430752312645023, + "timestamp": 6.8358320536695585 + }, + { + "x": 7.41422585651022, + "y": 5.941821981236076, + "heading": 0.11081246572396981, + "angularVelocity": 4.0935209556931575e-7, + "velocityX": -3.5607620079085063, + "velocityY": 0.6633402774142934, + "timestamp": 6.9156633112008485 + }, + { + "x": 7.129965619426801, + "y": 5.994776581882574, + "heading": 0.11081249814230922, + "angularVelocity": 4.060857915201533e-7, + "velocityX": -3.5607636140768695, + "velocityY": 0.6633316608565432, + "timestamp": 6.995494568732139 + }, + { + "x": 6.8457053823426035, + "y": 6.047731182524891, + "heading": 0.11081253056064869, + "angularVelocity": 4.060857924168534e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 7.075325826263429 + }, + { + "x": 6.561445145258405, + "y": 6.100685783167208, + "heading": 0.11081256297898824, + "angularVelocity": 4.060857934452354e-7, + "velocityX": -3.5607636140866235, + "velocityY": 0.6633316608041844, + "timestamp": 7.155157083794719 + }, + { + "x": 6.277184908174208, + "y": 6.153640383809526, + "heading": 0.11081259539732799, + "angularVelocity": 4.060857956958186e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041847, + "timestamp": 7.234988341326009 + }, + { + "x": 5.99292467109001, + "y": 6.206594984451843, + "heading": 0.11081262781566778, + "angularVelocity": 4.060857964387038e-7, + "velocityX": -3.560763614086623, + "velocityY": 0.6633316608041848, + "timestamp": 7.314819598857299 + }, + { + "x": 5.708664434005791, + "y": 6.259549585094043, + "heading": 0.11081266023400761, + "angularVelocity": 4.060857968841719e-7, + "velocityX": -3.5607636140868966, + "velocityY": 0.6633316608027189, + "timestamp": 7.394650856388589 + }, + { + "x": 5.424404193332451, + "y": 6.312504166469845, + "heading": 0.11081269265234713, + "angularVelocity": 4.060857929641026e-7, + "velocityX": -3.5607636590457346, + "velocityY": 0.6633314194636794, + "timestamp": 7.474482113919879 + }, + { + "x": 5.139572620391846, + "y": 6.3622941970825195, + "heading": 0.11081272507571231, + "angularVelocity": 4.061487466795785e-7, + "velocityX": -3.567920407980088, + "velocityY": 0.6236909219820286, + "timestamp": 7.554313371451169 + }, + { + "x": 4.8822045552292, + "y": 6.360012293211402, + "heading": 0.1108127534765096, + "angularVelocity": 3.996777164840354e-7, + "velocityX": -3.6218800325558864, + "velocityY": -0.03211269456367951, + "timestamp": 7.625372617712571 + }, + { + "x": 4.624881625896748, + "y": 6.3546795533312395, + "heading": 0.11081278178743512, + "angularVelocity": 3.98412972255652e-7, + "velocityX": -3.6212448466713205, + "velocityY": -0.07504638960770253, + "timestamp": 7.696431863973974 + }, + { + "x": 4.367558697083316, + "y": 6.349346788406661, + "heading": 0.11081281009836048, + "angularVelocity": 3.9841297029584103e-7, + "velocityX": -3.62124483936728, + "velocityY": -0.07504674205185301, + "timestamp": 7.767491110235376 + }, + { + "x": 4.110235768269888, + "y": 6.344014023481877, + "heading": 0.11081283840928584, + "angularVelocity": 3.9841296996573985e-7, + "velocityX": -3.6212448393672196, + "velocityY": -0.075046742054734, + "timestamp": 7.8385503564967784 + }, + { + "x": 3.852912839456459, + "y": 6.338681258557136, + "heading": 0.11081286672021123, + "angularVelocity": 3.984129707844367e-7, + "velocityX": -3.621244839367232, + "velocityY": -0.07504674205415067, + "timestamp": 7.909609602758181 + }, + { + "x": 3.5955899105380604, + "y": 6.333348498701343, + "heading": 0.110812895031426, + "angularVelocity": 3.98417042898143e-7, + "velocityX": -3.6212448408444415, + "velocityY": -0.07504667072002474, + "timestamp": 7.980668849019583 + }, + { + "x": 3.3629508998707167, + "y": 6.328797181710379, + "heading": 0.15195588309543634, + "angularVelocity": 0.5789955597426332, + "velocityX": -3.2738738856241922, + "velocityY": -0.06404960973301323, + "timestamp": 8.051728095280986 + }, + { + "x": 3.176840519639808, + "y": 6.325164863882251, + "heading": 0.18492044940341654, + "angularVelocity": 0.4639025607830811, + "velocityX": -2.6190874519872214, + "velocityY": -0.05111675143253586, + "timestamp": 8.122787341542388 + }, + { + "x": 3.0372579905429093, + "y": 6.322444909479725, + "heading": 0.2096671002374919, + "angularVelocity": 0.3482537760538718, + "velocityX": -1.9643119852893276, + "velocityY": -0.03827727629589979, + "timestamp": 8.19384658780379 + }, + { + "x": 2.944202841875484, + "y": 6.320632970128358, + "heading": 0.22616886731909797, + "angularVelocity": 0.2322254731059452, + "velocityX": -1.3095431427052717, + "velocityY": -0.025498994806420823, + "timestamp": 8.264905834065193 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 0.11605269709570704, + "velocityX": -0.6547748103081646, + "velocityY": -0.012750292274397987, + "timestamp": 8.335965080326595 + }, + { + "x": 2.897675037384033, + "y": 6.319726943969727, + "heading": 0.23441548450132177, + "angularVelocity": 6.477597733926451e-26, + "velocityX": -1.5355730051117952e-25, + "velocityY": -1.2183012914286086e-25, + "timestamp": 8.407024326587997 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Note Source Choreo.auto b/src/main/deploy/pathplanner/autos/3 Note Source Choreo.auto new file mode 100644 index 0000000..254a6a0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Note Source Choreo.auto @@ -0,0 +1,188 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6476043462753296, + "y": 4.391018867492676 + }, + "rotation": -60.25510756341669 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "armDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3PieceSourceSide.1" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "3PieceSourceSide.2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3PieceSourceSide.3" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "3PieceSourceSide.4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Note Source.auto b/src/main/deploy/pathplanner/autos/3 Note Source.auto new file mode 100644 index 0000000..884c698 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Note Source.auto @@ -0,0 +1,188 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6706475505032715, + "y": 4.417916777185551 + }, + "rotation": -61.19140611834038 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "armDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SubToCenterline4" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Centerline 4 to shot point" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "shot point to center line 3" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "centerline 3 to shot point" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 note centerline note 4-5.auto b/src/main/deploy/pathplanner/autos/3 note centerline note 4-5.auto new file mode 100644 index 0000000..1f15a3a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 note centerline note 4-5.auto @@ -0,0 +1,188 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6706475505032715, + "y": 4.417916777185551 + }, + "rotation": -61.19140611834038 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "armDown" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sub to Centerline 5" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "centerline 5 to shot point" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "shot point to centerline 4" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Centerline 4 to shot point" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Note.auto b/src/main/deploy/pathplanner/autos/4 Note.auto new file mode 100644 index 0000000..80a9423 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 Note.auto @@ -0,0 +1,183 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3239288017037572, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToTopNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopNoteToCenterNote" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CenterNoteToBottomNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/5 Note.auto b/src/main/deploy/pathplanner/autos/5 Note.auto new file mode 100644 index 0000000..7fc8c14 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/5 Note.auto @@ -0,0 +1,233 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3239288017037572, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToTopNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopNoteToCenterNote" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CenterNoteToBottomNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Spike to Centerline 1" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Centerline 1 to Shot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 Note.auto b/src/main/deploy/pathplanner/autos/6 Note.auto new file mode 100644 index 0000000..ee28f0e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/6 Note.auto @@ -0,0 +1,283 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3239288017037572, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "enableShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToTopNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopNoteToCenterNote" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "feederOn" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CenterNoteToBottomNote" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "armDown" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Spike to Centerline 1" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Centerline 1 to Shot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "shot to 2 centerline" + } + }, + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Centerline 2 to Shot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Subwoofer Taxi.auto b/src/main/deploy/pathplanner/autos/Subwoofer Taxi.auto new file mode 100644 index 0000000..6449003 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Subwoofer Taxi.auto @@ -0,0 +1,69 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.11, + "y": 5.54 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "runShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "subShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feederOn" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "TaxiPath" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Taxi.auto similarity index 86% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Taxi.auto index 4a40188..1cd545c 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -8,7 +8,7 @@ { "type": "path", "data": { - "pathName": "Example Path" + "pathName": "TaxiPath" } } ] diff --git a/src/main/deploy/pathplanner/paths/BottomNoteToCenterLineNote1.path b/src/main/deploy/pathplanner/paths/BottomNoteToCenterLineNote1.path new file mode 100644 index 0000000..568cec2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BottomNoteToCenterLineNote1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.67, + "y": 6.97 + }, + "prevControl": null, + "nextControl": { + "x": 7.946866950862808, + "y": 7.3944135162101166 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.287521550493405, + "y": 7.43 + }, + "prevControl": { + "x": 4.652958749005606, + "y": 7.120220093500354 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "sensorIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 640.0, + "maxAngularAcceleration": 820.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 33.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Spike to Right Spike.path b/src/main/deploy/pathplanner/paths/Center Spike to Right Spike.path new file mode 100644 index 0000000..56f4753 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Spike to Right Spike.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.75, + "y": 5.53 + }, + "prevControl": null, + "nextControl": { + "x": 2.5156594590018955, + "y": 5.059538823310532 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.75, + "y": 4.204940416823851 + }, + "prevControl": { + "x": 2.6292418978024292, + "y": 4.613241981000564 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.55, + "rotationDegrees": -52.6984793631279, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.24203395554145, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterLineNote1ToBottomNote.path b/src/main/deploy/pathplanner/paths/CenterLineNote1ToBottomNote.path new file mode 100644 index 0000000..2aa58eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CenterLineNote1ToBottomNote.path @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.29, + "y": 7.43 + }, + "prevControl": null, + "nextControl": { + "x": 3.5612306451367943, + "y": 7.0766856121097454 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.67, + "y": 6.97 + }, + "prevControl": { + "x": 5.614998833348084, + "y": 7.239024920993821 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "runShooter" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 640.0, + "maxAngularAcceleration": 820.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 33.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path b/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path new file mode 100644 index 0000000..17a1ba6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.71, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.7646700551619987, + "y": 6.150625753187941 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.82, + "y": 6.83 + }, + "prevControl": { + "x": 1.8995074325107804, + "y": 6.471030737761858 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "WallSpike" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 31.949371771711753, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 26.97, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": -51.06999999999999, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path b/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path new file mode 100644 index 0000000..3d9a302 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.44, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 4.682284376332917, + "y": 6.118910565260494 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.71, + "y": 5.55 + }, + "prevControl": { + "x": 7.211301936709776, + "y": 7.058691480664211 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -2.956856243509383, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.2159719365587969, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 60.59, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Centerline 2 to Shot.path b/src/main/deploy/pathplanner/paths/Centerline 2 to Shot.path new file mode 100644 index 0000000..d0d75ce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Centerline 2 to Shot.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.29, + "y": 5.78 + }, + "prevControl": null, + "nextControl": { + "x": 7.15294089683173, + "y": 6.727362969458998 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.71, + "y": 5.55 + }, + "prevControl": { + "x": 4.907787339039305, + "y": 7.071877489430977 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path b/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path new file mode 100644 index 0000000..3324bc4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.563175552421708, + "y": 2.5861525478838003 + }, + "prevControl": null, + "nextControl": { + "x": 5.220329239171783, + "y": 1.0906197179259107 + }, + "isLocked": false, + "linkedName": "Centerline4" + }, + { + "anchor": { + "x": 1.8995533548792793, + "y": 3.3091489994467262 + }, + "prevControl": { + "x": 4.1148072171774785, + "y": 1.504780349665863 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceShotPoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": -70.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Centerline 5 to Sub.path b/src/main/deploy/pathplanner/paths/Centerline 5 to Sub.path new file mode 100644 index 0000000..a69c719 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Centerline 5 to Sub.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.284577350658683, + "y": 0.7785395486367217 + }, + "prevControl": null, + "nextControl": { + "x": 6.251923092479073, + "y": 0.7340185250786149 + }, + "isLocked": false, + "linkedName": "centerline5" + }, + { + "anchor": { + "x": 0.6472211773226715, + "y": 4.402820011287809 + }, + "prevControl": { + "x": 1.5966289454483753, + "y": 1.4756745662037338 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SubwooferBottom" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.27854712151056, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0.39328295759580234, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Drive.path b/src/main/deploy/pathplanner/paths/Drive.path new file mode 100644 index 0000000..17cc8eb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Drive.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.65, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.65, + "y": 2.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 2.0 + }, + "prevControl": { + "x": 2.0, + "y": 2.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 6974417..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,81 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.8506271225505415, - "y": 7.261284207753602 - }, - "prevControl": null, - "nextControl": { - "x": 1.8816257678811654, - "y": 7.375124060433307 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.366769677983801, - "y": 6.999220947515969 - }, - "prevControl": { - "x": 3.748746093323321, - "y": 8.360872157673832 - }, - "nextControl": { - "x": 3.0338608547587467, - "y": 5.812483537004111 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.80754699009398, - "y": 4.25601898888983 - }, - "prevControl": { - "x": 3.1785906475237007, - "y": 4.666531929436711 - }, - "nextControl": { - "x": 1.991368098476938, - "y": 3.353020136595829 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.982031227951344, - "y": 7.261284207753602 - }, - "prevControl": { - "x": 7.210601698981175, - "y": 8.137322552422887 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Spike to Center Spike.path b/src/main/deploy/pathplanner/paths/Left Spike to Center Spike.path new file mode 100644 index 0000000..e692e95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Spike to Center Spike.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.75, + "y": 6.92 + }, + "prevControl": null, + "nextControl": { + "x": 2.5156594590018955, + "y": 6.449538823310531 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.75, + "y": 5.53129077523446 + }, + "prevControl": { + "x": 2.6292418978024292, + "y": 5.939592339411173 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.55, + "rotationDegrees": -51.32706361472603, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": 34.66, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerToCenterNote.path b/src/main/deploy/pathplanner/paths/SpeakerToCenterNote.path new file mode 100644 index 0000000..3447210 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerToCenterNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3239288017037572, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.6336826162025364, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.89, + "y": 5.55 + }, + "prevControl": { + "x": 2.8400000000000003, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path b/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path new file mode 100644 index 0000000..9e52cc9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.35, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.4090249243143167, + "y": 4.529749423648154 + }, + "isLocked": false, + "linkedName": "BottomNote" + }, + { + "anchor": { + "x": 2.7298951438994457, + "y": 4.2952682214771984 + }, + "prevControl": { + "x": 2.1452266447820207, + "y": 4.798162467049247 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -35.50541720876824, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.98000000000002, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stage to Left Spike.path b/src/main/deploy/pathplanner/paths/Stage to Left Spike.path new file mode 100644 index 0000000..6849343 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stage to Left Spike.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.11, + "y": 5.54 + }, + "prevControl": null, + "nextControl": { + "x": 1.9253135132082355, + "y": 6.207797868343774 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.077529297012804, + "y": 7.147903698612302 + }, + "prevControl": { + "x": 2.4794843872442565, + "y": 6.607142608020509 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 47.010941620320914, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.65996494166849, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sub to Centerline 5.path b/src/main/deploy/pathplanner/paths/Sub to Centerline 5.path new file mode 100644 index 0000000..ee1400d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sub to Centerline 5.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.67, + "y": 4.417916777185551 + }, + "prevControl": null, + "nextControl": { + "x": 1.440818731476351, + "y": 3.5180592510994035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 0.7785395486367217 + }, + "prevControl": { + "x": 4.320732882002873, + "y": 0.8229152577765951 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "centerline5" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SubToCenterline4.path b/src/main/deploy/pathplanner/paths/SubToCenterline4.path new file mode 100644 index 0000000..e18d961 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SubToCenterline4.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6472211773226715, + "y": 4.402820011287809 + }, + "prevControl": null, + "nextControl": { + "x": 3.461390197096674, + "y": 1.1646163146235593 + }, + "isLocked": false, + "linkedName": "SubwooferBottom" + }, + { + "anchor": { + "x": 8.563175552421708, + "y": 2.5861525478838003 + }, + "prevControl": { + "x": 5.987310625418366, + "y": 1.2205725045183329 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Centerline4" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": 22.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.9, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": -59.338469266805795, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TaxiPath.path b/src/main/deploy/pathplanner/paths/TaxiPath.path new file mode 100644 index 0000000..83de411 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TaxiPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.1000000000000014, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9026396514331485, + "y": 5.55 + }, + "prevControl": { + "x": 1.9026396514331485, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.39013790017844, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -179.12098285070883, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path new file mode 100644 index 0000000..554f213 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.82, + "y": 6.83 + }, + "prevControl": null, + "nextControl": { + "x": 3.478494160289644, + "y": 7.1328920181305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.442, + "y": 7.44 + }, + "prevControl": { + "x": 3.5519357836362166, + "y": 7.538075620279659 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.9, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 26.97, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Spike to Centerline 2.path b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 2.path new file mode 100644 index 0000000..1a65efd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 2.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.82, + "y": 6.83 + }, + "prevControl": null, + "nextControl": { + "x": 3.478494160289644, + "y": 7.1328920181305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.287089431302343, + "y": 5.776567709683814 + }, + "prevControl": { + "x": 5.602825564103819, + "y": 7.3345503493375155 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": -19.197529689544183, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.7577307040435, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 26.97, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path b/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path new file mode 100644 index 0000000..24142af --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.73, + "y": 4.3 + }, + "prevControl": null, + "nextControl": { + "x": 2.4062484896468637, + "y": 4.5961442632480205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.71, + "y": 5.5548446759057155 + }, + "prevControl": { + "x": 1.0143476058462082, + "y": 5.320961587056917 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "4 Note", + "previewStartingState": { + "rotation": -34.31999999999999, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/centerline 3 to shot point.path b/src/main/deploy/pathplanner/paths/centerline 3 to shot point.path new file mode 100644 index 0000000..f53514d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/centerline 3 to shot point.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.564971929857055, + "y": 4.110976357080029 + }, + "prevControl": null, + "nextControl": { + "x": 5.316677111711016, + "y": 5.06609501912327 + }, + "isLocked": false, + "linkedName": "Centerline3" + }, + { + "anchor": { + "x": 1.8995533548792793, + "y": 3.3091489994467262 + }, + "prevControl": { + "x": 4.05841359226612, + "y": 2.2818001416707445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceShotPoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -70.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path b/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path new file mode 100644 index 0000000..20f1435 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.28, + "y": 0.78 + }, + "prevControl": null, + "nextControl": { + "x": 5.175604333196231, + "y": 1.9905671990919738 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8995533548792793, + "y": 3.3091489994467262 + }, + "prevControl": { + "x": 6.118110187327796, + "y": 1.1904956371542255 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceShotPoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -41.31353520608083, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/shot point to center line 3.path b/src/main/deploy/pathplanner/paths/shot point to center line 3.path new file mode 100644 index 0000000..a57419d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/shot point to center line 3.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8995533548792793, + "y": 3.3091489994467262 + }, + "prevControl": null, + "nextControl": { + "x": 2.48421161723567, + "y": 2.885214536893553 + }, + "isLocked": false, + "linkedName": "SourceShotPoint" + }, + { + "anchor": { + "x": 3.9795879857171372, + "y": 3.236654256651329 + }, + "prevControl": { + "x": 3.314220563010169, + "y": 2.9535480185028966 + }, + "nextControl": { + "x": 5.9607062403254725, + "y": 4.079597444503143 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.564971929857055, + "y": 4.110976357080029 + }, + "prevControl": { + "x": 6.075756556701133, + "y": 4.110976357080029 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Centerline3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": -43.08818935848279, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/shot point to centerline 4.path b/src/main/deploy/pathplanner/paths/shot point to centerline 4.path new file mode 100644 index 0000000..7e6e176 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/shot point to centerline 4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8995533548792793, + "y": 3.3091489994467262 + }, + "prevControl": null, + "nextControl": { + "x": 3.583059974514634, + "y": 1.2532983091367629 + }, + "isLocked": false, + "linkedName": "SourceShotPoint" + }, + { + "anchor": { + "x": 8.27, + "y": 2.44 + }, + "prevControl": { + "x": 5.778838244119921, + "y": 1.1896756147827967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 26.705564141133653, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": -47.23186844477018, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/shot to 2 centerline.path b/src/main/deploy/pathplanner/paths/shot to 2 centerline.path new file mode 100644 index 0000000..7980b4e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/shot to 2 centerline.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.71, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 5.347615643774445, + "y": 6.911095659906697 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.29, + "y": 5.78 + }, + "prevControl": { + "x": 5.723621125243307, + "y": 6.54406357110016 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/edu/wpi/first/wpilibj2/command/BionicWaitCommand.java b/src/main/java/edu/wpi/first/wpilibj2/command/BionicWaitCommand.java deleted file mode 100644 index e26b193..0000000 --- a/src/main/java/edu/wpi/first/wpilibj2/command/BionicWaitCommand.java +++ /dev/null @@ -1,66 +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 edu.wpi.first.wpilibj2.command; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.Timer; -import java.util.function.DoubleSupplier; - -/** - * A command that does nothing but takes a specified amount of time to finish. - * - *

This class is provided by the NewCommands VendorDep - */ -public class BionicWaitCommand extends Command { - /** The timer used for waiting. */ - protected Timer m_timer = new Timer(); - - // private final double m_duration; - private final DoubleSupplier m_delaySeconds; - - /** - * Creates a new WaitCommand. This command will do nothing, and end after the specified duration. - * - * @param seconds the time to wait, in seconds - */ - @SuppressWarnings("this-escape") - public BionicWaitCommand(double seconds) { - // m_duration = seconds; - m_delaySeconds = () -> seconds; - SendableRegistry.setName(this, getName() + ": " + seconds + " seconds"); - } - - public BionicWaitCommand(DoubleSupplier delaySeconds) { - m_delaySeconds = delaySeconds; - SendableRegistry.setName(this, getName() + ": DYNAMIC seconds"); - } - - @Override - public void initialize() { - m_timer.restart(); - } - - @Override - public void end(boolean interrupted) { - m_timer.stop(); - } - - @Override - public boolean isFinished() { - return m_timer.hasElapsed(m_delaySeconds.getAsDouble()); - } - - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("duration", () -> m_delaySeconds.getAsDouble(), null); - } -} diff --git a/src/main/java/frc/lib/LocalADStarAK.java b/src/main/java/frc/lib/LocalADStarAK.java deleted file mode 100644 index 6496b60..0000000 --- a/src/main/java/frc/lib/LocalADStarAK.java +++ /dev/null @@ -1,151 +0,0 @@ -package frc.lib; - -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; -import com.pathplanner.lib.pathfinding.LocalADStar; -import com.pathplanner.lib.pathfinding.Pathfinder; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -// NOTE: This file is available at -// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d - -public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - - if (io.currentPathPoints.isEmpty()) { - return null; - } - - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - - private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - List pathPoints = new ArrayList<>(); - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add( - new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } -} diff --git a/src/main/java/frc/lib/LoggedTunableNumber.java b/src/main/java/frc/lib/LoggedTunableNumber.java new file mode 100644 index 0000000..40bd2c2 --- /dev/null +++ b/src/main/java/frc/lib/LoggedTunableNumber.java @@ -0,0 +1,86 @@ +package frc.lib; + +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (frc.robot.Constants.kInTuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return frc.robot.Constants.kInTuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java new file mode 100644 index 0000000..0d47677 --- /dev/null +++ b/src/main/java/frc/robot/Autos.java @@ -0,0 +1,69 @@ +package frc.robot; + +import com.choreo.lib.Choreo; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.drivetrain.Drivetrain; +import frc.robot.feeder.Feeder; +import frc.robot.intake.Intake; +import frc.robot.shooter.Shooter; + +public class Autos { + + private final Drivetrain m_drivetrain; + private final Shooter m_shooter; + private final Feeder m_feeder; + private final Intake m_intake; + + public Autos(Drivetrain drivetrain, Shooter shooter, Feeder feeder, Intake intake) { + m_drivetrain = drivetrain; + m_shooter = shooter; + m_feeder = feeder; + m_intake = intake; + } + + public Command centerlineTwoPiece() { + return Commands.sequence( + resetPose("Centerline Auto"), + shoot(), + intake().deadlineWith(getPathFollowingCommand("Centerline Auto.1")), + getPathFollowingCommand("Centerline Auto.2"), + shoot(), + intake().deadlineWith(getPathFollowingCommand("Centerline Auto.3")), + getPathFollowingCommand("Centerline Auto.4"), + shoot()) + .withName("Centerline 3 Piece"); + } + + private Command getPathFollowingCommand(String trajectoryName) { + return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(trajectoryName)); + } + + private Command resetPose(String trajectoryName) { + Pose2d startingPose = Choreo.getTrajectory(trajectoryName).getInitialPose(); + return Commands.runOnce( + () -> + m_drivetrain.resetPose.accept( + m_drivetrain.onRedAllianceSupplier.getAsBoolean() + ? GeometryUtil.flipFieldPose(startingPose) + : startingPose)); + } + + private Command intake() { + return Superstructure.sensorIntake(m_feeder, m_intake) + .andThen(m_intake.stop(), m_feeder.stop()); + } + + private Command shoot() { + return Commands.race( + m_shooter.runShooter(), + Commands.waitUntil(m_shooter.readyToShoot) + .andThen(m_feeder.shoot().alongWith(m_intake.intake()).withTimeout(0.5))) + .andThen(m_shooter.stop(), m_feeder.stop(), m_intake.stop()) + .finallyDo(() -> System.out.println("Shooting complete!")); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0cefb68..c78cd71 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,9 +1,42 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.nio.file.Path; +import java.util.function.BooleanSupplier; + public final class Constants { public static final Mode kCurrentMode = Mode.kReal; - public static final RobotType kRobot = RobotType.kPractice; + public static final RobotName kRobot = RobotName.kViper; public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim); + public static final String kDrivetrainCanBus = "CANivore1"; + public static final String kSuperstructureCanBus = "CANivore2"; + public static final boolean kInTuningMode = true; + public static final BooleanSupplier onRedAllianceSupplier = + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + private static final boolean kUseWpiFieldLayout = false; + public static final AprilTagFieldLayout fieldLayout; + + static { + if (kUseWpiFieldLayout) { + try { + fieldLayout = + new AprilTagFieldLayout( + Path.of( + Filesystem.getDeployDirectory().getPath(), + "apriltags", + "2024-wpi-custom.json")); + } catch (IOException e) { + throw new RuntimeException(e); + } + } else fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); + } public static enum Mode { kReal, @@ -11,8 +44,8 @@ public static enum Mode { kReplay } - public static enum RobotType { - kCompetition, - kPractice + public static enum RobotName { + kViper, + kCobra } } diff --git a/src/main/java/frc/robot/FieldPositions.java b/src/main/java/frc/robot/FieldPositions.java new file mode 100644 index 0000000..dd25207 --- /dev/null +++ b/src/main/java/frc/robot/FieldPositions.java @@ -0,0 +1,46 @@ +package frc.robot; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +public class FieldPositions { + public static class NotePositions { + public static final double fieldLength = Constants.fieldLayout.getFieldLength(); + private static final double kCenterlineX = fieldLength / 2.0; + private static final double kCenterlineFirstY = Units.inchesToMeters(29.638); + private static final double kCenterlineSeparationY = Units.inchesToMeters(66); + private static final double kSpikeX = Units.inchesToMeters(114); + private static final double kSpikeFirstY = Units.inchesToMeters(161.638); + private static final double kSpikeSeparationY = Units.inchesToMeters(57); + private static final double kCenterlineNoteCount = 5; + private static final double kSpikeNoteCount = 3; + public static final Translation2d[] noteTranslations = new Translation2d[11]; + + static { + int i = 0; + for (int j = 0; j < kCenterlineNoteCount; j++, i++) + noteTranslations[i] = + new Translation2d(kCenterlineX, kCenterlineFirstY + (j * kCenterlineSeparationY)); + for (int j = 0; j < kSpikeNoteCount; j++, i++) + noteTranslations[i] = new Translation2d(kSpikeX, kSpikeFirstY + (j * kSpikeSeparationY)); + for (int j = 0; j < kSpikeNoteCount; j++, i++) + noteTranslations[i] = + GeometryUtil.flipFieldPosition( + new Translation2d(kSpikeX, kSpikeFirstY + (j * kSpikeSeparationY))); + } + } + + public static final class Speaker { + private static final Translation3d topRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(83.091)); + private static final Translation3d bottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + public static final Translation3d centerSpeakerOpening = + bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5); + } +} diff --git a/src/main/java/frc/robot/NoteVisualizer.java b/src/main/java/frc/robot/NoteVisualizer.java new file mode 100644 index 0000000..86a8ca4 --- /dev/null +++ b/src/main/java/frc/robot/NoteVisualizer.java @@ -0,0 +1,124 @@ +package frc.robot; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +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.ScheduleCommand; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.Stream; +import org.littletonrobotics.junction.Logger; + +public class NoteVisualizer { + private static final double kShotSpeedMps = Units.feetToMeters(35.0); + private static Supplier m_wristPoseSupplier; + private static boolean m_hasNote = false; + private static final List m_notes = new ArrayList<>(); + + public static void showStagedNotes() { + if (m_notes.isEmpty()) { + Logger.recordOutput("NoteVisualizer/StagedNotes"); + } + // Show auto notes + Stream presentNotes = m_notes.stream().filter(Objects::nonNull); + Logger.recordOutput( + "NoteVisualizer/StagedNotes", + presentNotes + .map( + translation -> + new Pose3d( + translation.getX(), + translation.getY(), + Units.inchesToMeters(1.0), + new Rotation3d())) + .toArray(Pose3d[]::new)); + } + + public static void showHeldNotes() { + if (m_hasNote) { + Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d[] {getNotePose()}); + } else { + Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d()); + } + } + + public static void setHasNote(boolean hasNote) { + m_hasNote = hasNote; + } + + public static void removeNote(int note) { + m_notes.set(note, null); + m_hasNote = true; + } + + public static void resetNotes() { + m_notes.clear(); + for (Translation2d note : FieldPositions.NotePositions.noteTranslations) m_notes.add(note); + } + + public static Command shoot() { + return new ScheduleCommand( + Commands.defer( + () -> { + if (!m_hasNote) return Commands.none(); + m_hasNote = false; + final Pose3d startPose = getNotePose(); + final Translation3d speakerTranslation = + FieldPositions.Speaker.centerSpeakerOpening; + final Pose3d endPose; + if (Constants.onRedAllianceSupplier.getAsBoolean()) { + double flippedX = + GeometryUtil.flipFieldPosition(speakerTranslation.toTranslation2d()).getX(); + endPose = + new Pose3d( + new Translation3d( + flippedX, speakerTranslation.getY(), speakerTranslation.getZ()), + startPose.getRotation()); + } else { + endPose = new Pose3d(speakerTranslation, startPose.getRotation()); + } + final double duration = + startPose.getTranslation().getDistance(endPose.getTranslation()) + / kShotSpeedMps; + final Timer timer = new Timer(); + timer.start(); + return Commands.run( + () -> + Logger.recordOutput( + "NoteVisualizer/ShotNotes", + new Pose3d[] { + startPose.interpolate(endPose, timer.get() / duration) + })) + .until(() -> timer.hasElapsed(duration)) + .finallyDo(() -> Logger.recordOutput("NoteVisualizer/ShotNotes")); + }, + Set.of()) + .ignoringDisable(true)); + } + + public static void setWristPoseSupplier(Supplier wristPoseSupplier) { + m_wristPoseSupplier = wristPoseSupplier; + } + + private static Pose3d getNotePose() { + if (m_wristPoseSupplier == null) { + return new Pose3d(); + } + Pose3d wristPose = m_wristPoseSupplier.get(); + Transform3d noteTransform = + new Transform3d( + wristPose.getX(), wristPose.getY(), wristPose.getZ(), wristPose.getRotation()) + .plus(new Transform3d(0.0, 0.0, 0.0, new Rotation3d())); + return new Pose3d(PoseEstimation.getInstance().getPose()).transformBy(noteTransform); + } +} diff --git a/src/main/java/frc/robot/PoseEstimation.java b/src/main/java/frc/robot/PoseEstimation.java new file mode 100644 index 0000000..0a32b24 --- /dev/null +++ b/src/main/java/frc/robot/PoseEstimation.java @@ -0,0 +1,159 @@ +package frc.robot; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import frc.robot.drivetrain.Drivetrain; +import frc.robot.vision.Vision.VisionUpdate; +import org.littletonrobotics.junction.AutoLogOutput; + +public class PoseEstimation { + + private static PoseEstimation m_instance; + private final SwerveDrivePoseEstimator m_poseEstimator; + private final boolean kLookaheadDisable = true; + private final double kLookaheadSeconds = 0.0; + private Twist2d m_robotVelocity = new Twist2d(); + private AimingParameters m_lastAimingParameters = null; + + private static final InterpolatingDoubleTreeMap wristAngleMap = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap elbowAngleMap = new InterpolatingDoubleTreeMap(); + + static { + wristAngleMap.put(1.0, 0.0); + wristAngleMap.put(1.0, 0.0); + } + + static { + elbowAngleMap.put(1.0, 0.0); + } + + public record AimingParameters( + Rotation2d driveHeading, + Rotation2d armAngle, + double effectiveDistance, + double driveFeedVelocity) {} + + public PoseEstimation() { + m_poseEstimator = + new SwerveDrivePoseEstimator( + Drivetrain.swerveKinematics, + new Rotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + new Pose2d()); + } + + public void addOdometryMeasurement( + double timestamp, Rotation2d yawPosition, SwerveModulePosition[] modulePositions) { + m_lastAimingParameters = null; + m_poseEstimator.updateWithTime(timestamp, yawPosition, modulePositions); + } + + public void setVelocity(Twist2d robotVelocity) { + m_lastAimingParameters = null; + m_robotVelocity = robotVelocity; + } + + public void addVisionMeasurement(VisionUpdate visionUpdate) { + m_lastAimingParameters = null; + m_poseEstimator.addVisionMeasurement( + visionUpdate.pose(), visionUpdate.timestampSeconds(), visionUpdate.standardDeviations()); + } + + @AutoLogOutput(key = "PoseEstimation/EstimatedPose") + public Pose2d getPose() { + return m_poseEstimator + .getEstimatedPosition() + .plus(new Transform2d(new Translation2d(Units.inchesToMeters(2.5), 0.0), new Rotation2d())); + } + + @AutoLogOutput(key = "PoseEstimation/FieldVelocity") + public Twist2d getFieldVelocity() { + final Translation2d linearFieldVelocity = + new Translation2d(m_robotVelocity.dx, m_robotVelocity.dy).rotateBy(getPose().getRotation()); + return new Twist2d( + linearFieldVelocity.getX(), linearFieldVelocity.getY(), m_robotVelocity.dtheta); + } + + public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) { + return getPose() + .exp( + new Twist2d( + m_robotVelocity.dx * translationLookaheadS, + m_robotVelocity.dy * translationLookaheadS, + m_robotVelocity.dtheta * rotationLookaheadS)); + } + + public AimingParameters getAimingParameters() { + if (m_lastAimingParameters != null) return m_lastAimingParameters; + + Translation2d speakerPosition = FieldPositions.Speaker.centerSpeakerOpening.toTranslation2d(); + speakerPosition = + Constants.onRedAllianceSupplier.getAsBoolean() + ? GeometryUtil.flipFieldPosition(speakerPosition) + : speakerPosition; + final Transform2d fieldToTarget = new Transform2d(speakerPosition, new Rotation2d()); + final Pose2d fieldToPredictedVehicle = + kLookaheadDisable ? getPose() : getPredictedPose(kLookaheadSeconds, kLookaheadSeconds); + final Pose2d fieldToPredictedVehicleFixed = + new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d()); + + final Translation2d predictedVehicleToTargetTranslation = + poseInverse(fieldToPredictedVehicle).transformBy(fieldToTarget).getTranslation(); + final Translation2d predictedVehicleFixedToTargetTranslation = + poseInverse(fieldToPredictedVehicleFixed).transformBy(fieldToTarget).getTranslation(); + + /** + * Rotate both angles by 180 degrees so that the back (shooter) points towards the target and + * not the front (intake) + */ + final Rotation2d vehicleToGoalDirection = + predictedVehicleToTargetTranslation.getAngle().rotateBy(new Rotation2d(Math.PI)); + final Rotation2d targetVehicleDirection = + predictedVehicleFixedToTargetTranslation.getAngle().rotateBy(new Rotation2d(Math.PI)); + + double targetDistance = predictedVehicleToTargetTranslation.getNorm(); + + double feedVelocity = + m_robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance + - m_robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance; + + m_lastAimingParameters = + new AimingParameters( + targetVehicleDirection, + Rotation2d.fromDegrees(elbowAngleMap.get(targetDistance)), + targetDistance, + feedVelocity); + return m_lastAimingParameters; + } + + private Pose2d poseInverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + public void resetPose( + Rotation2d currentYawPosition, + SwerveModulePosition[] currentModulePositions, + Pose2d newPose) { + m_poseEstimator.resetPosition(currentYawPosition, currentModulePositions, newPose); + } + + public static PoseEstimation getInstance() { + if (m_instance == null) m_instance = new PoseEstimation(); + return m_instance; + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80ac22d..430cf7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,17 +2,20 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RepeatCommand; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.arm.Arm; +import frc.robot.climber.Climber; import frc.robot.drivetrain.Drivetrain; +import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; +import frc.robot.lights.Lights; import frc.robot.shooter.Shooter; import frc.robot.vision.Vision; import org.littletonrobotics.junction.LogFileUtil; @@ -28,24 +31,28 @@ public class Robot extends LoggedRobot { private final LoggedDashboardChooser m_autoChooser; private final Drivetrain m_drivetrain; private final Vision m_vision; - private final Intake m_intake = new Intake(); - private final Shooter m_shooter = new Shooter(); - private TimeOfFlight mytimeofflight = new TimeOfFlight(12); + private final Intake m_intake; + private final Arm m_arm; + private final Climber m_climber; + private final Shooter m_shooter; + private final Feeder m_feeder; + private final Lights m_lights; + + @SuppressWarnings("unused") + private final PoseEstimation m_poseEstimation = PoseEstimation.getInstance(); private final CommandXboxController m_driverController = new CommandXboxController(0); private final CommandXboxController m_operatorController = new CommandXboxController(1); public Robot() { recordMetadeta(); - + DriverStation.silenceJoystickConnectionWarning(true); switch (Constants.kCurrentMode) { case kReal: - // TODO find out why this causes weird errors - // Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs")); Logger.addDataReceiver(new NT4Publisher()); break; case kSim: - DriverStation.silenceJoystickConnectionWarning(true); Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); break; @@ -63,152 +70,154 @@ public Robot() { case kReal: m_drivetrain = Subsystems.createTalonFXDrivetrain(); m_vision = Subsystems.createFourCameraVision(); + m_intake = Subsystems.createSparkMAXIntake(); + m_arm = Subsystems.createTalonFXArm(); + m_climber = Subsystems.createSparkMAXClimber(); + m_shooter = Subsystems.createTalonFXShooter(); + m_feeder = Subsystems.createTalonFXFeeder(); break; case kSim: - m_drivetrain = Subsystems.createTalonFXDrivetrain(); + m_drivetrain = Subsystems.createSimDrivetrain(); m_vision = Subsystems.createFourCameraVision(); + m_intake = Subsystems.createSimIntake(); + m_arm = Subsystems.createTalonFXArm(); + m_climber = Subsystems.createBlankClimber(); + m_shooter = Subsystems.createSimShooter(); + m_feeder = Subsystems.createSimFeeder(); break; default: m_drivetrain = Subsystems.createBlankDrivetrain(); m_vision = Subsystems.createBlankFourCameraVision(); + m_intake = Subsystems.createBlankIntake(); + m_arm = Subsystems.createBlankArm(); + m_climber = Subsystems.createBlankClimber(); + m_shooter = Subsystems.createBlankShooter(); + m_feeder = Subsystems.createBlankFeeder(); break; } - m_vision.setVisionPoseConsumer(m_drivetrain.getVisionPoseConsumer()); - m_drivetrain.setDefaultCommand( - m_drivetrain.joystickDrive( - () -> -m_driverController.getLeftY(), - () -> -m_driverController.getLeftX(), - // This needs to be getRawAxis(2) when using sim on a Mac - () -> -m_driverController.getRightX())); + m_lights = new Lights(); + // NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier); + NoteVisualizer.resetNotes(); + NoteVisualizer.showStagedNotes(); + final Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake); + NamedCommands.registerCommand("intake", m_intake.intake()); + NamedCommands.registerCommand("intakeOff", m_intake.idle()); + NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter())); + NamedCommands.registerCommand("runShooter", m_shooter.runShooter().withTimeout(0.1)); + NamedCommands.registerCommand("subShot", m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0)); + NamedCommands.registerCommand("feederOn", m_feeder.feed().withTimeout(.3)); + NamedCommands.registerCommand("feederOnTest", m_feeder.feed()); + NamedCommands.registerCommand("feederOff", m_feeder.idle().withTimeout(1)); + NamedCommands.registerCommand("sensorIntake", Superstructure.sensorIntake(m_feeder, m_intake)); + NamedCommands.registerCommand("armDown", m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser()); - NamedCommands.registerCommand("Stop", m_shooter.Stop()); - NamedCommands.registerCommand("SensorIntake", SensorIntake()); - m_shooter.setDefaultCommand(m_shooter.Stop()); - m_intake.setDefaultCommand(m_intake.Stop()); m_autoChooser.addOption( "Drive SysId (Quasistatic Forward)", - m_drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_drivetrain.sysIdDriveQuasistatic(SysIdRoutine.Direction.kForward)); m_autoChooser.addOption( "Drive SysId (Quasistatic Reverse)", - m_drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_drivetrain.sysIdDriveQuasistatic(SysIdRoutine.Direction.kReverse)); m_autoChooser.addOption( "Drive SysId (Dynamic Forward)", - m_drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_drivetrain.sysIdDriveDynamic(SysIdRoutine.Direction.kForward)); m_autoChooser.addOption( "Drive SysId (Dynamic Reverse)", - m_drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - SmartDashboard.putNumber("Distance", mytimeofflight.getRange()); - - // m_driverController.povDown().whileTrue(m_shooter.Shoot()); - // m_driverController.povRight().whileTrue(m_shooter.Feeder()); - // m_driverController.x().whileTrue(m_shooter.Stop()); - - // m_driverController.a().whileTrue(m_intake.Spit()); - - // m_driverController.leftTrigger().whileTrue(new - // RepeatCommand(m_shooter.Intake())); - // m_driverController.leftTrigger().whileTrue(m_intake.intake()); - - // m_driverController.b().whileTrue(m_intake.Stop()); - // final double defaultStopDistance = 16; - // m_driverController.leftBumper().whileTrue(m_intake.intake()); - // m_driverController.rightBumper().onTrue(m_shooter.Feeder()); - // m_driverController.b().onTrue(m_shooter.Stop()); - // SmartDashboard.putNumber("StopDistance", defaultStopDistance); - - // m_driverController - // .povUp() - // .onTrue( - // new SequentialCommandGroup(m_intake.intake(), m_shooter.Intake()) - // .until( - // () -> { - // return mytimeofflight.getRange() - // <= SmartDashboard.getNumber("StopDistance", defaultStopDistance); - // })); - - // m_driverController - // .leftTrigger() - // .whileTrue(new ParallelRaceGroup(m_intake.intake(), - // m_shooter.Intake()).repeatedly()); - - // this is here to make the value be editable on the dashboard - - // SmartDashboard.putNumber("Intake/CurrentStopInput", 10); + m_drivetrain.sysIdDriveDynamic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Rotation SysId (Quasistatic Forward)", + m_drivetrain.sysIdRotationQuasistatic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Rotation SysId (Quasistatic Reverse)", + m_drivetrain.sysIdRotationQuasistatic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Rotation SysId (Dynamic Forward)", + m_drivetrain.sysIdRotationDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Rotation SysId (Dynamic Reverse)", + m_drivetrain.sysIdRotationDynamic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption("Slip Current SysId", m_drivetrain.sysIdSlipCurrent()); + m_autoChooser.addOption( + "Shooter SysId (Quasistatic Forward)", + m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Shooter SysId (Quasistatic Reverse)", + m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Shooter SysId (Dynamic Forward)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Shooter SysId (Dynamic Reverse)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption("3 Piece Centerline", autos.centerlineTwoPiece()); + m_drivetrain.setDefaultCommand( + m_drivetrain.joystickDrive( + () -> -m_driverController.getLeftY(), + () -> -m_driverController.getLeftX(), + () -> -m_driverController.getRightX())); - // m_driverController - // .leftBumper() - // .whileTrue( - // new ParallelRaceGroup(m_intake.intake(), m_shooter.Intake()) - // .repeatedly() - // .until( - // () -> { - // double defaultIntakeStopCurrent = 10; - // return m_shooter.getCurrent() > SmartDashboard.getNumber( - // "Intake/CurrentStopInput", defaultIntakeStopCurrent); - // })); + m_intake + .hasIntookPieceSim + .or(m_feeder.hasNote) + .onTrue(Commands.runOnce(() -> NoteVisualizer.setHasNote(true))); + m_feeder.hasNote.whileTrue(m_lights.setBlink(Color.kOrangeRed)); + m_drivetrain.inRangeOfGoal.whileTrue(m_lights.setBlink(Color.kBlue)); - // ____________________driverController_______________________\\ m_driverController .rightTrigger() - .onTrue(new ParallelRaceGroup(m_intake.intake(), m_shooter.Feeder())); - + .whileTrue(Commands.parallel(m_intake.intake(), m_feeder.shoot())) + .onFalse(Commands.runOnce(() -> m_shooter.getCurrentCommand().cancel())); m_driverController - .rightBumper() - .whileTrue(new ParallelRaceGroup(m_intake.Spit(), m_shooter.FeederOut())); - - m_driverController.button(7).onTrue(m_drivetrain.zeroGyro()); - - m_driverController - .leftBumper() + .leftTrigger() + .whileTrue(Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_lights)); + + m_driverController.start().onTrue(m_drivetrain.zeroGyro()); + // m_driverController.leftStick().toggleOnTrue(m_arm.aimElbowForTuning()); + // m_driverController.rightStick().toggleOnTrue(m_arm.aimWristForTuning()); + m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake)); + m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(1.633, -2.371, 0.0, 0.0)); + m_driverController.b().whileTrue(Commands.parallel(m_arm.idleCoast(), m_climber.windWinch())); + m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake)); + + m_operatorController + .leftTrigger() .whileTrue( - new ParallelRaceGroup(m_intake.intake(), m_shooter.Intake()) - .repeatedly() - .until( - () -> { - double defaultIntakeStopCurrent = 10; - return m_shooter.getCurrent() - > SmartDashboard.getNumber( - "Intake/CurrentStopInput", defaultIntakeStopCurrent); - })) - .onFalse(m_shooter.PullBack()); - - // ___________________OperatorController______________________\\ - m_operatorController.y().onTrue(m_shooter.Shoot()); - } - - public Command SensorIntake() { - final double defaultStopDistance = 35; - - // this is here to make the value be editable on the dashboard - SmartDashboard.putNumber("StopDistance", defaultStopDistance); - return new ParallelRaceGroup( - new RepeatCommand(m_intake.intake()), - new RepeatCommand(m_shooter.Intake()) - .until( - () -> { - return mytimeofflight.getRange() - <= SmartDashboard.getNumber("StopDistance", defaultStopDistance); - })); + Commands.parallel( + m_intake.feed(), + m_shooter.ampShot(), + m_arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0))) + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + m_operatorController.rightTrigger().onTrue(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + m_operatorController + .povUp() + .onTrue( + Commands.parallel(m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0), m_shooter.runShooter())) + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + + m_operatorController.y().whileTrue(m_shooter.runShooter()); + m_operatorController.a().whileTrue(m_shooter.ampShot()); + m_operatorController + .leftBumper() + .onTrue(Superstructure.sensorCatch(m_shooter, m_feeder, m_intake, m_arm)) + .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); } /** * This function is called every robot packet, no matter the mode. Use this for items like * diagnostics that you want ran during disabled, autonomous, teleoperated and test. * - *

This runs after the mode specific periodic functions, but before LiveWindow and + *

This runs after the mode specific periodic functions, but before LiveWindow and` * SmartDashboard integrated updating. */ @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); m_vision.periodic(); - SmartDashboard.putNumber("Distance", mytimeofflight.getRange()); - SmartDashboard.putNumber("Intake/Current", m_shooter.getCurrent()); + NoteVisualizer.showHeldNotes(); } @Override public void autonomousInit() { + NoteVisualizer.resetNotes(); + NoteVisualizer.setHasNote(true); Command autonomousCommand = m_autoChooser.get(); if (autonomousCommand != null) { autonomousCommand.schedule(); @@ -241,7 +250,8 @@ public void simulationInit() {} @Override public void simulationPeriodic() { - m_vision.updateSim(m_drivetrain.getPose()); + if (Constants.kIsSim) m_vision.updateSim(PoseEstimation.getInstance().getPose()); + NoteVisualizer.showStagedNotes(); } private void recordMetadeta() { diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 72ac92e..624e9a2 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -3,11 +3,31 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import frc.robot.arm.Arm; +import frc.robot.arm.ArmIO; +import frc.robot.arm.ArmIOSim; +import frc.robot.arm.ArmIOTalonFX; +import frc.robot.climber.Climber; +import frc.robot.climber.ClimberIO; +import frc.robot.climber.ClimberIOSparkMAX; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.ImuIO; import frc.robot.drivetrain.ImuIOPigeon2; import frc.robot.drivetrain.ModuleIO; import frc.robot.drivetrain.ModuleIOTalonFX; +import frc.robot.feeder.Feeder; +import frc.robot.feeder.FeederIO; +import frc.robot.feeder.FeederIOSim; +import frc.robot.feeder.FeederIOTalonFX; +import frc.robot.intake.Intake; +import frc.robot.intake.IntakeIO; +import frc.robot.intake.IntakeIOSim; +import frc.robot.intake.IntakeIOSparkMAX; +import frc.robot.shooter.Shooter; +import frc.robot.shooter.ShooterIO; +import frc.robot.shooter.ShooterIOSim; +import frc.robot.shooter.ShooterIOTalonFX; import frc.robot.vision.Vision; import frc.robot.vision.VisionIO; import frc.robot.vision.VisionIOPhotonVision; @@ -23,6 +43,15 @@ public static Drivetrain createTalonFXDrivetrain() { new ModuleIOTalonFX(3)); } + public static Drivetrain createSimDrivetrain() { + return new Drivetrain( + new ImuIO() {}, + new ModuleIOTalonFX(0), + new ModuleIOTalonFX(1), + new ModuleIOTalonFX(2), + new ModuleIOTalonFX(3)); + } + public static Drivetrain createBlankDrivetrain() { return new Drivetrain( new ImuIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); @@ -31,10 +60,86 @@ public static Drivetrain createBlankDrivetrain() { public static Vision createFourCameraVision() { return new Vision( new VisionIOPhotonVision( - "front-cam", new Transform3d(new Translation3d(1, 1, 1), new Rotation3d(1, 1, 1)))); + "back-left-cam", + new Transform3d( + new Translation3d( + Units.inchesToMeters(-9.124 + 2.5), + Units.inchesToMeters(10.646), + Units.inchesToMeters(8.25)), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-28.125), + Units.degreesToRadians(150.0)))), + new VisionIOPhotonVision( + "back-right-cam", + new Transform3d( + new Translation3d( + Units.inchesToMeters(-9.124 + 2.5), + Units.inchesToMeters(-10.646), + Units.inchesToMeters(8.25)), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-28.125), + Units.degreesToRadians(-150.0))))); } public static Vision createBlankFourCameraVision() { return new Vision(new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}); } + + public static Intake createSparkMAXIntake() { + return new Intake(new IntakeIOSparkMAX()); + } + + public static Intake createSimIntake() { + return new Intake(new IntakeIOSim()); + } + + public static Intake createBlankIntake() { + return new Intake(new IntakeIO() {}); + } + + public static Arm createTalonFXArm() { + return new Arm(new ArmIOTalonFX()); + } + + public static Arm createSimArm() { + return new Arm(new ArmIOSim()); + } + + public static Arm createBlankArm() { + return new Arm(new ArmIO() {}); + } + + public static Climber createSparkMAXClimber() { + return new Climber(new ClimberIOSparkMAX()); + } + + public static Climber createBlankClimber() { + return new Climber(new ClimberIO() {}); + } + + public static Shooter createTalonFXShooter() { + return new Shooter(new ShooterIOTalonFX()); + } + + public static Shooter createSimShooter() { + return new Shooter(new ShooterIOSim()); + } + + public static Shooter createBlankShooter() { + return new Shooter(new ShooterIO() {}); + } + + public static Feeder createTalonFXFeeder() { + return new Feeder(new FeederIOTalonFX()); + } + + public static Feeder createSimFeeder() { + return new Feeder(new FeederIOSim()); + } + + public static Feeder createBlankFeeder() { + return new Feeder(new FeederIO() {}); + } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java new file mode 100644 index 0000000..b2a73dc --- /dev/null +++ b/src/main/java/frc/robot/Superstructure.java @@ -0,0 +1,59 @@ +package frc.robot; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.arm.Arm; +import frc.robot.drivetrain.Drivetrain; +import frc.robot.feeder.Feeder; +import frc.robot.intake.Intake; +import frc.robot.lights.Lights; +import frc.robot.shooter.Shooter; + +/** Class of factories for commands that combine multiple subsystems. */ +public class Superstructure { + public static Command sensorIntake(Feeder feeder, Intake intake) { + // In sim just wait 5 seconds since we don't have a sensor + final Command feedUntilHasNote = + Commands.either( + feeder.feed().until(intake.hasIntookPieceSim), + feeder.feed().until(feeder.hasNote), + () -> Constants.kIsSim); + return Commands.deadline(feedUntilHasNote, intake.intake()) + .andThen(feeder.idle().withTimeout(0.01)) + .withName("Sensor Intake"); + } + + public static Command sensorCatch(Shooter shooter, Feeder feeder, Intake intake, Arm arm) { + return Commands.parallel( + arm.goToSetpoint(-0.558, 2.264 - Units.degreesToRadians(5.0), 0, 0), + feeder.enterCoast(), + shooter.catchNote()) + .andThen(feeder.pullBack().until(feeder.hasNote)) + .withName("Sensor Catch"); + } + + public static Command ampShot(Arm arm, Shooter shooter) { + return Commands.parallel( + arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0), shooter.runShooter()) + .finallyDo(() -> arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + } + + public static Command spit(Shooter shooter, Feeder feeder, Intake intake) { + return Commands.parallel(shooter.spit(), feeder.spit(), intake.spit()).withName("Spit"); + } + + public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Lights lights) { + return Commands.parallel( + Commands.startEnd( + () -> + drivetrain.setHeadingGoal( + () -> PoseEstimation.getInstance().getAimingParameters().driveHeading()), + drivetrain::clearHeadingGoal), + shooter.runShooter(), + lights.showReadyToShootStatus(drivetrain.atHeadingGoal.and(shooter.readyToShoot))) + .finallyDo(() -> lights.getCurrentCommand().cancel()) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } +} diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java new file mode 100644 index 0000000..9430387 --- /dev/null +++ b/src/main/java/frc/robot/arm/Arm.java @@ -0,0 +1,333 @@ +package frc.robot.arm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.LoggedTunableNumber; +import frc.robot.Constants; +import java.util.Optional; +import java.util.function.BiFunction; +import org.littletonrobotics.junction.Logger; + +public class Arm extends SubsystemBase { + + private final double kSDeadband = 0.05; + private final ArmIO m_io; + private final ArmIOInputsAutoLogged m_inputs = new ArmIOInputsAutoLogged(); + private final ArmModel m_armModel = new ArmModel(); + private final ArmKinematics m_kinematics = new ArmKinematics(); + private final ArmVisualizer m_measuredVisualizer; + private final ArmVisualizer m_setpointVisualizer; + private ProfiledPIDController m_elbowController = + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); + private ProfiledPIDController m_wristController = + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); + + private static final LoggedTunableNumber elbowkP = new LoggedTunableNumber("Arm/Elbow/kP"); + private static final LoggedTunableNumber elbowkD = new LoggedTunableNumber("Arm/Elbow/kD"); + private static final LoggedTunableNumber elbowkS = new LoggedTunableNumber("Arm/Elbow/kS"); + private static final LoggedTunableNumber elbowCruiseVelocityRadPerSec = + new LoggedTunableNumber("Arm/Elbow/CruiseVelocity"); + private static final LoggedTunableNumber elbowMaxAccelerationRadPerSecSq = + new LoggedTunableNumber("Arm/Elbow/MaxAcceleration"); + private static final LoggedTunableNumber wristkP = new LoggedTunableNumber("Arm/Wrist/kP"); + private static final LoggedTunableNumber wristkD = new LoggedTunableNumber("Arm/Wrist/kD"); + private static final LoggedTunableNumber wristkS = new LoggedTunableNumber("Arm/Wrist/kS"); + private static final LoggedTunableNumber wristCruiseVelocityRadPerSec = + new LoggedTunableNumber("Arm/Wrist/CruiseVelocity"); + private static final LoggedTunableNumber wristMaxAccelerationRadPerSecSq = + new LoggedTunableNumber("Arm/Wrist/MaxAcceleration"); + + // We don't want to add kS to the feedback when the output is close to 0 + private final BiFunction m_deadbandkS = + (volts, kS) -> Math.abs(volts) < kSDeadband ? volts : volts + Math.copySign(kS, volts); + + private Vector m_profileInitialAngles; + private double m_elbowPositionRad, m_wristPositionRad; + + static { + switch (Constants.kCurrentMode) { + case kReal: + elbowkP.initDefault(3.8); + elbowkD.initDefault(0); + elbowkS.initDefault(0.55); + elbowCruiseVelocityRadPerSec.initDefault(5.0); + elbowMaxAccelerationRadPerSecSq.initDefault(8.0); + wristkP.initDefault(3.3); + wristkD.initDefault(0); + wristkS.initDefault(0.4); + wristCruiseVelocityRadPerSec.initDefault(4.0); + wristMaxAccelerationRadPerSecSq.initDefault(8.0); + break; + case kSim: + // We do not want kS in simulation + elbowkP.initDefault(5.0); + elbowkD.initDefault(0.0); + elbowCruiseVelocityRadPerSec.initDefault(3.0); + elbowMaxAccelerationRadPerSecSq.initDefault(2.0); + wristkP.initDefault(20.0); + wristkD.initDefault(0.0); + wristCruiseVelocityRadPerSec.initDefault(5.0); + wristMaxAccelerationRadPerSecSq.initDefault(2.0); + break; + default: + break; + } + } + + public Arm(ArmIO io) { + m_io = io; + m_measuredVisualizer = new ArmVisualizer("ArmMeasured"); + m_setpointVisualizer = new ArmVisualizer("ArmSetpoint"); + m_wristController.enableContinuousInput(-Math.PI, Math.PI); + m_elbowController.setTolerance(Units.degreesToRadians(0.25)); + m_wristController.setTolerance(Units.degreesToRadians(0.25)); + setDefaultCommand(idle()); + } + + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("ArmInputs", m_inputs); + + m_elbowPositionRad = m_inputs.elbowRelativePositionRad; + m_wristPositionRad = m_inputs.wristRelativePositionRad; + + if (DriverStation.isDisabled()) { + m_io.setElbowVoltage(0.0); + m_io.setWristVoltage(0.0); + } + + // #region Update controllers wßhen tunables change. + if (elbowkP.hasChanged(hashCode()) || elbowkD.hasChanged(hashCode())) { + m_elbowController.setP(elbowkP.get()); + m_elbowController.setD(elbowkD.get()); + } + if (wristkP.hasChanged(hashCode()) || wristkD.hasChanged(hashCode())) { + m_wristController.setP(wristkP.get()); + m_wristController.setD(wristkD.get()); + } + + if (elbowCruiseVelocityRadPerSec.hasChanged(hashCode()) + || elbowMaxAccelerationRadPerSecSq.hasChanged(hashCode())) { + m_elbowController.setConstraints( + new TrapezoidProfile.Constraints( + elbowCruiseVelocityRadPerSec.get(), elbowMaxAccelerationRadPerSecSq.get())); + } + if (wristCruiseVelocityRadPerSec.hasChanged(hashCode()) + || wristMaxAccelerationRadPerSecSq.hasChanged(hashCode())) { + m_wristController.setConstraints( + new TrapezoidProfile.Constraints( + wristCruiseVelocityRadPerSec.get(), wristMaxAccelerationRadPerSecSq.get())); + } + // #endregion + + Logger.recordOutput( + "Arm/Forward Kinematics Position", + m_kinematics.forward(VecBuilder.fill(m_elbowPositionRad, m_wristPositionRad))); + m_measuredVisualizer.update(m_elbowPositionRad, m_wristPositionRad); + Logger.recordOutput( + "Arm/Current Command", + getCurrentCommand() == null ? "Null" : getCurrentCommand().getName()); + } + + private void setSetpoint(Vector setpointAnglesRad) { + double elbowAngle = setpointAnglesRad.get(0, 0); + double wristAngle = setpointAnglesRad.get(1, 0); + Logger.recordOutput("Arm/Goal Arm Position", m_kinematics.forward(setpointAnglesRad)); + Logger.recordOutput("Arm/Goal Elbow Angle", elbowAngle); + Logger.recordOutput("Arm/Goal Wrist Angle", wristAngle); + m_setpointVisualizer.update(elbowAngle, wristAngle); + m_elbowController.reset(m_elbowPositionRad); + m_elbowController.setGoal(elbowAngle); + m_wristController.reset(m_wristPositionRad); + m_wristController.setGoal(wristAngle); + m_profileInitialAngles = VecBuilder.fill(m_elbowPositionRad, m_wristPositionRad); + } + + private void setSetpoint(Translation2d setpoint) { + Optional> setpointAnglesRad = m_kinematics.inverse(setpoint); + setpointAnglesRad.ifPresent(angles -> setSetpoint(angles)); + } + + /** + * @param elbowDelay The percent that the wrist must travel before the elbow moves. + * @param wristDelay The percent that the elbow must travel before the wrist moves. + */ + private void runSetpoint(double elbowDelay, double wristDelay) { + if (elbowDelay > 0.0 && wristDelay > 0.0) + throw new IllegalArgumentException("Only one joint can be delayed at a time."); + if (elbowDelay < 0.0 || wristDelay < 0.0) + throw new IllegalArgumentException("Percent delay can't be negative."); + double elbowGoalPosition = m_elbowController.getSetpoint().position; + double wristGoalPosition = m_wristController.getSetpoint().position; + double elbowGoalVelocity = m_elbowController.getSetpoint().velocity; + double wristGoalVelocity = m_wristController.getSetpoint().velocity; + double elbowProgress = + Math.abs(m_elbowPositionRad - m_profileInitialAngles.get(0, 0)) + / Math.abs(m_elbowController.getGoal().position - m_profileInitialAngles.get(0, 0)); + double wristProgress = + Math.abs(m_wristPositionRad - m_profileInitialAngles.get(1, 0)) + / Math.abs(m_wristController.getGoal().position - m_profileInitialAngles.get(1, 0)); + Logger.recordOutput("Arm/Elbow Progress", elbowProgress); + Logger.recordOutput("Arm/Wrist Progress", wristProgress); + double elbowFeedbackVolts = 0.0; + double wristFeedbackVolts = 0.0; + if (wristProgress < elbowDelay) { + elbowGoalPosition = m_elbowPositionRad; + elbowGoalVelocity = 0.0; + } else + elbowFeedbackVolts = + m_deadbandkS.apply(m_elbowController.calculate(m_elbowPositionRad), elbowkS.get()); + if (elbowProgress < wristDelay) { + wristGoalPosition = m_wristPositionRad; + wristGoalVelocity = 0.0; + } else + wristFeedbackVolts = + m_deadbandkS.apply(m_wristController.calculate(m_wristPositionRad), wristkS.get()); + Vector feedforwardVolts = + m_armModel.feedforward( + VecBuilder.fill(elbowGoalPosition, wristGoalPosition), + VecBuilder.fill(elbowGoalVelocity, wristGoalVelocity)); + double elbowFeedForwardVolts = feedforwardVolts.get(0, 0); + double wristFeedForwardVolts = feedforwardVolts.get(1, 0); + Logger.recordOutput("Arm/Elbow Feed Forward", elbowFeedForwardVolts); + Logger.recordOutput("Arm/Wrist Feed Forward", wristFeedForwardVolts); + m_io.setElbowVoltage(elbowFeedbackVolts + elbowFeedForwardVolts); + m_io.setWristVoltage(wristFeedbackVolts + wristFeedForwardVolts); + } + + public Command goToSetpoint(ArmSetpoints setpoint) { + return this.runOnce( + () -> { + setSetpoint(setpoint.getTranslation()); + }) + .andThen( + this.run( + () -> runSetpoint(setpoint.getDelay().getFirst(), setpoint.getDelay().getSecond()))) + .withName("Set Inverse Setpoint"); + } + + public Command goToSetpoint( + double elbowAngleRad, double wristAngleRad, double elbowDelay, double wristDelay) { + return this.runOnce(() -> setSetpoint(VecBuilder.fill(elbowAngleRad, wristAngleRad))) + .andThen(this.run(() -> runSetpoint(elbowDelay, wristDelay))) + .withName("Set Forward Setpoint"); + } + + public Command idle() { + return this.run( + () -> { + m_io.setElbowVoltage(0.0); + m_io.setWristVoltage(0.0); + }) + .withName("Idle"); + } + + public Command idleCoast() { + return this.runOnce(() -> m_io.setBrakeMode(false)) + .andThen( + this.run( + () -> { + m_io.setElbowVoltage(0.0); + m_io.setWristVoltage(0.0); + })) + .finallyDo(() -> m_io.setBrakeMode(true)) + .ignoringDisable(true) + .withName("Idle Coast"); + } + + public Command setBrake() { + return this.runOnce(() -> m_io.setBrakeMode(true)); + } + + class ArmKinematics { + + public Translation2d forward(Vector angles) { + + double elbowAngle = angles.get(0, 0); + double wristAngle = angles.get(1, 0); + + return new Translation2d( + ArmModel.origin.getX() + + ArmModel.kElbowLengthMeters * Math.cos(elbowAngle) + + ArmModel.kWristLengthMeters * Math.cos(elbowAngle + (wristAngle - elbowAngle)), + ArmModel.origin.getY() + + ArmModel.kElbowLengthMeters * Math.sin(elbowAngle) + + ArmModel.kWristLengthMeters * Math.sin(elbowAngle + (wristAngle - elbowAngle))); + } + + public Translation2d forwardWristRelativeToElbow(Vector angles) { + + return new Translation2d( + ArmModel.origin.getX() + + ArmModel.kElbowLengthMeters * Math.cos(angles.get(0, 0)) + + ArmModel.kWristLengthMeters * Math.cos(angles.get(0, 0) + angles.get(1, 0)), + ArmModel.origin.getY() + + ArmModel.kElbowLengthMeters * Math.sin(angles.get(0, 0)) + + ArmModel.kWristLengthMeters * Math.sin(angles.get(0, 0) + angles.get(1, 0))); + } + + // This is still broken + public Optional> inverse(Translation2d position) { + Translation2d relativePosition = position.minus(ArmModel.origin); + + // Flip when X is negative + boolean isFlipped = true; // relativePosition.getX() < 0.0; + if (isFlipped) + relativePosition = new Translation2d(-relativePosition.getX(), relativePosition.getY()); + + // Calculate angles + double wristAngle = + -Math.acos( + MathUtil.clamp( + (Math.pow(relativePosition.getX(), 2) + + Math.pow(relativePosition.getY(), 2) + - Math.pow(ArmModel.kElbowLengthMeters, 2) + - Math.pow(ArmModel.kWristLengthMeters, 2)) + / (2 * ArmModel.kElbowLengthMeters * ArmModel.kWristLengthMeters), + -1.0, + 1.0)); + double elbowAngle = + Math.atan2(relativePosition.getY(), relativePosition.getX()) + - Math.atan2( + (ArmModel.kWristLengthMeters * Math.sin(wristAngle)), + (ArmModel.kElbowLengthMeters + + ArmModel.kWristLengthMeters * Math.cos(wristAngle))); + + // Invert elbow angle if invalid + Translation2d testPosition = + forwardWristRelativeToElbow(VecBuilder.fill(elbowAngle, wristAngle)) + .minus(ArmModel.origin); + if (testPosition.getDistance(relativePosition) > 1e-3) { + elbowAngle += Math.PI; + } + + // Flip angles + if (isFlipped) { + elbowAngle = Math.PI - elbowAngle; + wristAngle = -wristAngle; + } + + // Wrap angles to correct ranges + elbowAngle = MathUtil.angleModulus(elbowAngle); + wristAngle = MathUtil.angleModulus(wristAngle); + + // Exit if outside valid ranges for the joints + if (elbowAngle < ArmModel.kElbowMinAngleRad + || elbowAngle > ArmModel.kElbowMaxAngleRad + || wristAngle < ArmModel.kWristMinAngleRad + || wristAngle > ArmModel.kWristMaxAngleRad) { + return Optional.empty(); + } + return Optional.of(VecBuilder.fill(elbowAngle, wristAngle + elbowAngle)); + } + } +} diff --git a/src/main/java/frc/robot/arm/ArmIO.java b/src/main/java/frc/robot/arm/ArmIO.java new file mode 100644 index 0000000..f6ab4ef --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmIO.java @@ -0,0 +1,35 @@ +package frc.robot.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmIO { + + @AutoLog + public static class ArmIOInputs { + public double elbowAbsolutePositionRad = 0.0; + public boolean elbowAbsoluteEncoderConnected = false; + public double elbowEncoderRaw = 0.0; + public double elbowRelativePositionRad = 0.0; + public double elbowVelocityRadPerSec = 0.0; + public double elbowAppliedVolts = 0.0; + public double[] elbowCurrentAmps = new double[] {}; + + public double wristAbsolutePositionRad = 0.0; + public boolean wristAbsoluteEncoderConnected = false; + public double wristEncoderRaw = 0.0; + public double wristRelativePositionRad = 0.0; + public double wristVelocityRadPerSec = 0.0; + public double wristAppliedVolts = 0.0; + public double[] wristCurrentAmps = new double[] {}; + + public boolean allMotorsConnected = false; + } + + public default void updateInputs(ArmIOInputs inputs) {} + + public default void setElbowVoltage(double volts) {} + + public default void setWristVoltage(double volts) {} + + public default void setBrakeMode(boolean enableBrakeMode) {} +} diff --git a/src/main/java/frc/robot/arm/ArmIOSim.java b/src/main/java/frc/robot/arm/ArmIOSim.java new file mode 100644 index 0000000..111e365 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmIOSim.java @@ -0,0 +1,52 @@ +package frc.robot.arm; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N4; + +public class ArmIOSim implements ArmIO { + private Vector m_elbowWristSimStates = VecBuilder.fill(0.0, 0.0, 0.0, 0.0); + private ArmModel m_model = new ArmModel(); + + private double m_elbowAppliedVolts = 0.0; + private double m_wristAppliedVolts = 0.0; + + public ArmIOSim() {} + + public void updateInputs(ArmIOInputs inputs) { + + m_elbowWristSimStates = + m_model.simulate( + m_elbowWristSimStates, VecBuilder.fill(m_elbowAppliedVolts, m_wristAppliedVolts), 0.02); + inputs.elbowAbsolutePositionRad = m_elbowWristSimStates.get(0, 0); + inputs.elbowAbsoluteEncoderConnected = true; + inputs.elbowRelativePositionRad = m_elbowWristSimStates.get(0, 0); + inputs.elbowVelocityRadPerSec = m_elbowWristSimStates.get(2, 0); + inputs.elbowAppliedVolts = m_elbowAppliedVolts; + inputs.elbowCurrentAmps = + new double[] { + ArmModel.kElbowGearbox.getCurrent(m_elbowWristSimStates.get(2, 0), m_elbowAppliedVolts) + }; + + inputs.wristAbsolutePositionRad = m_elbowWristSimStates.get(1, 0); + inputs.wristAbsoluteEncoderConnected = true; + inputs.wristRelativePositionRad = m_elbowWristSimStates.get(1, 0); + inputs.wristVelocityRadPerSec = m_elbowWristSimStates.get(3, 0); + inputs.wristAppliedVolts = m_wristAppliedVolts; + inputs.wristCurrentAmps = + new double[] { + ArmModel.kWristGearbox.getCurrent(m_elbowWristSimStates.get(3, 0), m_wristAppliedVolts) + }; + + inputs.allMotorsConnected = true; + } + + public void setElbowVoltage(double volts) { + m_elbowAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } + + public void setWristVoltage(double volts) { + m_wristAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } +} diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java new file mode 100644 index 0000000..0d8a0d1 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -0,0 +1,197 @@ +package frc.robot.arm; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.Constants; + +public class ArmIOTalonFX implements ArmIO { + + // Offsets to the horizontal + private final double kElbowRelativeEncoderOffsetRad = 0.558; // .558; + private final double kWristRelativeEncoderOffsetRad = 2.5; // 2.5; + private final boolean kInvertWristAbsoluteEncoder = false; + private final boolean kInvertElbowAbsoluteEncoder = false; + private final TalonFX m_elbowLeftMotor, + m_elbowRightFollowerMotor, + m_wristLeftMotor, + m_wristRightFollowerMotor; + private final DutyCycleEncoder m_elbowAbsoluteEncoder, m_wristAbsoluteEncoder; + private final VoltageOut m_elbowControl, m_wristControl; + private final StatusSignal m_elbowPositionSignal, + m_elbowVelocitySignal, + m_elbowAppliedVoltsSignal, + m_elbowCurrentSignal, + m_elbowFollowerCurrentSignal, + m_wristPositionSignal, + m_wristVelocitySignal, + m_wristAppliedVoltsSignal, + m_wristCurrentSignal, + m_wristFollowerCurrentSignal; + + private final Rotation2d elbowAbsoluteEncoderOffset = new Rotation2d(2.471); + private final Rotation2d wristAbsoluteEncoderOffset = new Rotation2d(0.646); + // private final Rotation2d elbowAbsoluteEncoderOffset; + // private final Rotation2d wristAbsoluteEncoderOffset; + + public ArmIOTalonFX() { + m_elbowLeftMotor = new TalonFX(15, Constants.kSuperstructureCanBus); + m_elbowRightFollowerMotor = new TalonFX(17, Constants.kSuperstructureCanBus); + m_wristLeftMotor = new TalonFX(16, Constants.kSuperstructureCanBus); + m_wristRightFollowerMotor = new TalonFX(18, Constants.kSuperstructureCanBus); + + m_elbowAbsoluteEncoder = new DutyCycleEncoder(8); + m_wristAbsoluteEncoder = new DutyCycleEncoder(9); + m_elbowAbsoluteEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0); + m_wristAbsoluteEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0); + + m_elbowLeftMotor.setPosition(0.0); + m_wristLeftMotor.setPosition(0.0); + m_elbowRightFollowerMotor.setPosition(0.0); + m_wristRightFollowerMotor.setPosition(0.0); + + final CurrentLimitsConfigs currentLimitsConfig = new CurrentLimitsConfigs(); + currentLimitsConfig.SupplyCurrentLimit = 80.0; + currentLimitsConfig.SupplyCurrentLimitEnable = true; + final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration(); + m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig); + elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig; + elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -4.0; + m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig); + + final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); + m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig); + wristLeftMotorConfig.CurrentLimits = currentLimitsConfig; + wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig); + + final TalonFXConfiguration elbowRightMotorConfig = new TalonFXConfiguration(); + m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig); + elbowRightMotorConfig.CurrentLimits = currentLimitsConfig; + elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig); + m_elbowRightFollowerMotor.setControl(new Follower(m_elbowLeftMotor.getDeviceID(), true)); + + final TalonFXConfiguration wristRightMotorConfig = new TalonFXConfiguration(); + m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig); + wristRightMotorConfig.CurrentLimits = currentLimitsConfig; + wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig); + m_wristRightFollowerMotor.setControl(new Follower(m_wristLeftMotor.getDeviceID(), true)); + + m_elbowPositionSignal = m_elbowLeftMotor.getPosition(); + m_elbowVelocitySignal = m_elbowLeftMotor.getVelocity(); + m_elbowAppliedVoltsSignal = m_elbowLeftMotor.getMotorVoltage(); + m_elbowCurrentSignal = m_elbowLeftMotor.getStatorCurrent(); + m_elbowFollowerCurrentSignal = m_elbowRightFollowerMotor.getStatorCurrent(); + m_wristPositionSignal = m_wristLeftMotor.getPosition(); + m_wristVelocitySignal = m_wristLeftMotor.getVelocity(); + m_wristAppliedVoltsSignal = m_wristLeftMotor.getMotorVoltage(); + m_wristCurrentSignal = m_wristLeftMotor.getStatorCurrent(); + m_wristFollowerCurrentSignal = m_wristRightFollowerMotor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + m_elbowPositionSignal, + m_elbowVelocitySignal, + m_elbowAppliedVoltsSignal, + m_elbowCurrentSignal, + m_elbowFollowerCurrentSignal, + m_wristPositionSignal, + m_wristVelocitySignal, + m_wristAppliedVoltsSignal, + m_wristCurrentSignal, + m_wristFollowerCurrentSignal); + + ParentDevice.optimizeBusUtilizationForAll( + m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor); + + m_elbowControl = new VoltageOut(0.0, true, false, false, false).withUpdateFreqHz(0.0); + m_wristControl = new VoltageOut(0.0, true, false, false, false).withUpdateFreqHz(0.0); + } + + public void updateInputs(ArmIOInputs inputs) { + inputs.allMotorsConnected = + BaseStatusSignal.refreshAll( + m_elbowPositionSignal, + m_elbowVelocitySignal, + m_elbowAppliedVoltsSignal, + m_elbowCurrentSignal, + m_elbowFollowerCurrentSignal, + m_wristPositionSignal, + m_wristVelocitySignal, + m_wristAppliedVoltsSignal, + m_wristCurrentSignal, + m_wristFollowerCurrentSignal) + .equals(StatusCode.OK); + + inputs.elbowEncoderRaw = m_elbowPositionSignal.getValue(); + inputs.wristEncoderRaw = m_wristPositionSignal.getValue(); + inputs.elbowAbsolutePositionRad = + MathUtil.angleModulus( + Units.rotationsToRadians(m_elbowAbsoluteEncoder.get() / ArmModel.kElbowChainReduction) + * (kInvertElbowAbsoluteEncoder ? -1 : 1) + - elbowAbsoluteEncoderOffset.getRadians()); + inputs.elbowAbsoluteEncoderConnected = m_elbowAbsoluteEncoder.isConnected(); + inputs.elbowRelativePositionRad = + MathUtil.angleModulus( + Units.rotationsToRadians( + m_elbowPositionSignal.getValue() / ArmModel.kElbowFinalReduction) + - kElbowRelativeEncoderOffsetRad); + inputs.elbowVelocityRadPerSec = + Units.rotationsToRadians(m_elbowVelocitySignal.getValue() / ArmModel.kElbowFinalReduction); + inputs.elbowAppliedVolts = m_elbowAppliedVoltsSignal.getValue(); + inputs.elbowCurrentAmps = + new double[] {m_elbowCurrentSignal.getValue(), m_elbowFollowerCurrentSignal.getValue()}; + + inputs.wristAbsolutePositionRad = + MathUtil.angleModulus( + Units.rotationsToRadians(m_wristAbsoluteEncoder.get() / ArmModel.kWristChainReduction) + * (kInvertWristAbsoluteEncoder ? -1 : 1) + - wristAbsoluteEncoderOffset.getRadians()); + + inputs.wristAbsoluteEncoderConnected = m_wristAbsoluteEncoder.isConnected(); + inputs.wristRelativePositionRad = + MathUtil.angleModulus( + Units.rotationsToRadians( + m_wristPositionSignal.getValue() / ArmModel.kWristFinalReduction) + + kWristRelativeEncoderOffsetRad); + inputs.wristVelocityRadPerSec = + Units.rotationsToRadians(m_wristVelocitySignal.getValue() / ArmModel.kWristFinalReduction); + inputs.wristAppliedVolts = m_wristAppliedVoltsSignal.getValue(); + inputs.wristCurrentAmps = + new double[] {m_wristCurrentSignal.getValue(), m_wristFollowerCurrentSignal.getValue()}; + } + + public void setElbowVoltage(double volts) { + m_elbowLeftMotor.setControl(m_elbowControl.withOutput(volts)); + } + + public void setWristVoltage(double volts) { + m_wristLeftMotor.setControl(m_wristControl.withOutput(volts)); + } + + public void setBrakeMode(boolean enableBrakeMode) { + final NeutralModeValue neutralModeValue = + enableBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast; + m_elbowLeftMotor.setNeutralMode(neutralModeValue); + m_wristLeftMotor.setNeutralMode(neutralModeValue); + m_elbowRightFollowerMotor.setNeutralMode(neutralModeValue); + m_wristRightFollowerMotor.setNeutralMode(neutralModeValue); + } +} diff --git a/src/main/java/frc/robot/arm/ArmModel.java b/src/main/java/frc/robot/arm/ArmModel.java new file mode 100644 index 0000000..6ed7c7a --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmModel.java @@ -0,0 +1,103 @@ +package frc.robot.arm; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N4; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class ArmModel { + + public static final Translation2d origin = + new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20)); + + public static final double kElbowGearboxReduction = 3.0 * 5.0 * 5.0; + public static final double kElbowChainReduction = 36.0 / 22.0; + public static final double kElbowFinalReduction = kElbowGearboxReduction * kElbowChainReduction; + public static final double kElbowLengthMeters = Units.inchesToMeters(19.0); + public static final double kElbowMinAngleRad = -Math.PI; + public static final double kElbowMaxAngleRad = Math.PI; + public static final DCMotor kElbowGearbox = DCMotor.getFalcon500Foc(2); + private final double kElbowMassKg = Units.lbsToKilograms(7.0); + private final double kElbowMoiKgMetersSq = + SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg); + private final double elbowkG = 0.15; + private final double elbowkV = 2.31; + + public static final double kWristGearboxReduction = 3.0 * 5.0; + public static final double kWristChainReduction = 36.0 / 22.0; + public static final double kWristFinalReduction = kWristGearboxReduction * kWristChainReduction; + public static final double kWristLengthMeters = Units.inchesToMeters(15.0); + public static final double kWristMinAngleRad = -Math.PI; + public static final double kWristMaxAngleRad = Math.PI; + public static final DCMotor kWristGearbox = DCMotor.getFalcon500Foc(2); + private final double kWristMassKg = Units.lbsToKilograms(11.5); + private final double kWristMoiKgMetersSq = + SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg); + private final double wristkG = 0.51; + private final double wristkV = 0.8; + + private final ArmFeedforward m_elbowFeedForward, m_wristFeedForward; + private final SingleJointedArmSim m_elbowSim, m_wristSim; + + public ArmModel() { + m_elbowFeedForward = new ArmFeedforward(0.0, elbowkG, elbowkV, 0.0); + m_wristFeedForward = new ArmFeedforward(0.0, wristkG, wristkV, 0.0); + m_elbowSim = + new SingleJointedArmSim( + kElbowGearbox, + kElbowFinalReduction, + kElbowMoiKgMetersSq, + kElbowLengthMeters, + kElbowMinAngleRad, + kElbowMaxAngleRad, + true, + 0.0); + m_wristSim = + new SingleJointedArmSim( + kWristGearbox, + kWristFinalReduction, + kWristMoiKgMetersSq, + kWristLengthMeters, + kWristMinAngleRad, + kWristMaxAngleRad, + true, + 0.0); + } + + public Vector feedforward(Vector position) { + return feedforward(position, VecBuilder.fill(0.0, 0.0)); + } + + public Vector feedforward(Vector position, Vector velocity) { + return VecBuilder.fill( + m_elbowFeedForward.calculate(position.get(0, 0), velocity.get(0, 0)), + m_wristFeedForward.calculate(position.get(0, 0), velocity.get(1, 0))); + } + + /** + * Adjusts the simulated state of the arm based on applied voltages. + * + * @param state The current state of the arm as (position_0, position_1, velocity_0, velocity_1) + * @param voltage The applied voltage of each joint. + * @param dt The step length in seconds. + * @return The new state of the arm as (position_0, position_1, velocity_0, velocity_1) + */ + public Vector simulate(Vector state, Vector voltage, double dt) { + m_elbowSim.setState(state.get(0, 0), state.get(2, 0)); + m_wristSim.setState(state.get(1, 0), state.get(3, 0)); + m_elbowSim.setInputVoltage(voltage.get(0, 0)); + m_wristSim.setInputVoltage(voltage.get(1, 0)); + m_elbowSim.update(dt); + m_wristSim.update(dt); + return VecBuilder.fill( + m_elbowSim.getAngleRads(), + m_wristSim.getAngleRads(), + m_elbowSim.getVelocityRadPerSec(), + m_wristSim.getVelocityRadPerSec()); + } +} diff --git a/src/main/java/frc/robot/arm/ArmSetpoints.java b/src/main/java/frc/robot/arm/ArmSetpoints.java new file mode 100644 index 0000000..d766200 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmSetpoints.java @@ -0,0 +1,37 @@ +package frc.robot.arm; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +public enum ArmSetpoints { + kStowed(new Translation2d(-0.13, 0.48), 0.95, 0.0), + kAmp(new Translation2d(-0.412, 0.785), 0.0, 0.2); + + private final Translation2d m_setpoint; + private final double m_elbowDelay; + private final double m_wristDelay; + + private ArmSetpoints(Translation2d setpoint, double elbowDelay, double wristDelay) { + m_setpoint = setpoint; + m_elbowDelay = elbowDelay; + m_wristDelay = wristDelay; + } + + private ArmSetpoints( + double xPositionInches, double yPositionInches, double elbowDelay, double wristDelay) { + this( + new Translation2d( + Units.inchesToMeters(xPositionInches), Units.inchesToMeters(yPositionInches)), + elbowDelay, + wristDelay); + } + + public Translation2d getTranslation() { + return m_setpoint; + } + + public Pair getDelay() { + return Pair.of(m_elbowDelay, m_wristDelay); + } +} diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java new file mode 100644 index 0000000..3ee6a64 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmVisualizer.java @@ -0,0 +1,66 @@ +package frc.robot.arm; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import org.littletonrobotics.junction.Logger; + +public class ArmVisualizer { + + // Value from CAD, angle from horizontal to shoulder. + private final double kShoulderAngleDegrees = 103.985388; + private final String m_logKey; + private final Mechanism2d m_mechanism; + private final MechanismRoot2d m_mechanismRoot; + private final MechanismLigament2d m_fixedShoulderLigament; + private final MechanismLigament2d m_elbowLigament; + private final MechanismLigament2d m_wristLigament; + + public ArmVisualizer(String logKey) { + m_logKey = logKey; + m_mechanism = new Mechanism2d(4, 3, new Color8Bit(Color.kGray)); + m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + ArmModel.origin.getX(), 0); + m_fixedShoulderLigament = + m_mechanismRoot.append( + new MechanismLigament2d( + "Shoulder", + ArmModel.origin.getY(), + kShoulderAngleDegrees, + 6, + new Color8Bit(Color.kBlack))); + m_elbowLigament = + m_fixedShoulderLigament.append( + new MechanismLigament2d( + "Elbow", ArmModel.kElbowLengthMeters, 0, 4, new Color8Bit(Color.kGreen))); + m_wristLigament = + m_elbowLigament.append( + new MechanismLigament2d( + "Wrist", ArmModel.kWristLengthMeters, 0, 4, new Color8Bit(Color.kDarkGreen))); + } + + public void update(double elbowAngle, double wristAngle) { + m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngle) - kShoulderAngleDegrees); + m_wristLigament.setAngle(Units.radiansToDegrees(wristAngle - elbowAngle)); + Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism); + + Pose3d elbowPose = + new Pose3d( + ArmModel.origin.getX(), + 0.0, + ArmModel.origin.getY(), + new Rotation3d(0.0, -elbowAngle, 0.0)); + Pose3d wristPose = + elbowPose.transformBy( + new Transform3d( + new Translation3d(ArmModel.kElbowLengthMeters, 0.0, 0.0), + new Rotation3d(0.0, -(wristAngle - elbowAngle), 0.0))); + Logger.recordOutput("Mechanism3d/" + m_logKey, elbowPose, wristPose); + } +} diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java new file mode 100644 index 0000000..941a399 --- /dev/null +++ b/src/main/java/frc/robot/climber/Climber.java @@ -0,0 +1,45 @@ +package frc.robot.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Climber extends SubsystemBase { + private final ClimberIO m_io; + private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged(); + + public Climber(ClimberIO climberIO) { + m_io = climberIO; + setDefaultCommand(idle()); + } + + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("ClimberInputs", m_inputs); + } + + public Command idle() { + return this.run( + () -> { + m_io.setLeftDutyCycle(0.0); + m_io.setRightDutyCycle(0.0); + }); + } + + public Command windWinch() { + return this.run( + () -> { + m_io.setLeftDutyCycle(1.0); + m_io.setRightDutyCycle(1.0); + }); + } + + public Command unwindWinch() { + return this.run( + () -> { + m_io.setLeftDutyCycle(-0.25); + m_io.setRightDutyCycle(-0.25); + }); + } +} diff --git a/src/main/java/frc/robot/climber/ClimberIO.java b/src/main/java/frc/robot/climber/ClimberIO.java new file mode 100644 index 0000000..5d7410a --- /dev/null +++ b/src/main/java/frc/robot/climber/ClimberIO.java @@ -0,0 +1,24 @@ +package frc.robot.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public class ClimberIOInputs { + public double leftWinchPositionRad = 0.0; + public double leftWinchVelocityRadPerSec = 0.0; + public double leftWinchAppliedVolts = 0.0; + public double leftWinchCurrentAmps = 0.0; + + public double rightWinchPositionRad = 0.0; + public double rightWinchVelocityRadPerSec = 0.0; + public double rightWinchAppliedVolts = 0.0; + public double rightWinchCurrentAmps = 0.0; + } + + public default void updateInputs(ClimberIOInputs inputs) {} + + public default void setLeftDutyCycle(double outputDutyCycle) {} + + public default void setRightDutyCycle(double outputDutyCycle) {} +} diff --git a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java new file mode 100644 index 0000000..4c019de --- /dev/null +++ b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java @@ -0,0 +1,59 @@ +package frc.robot.climber; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.util.Units; + +public class ClimberIOSparkMAX implements ClimberIO { + + private final double kWinchReduction = 1.0; + private final CANSparkMax m_leftWinchMotor, m_rightWinchMotor; + private final RelativeEncoder m_leftWinchEncoder, m_rightWinchEncoder; + + public ClimberIOSparkMAX() { + + m_leftWinchMotor = new CANSparkMax(9, CANSparkMax.MotorType.kBrushless); + m_rightWinchMotor = new CANSparkMax(10, CANSparkMax.MotorType.kBrushless); + + m_leftWinchMotor.restoreFactoryDefaults(); + m_rightWinchMotor.restoreFactoryDefaults(); + + m_leftWinchEncoder = m_leftWinchMotor.getEncoder(); + m_rightWinchEncoder = m_rightWinchMotor.getEncoder(); + + m_leftWinchMotor.setIdleMode(IdleMode.kBrake); + m_rightWinchMotor.setIdleMode(IdleMode.kBrake); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.leftWinchPositionRad = + Units.rotationsToRadians(m_leftWinchEncoder.getPosition() / kWinchReduction); + inputs.leftWinchVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + m_leftWinchEncoder.getVelocity() / kWinchReduction); + inputs.leftWinchAppliedVolts = + m_leftWinchMotor.getAppliedOutput() * m_leftWinchMotor.getBusVoltage(); + inputs.leftWinchCurrentAmps = m_leftWinchMotor.getOutputCurrent(); + + inputs.rightWinchPositionRad = + Units.rotationsToRadians(m_rightWinchEncoder.getPosition() / kWinchReduction); + inputs.rightWinchVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + m_rightWinchEncoder.getVelocity() / kWinchReduction); + inputs.rightWinchAppliedVolts = + m_rightWinchMotor.getAppliedOutput() * m_rightWinchMotor.getBusVoltage(); + inputs.rightWinchCurrentAmps = m_rightWinchMotor.getOutputCurrent(); + } + + @Override + public void setLeftDutyCycle(double outputDutyCycle) { + m_leftWinchMotor.set(outputDutyCycle); + } + + @Override + public void setRightDutyCycle(double outputDutyCycle) { + m_rightWinchMotor.set(outputDutyCycle); + } +} diff --git a/src/main/java/frc/robot/drivetrain/DriveToPose.java b/src/main/java/frc/robot/drivetrain/DriveToPose.java new file mode 100644 index 0000000..37cbb83 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/DriveToPose.java @@ -0,0 +1,118 @@ +package frc.robot.drivetrain; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.PoseEstimation; +import org.littletonrobotics.junction.Logger; + +public class DriveToPose extends Command { + + private final Drivetrain m_drivetrain; + private final ProfiledPIDController m_translationController, m_thetaController; + private Translation2d m_lastSetpointTranslation; + private Pose2d m_pose; + private Pose2d m_goalPose; + + public DriveToPose(Pose2d pose, Drivetrain drivetrain) { + m_pose = pose; + m_translationController = + new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(3.5, 2.2)); + m_thetaController = + new ProfiledPIDController( + 5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, 4 * Math.PI)); + this.m_drivetrain = drivetrain; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + m_goalPose = + m_drivetrain.onRedAllianceSupplier.getAsBoolean() + ? GeometryUtil.flipFieldPose(m_pose) + : m_pose; + Pose2d initialPose = PoseEstimation.getInstance().getPose(); + m_translationController.setTolerance(0.01); + m_thetaController.setTolerance(Units.degreesToRadians(1.0)); + m_thetaController.enableContinuousInput(-Math.PI, Math.PI); + Twist2d fieldVelocity = PoseEstimation.getInstance().getFieldVelocity(); + m_translationController.reset( + initialPose.getTranslation().getDistance(m_goalPose.getTranslation()), + Math.min( + 0.0, + -new Translation2d(fieldVelocity.dx, fieldVelocity.dy) + .rotateBy( + m_goalPose + .getTranslation() + .minus(initialPose.getTranslation()) + .getAngle() + .unaryMinus()) + .getX())); + m_thetaController.reset( + initialPose.getRotation().getRadians(), + PoseEstimation.getInstance().getFieldVelocity().dtheta); + m_lastSetpointTranslation = initialPose.getTranslation(); + } + + @Override + public void execute() { + Pose2d currentPose = PoseEstimation.getInstance().getPose(); + double currentDistance = currentPose.getTranslation().getDistance(m_goalPose.getTranslation()); + double ffScaler = MathUtil.clamp((currentDistance - 0.2) / (0.8 - 0.2), 0.0, 1.0); + m_translationController.reset( + m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()), + m_translationController.getSetpoint().velocity); + double driveVelocityScalar = + m_translationController.getSetpoint().velocity * ffScaler + + m_translationController.calculate(currentDistance, 0.0); + if (currentDistance < m_translationController.getPositionTolerance()) driveVelocityScalar = 0.0; + m_lastSetpointTranslation = + new Pose2d( + m_goalPose.getTranslation(), + currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d( + new Translation2d(m_translationController.getSetpoint().position, 0.0), + new Rotation2d())) + .getTranslation(); + + double thetaVelocity = + m_thetaController.getSetpoint().velocity * ffScaler + + m_thetaController.calculate( + currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians()); + double thetaErrorAbs = + Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians()); + if (thetaErrorAbs < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0; + + Translation2d driveVelocity = + new Pose2d( + new Translation2d(), + currentPose.getTranslation().minus(m_goalPose.getTranslation()).getAngle()) + .transformBy( + new Transform2d(new Translation2d(driveVelocityScalar, 0.0), new Rotation2d())) + .getTranslation(); + m_drivetrain.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); + + Logger.recordOutput("DriveToPose/DistanceMeasured", currentDistance); + Logger.recordOutput( + "DriveToPose/DistanceSetpoint", m_translationController.getSetpoint().position); + Logger.recordOutput("DriveToPose/ThetaMeasured", currentPose.getRotation().getRadians()); + Logger.recordOutput("DriveToPose/ThetaSetpoint", m_thetaController.getSetpoint().position); + Logger.recordOutput( + "Odometry/DriveToPoseSetpoint", + new Pose2d( + m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position))); + Logger.recordOutput("Odometry/DriveToPoseGoal", m_goalPose); + } +} diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index b4bccad..b5d4b81 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -1,18 +1,19 @@ package frc.robot.drivetrain; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.GeometryUtil; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -23,53 +24,66 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.lib.LocalADStarAK; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.lib.LoggedTunableNumber; import frc.robot.Constants; -import frc.robot.vision.Vision.VisionUpdate; +import frc.robot.PoseEstimation; import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import java.util.function.UnaryOperator; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drivetrain extends SubsystemBase { - public final double kTrackwidthMeters = Units.inchesToMeters(26.0); - public final double kWheelbaseMeters = Units.inchesToMeters(26.0); - private final double kDriveBaseRadius = + public static final Lock odometryLock = new ReentrantLock(); + public final BooleanSupplier onRedAllianceSupplier; + + private static final double kTrackwidthMeters = Units.inchesToMeters(20.75); + private static double kWheelbaseMeters = Units.inchesToMeters(15.75); + private static final Translation2d[] m_modulePositions = + new Translation2d[] { + new Translation2d(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), + new Translation2d(kTrackwidthMeters / 2.0, -kWheelbaseMeters / 2.0), + new Translation2d(-kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), + new Translation2d(-kTrackwidthMeters / 2.0, -kWheelbaseMeters / 2.0) + }; + public static final SwerveDriveKinematics swerveKinematics = + new SwerveDriveKinematics(m_modulePositions); + public static final double kDriveBaseRadius = Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0); - private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16.5); + private static final LoggedTunableNumber inRangeRadius = + new LoggedTunableNumber("Drivetrain/InRangeRadius", 2.10); + private static final LoggedTunableNumber inRangeTolerance = + new LoggedTunableNumber("Drivetrain/InRangeTolerance", 0.25); + private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16); private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI; - private final double kDeadband = 0.07; - private final boolean kUseVisionCorrection = true; - - public static final Lock odometryLock = new ReentrantLock(); + private final double kDeadband = 0.1; private final ImuIO m_imuIO; private final ImuIOInputsAutoLogged m_imuInputs = new ImuIOInputsAutoLogged(); private final Module[] m_modules = new Module[4]; // FL, FR, BL, BR - private final SwerveDrivePoseEstimator m_poseEstimator; - private final SwerveDriveKinematics m_kinematics = - new SwerveDriveKinematics( - new Translation2d(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), - new Translation2d(kTrackwidthMeters / 2.0, -kWheelbaseMeters / 2.0), - new Translation2d(-kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), - new Translation2d(-kTrackwidthMeters / 2.0, -kWheelbaseMeters / 2.0)); - - // For calculating chassis position deltas in simulation. - private SwerveModulePosition[] m_lastModulePositions = - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; + private final SysIdRoutine m_sysIdRoutineDrive, m_sysIdRoutineRotation; + + private Rotation2d m_gyroRotation = new Rotation2d(); + private HeadingController m_headingController; + private SwerveModulePosition[] m_lastModulePositions = { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; - private final Consumer m_visionUpdateConsumer; - private final SysIdRoutine m_sysIdRoutine; + public final Trigger atHeadingGoal = new Trigger(this::atHeadingGoal); + public final Trigger inRangeOfGoal = new Trigger(this::inRange); + public final Pose2d m_sourcePoseBlueOrigin = + new Pose2d(15.40, 0.95, Rotation2d.fromDegrees(-60.0)); public Drivetrain( ImuIO imuIO, @@ -83,20 +97,12 @@ public Drivetrain( m_modules[2] = new Module(backLeftModuleIO, 2); m_modules[3] = new Module(backRightModuleIO, 3); PhoenixOdometryThread.getInstance().start(); - m_poseEstimator = - new SwerveDrivePoseEstimator( - m_kinematics, new Rotation2d(), getModulePositions(), new Pose2d()); - m_visionUpdateConsumer = - (VisionUpdate visionUpdate) -> { - if (!kUseVisionCorrection) { - return; - } - m_poseEstimator.addVisionMeasurement( - visionUpdate.pose(), - visionUpdate.timestampSeconds(), - visionUpdate.standardDeviations()); - }; - m_sysIdRoutine = + + onRedAllianceSupplier = + () -> + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + m_sysIdRoutineDrive = new SysIdRoutine( new SysIdRoutine.Config( null, @@ -111,6 +117,23 @@ public Drivetrain( }, null, this)); + m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + Volts.of(4.0), + null, + (state) -> Logger.recordOutput("Drivetrain/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (int i = 0; i < m_modules.length; ++i) { + m_modules[i].runCharacterization( + m_modulePositions[i].getAngle().plus(Rotation2d.fromDegrees(90)), + voltage.in(Volts)); + } + }, + null, + this)); configurePathing(); } @@ -125,7 +148,7 @@ public void periodic() { odometryLock.unlock(); Logger.processInputs("DrivetrainInputs/IMU", m_imuInputs); - for (var module : m_modules) { + for (Module module : m_modules) { module.periodic(); } @@ -137,7 +160,7 @@ public void periodic() { } } - double[] sampleTimestamps = m_modules[0].getOdometryTimestamps(); + final double[] sampleTimestamps = m_modules[0].getOdometryTimestamps(); for (int updateIndex = 0; updateIndex < sampleTimestamps.length; updateIndex++) { SwerveModulePosition[] newModulePositions = new SwerveModulePosition[m_modules.length]; SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; @@ -151,28 +174,35 @@ public void periodic() { newModulePositions[moduleIndex].angle); m_lastModulePositions[moduleIndex] = newModulePositions[moduleIndex]; } - if (Constants.kIsSim) { - Logger.recordOutput("Drivetrain/dtheta", m_kinematics.toTwist2d(moduleDeltas).dtheta); - // TODO we cannot update the gyro sim state in the same loop we read it, it will always be - // behind causing inaccurate behavior - // m_imuIO.updateSim(m_kinematics.toTwist2d(moduleDeltas).dtheta); + + // Update gyro angle + if (m_imuInputs.connected) { + m_gyroRotation = m_imuInputs.odometryYawPositions[updateIndex]; + } else { + final Twist2d twist = swerveKinematics.toTwist2d(moduleDeltas); + m_gyroRotation = m_gyroRotation.plus(new Rotation2d(twist.dtheta)); } - // The reason we are bothering with timestamps here is so vision updates can be properly - // chronologized with odometry updates. - m_poseEstimator.updateWithTime( - sampleTimestamps[updateIndex], - m_imuInputs.odometryYawPositions[updateIndex], - newModulePositions); + PoseEstimation.getInstance() + .addOdometryMeasurement( + sampleTimestamps[updateIndex], m_gyroRotation, newModulePositions); } + + final ChassisSpeeds robotRelativeVelocity = swerveKinematics.toChassisSpeeds(getModuleStates()); + PoseEstimation.getInstance() + .setVelocity( + new Twist2d( + robotRelativeVelocity.vxMetersPerSecond, + robotRelativeVelocity.vyMetersPerSecond, + m_imuInputs.connected + ? m_imuInputs.yawVelocityRadPerSec + : robotRelativeVelocity.omegaRadiansPerSecond)); } - private void runVelocity(ChassisSpeeds speeds) { - if (Constants.kIsSim) { - m_imuIO.updateSim(speeds.omegaRadiansPerSecond * 0.02); - } + public void runVelocity(ChassisSpeeds speeds) { + if (m_headingController != null) speeds.omegaRadiansPerSecond += m_headingController.update(); ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = m_kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveModuleState[] setpointStates = swerveKinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, kMaxLinearSpeedMetersPerSecond); SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -185,51 +215,82 @@ private void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } - public Command testDrive() { - return this.run( - () -> { - runVelocity(new ChassisSpeeds(1, 1, 0)); - }) - .withName("Test Drive"); - } - public Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { - UnaryOperator squareAxis = - (Double axisMagnitude) -> Math.copySign(Math.pow(axisMagnitude, 2), axisMagnitude); + UnaryOperator cubeAxis = axisMagnitude -> Math.pow(axisMagnitude, 3); return this.run( () -> { - var x = squareAxis.apply(MathUtil.applyDeadband(xSupplier.getAsDouble(), kDeadband)); - var y = squareAxis.apply(MathUtil.applyDeadband(ySupplier.getAsDouble(), kDeadband)); - var omega = - squareAxis.apply(MathUtil.applyDeadband(omegaSupplier.getAsDouble(), kDeadband)); + double x = cubeAxis.apply(MathUtil.applyDeadband(xSupplier.getAsDouble(), kDeadband)); + double y = cubeAxis.apply(MathUtil.applyDeadband(ySupplier.getAsDouble(), kDeadband)); + double omega = + cubeAxis.apply(MathUtil.applyDeadband(omegaSupplier.getAsDouble(), kDeadband)); + boolean isFlipped = Constants.onRedAllianceSupplier.getAsBoolean(); runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( x * kMaxLinearSpeedMetersPerSecond, y * kMaxLinearSpeedMetersPerSecond, omega * kMaxAngularSpeedRadPerSec, - m_imuInputs.yawPosition)); + isFlipped + ? PoseEstimation.getInstance() + .getPose() + .getRotation() + .plus(new Rotation2d(Math.PI)) + : PoseEstimation.getInstance().getPose().getRotation())); }) .withName("Joystick Drive"); } public Command zeroGyro() { - return Commands.runOnce(() -> m_imuIO.setGyroAngle(0.0)).ignoringDisable(true); + return Commands.runOnce( + () -> + resetPose.accept( + new Pose2d( + PoseEstimation.getInstance().getPose().getTranslation(), + onRedAllianceSupplier.getAsBoolean() + ? GeometryUtil.flipFieldRotation(new Rotation2d()) + : new Rotation2d()))) + .ignoringDisable(true); + } + + public Command sysIdDriveQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineDrive.quasistatic(direction); } - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.quasistatic(direction); + public Command sysIdDriveDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineDrive.dynamic(direction); } - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.dynamic(direction); + public Command sysIdRotationQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + + public Command sysIdRotationDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + public Command sysIdSlipCurrent() { + return new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.2).per(Seconds.of(1.0)), + Volts.of(0.0), + Seconds.of(30.0), + (state) -> Logger.recordOutput("Drivetrain/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> { + for (Module module : m_modules) { + module.runCharacterization(voltage.in(Volts)); + } + }, + null, + this)) + .quasistatic(Direction.kForward); } private void configurePathing() { AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> m_kinematics.toChassisSpeeds(getModuleStates()), + PoseEstimation.getInstance()::getPose, + resetPose, + () -> swerveKinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( new PIDConstants(5.0, 0.0, 0.0), @@ -237,11 +298,8 @@ private void configurePathing() { kMaxLinearSpeedMetersPerSecond, kDriveBaseRadius, new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, + onRedAllianceSupplier, this); - Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( @@ -253,25 +311,39 @@ private void configurePathing() { }); } - @AutoLogOutput(key = "SwerveStates/Measured") + @AutoLogOutput(key = "SwerveStates/StatesMeasured") private SwerveModuleState[] getModuleStates() { return Arrays.stream(m_modules).map(Module::getState).toArray(SwerveModuleState[]::new); } + @AutoLogOutput(key = "SwerveStates/PositionsMeasured") private SwerveModulePosition[] getModulePositions() { return Arrays.stream(m_modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); } - @AutoLogOutput(key = "Drivetrain/Estimated Pose") - public Pose2d getPose() { - return m_poseEstimator.getEstimatedPosition(); + public Consumer resetPose = + (newPose) -> + PoseEstimation.getInstance().resetPose(m_gyroRotation, getModulePositions(), newPose); + + public void setHeadingGoal(Supplier headingGoal) { + m_headingController = new HeadingController(headingGoal); + } + + public void clearHeadingGoal() { + m_headingController = null; + Logger.recordOutput("HeadingController/Setpoint", new Pose2d()); } - public void setPose(Pose2d pose) { - m_poseEstimator.resetPosition(m_imuInputs.yawPosition, getModulePositions(), pose); + @AutoLogOutput(key = "Drivetrain/AtHeadingGoal") + public boolean atHeadingGoal() { + return m_headingController != null && m_headingController.atGoal(); } - public Consumer getVisionPoseConsumer() { - return m_visionUpdateConsumer; + @AutoLogOutput(key = "Drivetrain/InRangeOfGoal") + public boolean inRange() { + return MathUtil.isNear( + inRangeRadius.get(), + PoseEstimation.getInstance().getAimingParameters().effectiveDistance(), + inRangeTolerance.get()); } } diff --git a/src/main/java/frc/robot/drivetrain/HeadingController.java b/src/main/java/frc/robot/drivetrain/HeadingController.java new file mode 100644 index 0000000..e3bdcb0 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/HeadingController.java @@ -0,0 +1,77 @@ +package frc.robot.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.lib.LoggedTunableNumber; +import frc.robot.PoseEstimation; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class HeadingController { + + private static final LoggedTunableNumber kP = + new LoggedTunableNumber("HeadingController/kP", 5.0); + private static final LoggedTunableNumber kD = + new LoggedTunableNumber("HeadingController/kD", 0.0); + private static final LoggedTunableNumber maxVelocityMultipler = + new LoggedTunableNumber("HeadingController/MaxVelocityMultipler", 0.8); + private static final LoggedTunableNumber maxAccelerationMultipler = + new LoggedTunableNumber("HeadingController/MaxAccelerationMultipler", 0.8); + private static final LoggedTunableNumber toleranceDegrees = + new LoggedTunableNumber("HeadingController/ToleranceDegrees", 1.0); + + private final ProfiledPIDController m_controller; + private final Supplier m_goalHeadingSupplier; + + public HeadingController(Supplier goalHeadingSupplier) { + m_controller = + new ProfiledPIDController( + kP.get(), 0, kD.get(), new TrapezoidProfile.Constraints(0.0, 0.0), 0.02); + m_controller.enableContinuousInput(-Math.PI, Math.PI); + m_controller.setTolerance(Units.degreesToRadians(toleranceDegrees.get())); + m_goalHeadingSupplier = goalHeadingSupplier; + + m_controller.reset( + PoseEstimation.getInstance().getPose().getRotation().getRadians(), + PoseEstimation.getInstance().getFieldVelocity().dtheta); + } + + public double update() { + m_controller.setPID(kP.get(), 0, kD.get()); + m_controller.setTolerance(Units.degreesToRadians(toleranceDegrees.get())); + + final double maxAngularAcceleration = + 9.0 / Drivetrain.kDriveBaseRadius * maxAccelerationMultipler.get(); + final double maxAngularVelocity = + 3.5 / Drivetrain.kDriveBaseRadius * maxVelocityMultipler.get(); + m_controller.setConstraints( + new TrapezoidProfile.Constraints(maxAngularVelocity, maxAngularAcceleration)); + + final double output = + m_controller.calculate( + PoseEstimation.getInstance().getPose().getRotation().getRadians(), + m_goalHeadingSupplier.get().getRadians()); + + Logger.recordOutput( + "HeadingController/Setpoint", + new Pose2d( + PoseEstimation.getInstance().getPose().getTranslation(), m_goalHeadingSupplier.get())); + Logger.recordOutput( + "HeadingController/HeadingErrorDegrees", + Units.radiansToDegrees(m_controller.getPositionError())); + return output; + } + + @AutoLogOutput(key = "HeadingController/AtGoal") + public boolean atGoal() { + return MathUtil.isNear( + m_controller.getGoal().position, + m_controller.getSetpoint().position, + Units.degreesToRadians(toleranceDegrees.get())); + } +} diff --git a/src/main/java/frc/robot/drivetrain/ImuIO.java b/src/main/java/frc/robot/drivetrain/ImuIO.java index d484c3c..37ceda8 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIO.java +++ b/src/main/java/frc/robot/drivetrain/ImuIO.java @@ -14,8 +14,4 @@ public static class ImuIOInputs { } public default void updateInputs(ImuIOInputs inputs) {} - - public default void setGyroAngle(double angleRad) {} - - public default void updateSim(double dThetaRad) {} } diff --git a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java index 2d261de..0d703ca 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java @@ -5,23 +5,23 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; import java.util.Queue; public class ImuIOPigeon2 implements ImuIO { - private final Pigeon2 m_imu = new Pigeon2(20, "CANivore1"); - private final StatusSignal yaw = m_imu.getYaw(); + private final Pigeon2 m_imu = new Pigeon2(20, Constants.kDrivetrainCanBus); + private final StatusSignal m_yawSignal = m_imu.getYaw(); + private final StatusSignal m_yawVelocitySignal = m_imu.getAngularVelocityZWorld(); private final Queue m_yawPositionQueue, m_yawTimestampQueue; - private final StatusSignal yawVelocity = m_imu.getAngularVelocityZWorld(); public ImuIOPigeon2() { - m_imu.getConfigurator().apply(new Pigeon2Configuration()); - m_imu.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); - yawVelocity.setUpdateFrequency(100.0); + final Pigeon2Configuration imuConfig = new Pigeon2Configuration(); + m_imu.getConfigurator().apply(imuConfig); + m_imu.setYaw(0.0); + m_yawSignal.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); + m_yawVelocitySignal.setUpdateFrequency(100.0); m_imu.optimizeBusUtilization(); m_yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_imu, m_imu.getYaw()); m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); @@ -29,9 +29,10 @@ public ImuIOPigeon2() { @Override public void updateInputs(ImuIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.connected = + BaseStatusSignal.refreshAll(m_yawSignal, m_yawVelocitySignal).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(m_yawSignal.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(m_yawVelocitySignal.getValueAsDouble()); inputs.odometryYawPositions = m_yawPositionQueue.stream() @@ -42,14 +43,4 @@ public void updateInputs(ImuIOInputs inputs) { m_yawPositionQueue.clear(); m_yawTimestampQueue.clear(); } - - public void setGyroAngle(double angleRad) { - m_imu.setYaw(angleRad); - } - - public void updateSim(double dThetaRad) { - final Pigeon2SimState imuSimState = m_imu.getSimState(); - imuSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - imuSimState.addYaw(Math.toDegrees(dThetaRad)); - } } diff --git a/src/main/java/frc/robot/drivetrain/Module.java b/src/main/java/frc/robot/drivetrain/Module.java index 4051e1f..461077b 100644 --- a/src/main/java/frc/robot/drivetrain/Module.java +++ b/src/main/java/frc/robot/drivetrain/Module.java @@ -9,16 +9,18 @@ public class Module { // https://www.swervedrivespecialties.com/products/mk4i-swerve-module - public static final double kDriveRatio = 6.75; + // However, our first stage is 50:13 instead of 50:14 because we have a different gear. + public static final double kDriveRatio = (50.0 / 13.0) * (16.0 / 28.0) * (45.0 / 15.0); public static final double kSteerRatio = 150.0 / 7.0; - public final double kWheelDiameterMeters = Units.inchesToMeters(4.0); - public final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0; - private final double kCouplingGearRatio = 50.0 / 14.0; + private final double kWheelDiameterMeters = Units.inchesToMeters(3.87); + private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0; + private final double kCouplingGearRatio = 50.0 / 13.0; private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); private final int m_index; private SwerveModulePosition[] m_odometryPositions = new SwerveModulePosition[] {}; + private SwerveModulePosition m_lastPosition = new SwerveModulePosition(); public Module(ModuleIO io, int index) { m_io = io; @@ -35,9 +37,13 @@ public void periodic() { int sampleCount = m_inputs.odometryTimestamps.length; m_odometryPositions = new SwerveModulePosition[sampleCount]; for (int i = 0; i < sampleCount; i++) { - double positionMeters = m_inputs.odometryDrivePositionsRad[i] * kWheelRadiusMeters; - Rotation2d angle = m_inputs.odometryTurnPositions[i]; - m_odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + double driveRotations = Units.radiansToRotations(m_inputs.odometryDrivePositionsRad[i]); + Rotation2d steerAngle = m_inputs.odometryTurnPositions[i]; + driveRotations -= steerAngle.getRotations() * kCouplingGearRatio; + double driveRadians = Units.rotationsToRadians(driveRotations); + double positionMeters = driveRadians / (kDriveRatio / kWheelRadiusMeters); + m_odometryPositions[i] = new SwerveModulePosition(positionMeters, steerAngle); + m_lastPosition = m_odometryPositions[i]; } } @@ -49,10 +55,14 @@ public SwerveModuleState setSetpoint(SwerveModuleState state) { * kDriveRatio; double angleError = optimizedState.angle.getRadians() - m_inputs.steerPosition.getRadians(); - setpointVelocityRPS *= Math.cos(angleError); + setpointVelocityRPS *= Math.max(0.0, Math.cos(angleError)); + + if (optimizedState.speedMetersPerSecond != 0.0) { + double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec); + double driveRateBackOut = azimuthVelocityRPS *= kCouplingGearRatio; + setpointVelocityRPS += driveRateBackOut; + } - Logger.recordOutput( - "Test/Desired Speed Module " + m_index, optimizedState.speedMetersPerSecond); m_io.setSteerRotations(optimizedState.angle.getRotations()); m_io.setDriveRPS(setpointVelocityRPS); return state; @@ -63,6 +73,11 @@ public void runCharacterization(double volts) { m_io.setDriveVoltage(volts); } + public void runCharacterization(Rotation2d angle, double volts) { + m_io.setSteerRotations(angle.getRotations()); + m_io.setDriveVoltage(volts); + } + public void stop() { m_io.setDriveVoltage(0.0); m_io.setSteerVoltage(0.0); @@ -76,10 +91,9 @@ public double[] getOdometryTimestamps() { return m_inputs.odometryTimestamps; } - // getNewPositions() should be used for performant odometry updates, not this. + // getOdometryPositions() should be used for performant odometry updates, not this. public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - m_inputs.drivePositionRad * kWheelRadiusMeters, m_inputs.steerPosition); + return m_lastPosition; } public SwerveModuleState getState() { diff --git a/src/main/java/frc/robot/drivetrain/ModuleIO.java b/src/main/java/frc/robot/drivetrain/ModuleIO.java index 1ac0160..4fdf464 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIO.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIO.java @@ -21,6 +21,8 @@ public static class ModuleIOInputs { public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + + public boolean devicesConnected = false; } public default void updateInputs(ModuleIOInputs inputs) {} diff --git a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java index 8bbc4a4..bd52459 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java @@ -1,12 +1,14 @@ package frc.robot.drivetrain; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -18,22 +20,22 @@ import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; import java.util.Queue; -import org.littletonrobotics.junction.Logger; public class ModuleIOTalonFX implements ModuleIO { - private final double kDrivekP = 0.05; - private final double kDrivekS = 0.2; - private final double kDrivekV = 0.1; + private final double kDrivekP = 0.031509; + private final double kDrivekS = 0.21806; + private final double kDrivekV = 0.13279; + private final double kDrivekA = 0.0057523; private final double kSteerkP = 100.0; private final double kSteerkD = 0.2; - private final double kSlipCurrent = 80; - private final String kCanBusName = "CANivore1"; + private final double kSlipCurrent = Constants.kIsSim ? 400.0 : 58.0; private final TalonFX m_driveMotor, m_steerMotor; private final CANcoder m_azimuthEncoder; @@ -51,39 +53,39 @@ public class ModuleIOTalonFX implements ModuleIO { private final Queue m_drivePositionQueue, m_steerPositionQueue, m_timestampQueue; private final MotionMagicExpoVoltage m_steerControl; private final VelocityVoltage m_driveControl; - private final DCMotorSim m_driveSim = - new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kDriveRatio, 0.001); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(kDrivekV, kDrivekA), + DCMotor.getKrakenX60Foc(1), + Module.kDriveRatio); private final DCMotorSim m_steerSim = - new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kSteerRatio, 0.00001); - private final int m_index; + new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kSteerRatio, 0.004); public ModuleIOTalonFX(int index) { - m_index = index; switch (index) { case 0: // FL - m_driveMotor = new TalonFX(4, kCanBusName); - m_steerMotor = new TalonFX(3, kCanBusName); - m_azimuthEncoder = new CANcoder(12, kCanBusName); - m_absoluteEncoderMagnetOffset = -0.333251953125; + m_driveMotor = new TalonFX(4, Constants.kDrivetrainCanBus); + m_steerMotor = new TalonFX(3, Constants.kDrivetrainCanBus); + m_azimuthEncoder = new CANcoder(12, Constants.kDrivetrainCanBus); + m_absoluteEncoderMagnetOffset = 0.37353515625; break; case 1: // FR - m_driveMotor = new TalonFX(6, kCanBusName); - m_steerMotor = new TalonFX(5, kCanBusName); - m_azimuthEncoder = new CANcoder(13, kCanBusName); - m_absoluteEncoderMagnetOffset = 0.311767578125; + m_driveMotor = new TalonFX(6, Constants.kDrivetrainCanBus); + m_steerMotor = new TalonFX(5, Constants.kDrivetrainCanBus); + m_azimuthEncoder = new CANcoder(13, Constants.kDrivetrainCanBus); + m_absoluteEncoderMagnetOffset = -0.090576171875; break; case 2: // BL - m_driveMotor = new TalonFX(2, kCanBusName); - m_steerMotor = new TalonFX(1, kCanBusName); - m_azimuthEncoder = new CANcoder(11, kCanBusName); - m_absoluteEncoderMagnetOffset = -0.138916015625; + m_driveMotor = new TalonFX(2, Constants.kDrivetrainCanBus); + m_steerMotor = new TalonFX(1, Constants.kDrivetrainCanBus); + m_azimuthEncoder = new CANcoder(11, Constants.kDrivetrainCanBus); + m_absoluteEncoderMagnetOffset = 0.275634765625; break; case 3: // BR - m_driveMotor = new TalonFX(7, kCanBusName); - m_steerMotor = new TalonFX(8, kCanBusName); - m_azimuthEncoder = new CANcoder(14, kCanBusName); - m_absoluteEncoderMagnetOffset = -0.336669921875; + m_driveMotor = new TalonFX(7, Constants.kDrivetrainCanBus); + m_steerMotor = new TalonFX(8, Constants.kDrivetrainCanBus); + m_azimuthEncoder = new CANcoder(14, Constants.kDrivetrainCanBus); + m_absoluteEncoderMagnetOffset = 0.314208984375; break; default: throw new RuntimeException("Invalid module index"); @@ -104,8 +106,6 @@ public ModuleIOTalonFX(int index) { driveMotorConfig.Slot0.kP = kDrivekP; driveMotorConfig.Slot0.kS = kDrivekS; driveMotorConfig.Slot0.kV = kDrivekV; - driveMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = kSlipCurrent; - driveMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -kSlipCurrent; driveMotorConfig.CurrentLimits.StatorCurrentLimit = kSlipCurrent; driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -133,11 +133,11 @@ public ModuleIOTalonFX(int index) { m_driveVelocitySignal = m_driveMotor.getVelocity(); m_driveAppliedVoltsSignal = m_driveMotor.getMotorVoltage(); m_driveCurrentSignal = m_driveMotor.getStatorCurrent(); - m_steerAbsolutePositionSignal = m_azimuthEncoder.getAbsolutePosition(); m_steerPositionSignal = m_steerMotor.getPosition(); m_steerVelocitySignal = m_steerMotor.getVelocity(); m_steerAppliedVoltsSignal = m_steerMotor.getMotorVoltage(); m_steerCurrentSignal = m_steerMotor.getStatorCurrent(); + m_steerAbsolutePositionSignal = m_azimuthEncoder.getAbsolutePosition(); m_drivePositionQueue = PhoenixOdometryThread.getInstance() @@ -158,28 +158,29 @@ public ModuleIOTalonFX(int index) { m_steerVelocitySignal, m_steerAppliedVoltsSignal, m_steerCurrentSignal); - m_driveMotor.optimizeBusUtilization(); - m_steerMotor.optimizeBusUtilization(); + ParentDevice.optimizeBusUtilizationForAll(m_driveMotor, m_steerMotor, m_azimuthEncoder); - m_steerControl = new MotionMagicExpoVoltage(0.0, true, 0.0, 0, true, false, false); - m_driveControl = new VelocityVoltage(0.0, 0.0, true, 0.0, 0, true, false, false); + m_steerControl = + new MotionMagicExpoVoltage(0.0, true, 0.0, 0, true, false, false).withUpdateFreqHz(0.0); + m_driveControl = + new VelocityVoltage(0.0, 0.0, true, 0.0, 0, true, false, false).withUpdateFreqHz(0.0); } @Override public void updateInputs(ModuleIOInputs inputs) { - if (Constants.kIsSim) { - updateSim(); - } - BaseStatusSignal.refreshAll( - m_drivePositionSignal, - m_driveVelocitySignal, - m_driveAppliedVoltsSignal, - m_driveCurrentSignal, - m_steerAbsolutePositionSignal, - m_steerPositionSignal, - m_steerVelocitySignal, - m_steerAppliedVoltsSignal, - m_steerCurrentSignal); + if (Constants.kIsSim) updateSim(); + inputs.devicesConnected = + BaseStatusSignal.refreshAll( + m_drivePositionSignal, + m_driveVelocitySignal, + m_driveAppliedVoltsSignal, + m_driveCurrentSignal, + m_steerAbsolutePositionSignal, + m_steerPositionSignal, + m_steerVelocitySignal, + m_steerAppliedVoltsSignal, + m_steerCurrentSignal) + .equals(StatusCode.OK); inputs.drivePositionRad = Units.rotationsToRadians(m_drivePositionSignal.getValueAsDouble()) / Module.kDriveRatio; @@ -190,7 +191,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.steerAbsolutePosition = Rotation2d.fromRotations(m_steerAbsolutePositionSignal.getValueAsDouble()); - + // V*s / rad -> V*s / rot /** * NOTE We do not need to divide by the steer ratio since we're fusing a cancoder with the ratio * already accounted for. @@ -203,7 +204,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryDrivePositionsRad = m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / Module.kDriveRatio) + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) .toArray(); inputs.odometryTurnPositions = m_steerPositionQueue.stream() @@ -231,16 +232,10 @@ public void updateSim() { m_driveSim.update(0.02); m_steerSim.update(0.02); - Logger.recordOutput( - "Drivetrain/Sim/Module " + m_index + "/Drive Motor Output V", - driveSimState.getMotorVoltage()); final double driveRotorVelocityRPS = Units.radiansToRotations(m_driveSim.getAngularVelocityRadPerSec()) * Module.kDriveRatio; driveSimState.addRotorPosition(driveRotorVelocityRPS * 0.02); driveSimState.setRotorVelocity(driveRotorVelocityRPS); - Logger.recordOutput( - "Drivetrain/Sim/Module " + m_index + "/Steer Motor Output V", - steerSimState.getMotorVoltage()); /* * Note that since we are simulating a fused cancoder we cannot simulate the * internal rotor's state, just the cancoder diff --git a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java index c3ef94e..6fa3d7e 100644 --- a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java @@ -1,25 +1,24 @@ package frc.robot.drivetrain; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; public class PhoenixOdometryThread extends Thread { public static final double kOdometryFrequencyHz = 250.0; - private final Lock signalsLock = new ReentrantLock(); - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; + private final Lock m_signalsLock = new ReentrantLock(); + private BaseStatusSignal[] m_signals = new BaseStatusSignal[0]; + private final List> m_queues = new ArrayList<>(); + private final List> m_timestampQueues = new ArrayList<>(); private static PhoenixOdometryThread instance = null; @@ -37,34 +36,31 @@ private PhoenixOdometryThread() { @Override public void start() { - if (timestampQueues.size() > 0) { - super.start(); - } + if (m_timestampQueues.size() > 0) super.start(); } public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayDeque<>(100); - signalsLock.lock(); + Queue queue = new ArrayBlockingQueue<>(20); + m_signalsLock.lock(); Drivetrain.odometryLock.lock(); try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); + BaseStatusSignal[] newSignals = new BaseStatusSignal[m_signals.length + 1]; + System.arraycopy(m_signals, 0, newSignals, 0, m_signals.length); + newSignals[m_signals.length] = signal; + m_signals = newSignals; + m_queues.add(queue); } finally { - signalsLock.unlock(); + m_signalsLock.unlock(); Drivetrain.odometryLock.unlock(); } return queue; } public Queue makeTimestampQueue() { - Queue queue = new ArrayDeque<>(100); + Queue queue = new ArrayBlockingQueue<>(20); Drivetrain.odometryLock.lock(); try { - timestampQueues.add(queue); + m_timestampQueues.add(queue); } finally { Drivetrain.odometryLock.unlock(); } @@ -74,35 +70,27 @@ public Queue makeTimestampQueue() { @Override public void run() { while (true) { - signalsLock.lock(); + m_signalsLock.lock(); try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / kOdometryFrequencyHz, signals); - } else { - Thread.sleep((long) (1000.0 / kOdometryFrequencyHz)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); + BaseStatusSignal.waitForAll(LoggedRobot.defaultPeriodSecs, m_signals); } finally { - signalsLock.unlock(); + m_signalsLock.unlock(); } - Drivetrain.odometryLock.lock(); try { double timestamp = Logger.getRealTimestamp() / 1e6; double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { + for (BaseStatusSignal signal : m_signals) { totalLatency += signal.getTimestamp().getLatency(); } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; + if (m_signals.length > 0) { + timestamp -= totalLatency / m_signals.length; } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); + for (int i = 0; i < m_signals.length; i++) { + m_queues.get(i).offer(m_signals[i].getValueAsDouble()); } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); + for (int i = 0; i < m_timestampQueues.size(); i++) { + m_timestampQueues.get(i).offer(timestamp); } } finally { Drivetrain.odometryLock.unlock(); diff --git a/src/main/java/frc/robot/feeder/Feeder.java b/src/main/java/frc/robot/feeder/Feeder.java new file mode 100644 index 0000000..2d93b2b --- /dev/null +++ b/src/main/java/frc/robot/feeder/Feeder.java @@ -0,0 +1,61 @@ +package frc.robot.feeder; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.NoteVisualizer; +import org.littletonrobotics.junction.Logger; + +public class Feeder extends SubsystemBase { + + public static final double kFeederReduction = 12.0; + public final Trigger hasNote; + + private final FeederIO m_io; + private final FeederIOInputsAutoLogged m_inputs = new FeederIOInputsAutoLogged(); + + public Feeder(FeederIO io) { + m_io = io; + hasNote = new Trigger(() -> m_inputs.topNoteSensorTripped); + setDefaultCommand(idle()); + } + + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("FeederInputs", m_inputs); + Logger.recordOutput( + "Feeder/Command", getCurrentCommand() == null ? "" : getCurrentCommand().getName()); + } + + public Command idle() { + return this.run(() -> m_io.setRollerSpeedVolts(0.0)); + } + + public Command stop() { + return this.runOnce(m_io::stopRoller); + } + + public Command shoot() { + return Commands.parallel( + this.run(() -> m_io.setRollerSpeedVolts(-5.0)), + Commands.waitUntil(hasNote.negate()).andThen(NoteVisualizer.shoot())); + } + + public Command feed() { + return this.run(() -> m_io.setRollerSpeedVolts(-6.0)); + } + + public Command spit() { + return this.run(() -> m_io.setRollerSpeedVolts(12.0)); + } + + public Command enterCoast() { + return this.startEnd(() -> m_io.setBrakeMode(false), () -> m_io.setBrakeMode(true)); + } + + public Command pullBack() { + return this.run(() -> m_io.setRollerSpeedVolts(3.0)); + } +} diff --git a/src/main/java/frc/robot/feeder/FeederIO.java b/src/main/java/frc/robot/feeder/FeederIO.java new file mode 100644 index 0000000..d6992b6 --- /dev/null +++ b/src/main/java/frc/robot/feeder/FeederIO.java @@ -0,0 +1,26 @@ +package frc.robot.feeder; + +import org.littletonrobotics.junction.AutoLog; + +public interface FeederIO { + + @AutoLog + public static class FeederIOInputs { + public double rollerPositionRot = 0.0; + public double rollerVelocityRps = 0.0; + public double rollerAppliedVolts = 0.0; + public double rollerCurrentAmps = 0.0; + + public boolean topNoteSensorTripped = false; + + public boolean feederMotorConnected = false; + } + + public default void updateInputs(FeederIOInputs inputs) {} + + public default void setRollerSpeedVolts(double volts) {} + + public default void setBrakeMode(boolean enableBrakeMode) {} + + public default void stopRoller() {} +} diff --git a/src/main/java/frc/robot/feeder/FeederIOSim.java b/src/main/java/frc/robot/feeder/FeederIOSim.java new file mode 100644 index 0000000..d6a8e96 --- /dev/null +++ b/src/main/java/frc/robot/feeder/FeederIOSim.java @@ -0,0 +1,29 @@ +package frc.robot.feeder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class FeederIOSim implements FeederIO { + private DCMotorSim m_feederSim = + new DCMotorSim(DCMotor.getFalcon500Foc(1), Feeder.kFeederReduction, 0.01); + private double m_appliedVoltage = 0.0; + + @Override + public void updateInputs(FeederIOInputs inputs) { + m_feederSim.setInputVoltage(m_appliedVoltage * 12.0); + m_feederSim.update(0.02); + + inputs.feederMotorConnected = true; + inputs.rollerPositionRot = m_feederSim.getAngularPositionRotations(); + inputs.rollerVelocityRps = Units.radiansToRotations(m_feederSim.getAngularVelocityRadPerSec()); + inputs.rollerCurrentAmps = m_feederSim.getCurrentDrawAmps(); + inputs.rollerAppliedVolts = m_appliedVoltage; + } + + @Override + public void setRollerSpeedVolts(double volts) { + m_appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + } +} diff --git a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java new file mode 100644 index 0000000..4bbe2aa --- /dev/null +++ b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java @@ -0,0 +1,73 @@ +package frc.robot.feeder; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.Constants; + +public class FeederIOTalonFX implements FeederIO { + private final TalonFX m_feederMotor; + private final DigitalInput m_topNoteSensor; + + private final StatusSignal m_rollerPositionSignal, + m_rollerVelocitySignal, + m_rollerAppliedVoltageSignal, + m_rollerCurrentSignal; + + public FeederIOTalonFX() { + m_feederMotor = new TalonFX(19, Constants.kSuperstructureCanBus); + m_topNoteSensor = new DigitalInput(0); + + final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration(); + m_feederMotor.getConfigurator().apply(feederMotorConfig); + feederMotorConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + feederMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + feederMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + m_feederMotor.getConfigurator().apply(feederMotorConfig); + + m_rollerPositionSignal = m_feederMotor.getPosition(); + m_rollerVelocitySignal = m_feederMotor.getVelocity(); + m_rollerAppliedVoltageSignal = m_feederMotor.getMotorVoltage(); + m_rollerCurrentSignal = m_feederMotor.getStatorCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, m_rollerVelocitySignal, m_rollerAppliedVoltageSignal, m_rollerCurrentSignal); + m_feederMotor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + inputs.feederMotorConnected = + BaseStatusSignal.refreshAll( + m_rollerPositionSignal, + m_rollerVelocitySignal, + m_rollerAppliedVoltageSignal, + m_rollerCurrentSignal) + .equals(StatusCode.OK); + + inputs.rollerPositionRot = m_rollerPositionSignal.getValue() / Feeder.kFeederReduction; + inputs.rollerVelocityRps = m_rollerVelocitySignal.getValue() / Feeder.kFeederReduction; + inputs.rollerAppliedVolts = m_rollerAppliedVoltageSignal.getValue(); + inputs.rollerCurrentAmps = m_rollerCurrentSignal.getValue(); + inputs.topNoteSensorTripped = m_topNoteSensor.get(); + } + + @Override + public void setRollerSpeedVolts(double volts) { + m_feederMotor.setVoltage(volts); + } + + public void setBrakeMode(boolean enableBrakeMode) { + final NeutralModeValue neutralModeValue = + enableBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast; + m_feederMotor.setNeutralMode(neutralModeValue); + } + + @Override + public void stopRoller() { + m_feederMotor.setVoltage(0.0); + } +} diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 6bc9b01..6e7c8c1 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,78 +1,108 @@ -// 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.intake; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; +import frc.robot.FieldPositions.NotePositions; +import frc.robot.NoteVisualizer; +import frc.robot.PoseEstimation; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { - // private final double Speed = 1; - private final double StopSpeed = 0; - private double defaultFrontRollerSpeed = -.8; - private double defaultBackRollerSpeed = -.8; - private double defaultFrontRollerSpeedSpit = .8; - private double defaultBackRollerSpeedSpit = .8; - - private CANSparkMax topRoller = new CANSparkMax(5, CANSparkMax.MotorType.kBrushless); - private CANSparkMax bottomRoller = new CANSparkMax(6, CANSparkMax.MotorType.kBrushless); - - public Intake() { - SmartDashboard.putNumber("FrontRollerSpeed", defaultFrontRollerSpeed); - SmartDashboard.putNumber("BackRollerSpeed", defaultBackRollerSpeed); - SmartDashboard.putNumber("FrontRollerSpeedSpit", defaultFrontRollerSpeed); - SmartDashboard.putNumber("BackRollerSpeedSpit", defaultBackRollerSpeed); + private final IntakeIO m_io; + private final IntakeIOInputsAutoLogged m_inputs = new IntakeIOInputsAutoLogged(); + private boolean m_isIntakingPieceSim = false; + private final double kIntakeTimeSimSeconds = 0.1; + private final List m_intookPieces = new ArrayList<>(); + public final Trigger hasIntookPieceSim = + new Trigger(() -> m_isIntakingPieceSim).debounce(kIntakeTimeSimSeconds, DebounceType.kRising); - topRoller.setIdleMode(IdleMode.kBrake); - bottomRoller.setIdleMode(IdleMode.kBrake); - - // frontRoller.getFault(FaultID.) + public Intake(IntakeIO io) { + m_io = io; + setDefaultCommand(idle()); } @Override public void periodic() { - // This method will be called once per scheduler run + m_io.updateInputs(m_inputs); + Logger.processInputs("IntakeInputs", m_inputs); + Logger.recordOutput("Intake/Sim/HasIntookPiece", hasIntookPieceSim.getAsBoolean()); } - public Command Stop() { - return new RunCommand( + public Command idle() { + return this.run( () -> { - topRoller.set(StopSpeed); - bottomRoller.set(StopSpeed); - }, - this); + m_io.setTopRollerDutyCycle(0.0); + m_io.setBottomRollerDutyCycle(0.0); + m_io.setCenteringMotorsDutyCycle(0.0); + }); } - public Command Spit() { - return new RunCommand( - () -> { - double FrontRollerSpeedSpit = - SmartDashboard.getNumber("FrontRollerSpeedSpit", defaultFrontRollerSpeedSpit); - topRoller.set(defaultFrontRollerSpeedSpit); + public Command stop() { + return this.runOnce(m_io::stopRollers); + } - double BackRollerSpeedSpit = - SmartDashboard.getNumber("BackRollerSpeedSpit", defaultBackRollerSpeedSpit); - bottomRoller.set(defaultBackRollerSpeedSpit); - }, - this); + public Command spit() { + return this.run( + () -> { + m_io.setTopRollerDutyCycle(-0.8); + m_io.setBottomRollerDutyCycle(-0.8); + m_io.setCenteringMotorsDutyCycle(-0.5); + }); } public Command intake() { - return new RunCommand( - () -> { - double FrontRollerSpeed = - SmartDashboard.getNumber("FrontRollerSpeed", defaultFrontRollerSpeed); - topRoller.set(defaultFrontRollerSpeed); + return this.run( + () -> { + m_io.setTopRollerDutyCycle(0.8); + m_io.setBottomRollerDutyCycle(0.8); + m_io.setCenteringMotorsDutyCycle(0.85); + }) + .alongWith( + Commands.run(() -> m_isIntakingPieceSim = simulateIsIntakingPiece()) + .until(() -> m_isIntakingPieceSim) + .andThen(Commands.run(() -> m_isIntakingPieceSim = true)) + .finallyDo(() -> m_isIntakingPieceSim = false) + .unless(() -> !Constants.kIsSim)); + } - double BackRollerSpeed = - SmartDashboard.getNumber("BackRollerSpeed", defaultBackRollerSpeed); - bottomRoller.set(defaultBackRollerSpeed); - }, - this); + private boolean simulateIsIntakingPiece() { + final Translation2d intakeOffsetFromRobotCenter = + new Translation2d(Units.inchesToMeters(10.0), 0.0); + final Translation2d intakePosition = + PoseEstimation.getInstance().getPose().getTranslation().plus(intakeOffsetFromRobotCenter); + final double kIntakeXRangeInches = 9.0; + final double kIntakeYRangeInches = 27.0 / 2.0; + for (int noteIndex = 0; noteIndex < NotePositions.noteTranslations.length; noteIndex++) { + if (m_intookPieces.contains(noteIndex)) continue; + final Translation2d intakeToNote = + NotePositions.noteTranslations[noteIndex].minus(intakePosition); + if (MathUtil.isNear( + 0, Math.abs(intakeToNote.getX()), Units.inchesToMeters(kIntakeXRangeInches)) + && MathUtil.isNear( + 0, Math.abs(intakeToNote.getY()), Units.inchesToMeters(kIntakeYRangeInches))) { + m_intookPieces.add(noteIndex); + NoteVisualizer.removeNote(noteIndex); + return true; + } + } + return false; + } + + public Command feed() { + return this.run( + () -> { + m_io.setTopRollerDutyCycle(0.4); + m_io.setBottomRollerDutyCycle(0.4); + m_io.setCenteringMotorsDutyCycle(0.5); + }); } } diff --git a/src/main/java/frc/robot/intake/IntakeIO.java b/src/main/java/frc/robot/intake/IntakeIO.java new file mode 100644 index 0000000..de895c3 --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeIO.java @@ -0,0 +1,29 @@ +package frc.robot.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public class IntakeIOInputs { + public double topRollerVelocityRadPerSec = 0.0; + public double topRollerAppliedVolts = 0.0; + public double topRollerCurrentAmps = 0.0; + + public double bottomRollerVelocityRadPerSec = 0.0; + public double bottomRollerAppliedVolts = 0.0; + public double bottomRollerCurrentAmps = 0.0; + + public double centeringBagMotorsAppliedVolts = 0.0; + public double centeringBagMotorsCurrentAmps = 0.0; + } + + public default void updateInputs(IntakeIOInputs inputs) {} + + public default void setTopRollerDutyCycle(double outputDutyCycle) {} + + public default void setBottomRollerDutyCycle(double outputDutyCycle) {} + + public default void setCenteringMotorsDutyCycle(double outputDutyCycle) {} + + public default void stopRollers() {} +} diff --git a/src/main/java/frc/robot/intake/IntakeIOSim.java b/src/main/java/frc/robot/intake/IntakeIOSim.java new file mode 100644 index 0000000..0094c83 --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeIOSim.java @@ -0,0 +1,58 @@ +package frc.robot.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class IntakeIOSim implements IntakeIO { + + private double m_topRollerAppliedVolts, m_bottomRollerAppliedVolts, m_centeringRollerAppliedVolts; + + private final DCMotorSim m_topRollerSim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.01); + private final DCMotorSim m_bottomRollerSim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.01); + private final DCMotorSim m_centeringRollersSim = new DCMotorSim(DCMotor.getBag(2), 1.0, 0.01); + + @Override + public void updateInputs(IntakeIOInputs inputs) { + + m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts); + m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts); + m_centeringRollersSim.setInputVoltage(m_centeringRollerAppliedVolts); + m_topRollerSim.update(0.02); + m_bottomRollerSim.update(0.02); + m_centeringRollersSim.update(0.02); + + inputs.topRollerVelocityRadPerSec = m_topRollerSim.getAngularVelocityRadPerSec(); + inputs.topRollerAppliedVolts = m_topRollerAppliedVolts; + inputs.topRollerCurrentAmps = m_topRollerSim.getCurrentDrawAmps(); + + inputs.bottomRollerVelocityRadPerSec = m_bottomRollerSim.getAngularVelocityRadPerSec(); + inputs.bottomRollerAppliedVolts = m_bottomRollerAppliedVolts; + inputs.bottomRollerCurrentAmps = m_bottomRollerSim.getCurrentDrawAmps(); + + inputs.centeringBagMotorsAppliedVolts = m_centeringRollerAppliedVolts; + inputs.centeringBagMotorsCurrentAmps = m_centeringRollersSim.getCurrentDrawAmps(); + } + + @Override + public void setTopRollerDutyCycle(double outputDutyCycle) { + m_topRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + } + + @Override + public void setBottomRollerDutyCycle(double outputDutyCycle) { + m_bottomRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + } + + @Override + public void setCenteringMotorsDutyCycle(double outputDutyCycle) { + m_centeringRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + } + + @Override + public void stopRollers() { + m_topRollerAppliedVolts = 0.0; + m_bottomRollerAppliedVolts = 0.0; + m_centeringRollerAppliedVolts = 0.0; + } +} diff --git a/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java new file mode 100644 index 0000000..04a5277 --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java @@ -0,0 +1,68 @@ +package frc.robot.intake; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.util.Units; + +public class IntakeIOSparkMAX implements IntakeIO { + + private final CANSparkMax topRollerMotor, bottomRollerMotor, centeringMotors; + private final RelativeEncoder m_topRollerEncoder, m_bottomRollerEncoder; + + public IntakeIOSparkMAX() { + topRollerMotor = new CANSparkMax(5, CANSparkMax.MotorType.kBrushless); + bottomRollerMotor = new CANSparkMax(6, CANSparkMax.MotorType.kBrushless); + centeringMotors = new CANSparkMax(8, CANSparkMax.MotorType.kBrushed); + + m_topRollerEncoder = topRollerMotor.getEncoder(); + m_bottomRollerEncoder = bottomRollerMotor.getEncoder(); + + topRollerMotor.restoreFactoryDefaults(); + bottomRollerMotor.restoreFactoryDefaults(); + + topRollerMotor.setSmartCurrentLimit(40); + bottomRollerMotor.setSmartCurrentLimit(40); + centeringMotors.setSmartCurrentLimit(40); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.topRollerVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(m_topRollerEncoder.getVelocity()); + inputs.topRollerAppliedVolts = + topRollerMotor.getAppliedOutput() * topRollerMotor.getBusVoltage(); + inputs.topRollerCurrentAmps = topRollerMotor.getOutputCurrent(); + + inputs.bottomRollerVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(m_bottomRollerEncoder.getVelocity()); + inputs.bottomRollerAppliedVolts = + bottomRollerMotor.getAppliedOutput() * bottomRollerMotor.getBusVoltage(); + inputs.bottomRollerCurrentAmps = bottomRollerMotor.getOutputCurrent(); + + inputs.centeringBagMotorsAppliedVolts = + centeringMotors.getAppliedOutput() * centeringMotors.getBusVoltage(); + inputs.bottomRollerCurrentAmps = centeringMotors.getOutputCurrent(); + } + + @Override + public void setTopRollerDutyCycle(double outputDutyCycle) { + topRollerMotor.set(outputDutyCycle); + } + + @Override + public void setBottomRollerDutyCycle(double outputDutyCycle) { + bottomRollerMotor.set(outputDutyCycle); + } + + @Override + public void setCenteringMotorsDutyCycle(double outputDutyCycle) { + centeringMotors.set(outputDutyCycle); + } + + @Override + public void stopRollers() { + topRollerMotor.stopMotor(); + bottomRollerMotor.stopMotor(); + centeringMotors.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java new file mode 100644 index 0000000..4c76f39 --- /dev/null +++ b/src/main/java/frc/robot/lights/Lights.java @@ -0,0 +1,96 @@ +package frc.robot.lights; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.StrobeAnimation; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; +import java.util.function.Function; +import org.littletonrobotics.junction.Logger; + +public class Lights extends SubsystemBase { + + private final int kLedCount = 8; + private final CANdle m_ledController; + + public Lights() { + m_ledController = new CANdle(1, "rio"); + CANdleConfiguration config = new CANdleConfiguration(); + config.stripType = LEDStripType.RGB; + config.disableWhenLOS = true; + config.brightnessScalar = 1; + m_ledController.configAllSettings(config); + setDefaultCommand(setRainbow()); + } + + @Override + public void periodic() { + if (Constants.kIsSim) return; + Logger.recordOutput("CANdle/5VRailVoltage", m_ledController.get5VRailVoltage()); + Logger.recordOutput("CANdle/Current", m_ledController.getCurrent()); + Logger.recordOutput("CANdle/Temperature", m_ledController.getTemperature()); + } + + public Command setRainbow() { + return this.runOnce(() -> m_ledController.animate(new RainbowAnimation(1.0, 0.1, kLedCount))) + .andThen(this.run(() -> {})) + .ignoringDisable(true); + } + + public Command setBlink(Color color) { + final int[] blinkColor = colorToRGB(color); + return Commands.sequence( + this.runOnce( + () -> + m_ledController.animate( + new StrobeAnimation( + blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount))), + this.run(() -> {})) + .ignoringDisable(true); + } + + public void setBlinkMethod(Color color) { + final int[] blinkColor = colorToRGB(color); + + m_ledController.animate( + new StrobeAnimation(blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount)); + } + + /** + * @param color + * @return an array of integers representing the RGB values of the given color: [0] is red, [1] is + * green, [2] is blue + */ + private int[] colorToRGB(Color color) { + final Function mapValue = value -> (int) (value * 255.0); + return new int[] { + mapValue.apply(color.red), mapValue.apply(color.green), mapValue.apply(color.blue) + }; + } + + public Command showReadyToShootStatus(Trigger readyToShoot) { + var lastState = + new Object() { + boolean value = false; + }; + return new FunctionalCommand( + () -> setBlinkMethod(Color.kRed), + () -> { + boolean currentTriggerState = readyToShoot.getAsBoolean(); + if (lastState.value == currentTriggerState) return; + if (readyToShoot.getAsBoolean()) setBlinkMethod(Color.kGreen); + else setBlinkMethod(Color.kRed); + lastState.value = readyToShoot.getAsBoolean(); + }, + (interrupted) -> {}, + () -> false, + this); + } +} diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index b37c3d8..c3832e2 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -1,108 +1,157 @@ -// 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.shooter; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { - private final double InSpeed = -1; - private final double OutSpeed = 1; - private final double StopSpeed = 0; - private double defaultDelay = .3; - - private TalonFX feeder = new TalonFX(19, "CANivore2"); - private TalonFX shooterTop = new TalonFX(17, "CANivore2"); - private TalonFX shooterBottom = new TalonFX(18, "CANivore2"); + private final double kFarShotVelocityRpm = 5600.0; + private final double kAmpshot = 5000.0; + private final double kReadyToShootToleranceRps = 3.0; + + // Denominator for gains here are in rotations + public static final double topRollerkS = 0.18039; + public static final double topRollerkV = 0.11968; + public static final double topRollerkA = 0.0089044; + public static final double bottomRollerkS = 0.19936; + public static final double bottomRollerkV = 0.12041; + public static final double bottomRollerkA = 0.0071461; + + private final double topRollerkP = 0.13085; + private final double bottomRollerkP = 0.11992; + + private final PIDController m_topRollerController, m_bottomRollerController; + private final SimpleMotorFeedforward m_topRollerFeedforward, m_bottomRollerFeedforward; + private final SysIdRoutine m_sysIdRoutine; + + private final ShooterIO m_io; + private final ShooterIOInputsAutoLogged m_inputs = new ShooterIOInputsAutoLogged(); + + public final Trigger readyToShoot = + new Trigger(() -> getRollersAtSetpoint()).debounce(0.1, DebounceType.kBoth); + + public Shooter(ShooterIO io) { + m_io = io; + + m_topRollerFeedforward = new SimpleMotorFeedforward(topRollerkS, topRollerkV, topRollerkA); + m_topRollerController = new PIDController(topRollerkP, 0.0, 0.0); + m_bottomRollerFeedforward = + new SimpleMotorFeedforward(bottomRollerkS, bottomRollerkV, bottomRollerkA); + m_bottomRollerController = new PIDController(bottomRollerkP, 0.0, 0.0); + + m_topRollerController.setTolerance(kReadyToShootToleranceRps); + m_bottomRollerController.setTolerance(kReadyToShootToleranceRps); + m_sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + state -> Logger.recordOutput("Shooter/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> { + m_io.setTopRollerVoltage(voltage.in(Volts)); + m_io.setBottomRollerVoltage(voltage.in(Volts)); + }, + null, + this)); + setDefaultCommand(idle()); + } - /** Creates a new Rev_1Shooter. */ - public Shooter() { - SmartDashboard.putNumber("ShooterDelay", defaultDelay); - shooterTop.getConfigurator().apply(new TalonFXConfiguration()); - shooterBottom.getConfigurator().apply(new TalonFXConfiguration()); - feeder.getConfigurator().apply(new TalonFXConfiguration()); + @Override + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("ShooterInputs", m_inputs); + Logger.recordOutput( + "Shooter/Command", getCurrentCommand() == null ? "" : getCurrentCommand().getName()); + } - TalonFXConfiguration brakeMode = new TalonFXConfiguration(); - brakeMode.MotorOutput.NeutralMode = NeutralModeValue.Brake; + private void setRollersSetpointRpm(double velocityRpm) { + double goalVelocityRps = velocityRpm / 60.0; + Logger.recordOutput("Shooter/Goal Roller RPS", goalVelocityRps); + double topRollerFeedforwardOutput = m_topRollerFeedforward.calculate(goalVelocityRps); + double bottomRollerFeedforwardOutput = m_bottomRollerFeedforward.calculate(goalVelocityRps); + double topRollerFeedbackOutput = + m_topRollerController.calculate(m_inputs.topRollerVelocityRps, goalVelocityRps); + double bottomRollerFeedbackOutput = + m_bottomRollerController.calculate(m_inputs.bottomRollerVelocityRps, goalVelocityRps); + m_io.setTopRollerVoltage(topRollerFeedforwardOutput + topRollerFeedbackOutput); + m_io.setBottomRollerVoltage(bottomRollerFeedforwardOutput + bottomRollerFeedbackOutput); + } - shooterTop.getConfigurator().apply(brakeMode); - shooterBottom.getConfigurator().apply(brakeMode); - feeder.getConfigurator().apply(brakeMode); + @AutoLogOutput(key = "Shooter/RollersAtSetpoint") + private boolean getRollersAtSetpoint() { + return m_bottomRollerController.atSetpoint() && m_topRollerController.atSetpoint(); } - @Override - public void periodic() {} + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } - public Command Shoot() { - return new InstantCommand( - () -> { - shooterTop.set(OutSpeed); - shooterBottom.set(OutSpeed); - }, - this) - .repeatedly(); + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); } - public Command Stop() { - return new InstantCommand( + public Command idle() { + return this.run( () -> { - SmartDashboard.putNumber("ShooterSpeed", StopSpeed); - - shooterTop.set(StopSpeed); - shooterBottom.set(StopSpeed); - feeder.set(StopSpeed); - }, - this) - .repeatedly(); + m_io.setTopRollerVoltage(0.0); + m_io.setBottomRollerVoltage(0.0); + }) + .withName("Idle"); } - public Command Intake() { - return new InstantCommand( - () -> { - System.out.println("InSpeed"); - SmartDashboard.putNumber("ShooterSpeed", InSpeed); + public Command stop() { + return this.runOnce(m_io::stopRollers); + } - feeder.set(InSpeed); - }, - this); + public Command runShooter() { + return this.run( + () -> { + setRollersSetpointRpm(kFarShotVelocityRpm); + }) + .finallyDo( + () -> { + Logger.recordOutput("Shooter/Goal Roller RPS", 0.0); + m_topRollerController.reset(); + m_bottomRollerController.reset(); + }); } - public Command PullBack() { - return new RunCommand(() -> feeder.set(0.3), this).repeatedly().withTimeout(0.25); + public Command ampShot() { + return this.run(() -> setRollersSetpointRpm(kAmpshot)); } - public Command Feeder() { - return new InstantCommand( + public Command catchNote() { + return this.run( () -> { - feeder.set(-OutSpeed); - }, - this) - .repeatedly() - .withTimeout(1) - .finallyDo(() -> Stop()); + m_io.setTopRollerVoltage(-9.0); + m_io.setBottomRollerVoltage(-9.0); + }) + .withName("Catch Note"); } - public double getCurrent() { - return feeder.getTorqueCurrent().getValue(); + public Command spit() { + return this.run( + () -> { + m_io.setTopRollerVoltage(-6.0); + m_io.setBottomRollerVoltage(-6.0); + }) + .withName("Spit"); } - public Command FeederOut() { - return new InstantCommand( - () -> { - feeder.set(OutSpeed); - }, - this) - .repeatedly() - .withTimeout(1) - .finallyDo(() -> Stop()); + public Command shootWithFeederDelay() { + return this.run(() -> setRollersSetpointRpm(kFarShotVelocityRpm)) + .until(() -> getRollersAtSetpoint()) + .andThen(runShooter()); } } diff --git a/src/main/java/frc/robot/shooter/ShooterIO.java b/src/main/java/frc/robot/shooter/ShooterIO.java new file mode 100644 index 0000000..f05ce8e --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterIO.java @@ -0,0 +1,31 @@ +package frc.robot.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + + @AutoLog + public static class ShooterIOInputs { + public double topRollerPositionRot = 0.0; + public double topRollerVelocityRps = 0.0; + public double topRollerAppliedVolts = 0.0; + public double topRollerCurrentAmps = 0.0; + + public double bottomRollerPositionRot = 0.0; + public double bottomRollerVelocityRps = 0.0; + public double bottomRollerAppliedVolts = 0.0; + public double bottomRollerCurrentAmps = 0.0; + + public boolean rollerMotorsConnected = false; + } + + public default void updateInputs(ShooterIOInputs inputs) {} + + public default void setTopRollerVoltage(double volts) {} + + public default void setBottomRollerVoltage(double volts) {} + + public default void stopRollers() {} + + public default void setRollerDutyCycle(double volts) {} +} diff --git a/src/main/java/frc/robot/shooter/ShooterIOSim.java b/src/main/java/frc/robot/shooter/ShooterIOSim.java new file mode 100644 index 0000000..49d8521 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterIOSim.java @@ -0,0 +1,72 @@ +package frc.robot.shooter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class ShooterIOSim implements ShooterIO { + + private final LinearSystem m_topRollerPlant = + LinearSystemId.createDCMotorSystem(Shooter.topRollerkV, Shooter.topRollerkA); + private final LinearSystem m_bottomRollerPlant = + LinearSystemId.createDCMotorSystem(Shooter.bottomRollerkV, Shooter.bottomRollerkA); + private final DCMotor m_rollerGearbox = DCMotor.getFalcon500Foc(1); + private final DCMotorSim m_topRollerSim = new DCMotorSim(m_topRollerPlant, m_rollerGearbox, 1.0); + private final DCMotorSim m_bottomRollerSim = + new DCMotorSim(m_bottomRollerPlant, m_rollerGearbox, 1.0); + + private double m_topRollerAppliedVolts = 0.0; + private double m_bottomRollerAppliedVolts = 0.0; + + @Override + public void updateInputs(ShooterIOInputs inputs) { + m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts); + m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts); + m_topRollerSim.update(0.02); + m_bottomRollerSim.update(0.02); + + inputs.rollerMotorsConnected = true; + /** + * The units here are weird. The plant was made with gains that were in volts/rotations. This + * means what the sim thinks is radians is actually rotations. However, for simulating current + * draw the DCMotor class expects speed in actual radians/s, so just for this we have to convert + * rot/s to rad/s, thus we cannot use the sim class's getCurrentDrawAmps() method. + */ + inputs.topRollerPositionRot = m_topRollerSim.getAngularPositionRad(); + inputs.topRollerVelocityRps = m_topRollerSim.getAngularVelocityRadPerSec(); + inputs.topRollerAppliedVolts = m_topRollerAppliedVolts; + inputs.topRollerCurrentAmps = + m_rollerGearbox.getCurrent( + Units.rotationsToRadians(inputs.topRollerVelocityRps), m_topRollerAppliedVolts) + * Math.signum(m_topRollerAppliedVolts); + inputs.bottomRollerPositionRot = m_bottomRollerSim.getAngularPositionRad(); + inputs.bottomRollerVelocityRps = m_bottomRollerSim.getAngularVelocityRadPerSec(); + inputs.bottomRollerAppliedVolts = m_bottomRollerAppliedVolts; + inputs.bottomRollerCurrentAmps = + m_rollerGearbox.getCurrent( + Units.rotationsToRadians(inputs.bottomRollerVelocityRps), + m_bottomRollerAppliedVolts) + * Math.signum(m_bottomRollerAppliedVolts); + } + + @Override + public void setTopRollerVoltage(double volts) { + m_topRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } + + @Override + public void setBottomRollerVoltage(double volts) { + m_bottomRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } + + @Override + public void stopRollers() { + m_topRollerAppliedVolts = 0.0; + m_bottomRollerAppliedVolts = 0.0; + } +} diff --git a/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java new file mode 100644 index 0000000..4503ae5 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java @@ -0,0 +1,102 @@ +package frc.robot.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.Constants; + +public class ShooterIOTalonFX implements ShooterIO { + + private final TalonFX m_topRoller, m_bottomRoller; + private final VoltageOut m_topRollerControl, m_bottomRollerControl; + private final StatusSignal m_topRollerPositionSignal, + m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerPositionSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal; + + public ShooterIOTalonFX() { + m_topRoller = new TalonFX(20, Constants.kSuperstructureCanBus); + m_bottomRoller = new TalonFX(21, Constants.kSuperstructureCanBus); + + final TalonFXConfiguration topRollerMotorConfig = new TalonFXConfiguration(); + m_topRoller.getConfigurator().apply(topRollerMotorConfig); + final TalonFXConfiguration bottomRollerMotorConfig = new TalonFXConfiguration(); + m_bottomRoller.getConfigurator().apply(bottomRollerMotorConfig); + + m_topRollerPositionSignal = m_topRoller.getPosition(); + m_topRollerVelocitySignal = m_topRoller.getVelocity(); + m_topRollerAppliedVoltageSignal = m_topRoller.getMotorVoltage(); + m_topRollerCurrentSignal = m_topRoller.getStatorCurrent(); + m_bottomRollerPositionSignal = m_bottomRoller.getPosition(); + m_bottomRollerVelocitySignal = m_bottomRoller.getVelocity(); + m_bottomRollerAppliedVoltageSignal = m_bottomRoller.getMotorVoltage(); + m_bottomRollerCurrentSignal = m_bottomRoller.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal); + + ParentDevice.optimizeBusUtilizationForAll(m_topRoller, m_bottomRoller); + + m_topRollerControl = new VoltageOut(0, true, false, false, false).withUpdateFreqHz(0.0); + m_bottomRollerControl = new VoltageOut(0, true, false, false, false).withUpdateFreqHz(0.0); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.rollerMotorsConnected = + BaseStatusSignal.refreshAll( + m_topRollerPositionSignal, + m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerPositionSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal) + .equals(StatusCode.OK); + + inputs.topRollerPositionRot = m_topRollerPositionSignal.getValue(); + inputs.topRollerVelocityRps = m_topRollerVelocitySignal.getValue(); + inputs.topRollerAppliedVolts = m_topRollerAppliedVoltageSignal.getValue(); + inputs.topRollerCurrentAmps = m_topRollerCurrentSignal.getValue(); + inputs.bottomRollerPositionRot = m_bottomRollerPositionSignal.getValue(); + inputs.bottomRollerVelocityRps = m_bottomRollerVelocitySignal.getValue(); + inputs.bottomRollerAppliedVolts = m_bottomRollerAppliedVoltageSignal.getValue(); + inputs.bottomRollerCurrentAmps = m_bottomRollerCurrentSignal.getValue(); + } + + @Override + public void setTopRollerVoltage(double volts) { + m_topRoller.setControl(m_topRollerControl.withOutput(volts)); + } + + @Override + public void setBottomRollerVoltage(double volts) { + m_bottomRoller.setControl(m_bottomRollerControl.withOutput(volts)); + } + + public void stopRollers() { + m_topRoller.setVoltage(0.0); + m_bottomRoller.setVoltage(0.0); + } + + public void setRollerDutyCycle(double volts) { + m_topRoller.setControl(new DutyCycleOut(volts)); + m_bottomRoller.setControl(new DutyCycleOut(volts)); + } +} diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 48bc03c..7b9afbc 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -1,26 +1,37 @@ package frc.robot.vision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; +import static frc.robot.Constants.fieldLayout; + +import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; 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.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.PoseEstimation; import frc.robot.vision.VisionIO.VisionIOInputs; import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; import java.util.List; +import java.util.Map; import java.util.Optional; -import java.util.function.Consumer; import java.util.stream.Collectors; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; @@ -29,15 +40,21 @@ public class Vision { - private final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - private final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + private static final double kTargetLogTimeSecs = 0.1; + // #region Set these to false to disable vision measurements; + private final boolean kTrustVisionXY = true; + private final boolean kTrustVisionTheta = true; + private final boolean kIgnoreVisionInSim = true; + private final boolean kIgnoreVisionInAuto = true; + // #endregion private final VisionIO[] io; private final VisionIOInputs[] m_inputs; private final PhotonPoseEstimator[] m_poseEstimators; private VisionSystemSim m_visionSimSystem = null; - private final AprilTagFieldLayout kTagLayout = - AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - private Consumer m_visionUpdateConsumer = null; + private Map m_lastDetectionTimeIds = new HashMap<>(); + private ArrayList m_newVisionUpdates; + private final double kXYStdDevCoefficient = 0.1; + private final double kThetaStdDevCoefficient = 0.1; public Vision(VisionIO... io) { this.io = io; @@ -45,17 +62,18 @@ public Vision(VisionIO... io) { m_poseEstimators = new PhotonPoseEstimator[io.length]; for (int i = 0; i < io.length; i++) { m_inputs[i] = new VisionIOInputs(); - io[i].updateInputs(m_inputs[i]); // This is for setting camera names and offsets. + io[i].updateInputs( + m_inputs[i]); // This is for initializing camera names and offsets into inputs. m_poseEstimators[i] = new PhotonPoseEstimator( - kTagLayout, + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, new PhotonCamera(m_inputs[i].cameraName), m_inputs[i].robotToCamera); } if (Constants.kIsSim) { m_visionSimSystem = new VisionSystemSim("sim_vision_system"); - m_visionSimSystem.addAprilTags(kTagLayout); + m_visionSimSystem.addAprilTags(fieldLayout); for (int index = 0; index < io.length; index++) { PhotonCameraSim cameraSim = new PhotonCameraSim( @@ -64,74 +82,125 @@ public Vision(VisionIO... io) { m_visionSimSystem.addCamera(cameraSim, m_inputs[index].robotToCamera); } } + + fieldLayout + .getTags() + .forEach( + (AprilTag tag) -> { + m_lastDetectionTimeIds.put(tag.ID, 0.0); + }); } public void periodic() { for (int index = 0; index < io.length; index++) { io[index].updateInputs(m_inputs[index]); - Logger.processInputs("Vision/Cam" + Integer.toString(index), m_inputs[index]); + Logger.processInputs("VisionInputs/Cam" + Integer.toString(index), m_inputs[index]); } - final List posesToLog = new ArrayList<>(); - final List tagPosesToLog = new ArrayList<>(); - for (int i = 0; i < io.length; i++) { - for (PhotonPipelineResult result : m_inputs[i].results) { - Optional poseOptional = m_poseEstimators[i].update(result); + m_newVisionUpdates = new ArrayList<>(); + final List estimatedPosesToLog = new ArrayList<>(); + for (int ioIndex = 0; ioIndex < io.length; ioIndex++) { + for (int resultIndex = 0; resultIndex < m_inputs[ioIndex].results.length; resultIndex++) { + final byte[] rawResult = m_inputs[ioIndex].results[resultIndex]; + final Packet dataPacket = new Packet(1); + dataPacket.setData(rawResult); + if (dataPacket.getSize() < 1) + DriverStation.reportError("Photonvision data packet is empty.", true); + final PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket); + final double latencyCompensatedTimestampSeconds = + (m_inputs[ioIndex].timestampsMillis[resultIndex] / 1e6) + - (result.getLatencyMillis() / 1e3); + result.setTimestampSeconds(latencyCompensatedTimestampSeconds); + final Optional poseOptional = m_poseEstimators[ioIndex].update(result); poseOptional.ifPresent( pose -> { - Pose2d estimatedPose = pose.estimatedPose.toPose2d(); + Pose3d estimatedPose = pose.estimatedPose; List targets = result.getTargets(); targets.forEach( target -> { - kTagLayout + fieldLayout .getTagPose(target.getFiducialId()) - .ifPresent(tagPose -> tagPosesToLog.add(tagPose)); + .ifPresent( + tagPose -> + m_lastDetectionTimeIds.put( + target.getFiducialId(), Timer.getFPGATimestamp())); }); - posesToLog.add(estimatedPose); - m_visionUpdateConsumer.accept( + estimatedPosesToLog.add( + estimatedPose.plus( + new Transform3d( + new Translation3d(Units.inchesToMeters(2.5), 0.0, 0.0), + new Rotation3d()))); + m_newVisionUpdates.add( new VisionUpdate( - estimatedPose, + estimatedPose.toPose2d(), result.getTimestampSeconds(), - getEstimationStdDevs(estimatedPose, targets))); + getEstimationStdDevs(estimatedPose.toPose2d(), targets))); }); } + + // Sort vision updates so more recent ones are given higher weight. + m_newVisionUpdates.stream() + .sorted(Comparator.comparingDouble(VisionUpdate::timestampSeconds)) + .forEach(PoseEstimation.getInstance()::addVisionMeasurement); } - Logger.recordOutput("Vision/EstimatedPoses", posesToLog.toArray(new Pose2d[posesToLog.size()])); - Logger.recordOutput("Vision/TagPoses", tagPosesToLog.toArray(new Pose3d[tagPosesToLog.size()])); + + Logger.recordOutput( + "Vision/EstimatedPoses", + estimatedPosesToLog.toArray(new Pose3d[estimatedPosesToLog.size()])); + List targetPose3ds = new ArrayList<>(); + for (Map.Entry detectionEntry : m_lastDetectionTimeIds.entrySet()) + if (Timer.getFPGATimestamp() - detectionEntry.getValue() < kTargetLogTimeSecs) + fieldLayout.getTagPose(detectionEntry.getKey()).ifPresent(pose -> targetPose3ds.add(pose)); + + Logger.recordOutput("Vision/TagPoses", targetPose3ds.toArray(new Pose3d[targetPose3ds.size()])); Logger.recordOutput( "Vision/TagPoses2D", - tagPosesToLog.stream() - .map(p -> p.toPose2d()) + targetPose3ds.stream() + .map(pose3d -> pose3d.toPose2d()) .collect(Collectors.toList()) - .toArray(new Pose2d[tagPosesToLog.size()])); + .toArray(new Pose2d[targetPose3ds.size()])); } public Matrix getEstimationStdDevs( Pose2d estimatedPose, List targets) { - var estimatedStdDevs = kSingleTagStdDevs; - int numTags = 0; - double avgDist = 0; - // TODO add in ambiguity checks and set max stddevs if it exceeds a certain threshold - for (var target : targets) { - var tagPose = kTagLayout.getTagPose(target.getFiducialId()); + if ((Constants.kIsSim && kIgnoreVisionInSim) + || (DriverStation.isAutonomous() && kIgnoreVisionInAuto)) + return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + int tagCount = 0; + double totalDistance = 0; + for (PhotonTrackedTarget target : targets) { + Optional tagPose = fieldLayout.getTagPose(target.getFiducialId()); + // Ignore tags whose ids are not in the field layout. if (tagPose.isEmpty()) continue; - numTags++; - avgDist += + tagCount++; + totalDistance += tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); } - if (numTags == 0) return estimatedStdDevs; - avgDist /= numTags; - if (numTags > 1) estimatedStdDevs = kMultiTagStdDevs; - // After 4 meters we can't trust vision - if (numTags == 1 && avgDist > 4) - estimatedStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estimatedStdDevs = estimatedStdDevs.times(1 + (avgDist * avgDist / 30)); - return estimatedStdDevs; + double averageDistance = totalDistance / tagCount; + + /** + * Reject tags is there are no valid tags OR there is only one tag and it has a high pose + * ambiguity OR there is only one tag and the tag is too far. + */ + if ((tagCount == 0) + || (tagCount == 1 && targets.get(0).getPoseAmbiguity() > 0.25) + || (tagCount == 1 && averageDistance > 4)) + return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + + double xyStdDev = + kTrustVisionXY + ? kXYStdDevCoefficient * Math.pow(averageDistance, 2) / tagCount + : Double.MAX_VALUE; + double thetaStdDev = + kTrustVisionTheta + ? kThetaStdDevCoefficient * Math.pow(averageDistance, 2) / tagCount + : Double.MAX_VALUE; + return VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev); } private SimCameraProperties getSimCameraProperties() { final SimCameraProperties cameraProperties = new SimCameraProperties(); - cameraProperties.setCalibration(1600, 1200, Rotation2d.fromDegrees(75)); + cameraProperties.setCalibration(1280, 720, Rotation2d.fromDegrees(75)); cameraProperties.setCalibError(0.35, 0.10); cameraProperties.setFPS(20); cameraProperties.setAvgLatencyMs(50); @@ -143,10 +212,6 @@ public void updateSim(Pose2d currentPose) { m_visionSimSystem.update(currentPose); } - public void setVisionPoseConsumer(Consumer consumer) { - m_visionUpdateConsumer = consumer; - } - public static record VisionUpdate( Pose2d pose, double timestampSeconds, Matrix standardDeviations) {} } diff --git a/src/main/java/frc/robot/vision/VisionIO.java b/src/main/java/frc/robot/vision/VisionIO.java index 08c0f56..d5c6967 100644 --- a/src/main/java/frc/robot/vision/VisionIO.java +++ b/src/main/java/frc/robot/vision/VisionIO.java @@ -3,20 +3,21 @@ import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.inputs.LoggableInputs; -import org.photonvision.targeting.PhotonPipelineResult; public interface VisionIO { public static class VisionIOInputs implements LoggableInputs { public String cameraName = ""; public Transform3d robotToCamera = new Transform3d(); - public PhotonPipelineResult[] results = new PhotonPipelineResult[] {}; + public double[] timestampsMillis = new double[] {}; + public byte[][] results = new byte[][] {}; public boolean connected = false; @Override public void toLog(LogTable table) { table.put("CameraName", cameraName); table.put("RobotToCamera", robotToCamera); + table.put("Timestamps", timestampsMillis); table.put("ResultCount", results.length); for (int i = 0; i < results.length; i++) { table.put("Result/" + Integer.toString(i), results[i]); @@ -28,10 +29,11 @@ public void toLog(LogTable table) { public void fromLog(LogTable table) { cameraName = table.get("CameraName", cameraName); robotToCamera = table.get("RobotToCamera", robotToCamera); + timestampsMillis = table.get("Timestamps", new double[] {0.0}); int resultCount = table.get("ResultCount", 0); - results = new PhotonPipelineResult[resultCount]; + results = new byte[resultCount][]; for (int i = 0; i < resultCount; i++) { - results[i] = table.get("Result/" + Integer.toString(i), new PhotonPipelineResult()); + results[i] = table.get("Result/" + i, new byte[] {}); } connected = table.get("Connected", false); } diff --git a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java index 8292519..c0a8583 100644 --- a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java @@ -6,8 +6,6 @@ import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.TimestampedRaw; import edu.wpi.first.wpilibj.Timer; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.PhotonPipelineResult; public class VisionIOPhotonVision implements VisionIO { @@ -35,27 +33,19 @@ public void updateInputs(VisionIOInputs inputs) { inputs.cameraName = m_cameraName; inputs.robotToCamera = m_robotToCamera; TimestampedRaw[] dataQueue = m_photonDataSubscriber.readQueue(); - inputs.results = new PhotonPipelineResult[dataQueue.length]; + inputs.results = new byte[dataQueue.length][]; + inputs.timestampsMillis = new double[dataQueue.length]; for (int index = 0; index < dataQueue.length; index++) { - Packet dataPacket = new Packet(1); - dataPacket.setData(dataQueue[index].value); - if (dataPacket.getSize() < 1) { - throw new NullPointerException("Data packet is empty. This should NEVER happen."); - } - PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket); - double timestampSeconds = - (dataQueue[index].timestamp / 1e6) - (result.getLatencyMillis() / 1e3); - result.setTimestampSeconds(timestampSeconds); - inputs.results[index] = result; + inputs.results[index] = dataQueue[index].value; + inputs.timestampsMillis[index] = dataQueue[index].timestamp; } if (dataQueue.length > 0) { + inputs.connected = true; m_disconnectedTimer.reset(); } else if (m_disconnectedTimer.hasElapsed(kDisconnectedTimeout)) { inputs.connected = false; - } else { - inputs.connected = true; } } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index dc693b5..55a634d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.2", + "version": "3.2.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.2" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.2" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.2" + "version": "3.2.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.2", + "version": "3.2.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json new file mode 100644 index 0000000..8fbf7ab --- /dev/null +++ b/vendordeps/ChoreoLib.json @@ -0,0 +1,43 @@ +{ + "fileName": "ChoreoLib.json", + "name": "ChoreoLib", + "version": "2024.1.3", + "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", + "frcYear": "2024", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "javaDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-java", + "version": "2024.1.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-cpp", + "version": "2024.1.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index b8d69c7..0cd337c 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..adf6e02 --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 42d1c3e..e9113c8 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 9e9b7c9..237b5e3 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.3", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8b19d38..cba2533 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.2", + "version": "v2024.2.10", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.2", + "version": "v2024.2.10", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.2", + "version": "v2024.2.10", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.2" + "version": "v2024.2.10" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.2" + "version": "v2024.2.10" } ] } diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json deleted file mode 100644 index 186694f..0000000 --- a/vendordeps/playingwithfusion2024.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "playingwithfusion2024.json", - "name": "PlayingWithFusion", - "version": "2024.01.10", - "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", - "frcYear": "2024", - "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", - "mavenUrls": [ - "https://www.playingwithfusion.com/frc/maven/" - ], - "javaDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-java", - "version": "2024.01.10" - } - ], - "jniDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.10", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-cpp", - "version": "2024.01.10", - "headerClassifier": "headers", - "sharedLibrary": false, - "libName": "PlayingWithFusion", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - }, - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.10", - "headerClassifier": "headers", - "sharedLibrary": true, - "libName": "PlayingWithFusionDriver", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - } - ] -}