diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json index 5dabee93..68109c91 100644 --- a/.OutlineViewer/outlineviewer.json +++ b/.OutlineViewer/outlineviewer.json @@ -37,6 +37,9 @@ "Gains": { "open": true }, + "Stow Gains": { + "open": true + }, "open": true }, "Funnel": { diff --git a/Superstructure.dot b/Superstructure.dot deleted file mode 100644 index 38b57b57..00000000 --- a/Superstructure.dot +++ /dev/null @@ -1,242 +0,0 @@ -digraph Superstructure { - /* - * Graphviz DOT file for the superstructure state machine. - * States are represented as nodes, and transitions are represented as edges. - */ - - // Node definitions - start [color = red] - stow_down [color = green] - - intake_coral [color = green] - - L1 [color = green] - L2 [color = green] - L3 [color = green] - L4 [color = green] - L4_plus [color = green] - - score_L1 [color = green] - score_L2 [color = green] - score_L3 [color = green] - score_L4 [color = green] - score_L4_plus [color = green] - - intermediate_wait_for_elevator [color = blue] - intermediate_wait_for_arm [color = blue] - - stow_up [color = blue] - - floor_acquisition [color = blue] - reef_acquisition_L2 [color = blue] - reef_acquisition_L3 [color = blue] - - barge [color = blue] - processor [color = blue] - - floor_intake [color = blue] - reef_intake_L2 [color = blue] - reef_intake_L3 [color = blue] - reef_drop_L2 [color = blue] - reef_drop_L3 [color = blue] - - score_barge [color = blue] - score_processor [color = blue] - - - - // start - start -> stow_down - - // stow down - stow_down -> intake_coral - stow_down -> L1 - stow_down -> L2 - stow_down -> L3 - stow_down -> L4 - - stow_down -> intermediate_wait_for_elevator - - stow_down -> floor_acquisition - - // intake coral - intake_coral -> stow_down - - // L1 - L1 -> stow_down - - L1 -> L2 - L1 -> L3 - L1 -> L4 - - L1 -> score_L1 - - L1 -> intermediate_wait_for_elevator - - L1 -> floor_acquisition - - // L2 - L2 -> stow_down - - L2 -> L1 - L2 -> L3 - L2 -> L4 - - L2 -> score_L2 - - L2 -> intermediate_wait_for_elevator - - L2 -> floor_acquisition - - // L3 - L3 -> stow_down - - L3 -> L1 - L3 -> L2 - L3 -> L4 - - L3 -> score_L3 - - L3 -> intermediate_wait_for_arm - - L3 -> reef_acquisition_L2 - L3 -> reef_acquisition_L3 - - // L4 - L4 -> stow_down - - L4 -> L1 - L4 -> L2 - L4 -> L3 - L4 -> L4_plus - - L4 -> score_L4 - - L4 -> intermediate_wait_for_arm - - L4 -> reef_acquisition_L2 - L4 -> reef_acquisition_L3 - - // L4_plus - L4_plus -> stow_down - - L4_plus -> score_L4_plus - - L4_plus -> intermediate_wait_for_arm - - // score_L1 - score_L1 -> L1 - - // score_L2 - score_L2 -> L2 - - // score_L3 - score_L3 -> L3 - - // score_L4 - score_L4 -> L4 - - // score_L4_plus - score_L4_plus -> L4_plus - - // intermediate_wait_for_elevator - intermediate_wait_for_elevator -> stow_up - - intermediate_wait_for_elevator -> reef_acquisition_L2 - intermediate_wait_for_elevator -> reef_acquisition_L3 - - intermediate_wait_for_elevator -> barge - intermediate_wait_for_elevator -> processor - - // intermediate_wait_for_arm - intermediate_wait_for_arm -> stow_down - - intermediate_wait_for_arm -> L1 - intermediate_wait_for_arm -> L2 - - intermediate_wait_for_arm -> floor_acquisition - - // stow_up - stow_up -> intermediate_wait_for_arm - - stow_up -> barge - stow_up -> processor - - intermediate_wait_for_arm -> stow_up - - barge -> stow_up - processor -> stow_up - // floor_acquisition - floor_acquisition -> stow_down - - floor_acquisition -> intermediate_wait_for_elevator - - floor_acquisition -> floor_intake - - // reef_acquisition_L2 - reef_acquisition_L2 -> intermediate_wait_for_arm - - reef_acquisition_L2 -> stow_up - - reef_acquisition_L2 -> reef_acquisition_L3 - - reef_acquisition_L2 -> barge - reef_acquisition_L2 -> processor - - reef_acquisition_L2 -> reef_intake_L2 - reef_acquisition_L2 -> reef_drop_L2 - - // reef_acquisition_L3 - reef_acquisition_L3 -> intermediate_wait_for_arm - - reef_acquisition_L3 -> stow_up - - reef_acquisition_L3 -> reef_acquisition_L2 - - reef_acquisition_L3 -> barge - reef_acquisition_L3 -> processor - - reef_acquisition_L3 -> reef_intake_L3 - reef_acquisition_L3 -> reef_drop_L3 - - // barge - barge -> intermediate_wait_for_arm - - barge -> reef_acquisition_L2 - barge -> reef_acquisition_L3 - - barge -> processor - - barge -> score_barge - - // processor - processor -> intermediate_wait_for_arm - - processor -> reef_acquisition_L2 - processor -> reef_acquisition_L3 - - processor -> barge - - processor -> score_processor - - // floor_intake - floor_intake -> floor_acquisition - - // reef_intake_L2 - reef_intake_L2 -> reef_acquisition_L2 - - // reef_intake_L3 - reef_intake_L3 -> reef_acquisition_L3 - - // reef_drop_L2 - reef_drop_L2 -> reef_acquisition_L2 - - // reef_drop_L3 - reef_drop_L3 -> reef_acquisition_L3 - - // score_barge - score_barge -> barge - - // score_processor - score_processor -> processor -} \ No newline at end of file diff --git a/advantagescopeassets/Robot_Poot/config.json b/advantagescopeassets/Robot_Poot/config.json new file mode 100644 index 00000000..406fc2e2 --- /dev/null +++ b/advantagescopeassets/Robot_Poot/config.json @@ -0,0 +1,91 @@ +{ + "name": "Poot", + "sourceUrl": "https://frc190.onshape.com/documents/fffbf0683934212b94752d43/w/aa43f9bee049f3b1d51e0915/e/de81482fe4fd1694f19df80a", + "disableSimplification": true, + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "zeroedPosition": [ + -0.1875, + 0, + -0.1363 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "zeroedPosition": [ + 0.177800, + 0, + -0.095250 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "zeroedPosition": [ + 0.15381615, + 0, + -0.120650 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "zeroedPosition": [ + 0.15381615, + 0, + -0.270855 + ] + } + ] +} diff --git a/advantagescopeassets/Robot_Poot/model.glb b/advantagescopeassets/Robot_Poot/model.glb new file mode 100644 index 00000000..55f0efc5 Binary files /dev/null and b/advantagescopeassets/Robot_Poot/model.glb differ diff --git a/advantagescopeassets/Robot_Poot/model_0.glb b/advantagescopeassets/Robot_Poot/model_0.glb new file mode 100644 index 00000000..bcffde66 Binary files /dev/null and b/advantagescopeassets/Robot_Poot/model_0.glb differ diff --git a/advantagescopeassets/Robot_Poot/model_1.glb b/advantagescopeassets/Robot_Poot/model_1.glb new file mode 100644 index 00000000..4821a73f Binary files /dev/null and b/advantagescopeassets/Robot_Poot/model_1.glb differ diff --git a/advantagescopeassets/Robot_Poot/model_2.glb b/advantagescopeassets/Robot_Poot/model_2.glb new file mode 100644 index 00000000..13415227 Binary files /dev/null and b/advantagescopeassets/Robot_Poot/model_2.glb differ diff --git a/advantagescopeassets/Robot_Poot/model_3.glb b/advantagescopeassets/Robot_Poot/model_3.glb new file mode 100644 index 00000000..ee1e0f02 Binary files /dev/null and b/advantagescopeassets/Robot_Poot/model_3.glb differ diff --git a/build.gradle b/build.gradle index eab042ee..1ec9747d 100755 --- a/build.gradle +++ b/build.gradle @@ -6,6 +6,11 @@ plugins { id "com.diffplug.spotless" version "6.12.0" } +ext { + deploySrcDir = "src/main/deploy" + deployRobotDir = "/home/lvuser/deploy" +} + java { sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 @@ -13,6 +18,41 @@ java { def ROBOT_MAIN_CLASS = "frc.robot.Main" +gradle.projectsEvaluated { + def buildDir = new File("${project.buildDir}/classes/java/main") + if (!buildDir.exists()) { + println "⚠️ Warning: compiled classes not found. Make sure compileJava runs first." + project.ext.deploySrcDir = "src/main/deploy" + return + } + + try { + def loader = new URLClassLoader([buildDir.toURI().toURL()] as URL[], this.class.classLoader) + def constantsClass = loader.loadClass("frc.robot.Constants") + def robotField = constantsClass.getDeclaredField("ROBOT") + robotField.setAccessible(true) + def robotEnumValue = robotField.get(null) // static field + + println "Detected RobotType: ${robotEnumValue}" + + def baseName = robotEnumValue.toString().toLowerCase().replaceAll(/_sim$/, "") + def candidateDir = "src/main/deploy/${baseName}" + + if (file(candidateDir).exists()) { + project.ext.deploySrcDir = candidateDir + project.ext.deployRobotDir = "/home/lvuser/deploy/${baseName}" + } else { + println "Folder '${candidateDir}' does not exist. Using default deploy folder." + } + + println "deploySrcDir set to: ${project.ext.deploySrcDir}" + + } catch (Throwable e) { + println "Could not read RobotType: ${e.message}" + project.ext.deploySrcDir = "src/main/deploy" + } +} + // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { @@ -37,10 +77,10 @@ deploy { // The options below may improve performance, but should only be enabled on the RIO 2 - // final MAX_JAVA_HEAP_SIZE_MB = 100; - // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") - // jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") - // jvmArgs.add("-XX:+AlwaysPreTouch") + final MAX_JAVA_HEAP_SIZE_MB = 100 + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:+AlwaysPreTouch") // Enable VisualVM connection // jvmArgs.add("-Dcom.sun.management.jmxremote=true") @@ -53,8 +93,8 @@ deploy { // Static files artifact frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' + files = project.fileTree(project.ext.deploySrcDir) + directory = deployRobotDir // Change to true to delete files on roboRIO that no // longer exist in deploy directory on roboRIO deleteOldFiles = false @@ -65,7 +105,8 @@ deploy { } // Grab the roboRIO comment via SSH -import com.jcraft.jsch.* + +import com.jcraft.jsch.JSch ext { // RoboRIO SSH details @@ -222,7 +263,7 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() implementation 'org.jgrapht:jgrapht-core:1.5.1' - + implementation 'org.jgrapht:jgrapht-io:1.5.1' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) @@ -282,12 +323,12 @@ tasks.withType(JavaCompile) { // Create version file project.compileJava.dependsOn(createVersionFile) gversion { - srcDir = "src/main/java/" + srcDir = "src/main/java/" classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" - indent = " " + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " } // Create commit with working changes on event branches diff --git a/Superstructure.py b/debug/Superstructure.py similarity index 68% rename from Superstructure.py rename to debug/Superstructure.py index 68d83cce..af497177 100644 --- a/Superstructure.py +++ b/debug/Superstructure.py @@ -1,8 +1,10 @@ import networkx as nx from networkx.drawing.nx_pydot import read_dot +import os # Load the graph from the DOT file -G = read_dot("Superstructure.dot") +dot_file_path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../src/main/java/frc/robot/subsystems/v3_Poot/superstructure/Superstructure.dot")) +G = read_dot(dot_file_path) # Specify your start and end nodes start = input("Enter the start node: ") diff --git a/debug/dh2fk.py b/debug/dh2fk.py new file mode 100644 index 00000000..fd495ab4 --- /dev/null +++ b/debug/dh2fk.py @@ -0,0 +1,45 @@ +from readline import redisplay +from numpy import disp +import sympy as sp + +sp.init_printing(use_unicode=True) + +# 1. Define the symbolic variables +l1, l3, d3, l5, theta5, l7 = sp.symbols('l1 l3 d3 l5 theta5 l7') + +# 2. Define the DH table using the symbolic variables +# [theta, d, a/r, alpha] in radians +dh_table = [ + [0, 0, -l1, 0], + [sp.pi/2, 0, 0, 0], + [0, l3+d3, 0, 0], + [0, 0, 0, sp.pi/2], + [0, l5, 0, 0], + [theta5, 0, 0, -sp.pi/2], + [-sp.pi/2, l7, 0, 0] +] + +def create_symbolic_transform(dh_row): + theta, d, a, alpha = dh_row + + return sp.Matrix([ + [sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)], + [sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)], + [0, sp.sin(alpha), sp.cos(alpha), d], + [0, 0, 0, 1] + ]) + +# 3. Initialize the final transformation matrix as a 4x4 identity matrix +T_final_symbolic = sp.eye(4) + +# 4. Loop through the DH table and multiply the matrices +for row in dh_table: + T_final_symbolic = T_final_symbolic * create_symbolic_transform(row) + +# 5. Simplify the final expression for a cleaner result +T_final_symbolic = sp.simplify(T_final_symbolic) + +# 6. Display the final symbolic transformation matrix row by row +print("Final Symbolic Transformation Matrix:") +for i in range(T_final_symbolic.rows): + print(T_final_symbolic.row(i)) diff --git a/ds_hid/ds_hid.py b/ds_hid/ds_hid.py deleted file mode 100644 index 95f9f839..00000000 --- a/ds_hid/ds_hid.py +++ /dev/null @@ -1,137 +0,0 @@ -import usb.core -import usb.util -import usb.backend.libusb1 -from usbmonitor import USBMonitor -from networktables import NetworkTables - -# XK-80 Vendor and Product ID -VENDOR_ID = 1523 -PRODUCT_ID = 1091 - -# Networktables stuff -SERVER = '10.1.90.2' -KEYBOARD = 0 -KEYS = 80 - -connected = False -# Set up NetworkTables connection -try: - NetworkTables.initialize(server=SERVER) -except Exception as e: - print(f'Failed to initialize NetworkTables with server {SERVER}: {e}.') - -keyboard_status_table = NetworkTables.getTable('AdvantageKit/DriverStation/Keyboard'+str(KEYBOARD)) - -def on_disconnect(device_id, device_info): - keyboard_status_table.putBoolean('isConnected', False) - connected = False - print(f"Device disconnected: {device_id}") - -def on_connect(device_id, device_info): - keyboard_status_table.putBoolean('isConnected', True) - connected = True - print(f"Device connected: {device_id}") - - -monitor = USBMonitor() -monitor.start_monitoring(on_connect=on_connect, on_disconnect=on_disconnect, check_every_seconds=0.02) - -def send_data(data): - keyboard_status_table.putBoolean('isConnected', True) - for key in data: - keyboard_status_table.putBoolean(str(key), True) - -def set_up_NT(): - for i in range(KEYS): - keyboard_status_table.putBoolean(str(i), False) - -set_up_NT() - - -# Find the XK-80 device -backend = usb.backend.libusb1.get_backend() -device = usb.core.find(idVendor=VENDOR_ID, idProduct=PRODUCT_ID, backend=backend) - -if device is None: - print("XK-80 not found! Check connections.") - exit(1) - -print("XK-80 found and ready!") - -# Detach kernel driver if necessary (Linux only) -if device.is_kernel_driver_active(0): - device.detach_kernel_driver(0) - -# Set device configuration -device.set_configuration() - -# Find the HID endpoint for reading input -cfg = device.get_active_configuration() -interface = cfg[(0, 0)] -endpoint = usb.util.find_descriptor( - interface, - custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN -) - -if endpoint is None: - print("No IN endpoint found on XK-80") - keyboard_status_table.putBoolean('isConnected', False) - exit(1) - -print("Listening for XK-80 key presses...") - - -try: - while True: - try: - if connected is True: - keyboard_status_table.putBoolean('isConnected', True) - pass - data = device.read(endpoint.bEndpointAddress, endpoint.wMaxPacketSize, timeout=5000) - - if data: - keyboard_status_table.putBoolean('isConnected', True) - print(f"Raw Data: {list(data)}") - - key_states = data[2:12] # 10 bytes, representing 10 columns - pressed_keys = [] - - for col_index, byte in enumerate(key_states): - for row_index in range(8): - if (byte >> row_index) & 1: - key_number = (col_index) * 8 + row_index - pressed_keys.append(key_number) - - set_up_NT() - if pressed_keys: - print(f"Keys Pressed: {pressed_keys}") - send_data(pressed_keys) - else: - print("No keys pressed.") - else: - print("No data received.") - keyboard_status_table.putBoolean('isConnected', False) - - except usb.core.USBTimeoutError: - pass # Ignore timeout errors - - except usb.core.USBError as e: - if e.errno == 5: # Input/Output Error = Disconnected device - print("Device disconnected! Exiting loop...") - keyboard_status_table.putBoolean('isConnected', False) - break # Exit the loop when the device disconnects - -except KeyboardInterrupt: - print("\nStopping script.") - -finally: - print("Cleaning up...") - monitor.stop_monitoring() - keyboard_status_table.putBoolean('isConnected', False) - NetworkTables.stopClient() - - try: - usb.util.release_interface(device, 0) - device.attach_kernel_driver(0) # Only needed on Linux - except: - pass # Ignore errors if device is already gone diff --git a/ds_hid/startup.bat b/ds_hid/startup.bat deleted file mode 100644 index 93b4e9a1..00000000 --- a/ds_hid/startup.bat +++ /dev/null @@ -1,3 +0,0 @@ -@echo off -python ./ds_hid.py -exit \ No newline at end of file diff --git a/src/main/deploy/choreo/A_LEFT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH1.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH2.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH3.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH4.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH4.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH4.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH4.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH4_ALT_ALT.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH4_ALT_ALT.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH4_ALT_ALT.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH4_ALT_ALT.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH_ALT3.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH_ALT3.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH_ALT3.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH_ALT3.traj diff --git a/src/main/deploy/choreo/A_LEFT_PATH_ALT4.traj b/src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH_ALT4.traj similarity index 100% rename from src/main/deploy/choreo/A_LEFT_PATH_ALT4.traj rename to src/main/deploy/v2_redundancy/choreo/A_LEFT_PATH_ALT4.traj diff --git a/src/main/deploy/choreo/A_RIGHT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/A_RIGHT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH1.traj diff --git a/src/main/deploy/choreo/A_RIGHT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/A_RIGHT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH2.traj diff --git a/src/main/deploy/choreo/A_RIGHT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/A_RIGHT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH3.traj diff --git a/src/main/deploy/choreo/A_RIGHT_PATH4.traj b/src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH4.traj similarity index 100% rename from src/main/deploy/choreo/A_RIGHT_PATH4.traj rename to src/main/deploy/v2_redundancy/choreo/A_RIGHT_PATH4.traj diff --git a/src/main/deploy/choreo/B_LEFT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/B_LEFT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH1.traj diff --git a/src/main/deploy/choreo/B_LEFT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/B_LEFT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH2.traj diff --git a/src/main/deploy/choreo/B_LEFT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/B_LEFT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/B_LEFT_PATH3.traj diff --git a/src/main/deploy/choreo/B_RIGHT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/B_RIGHT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH1.traj diff --git a/src/main/deploy/choreo/B_RIGHT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/B_RIGHT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH2.traj diff --git a/src/main/deploy/choreo/B_RIGHT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/B_RIGHT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/B_RIGHT_PATH3.traj diff --git a/src/main/deploy/choreo/C_LEFT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/C_LEFT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH1.traj diff --git a/src/main/deploy/choreo/C_LEFT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/C_LEFT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH2.traj diff --git a/src/main/deploy/choreo/C_LEFT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/C_LEFT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/C_LEFT_PATH3.traj diff --git a/src/main/deploy/choreo/C_RIGHT_PATH1.traj b/src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH1.traj similarity index 100% rename from src/main/deploy/choreo/C_RIGHT_PATH1.traj rename to src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH1.traj diff --git a/src/main/deploy/choreo/C_RIGHT_PATH2.traj b/src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH2.traj similarity index 100% rename from src/main/deploy/choreo/C_RIGHT_PATH2.traj rename to src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH2.traj diff --git a/src/main/deploy/choreo/C_RIGHT_PATH3.traj b/src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH3.traj similarity index 100% rename from src/main/deploy/choreo/C_RIGHT_PATH3.traj rename to src/main/deploy/v2_redundancy/choreo/C_RIGHT_PATH3.traj diff --git a/src/main/deploy/choreo/Choreo.chor b/src/main/deploy/v2_redundancy/choreo/Choreo.chor similarity index 100% rename from src/main/deploy/choreo/Choreo.chor rename to src/main/deploy/v2_redundancy/choreo/Choreo.chor diff --git a/src/main/deploy/choreo/D_CENTER_PATH.traj b/src/main/deploy/v2_redundancy/choreo/D_CENTER_PATH.traj similarity index 100% rename from src/main/deploy/choreo/D_CENTER_PATH.traj rename to src/main/deploy/v2_redundancy/choreo/D_CENTER_PATH.traj diff --git a/src/main/deploy/v3_poot/Superstructure.dot b/src/main/deploy/v3_poot/Superstructure.dot new file mode 100644 index 00000000..b2aea297 --- /dev/null +++ b/src/main/deploy/v3_poot/Superstructure.dot @@ -0,0 +1,229 @@ +digraph Superstructure { + START -> STOW_DOWN [type=UNCONSTRAINED] + + STOW_DOWN -> FLIP_DOWN [type=UNCONSTRAINED] + STOW_DOWN -> L2 [type=UNCONSTRAINED] + STOW_DOWN -> L3 [type=UNCONSTRAINED] + STOW_DOWN -> L4 [type=UNCONSTRAINED] + STOW_DOWN -> STOW_UP [type=UNCONSTRAINED] + STOW_DOWN -> L2_ALGAE [type=UNCONSTRAINED] + STOW_DOWN -> L3_ALGAE [type=UNCONSTRAINED] + STOW_DOWN -> PROCESSOR [type=UNCONSTRAINED] + STOW_DOWN -> BARGE [type=UNCONSTRAINED] + STOW_DOWN -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + GROUND_ACQUISITION -> FLIP_UP [type=UNCONSTRAINED] + GROUND_ACQUISITION -> GROUND_INTAKE [type=UNCONSTRAINED] + GROUND_ACQUISITION -> L1 [type=UNCONSTRAINED] + GROUND_ACQUISITION -> L4 [type=UNCONSTRAINED] + GROUND_ACQUISITION -> HANDOFF [type=UNCONSTRAINED] + GROUND_ACQUISITION -> L3_ALGAE [type=UNCONSTRAINED] + GROUND_ACQUISITION -> BARGE [type=UNCONSTRAINED] + + GROUND_INTAKE -> FLIP_UP [type=UNCONSTRAINED] + GROUND_INTAKE -> GROUND_ACQUISITION [type=UNCONSTRAINED] + GROUND_INTAKE -> L1 [type=UNCONSTRAINED] + GROUND_INTAKE -> L4 [type=UNCONSTRAINED] + GROUND_INTAKE -> HANDOFF [type=UNCONSTRAINED] + GROUND_INTAKE -> L3_ALGAE [type=UNCONSTRAINED] + GROUND_INTAKE -> BARGE [type=UNCONSTRAINED] + + L1 -> FLIP_UP [type=UNCONSTRAINED] + L1 -> GROUND_ACQUISITION [type=UNCONSTRAINED] + L1 -> L4 [type=UNCONSTRAINED] + L1 -> L1_SCORE [type=UNCONSTRAINED] + L1 -> HANDOFF [type=UNCONSTRAINED] + L1 -> L3_ALGAE [type=UNCONSTRAINED] + L1 -> BARGE [type=UNCONSTRAINED] + + L2 -> STOW_DOWN [type=UNCONSTRAINED] + L2 -> FLIP_DOWN [type=UNCONSTRAINED] + L2 -> L3 [type=UNCONSTRAINED] + L2 -> L4 [type=UNCONSTRAINED] + L2 -> L2_SCORE [type=UNCONSTRAINED] + L2 -> STOW_UP [type=UNCONSTRAINED] + L2 -> L2_ALGAE [type=UNCONSTRAINED] + L2 -> L3_ALGAE [type=UNCONSTRAINED] + L2 -> PROCESSOR [type=UNCONSTRAINED] + L2 -> BARGE [type=UNCONSTRAINED] + + L3 -> STOW_DOWN [type=UNCONSTRAINED] + L3 -> FLIP_DOWN [type=UNCONSTRAINED] + L3 -> L2 [type=UNCONSTRAINED] + L3 -> L4 [type=UNCONSTRAINED] + L3 -> L3_SCORE [type=UNCONSTRAINED] + L3 -> HANDOFF [type=UNCONSTRAINED] + L3 -> STOW_UP [type=UNCONSTRAINED] + L3 -> L2_ALGAE [type=UNCONSTRAINED] + L3 -> L3_ALGAE [type=UNCONSTRAINED] + L3 -> PROCESSOR [type=UNCONSTRAINED] + L3 -> BARGE [type=UNCONSTRAINED] + L3 -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L4 -> STOW_DOWN [type=UNCONSTRAINED] + L4 -> GROUND_ACQUISITION [type=UNCONSTRAINED] + L4 -> L1 [type=UNCONSTRAINED] + L4 -> L2 [type=UNCONSTRAINED] + L4 -> L3 [type=UNCONSTRAINED] + L4 -> L4_SCORE [type=UNCONSTRAINED] + L4 -> HANDOFF [type=UNCONSTRAINED] + L4 -> STOW_UP [type=UNCONSTRAINED] + L4 -> L2_ALGAE [type=UNCONSTRAINED] + L4 -> L3_ALGAE [type=UNCONSTRAINED] + L4 -> PROCESSOR [type=UNCONSTRAINED] + L4 -> BARGE [type=UNCONSTRAINED] + L4 -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L1_SCORE -> L1 [type=UNCONSTRAINED] + + L2_SCORE -> STOW_DOWN [type=UNCONSTRAINED] + L2_SCORE -> FLIP_DOWN [type=UNCONSTRAINED] + L2_SCORE -> L2 [type=UNCONSTRAINED] + L2_SCORE -> L3 [type=UNCONSTRAINED] + L2_SCORE -> L4 [type=UNCONSTRAINED] + L2_SCORE -> STOW_UP [type=UNCONSTRAINED] + L2_SCORE -> L2_ALGAE [type=UNCONSTRAINED] + L2_SCORE -> L3_ALGAE [type=UNCONSTRAINED] + L2_SCORE -> PROCESSOR [type=UNCONSTRAINED] + L2_SCORE -> BARGE [type=UNCONSTRAINED] + + L3_SCORE -> STOW_DOWN [type=UNCONSTRAINED] + L3_SCORE -> FLIP_DOWN [type=UNCONSTRAINED] + L3_SCORE -> L2 [type=UNCONSTRAINED] + L3_SCORE -> L4 [type=UNCONSTRAINED] + L3_SCORE -> L3 [type=UNCONSTRAINED] + L3_SCORE -> HANDOFF [type=UNCONSTRAINED] + L3_SCORE -> STOW_UP [type=UNCONSTRAINED] + L3_SCORE -> L2_ALGAE [type=UNCONSTRAINED] + L3_SCORE -> L3_ALGAE [type=UNCONSTRAINED] + L3_SCORE -> PROCESSOR [type=UNCONSTRAINED] + L3_SCORE -> BARGE [type=UNCONSTRAINED] + L3_SCORE -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L4_SCORE -> FLIP_UP [type=UNCONSTRAINED] + L4_SCORE -> GROUND_ACQUISITION [type=UNCONSTRAINED] + L4_SCORE -> L1 [type=UNCONSTRAINED] + L4_SCORE -> L3 [type=UNCONSTRAINED] + L4_SCORE -> L4 [type=UNCONSTRAINED] + L4_SCORE -> HANDOFF [type=UNCONSTRAINED] + L4_SCORE -> BARGE [type=UNCONSTRAINED] + + HANDOFF -> FLIP_UP [type=UNCONSTRAINED] + HANDOFF -> GROUND_ACQUISITION [type=UNCONSTRAINED] + HANDOFF -> L1 [type=UNCONSTRAINED] + HANDOFF -> L4 [type=UNCONSTRAINED] + HANDOFF -> L3_ALGAE [type=UNCONSTRAINED] + HANDOFF -> BARGE [type=UNCONSTRAINED] + + STOW_UP -> STOW_DOWN [type=UNCONSTRAINED] + STOW_UP -> FLIP_DOWN [type=UNCONSTRAINED] + STOW_UP -> L2 [type=UNCONSTRAINED] + STOW_UP -> L3 [type=UNCONSTRAINED] + STOW_UP -> L4 [type=UNCONSTRAINED] + STOW_UP -> L2_ALGAE [type=UNCONSTRAINED] + STOW_UP -> L3_ALGAE [type=UNCONSTRAINED] + STOW_UP -> PROCESSOR [type=UNCONSTRAINED] + STOW_UP -> BARGE [type=UNCONSTRAINED] + STOW_UP -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L2_ALGAE -> STOW_DOWN [type = NO_ALGAE] + L2_ALGAE -> FLIP_DOWN [type = NO_ALGAE] + L2_ALGAE -> L2 [type=UNCONSTRAINED] + L2_ALGAE -> L3 [type=UNCONSTRAINED] + L2_ALGAE -> L4 [type=UNCONSTRAINED] + L2_ALGAE -> HANDOFF [type = NO_ALGAE] + L2_ALGAE -> STOW_UP [type=UNCONSTRAINED] + L2_ALGAE -> L2_ALGAE_INTAKE [type=UNCONSTRAINED] + L2_ALGAE -> L3_ALGAE [type = NO_ALGAE] + L2_ALGAE -> PROCESSOR [type=UNCONSTRAINED] + L2_ALGAE -> BARGE [type=UNCONSTRAINED] + L2_ALGAE -> L2_ALGAE_DROP + L2_ALGAE -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L2_ALGAE_DROP -> L2_ALGAE + + L2_ALGAE_INTAKE -> FLIP_DOWN [type=UNCONSTRAINED] + L2_ALGAE_INTAKE -> L2_ALGAE [type=UNCONSTRAINED] + L2_ALGAE_INTAKE -> L2_ALGAE_DROP [type=UNCONSTRAINED] + + + L3_ALGAE -> STOW_DOWN [type = NO_ALGAE] + L3_ALGAE -> GROUND_ACQUISITION [type = NO_ALGAE] + L3_ALGAE -> L1 [type = NO_ALGAE] + L3_ALGAE -> L2 [type=UNCONSTRAINED] + L3_ALGAE -> L3 [type=UNCONSTRAINED] + L3_ALGAE -> L4 [type=UNCONSTRAINED] + L3_ALGAE -> HANDOFF [type = NO_ALGAE] + L3_ALGAE -> STOW_UP [type=UNCONSTRAINED] + L3_ALGAE -> L2_ALGAE [type = NO_ALGAE] + L3_ALGAE -> L3_ALGAE_INTAKE [type=UNCONSTRAINED] + L3_ALGAE -> PROCESSOR [type=UNCONSTRAINED] + L3_ALGAE -> BARGE [type=UNCONSTRAINED] + L3_ALGAE -> L3_ALGAE_DROP + L3_ALGAE -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + L3_ALGAE_DROP -> L3_ALGAE + + L3_ALGAE_INTAKE -> STOW_UP [type=UNCONSTRAINED] + L3_ALGAE_INTAKE -> L3_ALGAE [type=UNCONSTRAINED] + L3_ALGAE_INTAKE -> L3_ALGAE_DROP [type=UNCONSTRAINED] + + + PROCESSOR -> POST_PROCESSOR [type = NO_ALGAE] + PROCESSOR -> PROCESSOR_SCORE [type=UNCONSTRAINED] + PROCESSOR -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + PROCESSOR_SCORE -> PROCESSOR [type=UNCONSTRAINED] + + BARGE -> STOW_DOWN [type=UNCONSTRAINED] + BARGE -> GROUND_ACQUISITION [type=UNCONSTRAINED] + BARGE -> L1 [type=UNCONSTRAINED] + BARGE -> L2 [type=UNCONSTRAINED] + BARGE -> L3 [type=UNCONSTRAINED] + BARGE -> L4 [type=UNCONSTRAINED] + BARGE -> HANDOFF [type=UNCONSTRAINED] + BARGE -> STOW_UP [type=UNCONSTRAINED] + BARGE -> L2_ALGAE [type=UNCONSTRAINED] + BARGE -> L3_ALGAE [type=UNCONSTRAINED] + BARGE -> PROCESSOR [type=UNCONSTRAINED] + BARGE -> BARGE_SCORE [type=UNCONSTRAINED] + BARGE -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + + BARGE_SCORE -> BARGE [type=UNCONSTRAINED] + + L4_SCORE -> GROUND_AQUISITION_ALGAE + L2_SCORE -> GROUND_AQUISITION_ALGAE + + GROUND_AQUISITION_ALGAE -> PROCESSOR + GROUND_AQUISITION_ALGAE -> POST_PROCESSOR + + GROUND_AQUISITION_ALGAE -> GROUND_INTAKE_ALGAE + GROUND_INTAKE_ALGAE -> GROUND_AQUISITION_ALGAE + + FLIP_DOWN -> GROUND_ACQUISITION [type=UNCONSTRAINED] + FLIP_DOWN -> L1 [type=UNCONSTRAINED] + FLIP_DOWN -> HANDOFF [type=UNCONSTRAINED] + + FLIP_UP -> STOW_DOWN [type=UNCONSTRAINED] + FLIP_UP -> STOW_UP [type=UNCONSTRAINED] + FLIP_UP -> GROUND_AQUISITION_ALGAE [type=UNCONSTRAINED] + FLIP_UP -> L2 [type=UNCONSTRAINED] + FLIP_UP -> L3 [type=UNCONSTRAINED] + FLIP_UP -> PROCESSOR [type=UNCONSTRAINED] + + POST_PROCESSOR -> FLIP_DOWN [type = NO_ALGAE] + POST_PROCESSOR -> STOW_UP [type = UNCONSTRAINED] + POST_PROCESSOR -> L2_ALGAE [type = UNCONSTRAINED] + POST_PROCESSOR -> L3_ALGAE [type = UNCONSTRAINED] + POST_PROCESSOR -> L2 [type = UNCONSTRAINED] + POST_PROCESSOR -> L3 [type = UNCONSTRAINED] + POST_PROCESSOR -> L4 [type = UNCONSTRAINED] + POST_PROCESSOR -> BARGE [type = UNCONSTRAINED] + + HANDOFF_SPIN -> HANDOFF + HANDOFF -> HANDOFF_SPIN + + HANDOFF -> INVERSE_FLIP_UP + + INVERSE_FLIP_UP -> STOW_UP +} \ No newline at end of file diff --git a/src/main/deploy/v3_poot/choreo/Choreo.chor b/src/main/deploy/v3_poot/choreo/Choreo.chor new file mode 100644 index 00000000..dd321a96 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/Choreo.chor @@ -0,0 +1,86 @@ +{ + "name":"Choreo", + "version":1, + "type":"Swerve", + "variables":{ + "expressions":{ + "MAX_VELO":{ + "dimension":"Number", + "var":{ + "exp":"5.763493039050549", + "val":5.763493039050549 + } + } + }, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11.375 in", + "val":0.288925 + }, + "y":{ + "exp":"11.375 in", + "val":0.288925 + } + }, + "backLeft":{ + "x":{ + "exp":"-11.375 in", + "val":-0.288925 + }, + "y":{ + "exp":"11.375 in", + "val":0.288925 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"7.125", + "val":7.125 + }, + "radius":{ + "exp":"1.915 in", + "val":0.048641 + }, + "vmax":{ + "exp":"5557 RPM", + "val":581.9276791999494 + }, + "tmax":{ + "exp":"0.75 N * m", + "val":0.75 + }, + "cof":{ + "exp":"1", + "val":1.0 + }, + "bumper":{ + "front":{ + "exp":"18 in", + "val":0.4572 + }, + "side":{ + "exp":"18 in", + "val":0.4572 + }, + "back":{ + "exp":"18 in", + "val":0.4572 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1.traj new file mode 100644 index 00000000..edc3f462 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1.traj @@ -0,0 +1,94 @@ +{ + "name":"E_RIGHT_PATH_1", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.152472496032715, "y":0.4605357348918915, "heading":3.141592653589793, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.458690404891968, "y":1.516339898109436, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.624938726425171, "y":2.281198024749756, "heading":2.5967288478948136, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.152472496032715 m", "val":7.152472496032715}, "y":{"exp":"0.4605357348918915 m", "val":0.4605357348918915}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.4586904048919678 m", "val":3.458690404891968}, "y":{"exp":"1.516339898109436 m", "val":1.516339898109436}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.624938726425171 m", "val":3.624938726425171}, "y":{"exp":"2.281198024749756 m", "val":2.281198024749756}, "heading":{"exp":"2.5967288478948136 rad", "val":2.5967288478948136}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.43204,1.85318], + "samples":[ + {"t":0.0, "x":7.15247, "y":0.46054, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.43977, "ay":0.38656, "alpha":-0.2428, "fx":[-109.47113,-109.61816,-109.60954,-109.45564], "fy":[7.74027,5.25975,5.38163,7.91913]}, + {"t":0.0387, "x":7.14765, "y":0.46083, "heading":3.14159, "vx":-0.24924, "vy":0.01496, "omega":-0.0094, "ax":-6.43929, "ay":0.38653, "alpha":-0.2399, "fx":[-109.46388,-109.60917,-109.60043,-109.44843], "fy":[7.72601,5.27498,5.39572,7.90237]}, + {"t":0.07741, "x":7.13318, "y":0.46169, "heading":3.14123, "vx":-0.49847, "vy":0.02992, "omega":-0.01868, "ax":-6.43874, "ay":0.3865, "alpha":-0.23656, "fx":[-109.45549,-109.5988,-109.58997,-109.44016], "fy":[7.71006,5.29292,5.41146,7.88267]}, + {"t":0.11611, "x":7.10906, "y":0.46314, "heading":3.14051, "vx":-0.74767, "vy":0.04488, "omega":-0.02784, "ax":-6.4381, "ay":0.38647, "alpha":-0.23267, "fx":[-109.44569,-109.5867,-109.57781,-109.43056], "fy":[7.69186,5.31413,5.42939,7.85943]}, + {"t":0.15482, "x":7.0753, "y":0.46517, "heading":3.13943, "vx":-0.99685, "vy":0.05984, "omega":-0.03684, "ax":-6.43734, "ay":0.38643, "alpha":-0.22809, "fx":[-109.43412,-109.57241,-109.5635,-109.41925], "fy":[7.67065,5.33934,5.45026,7.83186]}, + {"t":0.19352, "x":7.0319, "y":0.46777, "heading":3.138, "vx":-1.246, "vy":0.07479, "omega":-0.04567, "ax":-6.43644, "ay":0.38638, "alpha":-0.22262, "fx":[-109.42025,-109.55531,-109.54639,-109.40573], "fy":[7.64539,5.36956,5.4751,7.79883]}, + {"t":0.23222, "x":6.97885, "y":0.47096, "heading":3.13623, "vx":-1.49512, "vy":0.08975, "omega":-0.05429, "ax":-6.43533, "ay":0.38632, "alpha":-0.21596, "fx":[-109.40336,-109.53448,-109.52556,-109.38925], "fy":[7.61457,5.40627,5.50536,7.75875]}, + {"t":0.27093, "x":6.91617, "y":0.47472, "heading":3.13413, "vx":-1.74419, "vy":0.1047, "omega":-0.06265, "ax":-6.43396, "ay":0.38625, "alpha":-0.20769, "fx":[-109.38233,-109.50854,-109.49965,-109.36873], "fy":[7.57595,5.45164,5.54322,7.70923]}, + {"t":0.30963, "x":6.84384, "y":0.47906, "heading":3.13171, "vx":-1.99321, "vy":0.11965, "omega":-0.07068, "ax":-6.4322, "ay":0.38616, "alpha":-0.19713, "fx":[-109.35546,-109.47538,-109.46653,-109.34246], "fy":[7.52609,5.50907,5.59201,7.6466]}, + {"t":0.34834, "x":6.76188, "y":0.48398, "heading":3.12897, "vx":-2.24216, "vy":0.1346, "omega":-0.07831, "ax":-6.42988, "ay":0.38604, "alpha":-0.18319, "fx":[-109.3199,-109.43148,-109.42271,-109.30764], "fy":[7.45932,5.58413,5.65719,7.56483]}, + {"t":0.38704, "x":6.67028, "y":0.48948, "heading":3.12594, "vx":-2.49102, "vy":0.14954, "omega":-0.0854, "ax":-6.42666, "ay":0.38587, "alpha":-0.16393, "fx":[-109.27061,-109.37061,-109.36204,-109.25932], "fy":[7.36566,5.68662,5.74832,7.45336]}, + {"t":0.42574, "x":6.56906, "y":0.49556, "heading":3.12264, "vx":-2.73976, "vy":0.16447, "omega":-0.09175, "ax":-6.4219, "ay":0.38562, "alpha":-0.13563, "fx":[-109.1977,-109.28059,-109.27253,-109.18785], "fy":[7.22578,5.83561,5.88381,7.29175]}, + {"t":0.46445, "x":6.45821, "y":0.50221, "heading":3.11909, "vx":-2.98831, "vy":0.1794, "omega":-0.097, "ax":-6.41415, "ay":0.38521, "alpha":-0.08993, "fx":[-109.07873,-109.13382,-109.12736,-109.0715], "fy":[6.99646,6.0737,6.10425,7.03481]}, + {"t":0.50315, "x":6.33774, "y":0.50944, "heading":3.11533, "vx":-3.23656, "vy":0.19431, "omega":-0.10048, "ax":-6.39933, "ay":0.38443, "alpha":-0.00371, "fx":[-108.84974,-108.85202,-108.85166,-108.84938], "fy":[6.55747,6.51934,6.5206,6.55874]}, + {"t":0.54185, "x":6.20768, "y":0.51725, "heading":3.11144, "vx":-3.48424, "vy":0.20919, "omega":-0.10062, "ax":-6.35974, "ay":0.38234, "alpha":0.22009, "fx":[-108.22813,-108.09176,-108.12865,-108.26091], "fy":[5.40004,7.67241,7.58401,5.35765]}, + {"t":0.58056, "x":6.06807, "y":0.52564, "heading":3.10755, "vx":-3.73039, "vy":0.22398, "omega":-0.0921, "ax":-5.94158, "ay":0.35983, "alpha":2.35627, "fx":[-100.83823,-99.32967,-101.47744,-102.61289], "fy":[-6.02327,18.72383,16.00682,-4.22516]}, + {"t":0.61926, "x":5.91924, "y":0.53457, "heading":3.10398, "vx":-3.96035, "vy":0.23791, "omega":-0.00091, "ax":-0.02747, "ay":0.03408, "alpha":0.01827, "fx":[-0.4216,-0.41802,-0.51279,-0.51638], "fy":[0.53056,0.62535,0.62889,0.53411]}, + {"t":0.65797, "x":5.76593, "y":0.54381, "heading":3.10395, "vx":-3.96142, "vy":0.23923, "omega":-0.0002, "ax":0.00641, "ay":0.10544, "alpha":0.00001, "fx":[0.10899,0.10899,0.10896,0.10896], "fy":[1.79344,1.79347,1.79347,1.79344]}, + {"t":0.69667, "x":5.61262, "y":0.55315, "heading":3.10394, "vx":-3.96117, "vy":0.24331, "omega":-0.0002, "ax":0.02411, "ay":0.38088, "alpha":0.0, "fx":[0.41015,0.41015,0.41015,0.41015], "fy":[6.47867,6.47867,6.47867,6.47867]}, + {"t":0.73537, "x":5.45932, "y":0.56285, "heading":3.10393, "vx":-3.96023, "vy":0.25805, "omega":-0.0002, "ax":0.09573, "ay":1.3341, "alpha":-0.00001, "fx":[1.6284,1.6284,1.62843,1.62843], "fy":[22.69273,22.6927,22.6927,22.69273]}, + {"t":0.77408, "x":5.30612, "y":0.57383, "heading":3.10393, "vx":-3.95653, "vy":0.30969, "omega":-0.0002, "ax":0.33472, "ay":3.49968, "alpha":-0.00003, "fx":[5.69333,5.69333,5.69355,5.69355], "fy":[59.52869,59.52857,59.52856,59.52868]}, + {"t":0.81278, "x":5.15323, "y":0.58844, "heading":3.10392, "vx":-3.94357, "vy":0.44514, "omega":-0.0002, "ax":0.72213, "ay":5.19375, "alpha":-0.00009, "fx":[12.28283,12.28289,12.28368,12.28363], "fy":[88.34433,88.34415,88.34406,88.34423]}, + {"t":0.85149, "x":5.00114, "y":0.60956, "heading":3.10391, "vx":-3.91562, "vy":0.64616, "omega":-0.0002, "ax":1.13019, "ay":5.79426, "alpha":-0.00019, "fx":[19.22313,19.22338,19.22516,19.22492], "fy":[98.55899,98.55877,98.55845,98.55866]}, + {"t":0.89019, "x":4.85044, "y":0.63891, "heading":3.1039, "vx":-3.87188, "vy":0.87042, "omega":-0.00021, "ax":1.53381, "ay":5.9638, "alpha":-0.00284, "fx":[26.07359,26.07905,26.10576,26.10029], "fy":[101.4472,101.44445,101.43787,101.44063]}, + {"t":0.92889, "x":4.70173, "y":0.67706, "heading":3.10389, "vx":-3.81252, "vy":1.10124, "omega":-0.00032, "ax":2.00103, "ay":5.94587, "alpha":-0.21879, "fx":[32.7525,33.30897,35.33727,34.74911], "fy":[101.58984,101.34351,100.67401,100.94296]}, + {"t":0.9676, "x":4.55567, "y":0.72414, "heading":3.10388, "vx":-3.73507, "vy":1.33137, "omega":-0.00879, "ax":4.04991, "ay":4.68021, "alpha":-4.5705, "fx":[43.89469,65.97451,92.55565,73.12646], "fy":[98.72123,84.24058,55.50597,79.96825]}, + {"t":1.0063, "x":4.41414, "y":0.77917, "heading":3.10354, "vx":-3.57832, "vy":1.51251, "omega":-0.18569, "ax":5.79589, "ay":2.51438, "alpha":-3.27227, "fx":[87.91838,103.02786,106.24749,97.15184], "fy":[63.95797,34.03173,23.74428,49.34162]}, + {"t":1.04501, "x":4.27999, "y":0.8396, "heading":3.09635, "vx":-3.354, "vy":1.60983, "omega":-0.31234, "ax":6.20747, "ay":1.50414, "alpha":-2.06547, "fx":[101.90409,107.62634,108.35079,104.46766], "fy":[39.02549,17.69701,13.68249,31.93485]}, + {"t":1.08371, "x":4.15483, "y":0.90303, "heading":3.08427, "vx":-3.11375, "vy":1.66804, "omega":-0.39228, "ax":6.33596, "ay":0.99799, "alpha":-1.43537, "fx":[106.17628,108.76291,109.02967,107.12276], "fy":[26.06197,10.92721,8.74739,22.16533]}, + {"t":1.12241, "x":4.03906, "y":0.96834, "heading":3.06908, "vx":-2.86852, "vy":1.70667, "omega":-0.44783, "ax":6.38873, "ay":0.70194, "alpha":-1.06629, "fx":[107.86408,109.19652,109.3244,108.29684], "fy":[18.55143,7.28733,5.82827,16.09189]}, + {"t":1.16112, "x":3.93282, "y":1.03492, "heading":3.05175, "vx":-2.62125, "vy":1.73384, "omega":-0.4891, "ax":6.41445, "ay":0.50922, "alpha":-0.82643, "fx":[108.66102,109.40457,109.47564,108.89046], "fy":[13.72622,5.02155,3.90489,11.99441]}, + {"t":1.19982, "x":3.83617, "y":1.10241, "heading":3.03282, "vx":-2.37299, "vy":1.75354, "omega":-0.52109, "ax":6.42847, "ay":0.37422, "alpha":-0.65862, "fx":[109.08469,109.5194,109.56212,109.21926], "fy":[10.38212,3.47483,2.54587,9.05881]}, + {"t":1.23852, "x":3.74914, "y":1.17056, "heading":3.01265, "vx":-2.12418, "vy":1.76803, "omega":-0.54658, "ax":6.43672, "ay":0.27453, "alpha":-0.53481, "fx":[109.32906,109.58901,109.61548,109.41337], "fy":[7.93202,2.34978,1.5377,6.85948]}, + {"t":1.27723, "x":3.67175, "y":1.23919, "heading":2.9915, "vx":-1.87505, "vy":1.77865, "omega":-0.56728, "ax":6.44185, "ay":0.19797, "alpha":-0.43979, "fx":[109.47835,109.63411,109.65035,109.53327], "fy":[6.06,1.49247,0.76258,5.15459]}, + {"t":1.31593, "x":3.604, "y":1.30818, "heading":2.96954, "vx":-1.62573, "vy":1.78632, "omega":-0.5843, "ax":6.44517, "ay":0.13735, "alpha":-0.36458, "fx":[109.57338,109.6648,109.67419,109.60972], "fy":[4.58224,0.81539,0.15018,3.79728]}, + {"t":1.35464, "x":3.54591, "y":1.37742, "heading":2.94693, "vx":-1.37628, "vy":1.79163, "omega":-0.59841, "ax":6.44739, "ay":0.08818, "alpha":-0.3036, "fx":[109.63561,109.68647,109.6911,109.65954], "fy":[3.38497,0.26524,-0.3441,2.69348]}, + {"t":1.39334, "x":3.49747, "y":1.44683, "heading":2.92377, "vx":-1.12674, "vy":1.79504, "omega":-0.61016, "ax":6.44889, "ay":0.0475, "alpha":-0.25316, "fx":[109.67712,109.70221,109.70349,109.69241], "fy":[2.3942,-0.19233,-0.74997,1.78022]}, + {"t":1.43204, "x":3.45869, "y":1.51634, "heading":2.90015, "vx":-0.87714, "vy":1.79688, "omega":-0.61996, "ax":6.01604, "ay":2.29643, "alpha":-0.26718, "fx":[101.63396,102.46005,103.00873,102.22165], "fy":[40.86201,38.73071,37.26678,39.38681]}, + {"t":1.4531, "x":3.44155, "y":1.55469, "heading":2.8871, "vx":-0.75046, "vy":1.84524, "omega":-0.62558, "ax":6.08604, "ay":2.09777, "alpha":-0.27934, "fx":[102.8568,103.65807,104.16555,103.40684], "fy":[37.58058,35.29395,33.79244,36.06255]}, + {"t":1.47416, "x":3.4271, "y":1.59401, "heading":2.87392, "vx":-0.62231, "vy":1.88941, "omega":-0.63147, "ax":6.17032, "ay":1.82664, "alpha":-0.29356, "fx":[104.34846,105.09883,105.53926,104.83528], "fy":[33.08136,30.5927,29.07247,31.536]}, + {"t":1.49521, "x":3.41537, "y":1.6342, "heading":2.86063, "vx":-0.49238, "vy":1.92787, "omega":-0.63765, "ax":6.25797, "ay":1.4865, "alpha":-0.31031, "fx":[105.92594,106.59066,106.94159,106.32679], "fy":[27.42067,24.68167,23.16814,25.86926]}, + {"t":1.51627, "x":3.40638, "y":1.67512, "heading":2.8472, "vx":-0.36061, "vy":1.95918, "omega":-0.64418, "ax":6.33804, "ay":1.07464, "alpha":-0.33007, "fx":[107.40912,107.9389,108.18139,107.70357], "fy":[20.5477,17.50817,16.03882,19.02247]}, + {"t":1.53733, "x":3.4002, "y":1.71661, "heading":2.83363, "vx":-0.22715, "vy":1.9818, "omega":-0.65113, "ax":6.3954, "ay":0.60598, "alpha":-0.35332, "fx":[108.53992,108.87494,109.00239,108.71831], "fy":[12.7094,9.32611,7.94554,11.24925]}, + {"t":1.55839, "x":3.39683, "y":1.75848, "heading":2.81992, "vx":-0.09248, "vy":1.99456, "omega":-0.65857, "ax":6.41722, "ay":0.12626, "alpha":-0.38073, "fx":[109.08632,109.1744,109.20078,109.15859], "fy":[4.68228,0.92656,-0.3329,3.31472]}, + {"t":1.57944, "x":3.39631, "y":1.8005, "heading":2.80606, "vx":0.04264, "vy":1.99722, "omega":-0.66659, "ax":6.40277, "ay":-0.33124, "alpha":-0.41343, "fx":[109.02546,108.82419,108.77494,109.01272], "fy":[-2.96031,-7.11594,-8.2375,-4.22325]}, + {"t":1.6005, "x":3.39862, "y":1.84249, "heading":2.79202, "vx":0.17747, "vy":1.99025, "omega":-0.67529, "ax":6.35549, "ay":-0.77065, "alpha":-0.45306, "fx":[108.42072,107.87611,107.77879,108.34467], "fy":[-10.28268,-14.88043,-15.84376,-11.42731]}, + {"t":1.62156, "x":3.40377, "y":1.88422, "heading":2.7778, "vx":0.31129, "vy":1.97402, "omega":-0.68483, "ax":6.27616, "ay":-1.19878, "alpha":-0.50194, "fx":[107.29362,106.33099,106.21898,107.17886], "fy":[-17.39518,-22.49542,-23.27118,-18.40159]}, + {"t":1.64261, "x":3.41172, "y":1.92553, "heading":2.76338, "vx":0.44345, "vy":1.94878, "omega":-0.6954, "ax":6.16425, "ay":-1.61578, "alpha":-0.56345, "fx":[105.64327,104.16226,104.08102,105.5219], "fy":[-24.29246,-29.97826,-30.52883,-25.13635]}, + {"t":1.66367, "x":3.42242, "y":1.9662, "heading":2.74874, "vx":0.57325, "vy":1.91475, "omega":-0.70727, "ax":6.01853, "ay":-2.01871, "alpha":-0.6427, "fx":[103.45864,101.32227,101.3374,103.37535], "fy":[-30.91102,-37.29848,-37.5766,-31.56441]}, + {"t":1.68473, "x":3.43583, "y":2.00607, "heading":2.73384, "vx":0.69998, "vy":1.87225, "omega":-0.7208, "ax":5.83647, "ay":-2.40249, "alpha":-0.74784, "fx":[100.71136,97.72617,97.93753,100.73181], "fy":[-37.14332,-44.39934,-44.34489,-37.57524]}, + {"t":1.70578, "x":3.45186, "y":2.04496, "heading":2.71867, "vx":0.82288, "vy":1.82166, "omega":-0.73655, "ax":5.61285, "ay":-2.75873, "alpha":-0.8924, "fx":[97.3326,93.21495,93.78452,97.56003], "fy":[-42.80975,-51.18173,-50.71867,-42.99038]}, + {"t":1.72684, "x":3.47043, "y":2.08271, "heading":2.70316, "vx":0.94107, "vy":1.76357, "omega":-0.75534, "ax":5.33623, "ay":-3.07172, "alpha":-1.10002, "fx":[93.1514,87.47291,88.68352,93.76334], "fy":[-47.57702,-57.44164,-56.48223,-47.49542]}, + {"t":1.7479, "x":3.49143, "y":2.11917, "heading":2.68725, "vx":1.05343, "vy":1.69889, "omega":-0.7785, "ax":4.97982, "ay":-3.3078, "alpha":-1.41334, "fx":[87.71364,79.83362,82.21973,89.05428], "fy":[-50.74356,-62.67413,-61.16442,-50.47688]}, + {"t":1.76895, "x":3.51472, "y":2.15421, "heading":2.67086, "vx":1.15829, "vy":1.62924, "omega":-0.80826, "ax":4.47607, "ay":-3.3847, "alpha":-1.90434, "fx":[79.70132,68.8403,73.46185,82.54346], "fy":[-50.65772,-65.41844,-63.58096,-50.63391]}, + {"t":1.79001, "x":3.5401, "y":2.18776, "heading":2.65384, "vx":1.25254, "vy":1.55796, "omega":-0.84836, "ax":3.65591, "ay":-3.09362, "alpha":-2.60749, "fx":[65.241,51.74951,60.34406,71.40908], "fy":[-43.50251,-61.20283,-60.5084,-45.27282]}, + {"t":1.81107, "x":3.56728, "y":2.21988, "heading":2.63598, "vx":1.32953, "vy":1.49282, "omega":-0.90327, "ax":2.28202, "ay":-2.10002, "alpha":-2.72235, "fx":[38.59745,28.51376,39.56193,48.59321], "fy":[-26.13813,-41.13501,-44.49542,-31.11458]}, + {"t":1.83212, "x":3.59578, "y":2.25085, "heading":2.61696, "vx":1.37758, "vy":1.4486, "omega":-0.96059, "ax":0.66237, "ay":-0.69928, "alpha":0.33448, "fx":[11.55793,12.46104,10.97672,10.07138], "fy":[-13.07051,-11.55398,-10.71467,-12.23905]}, + {"t":1.85318, "x":3.62494, "y":2.2812, "heading":2.59673, "vx":1.39153, "vy":1.43388, "omega":-0.95355, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1_BACK.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1_BACK.traj new file mode 100644 index 00000000..15ba82ac --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_1_BACK.traj @@ -0,0 +1,130 @@ +{ + "name":"E_RIGHT_PATH_1_BACK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.152472496032715, "y":0.4605357348918915, "heading":3.141592653589793, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.198220729827881, "y":2.1674070358276367, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.3785881996154785, "y":3.3401336669921875, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.937044858932495, "y":3.872309923171997, "heading":1.5707963267948966, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.152472496032715 m", "val":7.152472496032715}, "y":{"exp":"0.4605357348918915 m", "val":0.4605357348918915}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.198220729827881 m", "val":3.198220729827881}, "y":{"exp":"2.1674070358276367 m", "val":2.1674070358276367}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.3785881996154785 m", "val":2.3785881996154785}, "y":{"exp":"3.3401336669921875 m", "val":3.3401336669921875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.937044858932495 m", "val":2.937044858932495}, "y":{"exp":"3.872309923171997 m", "val":3.872309923171997}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.39551,1.9336,2.52308], + "samples":[ + {"t":0.0, "x":7.15247, "y":0.46054, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.95962, "ay":2.4391, "alpha":-1.77481, "fx":[-100.26693,-104.36205,-102.92747,-97.92916], "fy":[44.58879,33.89753,37.98584,49.48174]}, + {"t":0.03323, "x":7.14918, "y":0.46188, "heading":3.14159, "vx":-0.19802, "vy":0.08104, "omega":-0.05897, "ax":-5.9595, "ay":2.43902, "alpha":-1.74842, "fx":[-100.27856,-104.31851,-102.89834,-97.98193], "fy":[44.54375,34.00581,38.03992,49.35875]}, + {"t":0.06645, "x":7.13931, "y":0.46592, "heading":3.13963, "vx":-0.39603, "vy":0.16208, "omega":-0.11706, "ax":-5.95935, "ay":2.43892, "alpha":-1.71881, "fx":[-100.28551,-104.267,-102.87156,-98.04352], "fy":[44.50675,34.13486,38.08457,49.21546]}, + {"t":0.09968, "x":7.12287, "y":0.47265, "heading":3.13574, "vx":-0.59404, "vy":0.24312, "omega":-0.17417, "ax":-5.95918, "ay":2.43881, "alpha":-1.68531, "fx":[-100.28852,-104.20648,-102.84596,-98.11503], "fy":[44.47565,34.28691,38.12235,49.049]}, + {"t":0.13291, "x":7.09984, "y":0.48208, "heading":3.12996, "vx":-0.79204, "vy":0.32415, "omega":-0.23017, "ax":-5.95898, "ay":2.43868, "alpha":-1.64705, "fx":[-100.28854,-104.13559,-102.82006,-98.19786], "fy":[44.44768,34.46494,38.1565,48.85566]}, + {"t":0.16613, "x":7.07023, "y":0.49419, "heading":3.12231, "vx":-0.99004, "vy":0.40518, "omega":-0.2849, "ax":-5.95873, "ay":2.43852, "alpha":-1.60291, "fx":[-100.28676,-104.0525,-102.79197,-98.29385], "fy":[44.41926,34.67284,38.19118,48.63054]}, + {"t":0.19936, "x":7.03405, "y":0.509, "heading":3.11284, "vx":-1.18802, "vy":0.4862, "omega":-0.33816, "ax":-5.95842, "ay":2.43832, "alpha":-1.55139, "fx":[-100.28477,-103.95475,-102.7592,-98.40542], "fy":[44.38569,34.91588,38.23176,48.36718]}, + {"t":0.23259, "x":6.99128, "y":0.5265, "heading":3.10161, "vx":-1.386, "vy":0.56722, "omega":-0.3897, "ax":-5.95803, "ay":2.43808, "alpha":-1.49046, "fx":[-100.28465,-103.83895,-102.71842,-98.53573], "fy":[44.34062,35.20123,38.28531,48.05686]}, + {"t":0.26581, "x":6.94194, "y":0.5467, "heading":3.08866, "vx":-1.58396, "vy":0.64823, "omega":-0.43923, "ax":-5.95753, "ay":2.43777, "alpha":-1.41727, "fx":[-100.28928,-103.70031,-102.66511,-98.68909], "fy":[44.27527,35.5389,38.36143,47.68758]}, + {"t":0.29904, "x":6.88603, "y":0.56958, "heading":3.07406, "vx":-1.78191, "vy":0.72923, "omega":-0.48632, "ax":-5.95687, "ay":2.43737, "alpha":-1.32771, "fx":[-100.3027,-103.5318,-102.59286,-98.87148], "fy":[44.17702,35.94339,38.47344,47.24228]}, + {"t":0.33226, "x":6.82353, "y":0.59515, "heading":3.05791, "vx":-1.97984, "vy":0.81021, "omega":-0.53043, "ax":-5.95597, "ay":2.43684, "alpha":-1.21563, "fx":[-100.33084,-103.32268,-102.4923,-99.09147], "fy":[44.02708,36.43647,38.64061,46.69578]}, + {"t":0.36549, "x":6.75446, "y":0.62342, "heading":3.04028, "vx":-2.17773, "vy":0.89118, "omega":-0.57082, "ax":-5.95468, "ay":2.4361, "alpha":-1.07139, "fx":[-100.38281,-103.0557,-102.34897,-99.36191], "fy":[43.79583,37.05252,38.89195,46.00917]}, + {"t":0.39872, "x":6.67882, "y":0.65438, "heading":3.02132, "vx":-2.37559, "vy":0.97212, "omega":-0.60642, "ax":-5.95273, "ay":2.43501, "alpha":-0.87892, "fx":[-100.47343,-102.70109,-102.1392,-99.70297], "fy":[43.43354,37.84939,39.27379,45.11868]}, + {"t":0.43194, "x":6.5966, "y":0.68802, "heading":3.00117, "vx":-2.57337, "vy":1.05303, "omega":-0.63562, "ax":-5.94956, "ay":2.4333, "alpha":-0.60936, "fx":[-100.62873,-102.20279,-101.82111,-100.14844], "fy":[42.8491,38.93263,39.8655,43.91172]}, + {"t":0.46517, "x":6.50781, "y":0.72435, "heading":2.98005, "vx":-2.77106, "vy":1.13388, "omega":-0.65587, "ax":-5.94384, "ay":2.43033, "alpha":-0.2049, "fx":[-100.89912,-101.44136,-101.31237,-100.75921], "fy":[41.85436,40.51739,40.81641,42.16854]}, + {"t":0.4984, "x":6.41246, "y":0.76337, "heading":2.95825, "vx":-2.96855, "vy":1.21463, "omega":-0.66268, "ax":-5.93174, "ay":2.42431, "alpha":0.46992, "fx":[-101.39692,-100.11079,-100.42536,-101.6556], "fy":[39.98557,43.11887,42.44698,39.39567]}, + {"t":0.53162, "x":6.31055, "y":0.80506, "heading":2.93624, "vx":-3.16564, "vy":1.29518, "omega":-0.64707, "ax":-5.89785, "ay":2.4082, "alpha":1.82835, "fx":[-102.42406,-97.14036,-98.62137,-103.09704], "fy":[35.70318,48.33731,45.59377,34.21717]}, + {"t":0.56485, "x":6.20211, "y":0.84943, "heading":2.91474, "vx":-3.3616, "vy":1.3752, "omega":-0.58632, "ax":-5.71592, "ay":2.32082, "alpha":6.09969, "fx":[-104.69071,-85.1836,-93.54629,-105.48402], "fy":[19.2743,64.31934,53.27962,21.03273]}, + {"t":0.59808, "x":6.08726, "y":0.8964, "heading":2.89525, "vx":-3.55152, "vy":1.45231, "omega":-0.38365, "ax":-3.63431, "ay":1.5017, "alpha":11.50753, "fx":[-59.73238,-36.98712,-68.1641,-82.39082], "fy":[-8.99128,56.55541,48.85774,5.75224]}, + {"t":0.6313, "x":5.96725, "y":0.94549, "heading":2.88251, "vx":-3.67228, "vy":1.50221, "omega":-0.00129, "ax":0.00046, "ay":0.02394, "alpha":0.01216, "fx":[0.02351,0.03502,-0.00762,-0.01927], "fy":[0.37998,0.42323,0.43448,0.39129]}, + {"t":0.66453, "x":5.84523, "y":0.99541, "heading":2.88246, "vx":-3.67226, "vy":1.503, "omega":-0.00089, "ax":0.00294, "ay":0.0072, "alpha":0.00001, "fx":[0.04999,0.05,0.04998,0.04997], "fy":[0.12248,0.1225,0.12252,0.1225]}, + {"t":0.69776, "x":5.72322, "y":1.04536, "heading":2.88244, "vx":-3.67217, "vy":1.50324, "omega":-0.00089, "ax":0.00103, "ay":0.00251, "alpha":0.0, "fx":[0.01746,0.01746,0.01746,0.01746], "fy":[0.04264,0.04264,0.04265,0.04265]}, + {"t":0.73098, "x":5.60121, "y":1.0953, "heading":2.88241, "vx":-3.67213, "vy":1.50333, "omega":-0.00089, "ax":0.00036, "ay":0.00087, "alpha":0.0, "fx":[0.00608,0.00608,0.00608,0.00608], "fy":[0.01486,0.01486,0.01486,0.01486]}, + {"t":0.76421, "x":5.47919, "y":1.14526, "heading":2.88238, "vx":-3.67212, "vy":1.50335, "omega":-0.00089, "ax":0.00013, "ay":0.00031, "alpha":0.0, "fx":[0.00213,0.00213,0.00213,0.00213], "fy":[0.0052,0.0052,0.0052,0.0052]}, + {"t":0.79743, "x":5.35718, "y":1.19521, "heading":2.88235, "vx":-3.67212, "vy":1.50336, "omega":-0.00089, "ax":0.00005, "ay":0.00011, "alpha":0.0, "fx":[0.00077,0.00077,0.00077,0.00077], "fy":[0.00189,0.00189,0.00189,0.00189]}, + {"t":0.83066, "x":5.23517, "y":1.24516, "heading":2.88232, "vx":-3.67211, "vy":1.50337, "omega":-0.00089, "ax":0.00002, "ay":0.00005, "alpha":0.0, "fx":[0.00036,0.00036,0.00036,0.00036], "fy":[0.00088,0.00088,0.00088,0.00088]}, + {"t":0.86389, "x":5.11316, "y":1.29511, "heading":2.88229, "vx":-3.67211, "vy":1.50337, "omega":-0.00089, "ax":0.00002, "ay":0.00005, "alpha":0.0, "fx":[0.00038,0.00038,0.00038,0.00038], "fy":[0.00093,0.00093,0.00093,0.00093]}, + {"t":0.89711, "x":4.99115, "y":1.34506, "heading":2.88226, "vx":-3.67211, "vy":1.50337, "omega":-0.00089, "ax":0.00005, "ay":0.00012, "alpha":0.0, "fx":[0.00087,0.00087,0.00087,0.00087], "fy":[0.00213,0.00213,0.00213,0.00213]}, + {"t":0.93034, "x":4.86914, "y":1.39501, "heading":2.88223, "vx":-3.67211, "vy":1.50338, "omega":-0.00089, "ax":0.00014, "ay":0.00035, "alpha":0.0, "fx":[0.00242,0.00242,0.00242,0.00242], "fy":[0.00591,0.00591,0.00591,0.00591]}, + {"t":0.96357, "x":4.74713, "y":1.44496, "heading":2.8822, "vx":-3.67211, "vy":1.50339, "omega":-0.00089, "ax":0.00041, "ay":0.00099, "alpha":0.0, "fx":[0.00692,0.00692,0.00692,0.00692], "fy":[0.0169,0.0169,0.0169,0.0169]}, + {"t":0.99679, "x":4.62512, "y":1.49492, "heading":2.88217, "vx":-3.67209, "vy":1.50342, "omega":-0.00089, "ax":0.00117, "ay":0.00285, "alpha":0.0, "fx":[0.01986,0.01986,0.01986,0.01986], "fy":[0.04851,0.04851,0.04851,0.04851]}, + {"t":1.03002, "x":4.50311, "y":1.54487, "heading":2.88214, "vx":-3.67205, "vy":1.50352, "omega":-0.00089, "ax":0.00335, "ay":0.00819, "alpha":0.0, "fx":[0.05703,0.05703,0.05703,0.05703], "fy":[0.13927,0.13927,0.13927,0.13927]}, + {"t":1.06325, "x":4.3811, "y":1.59483, "heading":2.88211, "vx":-3.67194, "vy":1.50379, "omega":-0.00089, "ax":0.00963, "ay":0.02351, "alpha":0.0, "fx":[0.1638,0.1638,0.1638,0.1638], "fy":[0.39985,0.39985,0.39985,0.39985]}, + {"t":1.09647, "x":4.2591, "y":1.64481, "heading":2.88208, "vx":-3.67162, "vy":1.50457, "omega":-0.00089, "ax":0.02768, "ay":0.06748, "alpha":0.0, "fx":[0.47075,0.47075,0.47075,0.47075], "fy":[1.14777,1.14777,1.14777,1.14777]}, + {"t":1.1297, "x":4.13712, "y":1.69484, "heading":2.88205, "vx":-3.6707, "vy":1.50681, "omega":-0.00089, "ax":0.07962, "ay":0.19347, "alpha":0.0, "fx":[1.35426,1.35426,1.35427,1.35427], "fy":[3.2908,3.2908,3.2908,3.2908]}, + {"t":1.16293, "x":4.0152, "y":1.74501, "heading":2.88202, "vx":-3.66806, "vy":1.51324, "omega":-0.00089, "ax":0.22866, "ay":0.55033, "alpha":-0.00001, "fx":[3.88947,3.88946,3.88948,3.88949], "fy":[9.36101,9.361,9.36099,9.36101]}, + {"t":1.19615, "x":3.89345, "y":1.7956, "heading":2.88199, "vx":-3.66046, "vy":1.53152, "omega":-0.00089, "ax":0.63245, "ay":1.48316, "alpha":-0.00003, "fx":[10.75775,10.75772,10.75785,10.75788], "fy":[25.22814,25.22803,25.22799,25.22811]}, + {"t":1.22938, "x":3.77217, "y":1.8473, "heading":2.88196, "vx":-3.63945, "vy":1.5808, "omega":-0.00089, "ax":1.41654, "ay":3.13585, "alpha":-0.00014, "fx":[24.0946,24.09455,24.09517,24.09523], "fy":[53.34023,53.33985,53.33964,53.34002]}, + {"t":1.2626, "x":3.65203, "y":1.90156, "heading":2.88193, "vx":-3.59238, "vy":1.685, "omega":-0.00089, "ax":2.19972, "ay":4.44497, "alpha":-0.00036, "fx":[37.41566,37.4158,37.41768,37.41754], "fy":[75.60836,75.6077,75.60689,75.60755]}, + {"t":1.29583, "x":3.53388, "y":1.96, "heading":2.8819, "vx":-3.51929, "vy":1.83269, "omega":-0.00091, "ax":2.749, "ay":4.98144, "alpha":-0.00077, "fx":[46.75715,46.75792,46.76225,46.76148], "fy":[84.73455,84.73343,84.73121,84.73234]}, + {"t":1.32906, "x":3.41847, "y":2.02364, "heading":2.88187, "vx":-3.42795, "vy":1.9982, "omega":-0.00093, "ax":3.1731, "ay":5.11309, "alpha":-0.0219, "fx":[53.89606,53.92771,54.05096,54.0192], "fy":[87.02438,86.99288,86.91999,86.95162]}, + {"t":1.36228, "x":3.30632, "y":2.09286, "heading":2.88184, "vx":-3.32252, "vy":2.16809, "omega":-0.00166, "ax":4.16293, "ay":4.55065, "alpha":-2.08872, "fx":[63.73579,68.12214,78.0509,73.33188], "fy":[83.84918,79.50983,70.31928,75.94253]}, + {"t":1.39551, "x":3.19822, "y":2.16741, "heading":2.88179, "vx":-3.1842, "vy":2.31929, "omega":-0.07106, "ax":5.6711, "ay":1.65715, "alpha":-9.01765, "fx":[78.29136,101.83908,105.88003,99.84487], "fy":[70.74095,12.13437,-8.08864,37.9637]}, + {"t":1.41544, "x":3.13589, "y":2.21396, "heading":2.88037, "vx":-3.07118, "vy":2.35232, "omega":-0.25077, "ax":5.84735, "ay":0.76908, "alpha":-10.50223, "fx":[85.38776,103.45353,105.01485,103.9907], "fy":[64.03797,-17.1633,-23.10917,28.56184]}, + {"t":1.43537, "x":3.07584, "y":2.26099, "heading":2.87537, "vx":-2.95465, "vy":2.36765, "omega":-0.46007, "ax":5.94017, "ay":0.2869, "alpha":-10.53765, "fx":[91.73482,102.21689,104.15544,106.05532], "fy":[55.94792,-29.42495,-29.16339,22.16105]}, + {"t":1.4553, "x":3.01814, "y":2.30823, "heading":2.86621, "vx":-2.83626, "vy":2.37337, "omega":-0.67008, "ax":6.06192, "ay":-0.0401, "alpha":-9.27711, "fx":[98.47117,102.51781,103.99953,107.45779], "fy":[44.20584,-31.8813,-30.96693,15.91392]}, + {"t":1.47523, "x":2.96282, "y":2.35552, "heading":2.85285, "vx":-2.71546, "vy":2.37257, "omega":-0.85497, "ax":6.17235, "ay":-0.30348, "alpha":-7.55508, "fx":[103.72983,103.58265,104.26346,108.38381], "fy":[31.14002,-30.57449,-30.85263,9.63853]}, + {"t":1.49516, "x":2.90993, "y":2.40275, "heading":2.83581, "vx":-2.59245, "vy":2.36652, "omega":-1.00553, "ax":6.24043, "ay":-0.51169, "alpha":-6.09671, "fx":[106.66719,104.47295,104.58637,108.86514], "fy":[20.26299,-28.9227,-30.30187,4.14697]}, + {"t":1.51509, "x":2.8595, "y":2.44981, "heading":2.81577, "vx":-2.46808, "vy":2.35632, "omega":-1.12704, "ax":6.27815, "ay":-0.67313, "alpha":-4.97236, "fx":[108.12183,105.11432,104.87612,109.04615], "fy":[11.91594,-27.56385,-29.71395,-0.43729]}, + {"t":1.53501, "x":2.81156, "y":2.49663, "heading":2.79331, "vx":-2.34296, "vy":2.34291, "omega":-1.22613, "ax":6.29839, "ay":-0.79915, "alpha":-4.1066, "fx":[108.78741,105.57634,105.12373,109.04745], "fy":[5.54202,-26.51312,-29.16912,-4.23336]}, + {"t":1.55494, "x":2.76612, "y":2.54317, "heading":2.76888, "vx":-2.21744, "vy":2.32698, "omega":-1.30797, "ax":6.3088, "ay":-0.89917, "alpha":-3.4271, "fx":[109.04395,105.91691,105.33578,108.94703], "fy":[0.5913,-25.70607,-28.67559,-7.38826]}, + {"t":1.57487, "x":2.72318, "y":2.58936, "heading":2.74281, "vx":-2.09171, "vy":2.30906, "omega":-1.37627, "ax":6.31364, "ay":-0.98002, "alpha":-2.88198, "fx":[109.08663,106.17368,105.51995,108.79242], "fy":[-3.33992,-25.08464,-28.22639,-10.02866]}, + {"t":1.5948, "x":2.68275, "y":2.63519, "heading":2.71538, "vx":-1.96588, "vy":2.28953, "omega":-1.43371, "ax":6.31524, "ay":-1.04652, "alpha":-2.43577, "fx":[109.01643,106.37066,105.68248,108.61214], "fy":[-6.53045,-24.60584,-27.81287,-12.2551]}, + {"t":1.61473, "x":2.64482, "y":2.68061, "heading":2.68681, "vx":-1.84003, "vy":2.26867, "omega":-1.48225, "ax":6.31495, "ay":-1.10208, "alpha":-2.06404, "fx":[108.8869,106.52357,105.82802,108.42325], "fy":[-9.17192,-24.23879,-27.42779,-14.14561]}, + {"t":1.63466, "x":2.60941, "y":2.7256, "heading":2.65727, "vx":-1.71417, "vy":2.24671, "omega":-1.52338, "ax":6.31354, "ay":-1.14913, "alpha":-1.74963, "fx":[108.72719,106.64297,105.95998,108.23591], "fy":[-11.39773,-23.96112,-27.06568,-15.76062]}, + {"t":1.65459, "x":2.5765, "y":2.77015, "heading":2.62691, "vx":-1.58835, "vy":2.22381, "omega":-1.55825, "ax":6.31149, "ay":-1.18945, "alpha":-1.48022, "fx":[108.55349,106.73621,106.08081,108.05605], "fy":[-13.30245,-23.75637,-26.72261,-17.14722]}, + {"t":1.67452, "x":2.5461, "y":2.81423, "heading":2.59585, "vx":-1.46257, "vy":2.2001, "omega":-1.58775, "ax":6.30908, "ay":-1.22437, "alpha":-1.24678, "fx":[108.37494,106.80854,106.19231,107.88703], "fy":[-14.95438,-23.61213,-26.39585,-18.3424]}, + {"t":1.69445, "x":2.5182, "y":2.85783, "heading":2.56421, "vx":-1.33683, "vy":2.1757, "omega":-1.6126, "ax":6.3065, "ay":-1.2549, "alpha":-1.04251, "fx":[108.19674,106.86382,106.29582,107.73063], "fy":[-16.40381,-23.51887,-26.08353,-19.37556]}, + {"t":1.71438, "x":2.49281, "y":2.90094, "heading":2.53207, "vx":-1.21115, "vy":2.15069, "omega":-1.63338, "ax":6.30385, "ay":-1.2818, "alpha":-0.86225, "fx":[108.02181,106.90493,106.39232,107.58763], "fy":[-17.6884,-23.46909,-25.78444,-20.27028]}, + {"t":1.73431, "x":2.46993, "y":2.94355, "heading":2.49952, "vx":-1.08552, "vy":2.12515, "omega":-1.65056, "ax":6.3012, "ay":-1.30568, "alpha":-0.70198, "fx":[107.85181,106.93409,106.48253,107.45819], "fy":[-18.8368,-23.45678,-25.49783,-21.0457]}, + {"t":1.75424, "x":2.44955, "y":2.98564, "heading":2.46663, "vx":-0.95994, "vy":2.09913, "omega":-1.66455, "ax":6.2986, "ay":-1.32702, "alpha":-0.55853, "fx":[107.68759,106.95303,106.56703,107.3421], "fy":[-19.87111,-23.47705,-25.22328,-21.71749]}, + {"t":1.77416, "x":2.43167, "y":3.02721, "heading":2.43345, "vx":-0.83442, "vy":2.07268, "omega":-1.67568, "ax":6.29608, "ay":-1.3462, "alpha":-0.42939, "fx":[107.5296,106.96313,106.64625,107.23891], "fy":[-20.80863,-23.52582,-24.96058,-22.29867]}, + {"t":1.79409, "x":2.41629, "y":3.06825, "heading":2.40006, "vx":-0.70894, "vy":2.04585, "omega":-1.68424, "ax":6.29364, "ay":-1.36352, "alpha":-0.31252, "fx":[107.37797,106.96552,106.72052,107.14805], "fy":[-21.66304,-23.59967,-24.70968,-22.80011]}, + {"t":1.81402, "x":2.40341, "y":3.10876, "heading":2.36649, "vx":-0.58351, "vy":2.01868, "omega":-1.69047, "ax":6.2913, "ay":-1.37925, "alpha":-0.20626, "fx":[107.23275,106.96109,106.79012,107.06885], "fy":[-22.44527,-23.69568,-24.47062,-23.23098]}, + {"t":1.83395, "x":2.39303, "y":3.14871, "heading":2.3328, "vx":-0.45813, "vy":1.99119, "omega":-1.69458, "ax":6.28906, "ay":-1.39359, "alpha":-0.10922, "fx":[107.09385,106.95062,106.85528,107.00063], "fy":[-23.1642,-23.81129,-24.24351,-23.59913]}, + {"t":1.85388, "x":2.38515, "y":3.18812, "heading":2.29903, "vx":-0.3328, "vy":1.96342, "omega":-1.69675, "ax":6.28692, "ay":-1.40671, "alpha":-0.02028, "fx":[106.96116,106.93475,106.91618,106.94267], "fy":[-23.82703,-23.9443,-24.02847,-23.91129]}, + {"t":1.87381, "x":2.37976, "y":3.22697, "heading":2.26522, "vx":-0.2075, "vy":1.93538, "omega":-1.69716, "ax":6.28488, "ay":-1.41877, "alpha":0.06154, "fx":[106.83456,106.91403,106.97298,106.89427], "fy":[-24.43973,-24.09272,-23.82564,-24.17331]}, + {"t":1.89374, "x":2.37688, "y":3.26526, "heading":2.2314, "vx":-0.08225, "vy":1.90711, "omega":-1.69593, "ax":6.28293, "ay":-1.42988, "alpha":0.13704, "fx":[106.71392,106.88895,107.02584,106.85474], "fy":[-25.00722,-24.25479,-23.63513,-24.3903]}, + {"t":1.91367, "x":2.37648, "y":3.30298, "heading":2.1976, "vx":0.04296, "vy":1.87861, "omega":-1.6932, "ax":6.28108, "ay":-1.44015, "alpha":0.20691, "fx":[106.59909,106.85993,107.07488,106.8234], "fy":[-25.53366,-24.42892,-23.45706,-24.56677]}, + {"t":1.9336, "x":2.37859, "y":3.34013, "heading":2.16385, "vx":0.16814, "vy":1.84991, "omega":-1.68908, "ax":6.2666, "ay":-1.51557, "alpha":0.31738, "fx":[106.20287,106.61882,106.97212,106.57872], "fy":[-27.36787,-25.71095,-24.18592,-25.8528]}, + {"t":1.96039, "x":2.38534, "y":3.38916, "heading":2.11859, "vx":0.33605, "vy":1.8093, "omega":-1.68057, "ax":6.22395, "ay":-1.67613, "alpha":0.46607, "fx":[105.23012,105.90028,106.48053,105.85958], "fy":[-30.82796,-28.45725,-26.18372,-28.57324]}, + {"t":1.98719, "x":2.39658, "y":3.43704, "heading":2.07356, "vx":0.50282, "vy":1.76439, "omega":-1.66808, "ax":6.16557, "ay":-1.87221, "alpha":0.64871, "fx":[103.87813,104.91712,105.82307,104.87982], "fy":[-35.04018,-31.81735,-28.63423,-31.89133]}, + {"t":2.01398, "x":2.41227, "y":3.48364, "heading":2.02887, "vx":0.66803, "vy":1.71422, "omega":-1.6507, "ax":6.08298, "ay":-2.11612, "alpha":0.87757, "fx":[101.93904,103.53454,104.91372,103.49181], "fy":[-40.25397,-35.9828,-31.70212,-36.03957]}, + {"t":2.04078, "x":2.43235, "y":3.52881, "heading":1.98464, "vx":0.83102, "vy":1.65752, "omega":-1.62719, "ax":5.96148, "ay":-2.42588, "alpha":1.1709, "fx":[99.05325,101.52713,103.60374,101.42821], "fy":[-46.82048,-41.22172,-35.6351,-41.37668]}, + {"t":2.06757, "x":2.45676, "y":3.57236, "heading":1.94104, "vx":0.99076, "vy":1.59252, "omega":-1.59581, "ax":5.77415, "ay":-2.82799, "alpha":1.55638, "fx":[94.57323,98.50298,101.61984,98.17017], "fy":[-55.21823,-47.90734,-40.81798,-48.46938]}, + {"t":2.09437, "x":2.48538, "y":3.61401, "heading":1.89828, "vx":1.14547, "vy":1.51675, "omega":-1.55411, "ax":5.46905, "ay":-3.36023, "alpha":2.07595, "fx":[87.29973,93.75562,98.42239,92.62989], "fy":[-66.01775,-56.53597,-47.86426,-58.20852]}, + {"t":2.12116, "x":2.51803, "y":3.65345, "heading":1.85663, "vx":1.29202, "vy":1.42671, "omega":-1.49849, "ax":4.94206, "ay":-4.06833, "alpha":2.79114, "fx":[75.04691,85.98701,92.85683,82.36144], "fy":[-79.56515,-67.66104,-57.74892,-71.82914]}, + {"t":2.14796, "x":2.55443, "y":3.69021, "heading":1.81648, "vx":1.42444, "vy":1.3177, "omega":-1.4237, "ax":3.9899, "ay":-4.96735, "alpha":3.78381, "fx":[54.40719,72.8973,82.26866,61.89498], "fy":[-94.78255,-81.48551,-71.83176,-89.87298]}, + {"t":2.17475, "x":2.59403, "y":3.72374, "heading":1.77834, "vx":1.53135, "vy":1.1846, "omega":-1.32231, "ax":2.31559, "ay":-5.88607, "alpha":5.0694, "fx":[22.82832,51.17987,60.63871,22.90348], "fy":[-106.81068,-96.51781,-90.61682,-106.53607]}, + {"t":2.20155, "x":2.63589, "y":3.75337, "heading":1.7429, "vx":1.59339, "vy":1.02688, "omega":-1.18648, "ax":-0.08218, "ay":-6.28824, "alpha":5.94264, "fx":[-14.98003,19.15247,19.31265,-29.07625], "fy":[-108.18994,-107.49744,-107.14333,-105.01391]}, + {"t":2.22834, "x":2.67856, "y":3.77862, "heading":1.71111, "vx":1.59119, "vy":0.85839, "omega":-1.02725, "ax":-2.4374, "ay":-5.81359, "alpha":5.58264, "fx":[-47.53312,-17.4276,-33.29351,-67.5834], "fy":[-98.40791,-107.80099,-103.67549,-85.66555]}, + {"t":2.25514, "x":2.72032, "y":3.79954, "heading":1.68359, "vx":1.52588, "vy":0.70262, "omega":-0.87766, "ax":-4.03424, "ay":-4.88434, "alpha":4.93177, "fx":[-69.35715,-48.04634,-69.91561,-87.16588], "fy":[-84.56933,-98.14085,-83.71165,-65.90303]}, + {"t":2.28193, "x":2.75975, "y":3.81661, "heading":1.66007, "vx":1.41778, "vy":0.57174, "omega":-0.74551, "ax":-4.93632, "ay":-4.01227, "alpha":4.35838, "fx":[-82.58825,-68.63531,-88.06681,-96.5715], "fy":[-71.82388,-85.13726,-64.63704,-51.39215]}, + {"t":2.30873, "x":2.79597, "y":3.83049, "heading":1.6401, "vx":1.28552, "vy":0.46423, "omega":-0.62873, "ax":-5.44116, "ay":-3.33334, "alpha":3.87119, "fx":[-90.59388,-81.37837,-96.82264,-101.41544], "fy":[-61.53007,-73.16977,-50.85916,-41.23767]}, + {"t":2.33552, "x":2.82846, "y":3.84173, "heading":1.62325, "vx":1.13972, "vy":0.37492, "omega":-0.525, "ax":-5.73834, "ay":-2.82325, "alpha":3.47772, "fx":[-95.61759,-89.28023,-101.39812,-104.13389], "fy":[-53.48969,-63.39606,-41.21949,-33.98547]}, + {"t":2.36232, "x":2.85694, "y":3.85077, "heading":1.60918, "vx":0.98596, "vy":0.29927, "omega":-0.43182, "ax":-5.92391, "ay":-2.43678, "alpha":3.16658, "fx":[-98.91503,-94.35399,-104.0098,-105.77745], "fy":[-47.20189,-55.66092,-34.30001,-28.63278]}, + {"t":2.38911, "x":2.88123, "y":3.85791, "heading":1.59761, "vx":0.82723, "vy":0.23398, "omega":-0.34697, "ax":-6.04619, "ay":-2.13802, "alpha":2.92003, "fx":[-101.17374,-97.75276,-105.6162,-106.83282], "fy":[-42.22167,-49.53483,-29.15612,-24.55559]}, + {"t":2.41591, "x":2.90123, "y":3.86341, "heading":1.58831, "vx":0.66523, "vy":0.17669, "omega":-0.26873, "ax":-6.13051, "ay":-1.90198, "alpha":2.72231, "fx":[-102.78,-100.12346,-106.66547,-107.54399], "fy":[-38.21486,-44.62464,-25.20439,-21.36436]}, + {"t":2.4427, "x":2.91685, "y":3.86746, "heading":1.58111, "vx":0.50096, "vy":0.12573, "omega":-0.19579, "ax":-6.19093, "ay":-1.71166, "alpha":2.56138, "fx":[-103.95947,-101.83745,-107.3847,-108.04227], "fy":[-34.94106,-40.62848,-22.08069,-18.80903]}, + {"t":2.46949, "x":2.92805, "y":3.87022, "heading":1.57587, "vx":0.33508, "vy":0.07986, "omega":-0.12715, "ax":-6.23563, "ay":-1.55541, "alpha":2.42843, "fx":[-104.84945,-103.11574,-107.89734,-108.40268], "fy":[-32.22818,-37.32538,-19.55076,-16.72377]}, + {"t":2.49629, "x":2.93479, "y":3.8718, "heading":1.57246, "vx":0.16799, "vy":0.03818, "omega":-0.06209, "ax":-6.2696, "ay":-1.42508, "alpha":2.31705, "fx":[-105.5367,-104.09491,-108.27467,-108.67034], "fy":[-29.9518,-34.55494,-17.45881,-14.99505]}, + {"t":2.52308, "x":2.93704, "y":3.87231, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2.traj new file mode 100644 index 00000000..e5795611 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2.traj @@ -0,0 +1,151 @@ +{ + "name":"E_RIGHT_PATH_2", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.9948270320892334, "y":2.801387310028076, "heading":2.618314122167914, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.619781017303467, "y":2.2527506351470947, "heading":1.5707963267948966, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8137584924697876, "y":2.195854902267456, "heading":0.0, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.2163537740707395, "y":2.1863720417022705, "heading":1.5707963267948966, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7802374362945557, "y":1.8250209093093872, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.7930970191955566, "y":2.08547043800354, "heading":2.618314122167914, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.9948270320892334 m", "val":3.9948270320892334}, "y":{"exp":"2.801387310028076 m", "val":2.801387310028076}, "heading":{"exp":"2.6183141221679134 rad", "val":2.618314122167914}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.619781017303467 m", "val":2.619781017303467}, "y":{"exp":"2.2527506351470947 m", "val":2.2527506351470947}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8137584924697876 m", "val":1.8137584924697876}, "y":{"exp":"2.195854902267456 m", "val":2.195854902267456}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.2163537740707397 m", "val":1.2163537740707395}, "y":{"exp":"2.1863720417022705 m", "val":2.1863720417022705}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7802374362945557 m", "val":1.7802374362945557}, "y":{"exp":"1.8250209093093872 m", "val":1.8250209093093872}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.7930970191955566 m", "val":2.7930970191955566}, "y":{"exp":"2.08547043800354 m", "val":2.08547043800354}, "heading":{"exp":"2.6183141221679134 rad", "val":2.618314122167914}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.70417,0.93826,1.36875,1.86431,2.22482], + "samples":[ + {"t":0.0, "x":3.99483, "y":2.80139, "heading":2.61831, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.26392, "ay":-2.87502, "alpha":-7.77032, "fx":[-109.39894,-101.01273,-65.94747,-81.79183], "fy":[7.60574,-42.85708,-87.65929,-72.70251]}, + {"t":0.02347, "x":3.99338, "y":2.8006, "heading":2.61831, "vx":-0.12356, "vy":-0.06748, "omega":-0.18239, "ax":-5.29273, "ay":-2.85498, "alpha":-7.62651, "fx":[-109.43396,-101.06468,-66.6976,-82.91509], "fy":[6.95037,-42.71801,-87.0799,-71.40235]}, + {"t":0.04694, "x":3.98902, "y":2.79822, "heading":2.61403, "vx":-0.24779, "vy":-0.1345, "omega":-0.3614, "ax":-5.32062, "ay":-2.83415, "alpha":-7.49341, "fx":[-109.46121,-101.18553,-67.59775,-83.76433], "fy":[6.3406,-42.41274,-86.37231,-70.38778]}, + {"t":0.07042, "x":3.98174, "y":2.79429, "heading":2.60555, "vx":-0.37268, "vy":-0.20102, "omega":-0.53729, "ax":-5.3483, "ay":-2.81217, "alpha":-7.36616, "fx":[-109.48234,-101.37096,-68.64579,-84.3928], "fy":[5.75715,-41.94725,-85.53018,-69.61657]}, + {"t":0.09389, "x":3.97152, "y":2.78879, "heading":2.59294, "vx":-0.49821, "vy":-0.26703, "omega":-0.71019, "ax":-5.37648, "ay":-2.78857, "alpha":-7.24012, "fx":[-109.49868,-101.61583,-69.84067,-84.85454], "fy":[5.17716,-41.32753,-84.54473,-69.03632]}, + {"t":0.11736, "x":3.95834, "y":2.78176, "heading":2.57627, "vx":-0.62441, "vy":-0.33248, "omega":-0.88013, "ax":-5.40592, "ay":-2.76279, "alpha":-7.11045, "fx":[-109.51123,-101.91422,-71.18235,-85.20485], "fy":[4.57359,-40.55992,-83.4044,-68.58644]}, + {"t":0.14083, "x":3.9422, "y":2.77319, "heading":2.55561, "vx":-0.7513, "vy":-0.39733, "omega":-1.04703, "ax":-5.43737, "ay":-2.73419, "alpha":-6.97177, "fx":[-109.52049,-102.25936,-72.67194,-85.50075], "fy":[3.91446,-39.65156,-82.09435,-68.1996]}, + {"t":0.16431, "x":3.92306, "y":2.76311, "heading":2.53103, "vx":-0.87893, "vy":-0.46151, "omega":-1.21067, "ax":-5.47162, "ay":-2.70209, "alpha":-6.81778, "fx":[-109.52628,-102.64359,-74.31167,-85.80144], "fy":[3.16205,-38.61091,-80.59571,-67.80253]}, + {"t":0.18778, "x":3.90092, "y":2.75154, "heading":2.50262, "vx":-1.00736, "vy":-0.52493, "omega":-1.3707, "ax":-5.50948, "ay":-2.66579, "alpha":-6.64087, "fx":[-109.52728,-103.05833,-76.10511,-86.16832], "fy":[2.27211,-37.44845,-78.88454,-67.31618]}, + {"t":0.21125, "x":3.87576, "y":2.73848, "heading":2.47044, "vx":-1.13668, "vy":-0.58751, "omega":-1.52658, "ax":-5.55177, "ay":-2.62453, "alpha":-6.43183, "fx":[-109.52034,-103.49399,-78.05725,-86.66454], "fy":[1.19333,-36.1776,-76.93032,-66.65515]}, + {"t":0.23472, "x":3.84755, "y":2.72397, "heading":2.43461, "vx":-1.26699, "vy":-0.64911, "omega":-1.67755, "ax":-5.59927, "ay":-2.57747, "alpha":-6.17959, "fx":[-109.4994,-103.93988,-80.17476,-87.35394], "fy":[-0.13272,-34.81598,-74.69363,-65.72604]}, + {"t":0.2582, "x":3.81627, "y":2.70802, "heading":2.39524, "vx":-1.39842, "vy":-0.70961, "omega":-1.8226, "ax":-5.65271, "ay":-2.52363, "alpha":-5.87111, "fx":[-109.45402,-104.38403,-82.46643,-88.29924], "fy":[-1.77072,-33.38711,-72.12259,-64.42461]}, + {"t":0.28167, "x":3.78189, "y":2.69067, "heading":2.35246, "vx":-1.5311, "vy":-0.76884, "omega":-1.9604, "ax":-5.71267, "ay":-2.46169, "alpha":-5.49126, "fx":[-109.36739,-104.81299,-84.94358,-89.55933], "fy":[-3.78887,-31.92297,-69.14726,-62.63134]}, + {"t":0.30514, "x":3.74438, "y":2.67195, "heading":2.30644, "vx":-1.66519, "vy":-0.82663, "omega":-2.0893, "ax":-5.77951, "ay":-2.38976, "alpha":-5.02288, "fx":[-109.2142,-105.21125,-87.62078,-91.18497], "fy":[-6.25348,-30.46773,-65.67021,-60.20495]}, + {"t":0.32861, "x":3.7037, "y":2.65188, "heading":2.2574, "vx":-1.80085, "vy":-0.88272, "omega":-2.2072, "ax":-5.85324, "ay":-2.30496, "alpha":-4.44666, "fx":[-108.95902,-105.56037,-90.51614,-93.21215], "fy":[-9.21935,-29.08359,-61.55055,-56.97364]}, + {"t":0.35208, "x":3.65982, "y":2.63053, "heading":2.20559, "vx":-1.93824, "vy":-0.93682, "omega":-2.31157, "ax":-5.9333, "ay":-2.20279, "alpha":-3.74063, "fx":[-108.55676,-105.83691,-93.65035,-95.65094], "fy":[-12.71518,-27.86066,-56.57534,-52.7242]}, + {"t":0.37556, "x":3.61269, "y":2.60793, "heading":2.15133, "vx":-2.07751, "vy":-0.98853, "omega":-2.39937, "ax":-6.01821, "ay":-2.07609, "alpha":-2.87802, "fx":[-107.9573,-106.00795,-97.04026,-98.46654], "fy":[-16.72405,-26.93427,-50.40638,-47.19008]}, + {"t":0.39903, "x":3.56227, "y":2.58416, "heading":2.09501, "vx":-2.21877, "vy":-1.03726, "omega":-2.46692, "ax":-6.10474, "ay":-1.91362, "alpha":-1.82114, "fx":[-107.11723,-106.02064,-100.67422,-101.54738], "fy":[-21.16208,-26.51851,-42.47687,-40.04317]}, + {"t":0.4225, "x":3.5085, "y":2.55929, "heading":2.03711, "vx":-2.36206, "vy":-1.08217, "omega":-2.50967, "ax":-6.18577, "ay":-1.69791, "alpha":-0.50525, "fx":[-106.01918,-105.77526,-104.42477,-104.6538], "fy":[-25.862,-26.97639,-31.78404,-30.90166]}, + {"t":0.44597, "x":3.45136, "y":2.53342, "heading":1.9782, "vx":-2.50725, "vy":-1.12203, "omega":-2.52153, "ax":-6.24393, "ay":-1.4024, "alpha":1.202, "fx":[-104.69559,-105.04776,-107.73976,-107.34659], "fy":[-30.57066,-28.98305,-16.48219,-19.38165]}, + {"t":0.46945, "x":3.39079, "y":2.50669, "heading":1.91902, "vx":-2.65381, "vy":-1.15494, "omega":-2.49332, "ax":-6.231, "ay":-0.99144, "alpha":3.5965, "fx":[-103.24839,-103.22544,-108.55659,-108.91942], "fy":[-34.96374,-33.95052,6.7017,-5.24384]}, + {"t":0.49292, "x":3.32678, "y":2.47931, "heading":1.86049, "vx":-2.80007, "vy":-1.17822, "omega":-2.4089, "ax":-6.00765, "ay":-0.45801, "alpha":7.29003, "fx":[-101.86442,-98.19402,-100.2649,-108.43039], "fy":[-38.64635,-45.20472,41.4019,11.28671]}, + {"t":0.51639, "x":3.2594, "y":2.45153, "heading":1.80395, "vx":-2.94108, "vy":-1.18897, "omega":-2.23778, "ax":-5.32022, "ay":-0.01162, "alpha":12.74156, "fx":[-101.00456,-82.38648,-73.39751,-105.19331], "fy":[-40.67787,-68.27065,79.61808,28.54006]}, + {"t":0.53986, "x":3.1889, "y":2.42362, "heading":1.75142, "vx":-3.06596, "vy":-1.18924, "omega":-1.93871, "ax":-5.10488, "ay":0.37721, "alpha":13.86171, "fx":[-103.1214,-81.93908,-60.21168,-102.05798], "fy":[-34.66644,-67.65967,89.90471,38.08658]}, + {"t":0.56333, "x":3.11553, "y":2.39581, "heading":1.70592, "vx":-3.18578, "vy":-1.18039, "omega":-1.61334, "ax":-5.13163, "ay":0.94105, "alpha":13.04013, "fx":[-105.70065,-92.17528,-52.73232,-98.54215], "fy":[-25.23277,-51.4034,94.41524,46.24923]}, + {"t":0.58681, "x":3.03934, "y":2.36836, "heading":1.66805, "vx":-3.30624, "vy":-1.1583, "omega":-1.30726, "ax":-5.0578, "ay":1.69461, "alpha":11.9487, "fx":[-107.58361,-101.02941,-42.04124,-93.47294], "fy":[-14.31306,-25.6754,99.63103,55.65687]}, + {"t":0.61028, "x":2.96034, "y":2.34164, "heading":1.63737, "vx":-3.42495, "vy":-1.11852, "omega":-1.0268, "ax":-4.77075, "ay":2.80652, "alpha":10.18533, "fx":[-108.37041,-99.93368,-29.64943,-86.64288], "fy":[-1.2536,22.45382,104.04871,65.70317]}, + {"t":0.63375, "x":2.87863, "y":2.31616, "heading":1.61326, "vx":-3.53693, "vy":-1.05264, "omega":-0.78773, "ax":-4.26396, "ay":3.93501, "alpha":8.30008, "fx":[-106.6632,-76.51872,-26.98257,-79.95051], "fy":[18.28039,71.04657,104.80345,73.60335]}, + {"t":0.65722, "x":2.79444, "y":2.29253, "heading":1.59477, "vx":-3.63702, "vy":-0.96028, "omega":-0.5929, "ax":-3.74983, "ay":4.65649, "alpha":6.99814, "fx":[-100.04366,-56.67862,-26.0775,-72.33406], "fy":[40.96811,89.7431,105.08291,81.02812]}, + {"t":0.6807, "x":2.70804, "y":2.27128, "heading":1.58086, "vx":-3.72504, "vy":-0.85098, "omega":-0.42864, "ax":-2.97956, "ay":5.25477, "alpha":6.56005, "fx":[-89.87969,-33.5971,-17.07127,-62.17755], "fy":[59.99527,101.42117,107.03602,89.07613]}, + {"t":0.70417, "x":2.61978, "y":2.25275, "heading":1.5708, "vx":-3.79497, "vy":-0.72764, "omega":-0.27466, "ax":-1.83706, "ay":5.8024, "alpha":5.46868, "fx":[-66.16972,-8.53721,-5.08244,-45.20217], "fy":[83.8374,105.65009,107.45577,97.84566]}, + {"t":0.71977, "x":2.56033, "y":2.2421, "heading":1.56651, "vx":-3.82364, "vy":-0.63709, "omega":-0.18932, "ax":-0.67027, "ay":6.06863, "alpha":4.97282, "fx":[-42.10944,14.94103,9.8335,-28.26935], "fy":[97.51704,104.87195,106.88346,103.62987]}, + {"t":0.73538, "x":2.50058, "y":2.2329, "heading":1.56356, "vx":-3.8341, "vy":-0.54238, "omega":-0.11171, "ax":0.88059, "ay":6.07298, "alpha":4.27279, "fx":[-6.68931,41.34393,29.57237,-4.31249], "fy":[105.54042,97.65995,103.01141,106.98687]}, + {"t":0.75099, "x":2.44085, "y":2.22517, "heading":1.56181, "vx":-3.82036, "vy":-0.44761, "omega":-0.04503, "ax":2.56521, "ay":5.6297, "alpha":3.3189, "fx":[34.15022,64.28651,50.77929,25.31808], "fy":[100.09827,84.76829,94.34294,103.82906]}, + {"t":0.76659, "x":2.38154, "y":2.21887, "heading":1.56111, "vx":-3.78033, "vy":-0.35975, "omega":0.00676, "ax":3.98558, "ay":4.80171, "alpha":2.36868, "fx":[66.28304,80.4851,69.65893,54.74732], "fy":[83.07259,70.24337,81.54574,91.84095]}, + {"t":0.7822, "x":2.32303, "y":2.21384, "heading":1.56121, "vx":-3.71813, "vy":-0.28482, "omega":0.04373, "ax":4.95564, "ay":3.86696, "alpha":1.52657, "fx":[85.12563,90.73549,84.01131,77.30393], "fy":[64.67947,57.18033,66.98865,74.25501]}, + {"t":0.7978, "x":2.26561, "y":2.20987, "heading":1.5619, "vx":-3.64079, "vy":-0.22447, "omega":0.06755, "ax":5.54679, "ay":3.02732, "alpha":0.83279, "fx":[95.12739,97.04167,93.76988,91.45826], "fy":[49.95004,46.50374,52.95061,56.57113]}, + {"t":0.81341, "x":2.20947, "y":2.20674, "heading":1.56295, "vx":-3.55423, "vy":-0.17722, "omega":0.08055, "ax":5.89331, "ay":2.34284, "alpha":0.29678, "fx":[100.52959,100.97419,99.98163,99.48894], "fy":[39.05334,38.03584,40.62612,41.68875]}, + {"t":0.82902, "x":2.15472, "y":2.20426, "heading":1.56421, "vx":-3.46226, "vy":-0.14066, "omega":0.08518, "ax":6.09669, "ay":1.8023, "alpha":-0.10968, "fx":[103.60887,103.49769,103.79979,103.90533], "fy":[31.01636,31.32724,30.29268,29.99006]}, + {"t":0.84462, "x":2.10143, "y":2.20228, "heading":1.56554, "vx":-3.36711, "vy":-0.11254, "omega":0.08347, "ax":6.21793, "ay":1.37633, "alpha":-0.42086, "fx":[105.46649,105.16855,106.09785,106.32829], "fy":[24.97005,25.95753,21.78134,20.93497]}, + {"t":0.86023, "x":2.04964, "y":2.20069, "heading":1.56684, "vx":-3.27008, "vy":-0.09106, "omega":0.0769, "ax":6.29142, "ay":1.03728, "alpha":-0.66364, "fx":[106.64465,106.30768,107.45135,107.65733], "fy":[20.30811,21.59898,14.78924,13.87931]}, + {"t":0.87583, "x":1.99937, "y":2.1994, "heading":1.56804, "vx":-3.17189, "vy":-0.07487, "omega":0.06654, "ax":6.33643, "ay":0.7636, "alpha":-0.85689, "fx":[107.42394,107.10449,108.21967,108.3752], "fy":[16.62742,18.00962,9.01706,8.30025]}, + {"t":0.89144, "x":1.95065, "y":2.19832, "heading":1.56908, "vx":-3.07301, "vy":-0.06295, "omega":0.05317, "ax":6.364, "ay":0.53938, "alpha":-1.01359, "fx":[107.95762,107.67419,108.62379,108.74361], "fy":[13.65905,15.01309,4.21237,3.81421]}, + {"t":0.90705, "x":1.90346, "y":2.1974, "heading":1.56991, "vx":-2.97369, "vy":-0.05453, "omega":0.03735, "ax":6.38063, "ay":0.35307, "alpha":-1.14275, "fx":[108.33373,108.08906,108.79996,108.90798], "fy":[11.22011,12.48029,0.17483,0.14723]}, + {"t":0.92265, "x":1.85783, "y":2.1966, "heading":1.57049, "vx":-2.87411, "vy":-0.04902, "omega":0.01952, "ax":6.39025, "ay":0.19624, "alpha":-1.25076, "fx":[108.60515,108.3958,108.83302,108.95142], "fy":[9.1833,10.31576,-3.25088,-2.8964]}, + {"t":0.93826, "x":1.81376, "y":2.19585, "heading":1.5708, "vx":-2.77439, "vy":-0.04596, "omega":0.0, "ax":6.4348, "ay":0.12424, "alpha":0.0, "fx":[109.45411,109.45411,109.45411,109.45411], "fy":[2.11325,2.11325,2.11325,2.11325]}, + {"t":0.97137, "x":1.72541, "y":2.1944, "heading":1.5708, "vx":-2.5613, "vy":-0.04185, "omega":0.0, "ax":6.43792, "ay":0.11876, "alpha":0.0, "fx":[109.50724,109.50724,109.50724,109.50724], "fy":[2.02002,2.02002,2.02002,2.02002]}, + {"t":1.00449, "x":1.64413, "y":2.19308, "heading":1.5708, "vx":-2.34811, "vy":-0.03792, "omega":0.0, "ax":6.44031, "ay":0.11456, "alpha":0.0, "fx":[109.54787,109.54787,109.54787,109.54787], "fy":[1.94862,1.94862,1.94862,1.94862]}, + {"t":1.0376, "x":1.5699, "y":2.19189, "heading":1.5708, "vx":-2.13485, "vy":-0.03412, "omega":0.0, "ax":6.4422, "ay":0.11124, "alpha":0.0, "fx":[109.57994,109.57994,109.57994,109.57994], "fy":[1.89221,1.89221,1.89221,1.89221]}, + {"t":1.07072, "x":1.50274, "y":2.19082, "heading":1.5708, "vx":-1.92152, "vy":-0.03044, "omega":0.0, "ax":6.44372, "ay":0.10856, "alpha":0.0, "fx":[109.6059,109.6059,109.6059,109.6059], "fy":[1.8465,1.8465,1.8465,1.8465]}, + {"t":1.10383, "x":1.44264, "y":2.18987, "heading":1.5708, "vx":-1.70813, "vy":-0.02684, "omega":0.0, "ax":6.44498, "ay":0.10634, "alpha":0.0, "fx":[109.62735,109.62735,109.62735,109.62735], "fy":[1.80873,1.80873,1.80873,1.80873]}, + {"t":1.13695, "x":1.38961, "y":2.18904, "heading":1.5708, "vx":-1.49471, "vy":-0.02332, "omega":0.0, "ax":6.44604, "ay":0.10447, "alpha":0.0, "fx":[109.64535,109.64535,109.64535,109.64535], "fy":[1.77699,1.77699,1.77699,1.77699]}, + {"t":1.17006, "x":1.34365, "y":2.18832, "heading":1.5708, "vx":-1.28125, "vy":-0.01986, "omega":0.0, "ax":6.44695, "ay":0.10288, "alpha":0.0, "fx":[109.66069,109.66069,109.66069,109.66069], "fy":[1.74994,1.74994,1.74994,1.74994]}, + {"t":1.20317, "x":1.30476, "y":2.18772, "heading":1.5708, "vx":-1.06777, "vy":-0.01646, "omega":0.0, "ax":6.44772, "ay":0.10151, "alpha":0.0, "fx":[109.67391,109.67391,109.67391,109.67391], "fy":[1.72662,1.72662,1.72662,1.72662]}, + {"t":1.23629, "x":1.27293, "y":2.18723, "heading":1.5708, "vx":-0.85425, "vy":-0.01309, "omega":0.0, "ax":6.4484, "ay":0.10031, "alpha":0.0, "fx":[109.68542,109.68542,109.68542,109.68542], "fy":[1.70631,1.70631,1.70631,1.70631]}, + {"t":1.2694, "x":1.24818, "y":2.18686, "heading":1.5708, "vx":-0.64072, "vy":-0.00977, "omega":0.0, "ax":6.44899, "ay":0.09926, "alpha":0.0, "fx":[109.69554,109.69554,109.69554,109.69554], "fy":[1.68846,1.68846,1.68846,1.68846]}, + {"t":1.30252, "x":1.2305, "y":2.18659, "heading":1.5708, "vx":-0.42716, "vy":-0.00649, "omega":0.0, "ax":6.44952, "ay":0.09833, "alpha":0.0, "fx":[109.7045,109.7045,109.7045,109.7045], "fy":[1.67264,1.67264,1.67264,1.67264]}, + {"t":1.33563, "x":1.21989, "y":2.18643, "heading":1.5708, "vx":-0.21359, "vy":-0.00323, "omega":0.0, "ax":6.44999, "ay":0.09751, "alpha":0.0, "fx":[109.71248,109.71248,109.71248,109.71248], "fy":[1.65854,1.65854,1.65854,1.65854]}, + {"t":1.36875, "x":1.21635, "y":2.18637, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.24699, "ay":-5.47285, "alpha":3.39824, "fx":[35.40708,60.34585,76.16112,49.00745], "fy":[-103.80892,-91.61316,-78.89831,-98.04618]}, + {"t":1.39235, "x":1.21726, "y":2.18485, "heading":1.5708, "vx":0.07662, "vy":-0.12915, "omega":0.08019, "ax":3.41994, "ay":-5.36005, "alpha":3.4864, "fx":[37.84599,62.83436,79.21432,52.79431], "fy":[-102.93019,-89.91197,-75.81379,-96.03602]}, + {"t":1.41594, "x":1.22002, "y":2.18031, "heading":1.57269, "vx":0.15733, "vy":-0.25564, "omega":0.16247, "ax":3.61503, "ay":-5.22299, "alpha":3.58348, "fx":[40.675,65.64247,82.53204,57.11325], "fy":[-101.82844,-87.86824,-72.16627,-93.5034]}, + {"t":1.43954, "x":1.22474, "y":2.17282, "heading":1.57652, "vx":0.24264, "vy":-0.37889, "omega":0.24703, "ax":3.8356, "ay":-5.05446, "alpha":3.69037, "fx":[43.97625,68.81156,86.11704,62.06516], "fy":[-100.42698,-85.39231,-67.82249,-90.25806]}, + {"t":1.46314, "x":1.23153, "y":2.16247, "heading":1.58235, "vx":0.33315, "vy":-0.49817, "omega":0.33411, "ax":4.08506, "ay":-4.84469, "alpha":3.80811, "fx":[47.85306,72.38416,89.95044,67.75498], "fy":[-98.61496,-82.36597,-62.61887,-86.02703]}, + {"t":1.48674, "x":1.24053, "y":2.14937, "heading":1.59024, "vx":0.42955, "vy":-0.61249, "omega":0.42398, "ax":4.36627, "ay":-4.58037, "alpha":3.93823, "fx":[52.43368,76.39884,93.97613,74.26739], "fy":[-96.22919,-78.63301,-56.36054,-80.42006]}, + {"t":1.51034, "x":1.25188, "y":2.13364, "heading":1.60024, "vx":0.53259, "vy":-0.72058, "omega":0.51691, "ax":4.68027, "ay":-4.24366, "alpha":4.08329, "fx":[57.87199,80.87987,98.07839,81.60975], "fy":[-93.02528,-73.98797,-48.82795,-72.89265]}, + {"t":1.53393, "x":1.26575, "y":2.11545, "heading":1.61244, "vx":0.64303, "vy":-0.82072, "omega":0.61327, "ax":5.02367, "ay":-3.81152, "alpha":4.24747, "fx":[64.33788,85.81766,102.0535,89.59546], "fy":[-88.63313,-68.16479,-39.79885,-62.73438]}, + {"t":1.55753, "x":1.28233, "y":2.09502, "heading":1.62691, "vx":0.76158, "vy":-0.91067, "omega":0.71351, "ax":5.38436, "ay":-3.25668, "alpha":4.43508, "fx":[71.97935,91.13465,105.58235,97.64905], "fy":[-82.49311,-60.83118,-29.096,-49.16029]}, + {"t":1.58113, "x":1.3018, "y":2.07263, "heading":1.64375, "vx":0.88864, "vy":-0.98752, "omega":0.81817, "ax":5.73599, "ay":-2.55297, "alpha":4.64125, "fx":[80.818,96.63142,108.22235,104.5982], "fy":[-73.78433,-51.60218,-16.66808,-31.6465]}, + {"t":1.60473, "x":1.32437, "y":2.04861, "heading":1.66306, "vx":1.024, "vy":-1.04777, "omega":0.92769, "ax":6.03503, "ay":-1.68699, "alpha":4.82717, "fx":[90.51395,101.91564,109.44916,108.73772], "fy":[-61.41181,-40.09915,-2.69413,-10.57569]}, + {"t":1.62833, "x":1.35021, "y":2.02342, "heading":1.68495, "vx":1.16642, "vy":-1.08758, "omega":1.0416, "ax":6.22655, "ay":-0.67288, "alpha":4.89543, "fx":[99.97156,106.3427,108.77489,108.55822], "fy":[-44.25631,-26.08881,12.33389,12.2292]}, + {"t":1.65193, "x":1.37947, "y":1.99757, "heading":1.70953, "vx":1.31335, "vy":-1.10345, "omega":1.15713, "ax":6.25915, "ay":0.43941, "alpha":4.72737, "fx":[107.05142,109.04483,105.92526,103.84403], "fy":[-22.00917,-9.71086,27.62744,33.98945]}, + {"t":1.67552, "x":1.41221, "y":1.97165, "heading":1.73683, "vx":1.46106, "vy":-1.09308, "omega":1.26868, "ax":6.10325, "ay":1.56683, "alpha":4.29792, "fx":[109.2182,109.1497,100.98548,95.90505], "fy":[3.60014,8.30045,42.26618,52.43824]}, + {"t":1.69912, "x":1.44838, "y":1.94629, "heading":1.76677, "vx":1.60508, "vy":-1.05611, "omega":1.37011, "ax":5.77042, "ay":2.61125, "alpha":3.71792, "fx":[105.42862,106.17601,94.40367,86.60421], "fy":[28.81797,26.62754,55.45306,66.76791]}, + {"t":1.72272, "x":1.48787, "y":1.92209, "heading":1.79911, "vx":1.74125, "vy":-0.99449, "omega":1.45784, "ax":5.3163, "ay":3.49759, "alpha":3.12253, "fx":[97.1609,100.34465,86.83493,77.37431], "fy":[50.14726,43.76642,66.71226,77.34578]}, + {"t":1.74632, "x":1.53044, "y":1.8996, "heading":1.83351, "vx":1.86671, "vy":-0.91195, "omega":1.53153, "ax":4.81191, "ay":4.20077, "alpha":2.57958, "fx":[87.00396,92.49867,78.9317,68.96253], "fy":[66.30855,58.58817,75.9277,84.99096]}, + {"t":1.76992, "x":1.57583, "y":1.87925, "heading":1.86965, "vx":1.98026, "vy":-0.81282, "omega":1.5924, "ax":4.31255, "ay":4.73633, "alpha":2.10388, "fx":[76.91044,83.692,71.20039,61.61841], "fy":[77.86364,70.6353,83.24892,90.50672]}, + {"t":1.79351, "x":1.62376, "y":1.86139, "heading":1.90723, "vx":2.08203, "vy":-0.70105, "omega":1.64205, "ax":3.84916, "ay":5.13647, "alpha":1.69273, "fx":[67.79637,74.80358,63.96017,55.33228], "fy":[85.9762,80.02272,88.95832,94.52223]}, + {"t":1.81711, "x":1.67396, "y":1.84627, "heading":1.94598, "vx":2.17286, "vy":-0.57984, "omega":1.682, "ax":3.43405, "ay":5.43393, "alpha":1.33988, "fx":[59.90085,66.38808,57.37008,49.99], "fy":[91.70057,87.16227,93.36778,97.48772]}, + {"t":1.84071, "x":1.72619, "y":1.8341, "heading":1.98567, "vx":2.2539, "vy":-0.45161, "omega":1.71361, "ax":3.06885, "ay":5.65582, "alpha":1.03819, "fx":[53.15906,58.71268,51.47859,45.45073], "fy":[95.80555,92.53395,96.76204,99.71405]}, + {"t":1.86431, "x":1.78024, "y":1.82502, "heading":2.02611, "vx":2.32632, "vy":-0.31814, "omega":1.73811, "ax":2.89467, "ay":5.73933, "alpha":0.90021, "fx":[50.12311,54.97229,48.53218,43.32216], "fy":[97.25443,94.63391,98.11449,100.49481]}, + {"t":1.87933, "x":1.81551, "y":1.82089, "heading":2.05222, "vx":2.3698, "vy":-0.23193, "omega":1.75164, "ax":2.88367, "ay":5.74468, "alpha":0.81994, "fx":[49.98406,54.2601,48.27855,43.67877], "fy":[97.30533,95.02154,98.21622,100.31808]}, + {"t":1.89435, "x":1.85143, "y":1.81805, "heading":2.07853, "vx":2.41312, "vy":-0.14564, "omega":1.76395, "ax":2.87156, "ay":5.75041, "alpha":0.7318, "fx":[49.78883,53.48133,48.0384,44.06919], "fy":[97.38298,95.43723,98.30804,100.12292]}, + {"t":1.90937, "x":1.888, "y":1.81651, "heading":2.10502, "vx":2.45625, "vy":-0.05926, "omega":1.77495, "ax":2.85817, "ay":5.75657, "alpha":0.63458, "fx":[49.52957,52.62639,47.81463,44.49608], "fy":[97.49071,95.88389,98.38827,99.90733]}, + {"t":1.9244, "x":1.92522, "y":1.81627, "heading":2.13169, "vx":2.49919, "vy":0.02721, "omega":1.78448, "ax":2.84328, "ay":5.76319, "alpha":0.52686, "fx":[49.19709,51.68343,47.61071,44.96248], "fy":[97.63226,96.36492,98.45482,99.66904]}, + {"t":1.93942, "x":1.96309, "y":1.81733, "heading":2.15849, "vx":2.5419, "vy":0.11379, "omega":1.79239, "ax":2.82663, "ay":5.77033, "alpha":0.4069, "fx":[48.78047,50.63767,47.43086,45.47195], "fy":[97.81183,96.88438,98.50514,99.40529]}, + {"t":1.95444, "x":2.00159, "y":1.81969, "heading":2.18542, "vx":2.58436, "vy":0.20046, "omega":1.7985, "ax":2.8079, "ay":5.77802, "alpha":0.27252, "fx":[48.26662,49.47044,47.28029,46.02866], "fy":[98.03415,97.4471,98.53605,99.1128]}, + {"t":1.96946, "x":2.04073, "y":1.82336, "heading":2.21243, "vx":2.62654, "vy":0.28726, "omega":1.8026, "ax":2.78665, "ay":5.78632, "alpha":0.12104, "fx":[47.63971,48.15779,47.16552,46.6376], "fy":[98.30457,98.05881,98.54347,98.78757]}, + {"t":1.98448, "x":2.08049, "y":1.82832, "heading":2.23951, "vx":2.6684, "vy":0.37418, "omega":1.80442, "ax":2.76237, "ay":5.79525, "alpha":-0.05096, "fx":[46.88032,46.66855,47.0949,47.30486], "fy":[98.62911,98.72635,98.52213,98.42465]}, + {"t":1.9995, "x":2.12089, "y":1.8346, "heading":2.26662, "vx":2.70989, "vy":0.46123, "omega":1.80365, "ax":2.73436, "ay":5.80485, "alpha":-0.24792, "fx":[45.96441,44.96146,47.07926,48.03789], "fy":[99.01456,99.45777,98.46499,98.01784]}, + {"t":2.01452, "x":2.1619, "y":1.84218, "heading":2.29371, "vx":2.75097, "vy":0.54843, "omega":1.79993, "ax":2.70172, "ay":5.8151, "alpha":-0.47565, "fx":[44.86182,42.98088,47.13307,48.84605], "fy":[99.46852,100.26236,98.36248,97.55925]}, + {"t":2.02955, "x":2.20353, "y":1.85108, "heading":2.32075, "vx":2.79155, "vy":0.63578, "omega":1.79278, "ax":2.66321, "ay":5.82593, "alpha":-0.74198, "fx":[43.53424,40.6502,47.2761,49.7412], "fy":[99.99926,101.15032,98.20111,97.03863]}, + {"t":2.04457, "x":2.24577, "y":1.86128, "heading":2.34768, "vx":2.83155, "vy":0.72329, "omega":1.78164, "ax":2.61716, "ay":5.83712, "alpha":-1.0577, "fx":[41.93243,37.86155,47.53629,50.73863], "fy":[100.61557,102.13174,97.96134,96.44246]}, + {"t":2.05959, "x":2.2886, "y":1.87281, "heading":2.37444, "vx":2.87087, "vy":0.81098, "omega":1.76575, "ax":2.56125, "ay":5.84821, "alpha":-1.43807, "fx":[39.99237,34.4593,47.95441,51.8582], "fy":[101.32602,103.21351,97.61357,95.7526]}, + {"t":2.07461, "x":2.33201, "y":1.88565, "heading":2.40096, "vx":2.90934, "vy":0.89883, "omega":1.74415, "ax":2.49214, "ay":5.85818, "alpha":-1.90537, "fx":[37.63012,30.21337,48.59249,53.12603], "fy":[102.13744,104.39158,97.11095,94.9442]}, + {"t":2.08963, "x":2.37599, "y":1.89981, "heading":2.42716, "vx":2.94678, "vy":0.98682, "omega":1.71552, "ax":2.40506, "ay":5.8649, "alpha":-2.4931, "fx":[34.73571,24.77622,49.54942,54.57623], "fy":[103.05158,105.6316,96.37494,93.98281]}, + {"t":2.10465, "x":2.42053, "y":1.9153, "heading":2.45293, "vx":2.9829, "vy":1.07492, "omega":1.67807, "ax":2.29325, "ay":5.86377, "alpha":-3.25292, "fx":[31.16827,17.61913,50.99096,56.25183], "fy":[104.058,106.82087,95.26475,92.82069]}, + {"t":2.11967, "x":2.4656, "y":1.93211, "heading":2.47814, "vx":3.01735, "vy":1.16301, "omega":1.62921, "ax":2.14794, "ay":5.84482, "alpha":-4.26429, "fx":[26.76429,7.97171,53.20796,58.19956], "fy":[105.11874,107.65247,93.50862,91.39473]}, + {"t":2.1347, "x":2.51116, "y":1.95024, "heading":2.50261, "vx":3.04962, "vy":1.2508, "omega":1.56515, "ax":1.96245, "ay":5.78675, "alpha":-5.63424, "fx":[21.41412,-5.02787,56.70335,60.43295], "fy":[106.13552,107.39213,90.55366,89.64224]}, + {"t":2.14972, "x":2.55719, "y":1.96968, "heading":2.52612, "vx":3.0791, "vy":1.33773, "omega":1.48052, "ax":1.75459, "ay":5.65592, "alpha":-7.38497, "fx":[15.47327,-20.83885,62.00708,62.73903], "fy":[106.88307,104.85016,85.46276,87.62657]}, + {"t":2.16474, "x":2.60364, "y":1.99041, "heading":2.54836, "vx":3.10545, "vy":1.42269, "omega":1.36959, "ax":1.61548, "ay":5.48002, "alpha":-8.85884, "fx":[11.0779,-32.46262,67.23269,64.06733], "fy":[106.96315,100.91165,78.99091,85.98853]}, + {"t":2.17976, "x":2.65048, "y":2.0124, "heading":2.56894, "vx":3.12972, "vy":1.50501, "omega":1.23651, "ax":1.57828, "ay":5.34628, "alpha":-9.35931, "fx":[9.59628,-34.89201,68.87229,63.80797], "fy":[106.20397,98.41727,74.19302,84.94055]}, + {"t":2.19478, "x":2.69767, "y":2.03561, "heading":2.58751, "vx":3.15343, "vy":1.58532, "omega":1.09592, "ax":1.56711, "ay":5.15839, "alpha":-9.40568, "fx":[9.44218,-32.95599,67.51879,62.61931], "fy":[104.11935,95.28135,68.58349,82.98705]}, + {"t":2.2098, "x":2.74521, "y":2.06001, "heading":2.60397, "vx":3.17697, "vy":1.6628, "omega":0.95464, "ax":1.43234, "ay":4.31894, "alpha":-9.83483, "fx":[8.93122,-30.02417,59.1977,59.35025], "fy":[94.9271,79.54919,46.39146,72.98774]}, + {"t":2.22482, "x":2.7931, "y":2.08547, "heading":2.61831, "vx":3.19848, "vy":1.72768, "omega":0.8069, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[ + {"name":"STOW", "from":{"target":3, "targetTimestamp":1.36875, "offset":{"exp":"0.25 s", "val":0.25}}, "event":null}] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2_BACK.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2_BACK.traj new file mode 100644 index 00000000..02ccab3d --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_2_BACK.traj @@ -0,0 +1,147 @@ +{ + "name":"E_RIGHT_PATH_2_BACK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.173567533493042, "y":3.878880023956299, "heading":1.5707963267948966, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6164592504501345, "y":2.643705368041992, "heading":2.400500524589353, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2163537740707395, "y":2.1863720417022705, "heading":2.400500524589353, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.937044858932495, "y":3.56351637840271, "heading":1.5707963267948966, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":0, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.173567533493042 m", "val":3.173567533493042}, "y":{"exp":"3.878880023956299 m", "val":3.878880023956299}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":53, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.6164592504501343 m", "val":1.6164592504501345}, "y":{"exp":"2.643705368041992 m", "val":2.643705368041992}, "heading":{"exp":"2.400500524589353 rad", "val":2.400500524589353}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2163537740707397 m", "val":1.2163537740707395}, "y":{"exp":"2.1863720417022705 m", "val":2.1863720417022705}, "heading":{"exp":"2.400500524589353 rad", "val":2.400500524589353}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.937044858932495 m", "val":2.937044858932495}, "y":{"exp":"3.56351637840271 m", "val":3.56351637840271}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":0, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,2.06576,2.75258,3.9249], + "samples":[ + {"t":0.0, "x":3.17357, "y":3.87888, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.05765, "ay":-4.00362, "alpha":0.00539, "fx":[-86.02726,-86.01206,-86.0312,-86.04639], "fy":[-68.10304,-68.12218,-68.09795,-68.07882]}, + {"t":0.03898, "x":3.16973, "y":3.87584, "heading":1.5708, "vx":-0.19713, "vy":-0.15605, "omega":0.00021, "ax":-5.05469, "ay":-4.00128, "alpha":0.0071, "fx":[-85.97631,-85.95629,-85.98148,-86.00149], "fy":[-68.06401,-68.08919,-68.05729,-68.03211]}, + {"t":0.07795, "x":3.1582, "y":3.86672, "heading":1.5708, "vx":-0.39415, "vy":-0.312, "omega":0.00049, "ax":-5.04672, "ay":-3.99497, "alpha":0.01179, "fx":[-85.83913,-85.80584,-85.84757,-85.88083], "fy":[-67.95897,-68.00068,-67.94773,-67.90602]}, + {"t":0.11693, "x":3.13901, "y":3.85152, "heading":1.57082, "vx":-0.59085, "vy":-0.46772, "omega":0.00095, "ax":-4.95112, "ay":-3.91929, "alpha":0.06982, "fx":[-84.19754,-83.99827,-84.23678,-84.43563], "fy":[-66.70253,-66.94101,-66.62927,-66.3911]}, + {"t":0.15591, "x":3.11222, "y":3.83031, "heading":1.57086, "vx":-0.78383, "vy":-0.62048, "omega":0.00367, "ax":-0.00112, "ay":-0.00089, "alpha":2.58338, "fx":[-4.7421,4.70437,4.70402,-4.74269], "fy":[-4.73871,-4.73802,4.70863,4.70762]}, + {"t":0.19488, "x":3.08166, "y":3.80613, "heading":1.571, "vx":-0.78387, "vy":-0.62051, "omega":0.10436, "ax":0.0, "ay":0.0, "alpha":2.14367, "fx":[-3.91849,3.92019,3.91849,-3.92019], "fy":[-3.92019,-3.91849,3.92019,3.91849]}, + {"t":0.23386, "x":3.05111, "y":3.78194, "heading":1.57507, "vx":-0.78387, "vy":-0.62051, "omega":0.18791, "ax":0.0, "ay":0.0, "alpha":1.73467, "fx":[-3.15803,3.18502,3.15803,-3.18502], "fy":[-3.18502,-3.15803,3.18502,3.15803]}, + {"t":0.27284, "x":3.02056, "y":3.75776, "heading":1.5824, "vx":-0.78387, "vy":-0.62051, "omega":0.25552, "ax":0.0, "ay":0.0, "alpha":1.50054, "fx":[-2.7121,2.77451,2.7121,-2.77451], "fy":[-2.77451,-2.7121,2.77451,2.7121]}, + {"t":0.31181, "x":2.99001, "y":3.73357, "heading":1.59235, "vx":-0.78387, "vy":-0.62051, "omega":0.31401, "ax":0.0, "ay":0.0, "alpha":1.09541, "fx":[-1.95889,2.04569,1.95889,-2.04569], "fy":[-2.04569,-1.95889,2.04569,1.95889]}, + {"t":0.35079, "x":2.95945, "y":3.70939, "heading":1.60459, "vx":-0.78387, "vy":-0.62051, "omega":0.35671, "ax":0.0, "ay":0.0, "alpha":1.05051, "fx":[-1.85609,1.98316,1.85609,-1.98316], "fy":[-1.98316,-1.85609,1.98316,1.85609]}, + {"t":0.38977, "x":2.9289, "y":3.6852, "heading":1.6185, "vx":-0.78387, "vy":-0.62051, "omega":0.39765, "ax":0.0, "ay":0.0, "alpha":0.64528, "fx":[-1.12065,1.23609,1.12065,-1.23609], "fy":[-1.23609,-1.12065,1.23609,1.12065]}, + {"t":0.42874, "x":2.89835, "y":3.66102, "heading":1.634, "vx":-0.78387, "vy":-0.62051, "omega":0.4228, "ax":0.0, "ay":0.0, "alpha":0.70939, "fx":[-1.21398,1.37501,1.21398,-1.37501], "fy":[-1.37501,-1.21398,1.37501,1.21398]}, + {"t":0.46772, "x":2.8678, "y":3.63683, "heading":1.65048, "vx":-0.78387, "vy":-0.62051, "omega":0.45045, "ax":0.0, "ay":0.0, "alpha":0.35911, "fx":[-0.59917,0.70935,0.59917,-0.70935], "fy":[-0.70935,-0.59917,0.70935,0.59917]}, + {"t":0.5067, "x":2.83724, "y":3.61265, "heading":1.66803, "vx":-0.78387, "vy":-0.62051, "omega":0.46445, "ax":0.0, "ay":0.0, "alpha":0.44397, "fx":[-0.72952,0.88634,0.72952,-0.88634], "fy":[-0.88634,-0.72952,0.88634,0.72952]}, + {"t":0.54567, "x":2.80669, "y":3.58846, "heading":1.68614, "vx":-0.78387, "vy":-0.62051, "omega":0.48175, "ax":0.0, "ay":0.0, "alpha":0.19878, "fx":[-0.3154,0.40586,0.3154,-0.40586], "fy":[-0.40586,-0.3154,0.40586,0.3154]}, + {"t":0.58465, "x":2.77614, "y":3.56428, "heading":1.70491, "vx":-0.78387, "vy":-0.62051, "omega":0.4895, "ax":0.0, "ay":0.0, "alpha":0.24522, "fx":[-0.38295,0.50535,0.38295,-0.50535], "fy":[-0.50535,-0.38295,0.50535,0.38295]}, + {"t":0.62363, "x":2.74558, "y":3.54009, "heading":1.72399, "vx":-0.78387, "vy":-0.62051, "omega":0.49906, "ax":0.0, "ay":0.0, "alpha":0.12424, "fx":[-0.18651,0.26159,0.18651,-0.26159], "fy":[-0.26159,-0.18651,0.26159,0.18651]}, + {"t":0.6626, "x":2.71503, "y":3.5159, "heading":1.74344, "vx":-0.78387, "vy":-0.62051, "omega":0.5039, "ax":0.0, "ay":0.0, "alpha":0.10341, "fx":[-0.15036,0.22113,0.15036,-0.22113], "fy":[-0.22113,-0.15036,0.22113,0.15036]}, + {"t":0.70158, "x":2.68448, "y":3.49172, "heading":1.76308, "vx":-0.78387, "vy":-0.62051, "omega":0.50793, "ax":0.0, "ay":0.0, "alpha":0.10046, "fx":[-0.14334,0.21665,0.14334,-0.21665], "fy":[-0.21665,-0.14334,0.21665,0.14334]}, + {"t":0.74056, "x":2.65393, "y":3.46753, "heading":1.78288, "vx":-0.78387, "vy":-0.62051, "omega":0.51185, "ax":0.0, "ay":0.0, "alpha":0.0087, "fx":[-0.00724,0.0221,0.00724,-0.0221], "fy":[-0.0221,-0.00724,0.0221,0.00724]}, + {"t":0.77953, "x":2.62337, "y":3.44335, "heading":1.80283, "vx":-0.78387, "vy":-0.62051, "omega":0.51219, "ax":0.0, "ay":0.0, "alpha":0.10123, "fx":[-0.13758,0.22267,0.13758,-0.22267], "fy":[-0.22267,-0.13758,0.22267,0.13758]}, + {"t":0.81851, "x":2.59282, "y":3.41916, "heading":1.82279, "vx":-0.78387, "vy":-0.62051, "omega":0.51613, "ax":0.0, "ay":0.0, "alpha":-0.05119, "fx":[0.07314,-0.11052,-0.07314,0.11052], "fy":[0.11052,0.07314,-0.11052,-0.07314]}, + {"t":0.85749, "x":2.56227, "y":3.39498, "heading":1.84291, "vx":-0.78387, "vy":-0.62051, "omega":0.51414, "ax":0.0, "ay":0.0, "alpha":0.109, "fx":[-0.1401,0.24455,0.1401,-0.24455], "fy":[-0.24455,-0.1401,0.24455,0.1401]}, + {"t":0.89646, "x":2.53172, "y":3.37079, "heading":1.86295, "vx":-0.78387, "vy":-0.62051, "omega":0.51839, "ax":0.0, "ay":0.0, "alpha":-0.08785, "fx":[0.11367,-0.19678,-0.11367,0.19678], "fy":[0.19678,0.11367,-0.19678,-0.11367]}, + {"t":0.93544, "x":2.50116, "y":3.34661, "heading":1.88316, "vx":-0.78387, "vy":-0.62051, "omega":0.51496, "ax":0.0, "ay":0.0, "alpha":0.11412, "fx":[-0.13732,0.26119,0.13732,-0.26119], "fy":[-0.26119,-0.13732,0.26119,0.13732]}, + {"t":0.97442, "x":2.47061, "y":3.32242, "heading":1.90323, "vx":-0.78387, "vy":-0.62051, "omega":0.51941, "ax":0.0, "ay":0.0, "alpha":-0.10874, "fx":[0.12894,-0.24994,-0.12894,0.24994], "fy":[0.24994,0.12894,-0.24994,-0.12894]}, + {"t":1.01339, "x":2.44006, "y":3.29824, "heading":1.92347, "vx":-0.78387, "vy":-0.62051, "omega":0.51517, "ax":0.0, "ay":0.0, "alpha":0.11237, "fx":[-0.12533,0.26215,0.12533,-0.26215], "fy":[-0.26215,-0.12533,0.26215,0.12533]}, + {"t":1.05237, "x":2.4095, "y":3.27405, "heading":1.94355, "vx":-0.78387, "vy":-0.62051, "omega":0.51955, "ax":0.0, "ay":0.0, "alpha":-0.11852, "fx":[0.1282,-0.27842,-0.1282,0.27842], "fy":[0.27842,0.1282,-0.27842,-0.1282]}, + {"t":1.09135, "x":2.37895, "y":3.24986, "heading":1.9638, "vx":-0.78387, "vy":-0.62051, "omega":0.51493, "ax":0.0, "ay":0.0, "alpha":0.10139, "fx":[-0.10379,0.24078,0.10379,-0.24078], "fy":[-0.24078,-0.10379,0.24078,0.10379]}, + {"t":1.13032, "x":2.3484, "y":3.22568, "heading":1.98387, "vx":-0.78387, "vy":-0.62051, "omega":0.51888, "ax":0.0, "ay":0.0, "alpha":-0.12241, "fx":[0.11964,-0.29308,-0.11964,0.29308], "fy":[0.29308,0.11964,-0.29308,-0.11964]}, + {"t":1.1693, "x":2.31785, "y":3.20149, "heading":2.0041, "vx":-0.78387, "vy":-0.62051, "omega":0.51411, "ax":0.0, "ay":0.0, "alpha":0.08179, "fx":[-0.07613,0.19732,0.07613,-0.19732], "fy":[-0.19732,-0.07613,0.19732,0.07613]}, + {"t":1.20828, "x":2.28729, "y":3.17731, "heading":2.02414, "vx":-0.78387, "vy":-0.62051, "omega":0.5173, "ax":0.0, "ay":0.0, "alpha":-0.12415, "fx":[0.10826,-0.30221,-0.10826,0.30221], "fy":[0.30221,0.10826,-0.30221,-0.10826]}, + {"t":1.24725, "x":2.25674, "y":3.15312, "heading":2.0443, "vx":-0.78387, "vy":-0.62051, "omega":0.51246, "ax":0.0, "ay":0.0, "alpha":0.05322, "fx":[-0.04484,0.13012,0.04484,-0.13012], "fy":[-0.13012,-0.04484,0.13012,0.04484]}, + {"t":1.28623, "x":2.22619, "y":3.12894, "heading":2.06427, "vx":-0.78387, "vy":-0.62051, "omega":0.51454, "ax":0.0, "ay":0.0, "alpha":-0.12775, "fx":[0.09784,-0.3155,-0.09784,0.3155], "fy":[0.3155,0.09784,-0.3155,-0.09784]}, + {"t":1.32521, "x":2.19563, "y":3.10475, "heading":2.08433, "vx":-0.78387, "vy":-0.62051, "omega":0.50956, "ax":0.0, "ay":0.0, "alpha":0.01723, "fx":[-0.01394,0.04235,0.01394,-0.04235], "fy":[-0.04235,-0.01394,0.04235,0.01394]}, + {"t":1.36418, "x":2.16508, "y":3.08057, "heading":2.10419, "vx":-0.78387, "vy":-0.62051, "omega":0.51023, "ax":0.0, "ay":0.0, "alpha":-0.13562, "fx":[0.08942,-0.33907,-0.08942,0.33907], "fy":[0.33907,0.08942,-0.33907,-0.08942]}, + {"t":1.40316, "x":2.13453, "y":3.05638, "heading":2.12408, "vx":-0.78387, "vy":-0.62051, "omega":0.50494, "ax":0.0, "ay":0.0, "alpha":-0.02759, "fx":[0.01503,-0.06974,-0.01503,0.06974], "fy":[0.06974,0.01503,-0.06974,-0.01503]}, + {"t":1.44214, "x":2.10398, "y":3.03219, "heading":2.14376, "vx":-0.78387, "vy":-0.62051, "omega":0.50387, "ax":0.0, "ay":0.0, "alpha":-0.15319, "fx":[0.08484,-0.38692,-0.08484,0.38692], "fy":[0.38692,0.08484,-0.38692,-0.08484]}, + {"t":1.48111, "x":2.07342, "y":3.00801, "heading":2.1634, "vx":-0.78387, "vy":-0.62051, "omega":0.4979, "ax":0.0, "ay":0.0, "alpha":-0.08226, "fx":[0.03988,-0.20893,-0.03988,0.20893], "fy":[0.20893,0.03988,-0.20893,-0.03988]}, + {"t":1.52009, "x":2.04287, "y":2.98382, "heading":2.1828, "vx":-0.78387, "vy":-0.62051, "omega":0.49469, "ax":0.0, "ay":0.0, "alpha":-0.18502, "fx":[0.08333,-0.47109,-0.08333,0.47109], "fy":[0.47109,0.08333,-0.47109,-0.08333]}, + {"t":1.55907, "x":2.01232, "y":2.95964, "heading":2.20208, "vx":-0.78387, "vy":-0.62051, "omega":0.48748, "ax":0.0, "ay":0.0, "alpha":-0.15183, "fx":[0.05976,-0.38799,-0.05976,0.38799], "fy":[0.38799,0.05976,-0.38799,-0.05976]}, + {"t":1.59804, "x":1.98177, "y":2.93545, "heading":2.22108, "vx":-0.78387, "vy":-0.62051, "omega":0.48156, "ax":0.0, "ay":0.0, "alpha":-0.24002, "fx":[0.08403,-0.61489,-0.08403,0.61489], "fy":[0.61489,0.08403,-0.61489,-0.08403]}, + {"t":1.63702, "x":1.95121, "y":2.91127, "heading":2.23985, "vx":-0.78387, "vy":-0.62051, "omega":0.4722, "ax":0.0, "ay":0.0, "alpha":-0.24339, "fx":[0.0728,-0.6251,-0.0728,0.6251], "fy":[0.6251,0.0728,-0.6251,-0.0728]}, + {"t":1.676, "x":1.92066, "y":2.88708, "heading":2.25826, "vx":-0.78387, "vy":-0.62051, "omega":0.46272, "ax":0.0, "ay":0.0, "alpha":-0.32667, "fx":[0.0828,-0.84059,-0.0828,0.84059], "fy":[0.84059,0.0828,-0.84059,-0.0828]}, + {"t":1.71497, "x":1.89011, "y":2.8629, "heading":2.27629, "vx":-0.78387, "vy":-0.62051, "omega":0.44999, "ax":0.0, "ay":0.0, "alpha":-0.36691, "fx":[0.07562,-0.94567,-0.07562,0.94567], "fy":[0.94567,0.07562,-0.94567,-0.07562]}, + {"t":1.75395, "x":1.85955, "y":2.83871, "heading":2.29383, "vx":-0.78387, "vy":-0.62051, "omega":0.43568, "ax":0.0, "ay":0.0, "alpha":-0.45789, "fx":[0.07386,-1.18164,-0.07386,1.18164], "fy":[1.18164,0.07386,-1.18164,-0.07386]}, + {"t":1.79293, "x":1.829, "y":2.81453, "heading":2.31081, "vx":-0.78387, "vy":-0.62051, "omega":0.41784, "ax":0.0, "ay":0.0, "alpha":-0.53783, "fx":[0.06306,-1.3892,-0.06305,1.3892], "fy":[1.3892,0.06305,-1.3892,-0.06306]}, + {"t":1.8319, "x":1.79845, "y":2.79034, "heading":2.3271, "vx":-0.78387, "vy":-0.62051, "omega":0.39687, "ax":0.0, "ay":0.0, "alpha":-0.65251, "fx":[0.0491,-1.68644,-0.0491,1.68645], "fy":[1.68644,0.0491,-1.68645,-0.0491]}, + {"t":1.87088, "x":1.7679, "y":2.76615, "heading":2.34257, "vx":-0.78387, "vy":-0.62051, "omega":0.37144, "ax":0.0, "ay":0.0, "alpha":-0.77888, "fx":[0.02746,-2.01371,-0.02741,2.01376], "fy":[2.0137,0.0274,-2.01377,-0.02747]}, + {"t":1.90986, "x":1.73734, "y":2.74197, "heading":2.35705, "vx":-0.78387, "vy":-0.62051, "omega":0.34108, "ax":0.00005, "ay":-0.00006, "alpha":-0.93628, "fx":[-0.00124,-2.42007,0.00288,2.4217], "fy":[2.41986,-0.00309,-2.42192,0.00103]}, + {"t":1.94883, "x":1.70679, "y":2.71778, "heading":2.37034, "vx":-0.78387, "vy":-0.62051, "omega":0.30459, "ax":0.00158, "ay":-0.002, "alpha":-1.12247, "fx":[-0.01414,-2.87514,0.06796,2.9289], "fy":[2.86807,-0.07506,-2.93596,0.00706]}, + {"t":1.98781, "x":1.67624, "y":2.6936, "heading":2.38221, "vx":-0.78381, "vy":-0.62059, "omega":0.26084, "ax":0.05225, "ay":-0.06577, "alpha":-1.34594, "fx":[0.79971,-2.59117,0.9795,4.36703], "fy":[2.36123,-1.21107,-4.59632,-1.02871]}, + {"t":2.02679, "x":1.64573, "y":2.66936, "heading":2.39238, "vx":-0.78177, "vy":-0.62315, "omega":0.20838, "ax":1.58171, "ay":-1.79572, "alpha":-1.22491, "fx":[27.24521,23.64343,26.61486,30.11433], "fy":[-27.42182,-31.11682,-33.61133,-30.02854]}, + {"t":2.06576, "x":1.61646, "y":2.64371, "heading":2.4005, "vx":-0.72012, "vy":-0.69315, "omega":0.16064, "ax":1.4587, "ay":-1.39769, "alpha":-1.18392, "fx":[24.99607,21.73381,24.66729,27.85109], "fy":[-20.69479,-24.23163,-26.81034,-23.36052]}, + {"t":2.10869, "x":1.58689, "y":2.61266, "heading":2.4074, "vx":-0.65751, "vy":-0.75314, "omega":0.10982, "ax":0.03867, "ay":-0.03368, "alpha":-1.15058, "fx":[0.50611,-2.31383,0.81015,3.62833], "fy":[2.39867,-0.72588,-3.54356,-0.42089]}, + {"t":2.15162, "x":1.5587, "y":2.5803, "heading":2.41211, "vx":-0.65585, "vy":-0.75459, "omega":0.06043, "ax":0.00089, "ay":-0.00077, "alpha":-0.90665, "fx":[-0.11592,-2.32554,0.1461,2.3557], "fy":[2.32752,-0.14413,-2.35373,0.11789]}, + {"t":2.19454, "x":1.53055, "y":2.54791, "heading":2.4147, "vx":-0.65581, "vy":-0.75462, "omega":0.02151, "ax":0.00002, "ay":-0.00002, "alpha":-0.69877, "fx":[-0.10531,-1.80333,0.106,1.80402], "fy":[1.80337,-0.10595,-1.80397,0.10535]}, + {"t":2.23747, "x":1.5024, "y":2.51552, "heading":2.41563, "vx":-0.65581, "vy":-0.75462, "omega":-0.00849, "ax":0.0, "ay":0.0, "alpha":-0.51885, "fx":[-0.07968,-1.3392,0.07969,1.33921], "fy":[1.3392,-0.07969,-1.33921,0.07968]}, + {"t":2.28039, "x":1.47425, "y":2.48313, "heading":2.41526, "vx":-0.65581, "vy":-0.75462, "omega":-0.03076, "ax":0.0, "ay":0.0, "alpha":-0.35972, "fx":[-0.05491,-0.92849,0.05491,0.92849], "fy":[0.92849,-0.05491,-0.92849,0.05491]}, + {"t":2.32332, "x":1.4461, "y":2.45073, "heading":2.41394, "vx":-0.65581, "vy":-0.75462, "omega":-0.0462, "ax":0.0, "ay":0.0, "alpha":-0.215, "fx":[-0.03208,-0.55498,0.03208,0.55498], "fy":[0.55498,-0.03208,-0.55498,0.03208]}, + {"t":2.36625, "x":1.41795, "y":2.41834, "heading":2.41196, "vx":-0.65581, "vy":-0.75462, "omega":-0.05543, "ax":0.0, "ay":0.0, "alpha":-0.0789, "fx":[-0.01137,-0.2037,0.01137,0.2037], "fy":[0.2037,-0.01137,-0.2037,0.01137]}, + {"t":2.40917, "x":1.3898, "y":2.38595, "heading":2.40958, "vx":-0.65581, "vy":-0.75462, "omega":-0.05882, "ax":0.0, "ay":0.0, "alpha":0.054, "fx":[0.00745,0.13944,-0.00745,-0.13944], "fy":[-0.13944,0.00745,0.13944,-0.00745]}, + {"t":2.4521, "x":1.36164, "y":2.35355, "heading":2.40706, "vx":-0.65581, "vy":-0.75462, "omega":-0.0565, "ax":0.0, "ay":0.0, "alpha":0.18907, "fx":[0.02485,0.48824,-0.02485,-0.48824], "fy":[-0.48824,0.02485,0.48824,-0.02485]}, + {"t":2.49502, "x":1.33349, "y":2.32116, "heading":2.40463, "vx":-0.65581, "vy":-0.75462, "omega":-0.04838, "ax":0.0, "ay":0.0, "alpha":0.33172, "fx":[0.04153,0.85671,-0.04153,-0.85671], "fy":[-0.85671,0.04153,0.85671,-0.04153]}, + {"t":2.53795, "x":1.30534, "y":2.28877, "heading":2.40255, "vx":-0.65581, "vy":-0.75462, "omega":-0.03414, "ax":0.00002, "ay":0.00003, "alpha":0.48767, "fx":[0.05883,1.25999,-0.05804,-1.2592], "fy":[-1.25914,0.05888,1.26005,-0.05798]}, + {"t":2.58087, "x":1.27719, "y":2.25638, "heading":2.40109, "vx":-0.65581, "vy":-0.75462, "omega":-0.01321, "ax":2.59339, "ay":2.98416, "alpha":0.30156, "fx":[44.44219,44.91605,43.7906,43.30274], "fy":[50.00538,50.50693,51.50786,51.01855]}, + {"t":2.6238, "x":1.25143, "y":2.22673, "heading":2.40052, "vx":-0.54448, "vy":-0.62652, "omega":-0.00026, "ax":4.22267, "ay":4.85893, "alpha":0.00291, "fx":[71.83419,71.83465,71.81866,71.8182], "fy":[82.6422,82.6419,82.65579,82.65609]}, + {"t":2.66673, "x":1.23195, "y":2.20432, "heading":2.40051, "vx":-0.36322, "vy":-0.41795, "omega":-0.00014, "ax":4.22964, "ay":4.86695, "alpha":0.00181, "fx":[71.94979,71.95005,71.94009,71.93982], "fy":[82.78114,82.78093,82.78959,82.7898]}, + {"t":2.70965, "x":1.22025, "y":2.19086, "heading":2.4005, "vx":-0.18166, "vy":-0.20903, "omega":-0.00006, "ax":4.23197, "ay":4.86963, "alpha":0.00144, "fx":[71.9885,71.98871,71.98078,71.98057], "fy":[82.82769,82.82752,82.83441,82.83458]}, + {"t":2.75258, "x":1.21635, "y":2.18637, "heading":2.4005, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.01301, "ay":4.01602, "alpha":-2.80111, "fx":[78.56655,79.43799,92.86166,90.21305], "fy":[76.62122,75.6823,58.44621,62.4958]}, + {"t":2.7881, "x":1.21952, "y":2.18891, "heading":2.4005, "vx":0.17809, "vy":0.14267, "omega":-0.09951, "ax":5.01324, "ay":4.01595, "alpha":-2.76846, "fx":[78.64148,79.51587,92.77422,90.16333], "fy":[76.53273,75.58681,58.56775,62.55362]}, + {"t":2.82363, "x":1.22901, "y":2.19651, "heading":2.39697, "vx":0.35618, "vy":0.28534, "omega":-0.19786, "ax":5.01349, "ay":4.01586, "alpha":-2.73145, "fx":[78.7482,79.57594,92.66019,90.1279], "fy":[76.40966,75.50804,58.72831,62.58867]}, + {"t":2.85915, "x":1.24482, "y":2.20918, "heading":2.38994, "vx":0.53429, "vy":0.428, "omega":-0.29489, "ax":5.01378, "ay":4.01574, "alpha":-2.68898, "fx":[78.8881,79.62268,92.51697,90.10378], "fy":[76.2499,75.44091,58.93093,62.60482]}, + {"t":2.89468, "x":1.26697, "y":2.22692, "heading":2.37946, "vx":0.7124, "vy":0.57066, "omega":-0.39042, "ax":5.01409, "ay":4.01559, "alpha":-2.63954, "fx":[79.06314,79.6621,92.34089,90.08692], "fy":[76.05044,75.37859,59.17992,62.60725]}, + {"t":2.9302, "x":1.29544, "y":2.24972, "heading":2.36559, "vx":0.89053, "vy":0.71331, "omega":-0.48419, "ax":5.01445, "ay":4.0154, "alpha":-2.58115, "fx":[79.27616,79.70217,92.12697,90.07184], "fy":[75.80704,75.31185,59.4811,62.60293]}, + {"t":2.96573, "x":1.33024, "y":2.2776, "heading":2.34839, "vx":1.06866, "vy":0.85596, "omega":-0.57588, "ax":5.01484, "ay":4.01514, "alpha":-2.51099, "fx":[79.53114,79.75363,91.86835,90.05097], "fy":[75.5137,75.22822,59.84237,62.60145]}, + {"t":3.00125, "x":1.37137, "y":2.31054, "heading":2.32793, "vx":1.24682, "vy":0.9986, "omega":-0.66509, "ax":5.01528, "ay":4.01481, "alpha":-2.42507, "fx":[79.83375,79.83126,91.55556,90.01353], "fy":[75.16183,75.11034,60.27444,62.61639]}, + {"t":3.03678, "x":1.41883, "y":2.34855, "heading":2.3043, "vx":1.42498, "vy":1.14122, "omega":-0.75124, "ax":5.01577, "ay":4.01435, "alpha":-2.31743, "fx":[80.19245,79.95598,91.17508,89.94359], "fy":[74.73859,74.93329,60.79225,62.6677]}, + {"t":3.0723, "x":1.47261, "y":2.39162, "heading":2.27762, "vx":1.60317, "vy":1.28383, "omega":-0.83356, "ax":5.01628, "ay":4.01369, "alpha":-2.17883, "fx":[80.6204,80.15872,90.70667,89.8163], "fy":[74.2238,74.65947,61.41764,62.7861]}, + {"t":3.10783, "x":1.53273, "y":2.43976, "heading":2.248, "vx":1.78137, "vy":1.42642, "omega":-0.91097, "ax":5.01678, "ay":4.01268, "alpha":-1.99388, "fx":[81.1396,80.48798,90.11819,89.59024], "fy":[73.58343,74.22839,62.1845,63.02196]}, + {"t":3.14335, "x":1.59918, "y":2.49297, "heading":2.21564, "vx":1.95959, "vy":1.56897, "omega":-0.9818, "ax":5.01713, "ay":4.011, "alpha":-1.735, "fx":[81.79025,81.02559,89.35407,89.18961], "fy":[72.75452,73.53423,63.14991,63.46508]}, + {"t":3.17888, "x":1.67196, "y":2.55124, "heading":2.18076, "vx":2.13783, "vy":1.71146, "omega":-1.04344, "ax":5.01689, "ay":4.00786, "alpha":-1.34681, "fx":[82.65502,81.92418,88.30713,88.45731], "fy":[71.60482,72.36961,64.42073,64.29486]}, + {"t":3.2144, "x":1.75107, "y":2.61457, "heading":2.1437, "vx":2.31605, "vy":1.85384, "omega":-1.09128, "ax":5.01452, "ay":4.00094, "alpha":-0.69856, "fx":[83.93546,83.51007,86.73633,87.00064], "fy":[69.79981,70.25754,66.22755,65.93474]}, + {"t":3.24993, "x":1.83652, "y":2.68295, "heading":2.10493, "vx":2.49419, "vy":1.99597, "omega":-1.1161, "ax":5.00221, "ay":3.98071, "alpha":0.61505, "fx":[86.27981,86.6334,83.95577,83.47534], "fy":[66.1786,65.7889,69.18247,69.69291]}, + {"t":3.28545, "x":1.92828, "y":2.75637, "heading":2.06528, "vx":2.6719, "vy":2.13739, "omega":-1.09425, "ax":4.8964, "ay":3.85955, "alpha":4.90177, "fx":[93.31014,94.24242,76.94917,68.64388], "fy":[52.36214,52.30602,75.59013,82.34093]}, + {"t":3.32098, "x":2.02629, "y":2.83473, "heading":2.02641, "vx":2.84584, "vy":2.2745, "omega":-0.92011, "ax":-0.04487, "ay":-0.05809, "alpha":0.20237, "fx":[-0.93258,-0.26807,-0.59378,-1.25829], "fy":[-1.48304,-1.15758,-0.49298,-0.81856]}, + {"t":3.3565, "x":2.12736, "y":2.9155, "heading":1.99372, "vx":2.84425, "vy":2.27243, "omega":-0.91292, "ax":-4.86754, "ay":-3.82724, "alpha":-5.96025, "fx":[-94.3506,-96.29119,-76.66913,-63.87092], "fy":[-49.99059,-48.42901,-75.94026,-86.04143]}, + {"t":3.39203, "x":2.22533, "y":2.99381, "heading":1.96129, "vx":2.67133, "vy":2.13647, "omega":-1.12466, "ax":-5.00104, "ay":-3.97843, "alpha":-0.93531, "fx":[-86.54769,-87.6189,-83.73715,-82.36151], "fy":[-65.78224,-64.46258,-69.45772,-70.98531]}, + {"t":3.42755, "x":2.31707, "y":3.0672, "heading":1.92133, "vx":2.49367, "vy":1.99514, "omega":-1.15789, "ax":-5.01516, "ay":-4.0006, "alpha":0.58467, "fx":[-84.51434,-83.59338,-86.1586,-86.95971], "fy":[-69.09057,-70.16179,-66.97157,-65.97247]}, + {"t":3.46308, "x":2.40249, "y":3.13555, "heading":1.8802, "vx":2.3155, "vy":1.85302, "omega":-1.13712, "ax":-5.01724, "ay":-4.00749, "alpha":1.32861, "fx":[-83.77495,-81.28256,-87.21533,-89.0942], "fy":[-70.2947,-73.10019,-65.87761,-63.39237]}, + {"t":3.4986, "x":2.48158, "y":3.19885, "heading":1.8398, "vx":2.13726, "vy":1.71065, "omega":-1.08992, "ax":-5.01699, "ay":-4.0103, "alpha":1.76959, "fx":[-83.51542,-79.77247,-87.69534,-90.36727], "fy":[-70.77521,-74.90703,-65.41585,-61.75832]}, + {"t":3.53413, "x":2.55435, "y":3.25709, "heading":1.80108, "vx":1.95904, "vy":1.56818, "omega":-1.02705, "ax":-5.01625, "ay":-4.01164, "alpha":2.06033, "fx":[-83.47707,-78.70606,-87.88324,-91.23343], "fy":[-70.93033,-76.13333,-65.28452,-60.59924]}, + {"t":3.56965, "x":2.62077, "y":3.31027, "heading":1.7646, "vx":1.78083, "vy":1.42567, "omega":-0.95386, "ax":-5.01544, "ay":-4.01234, "alpha":2.26568, "fx":[-83.55071,-77.9153,-87.90876,-91.87033], "fy":[-70.91978,-77.01773,-65.33786,-59.71989]}, + {"t":3.60518, "x":2.68087, "y":3.35839, "heading":1.73071, "vx":1.60266, "vy":1.28313, "omega":-0.87337, "ax":-5.0147, "ay":-4.01274, "alpha":2.41802, "fx":[-83.68167,-77.30972,-87.84082,-92.36205], "fy":[-70.82118,-77.68157,-65.49548,-59.02427]}, + {"t":3.6407, "x":2.73464, "y":3.40144, "heading":1.69969, "vx":1.42451, "vy":1.14058, "omega":-0.78747, "ax":-5.01403, "ay":-4.01299, "alpha":2.53534, "fx":[-83.83925,-76.83533,-87.72031,-92.75397], "fy":[-70.67744,-78.19396,-65.70864,-58.45908]}, + {"t":3.67623, "x":2.78209, "y":3.43942, "heading":1.67171, "vx":1.24639, "vy":0.99802, "omega":-0.6974, "ax":-5.01344, "ay":-4.01315, "alpha":2.62844, "fx":[-84.00477,-76.4573,-87.57357,-93.07308], "fy":[-70.51453,-78.59786,-65.94575,-57.9918]}, + {"t":3.71175, "x":2.8232, "y":3.47235, "heading":1.64694, "vx":1.06829, "vy":0.85545, "omega":-0.60403, "ax":-5.01292, "ay":-4.01326, "alpha":2.70416, "fx":[-84.16614,-76.15168,-87.41863,-93.33674], "fy":[-70.34931,-78.92177,-66.1852,-57.60103]}, + {"t":3.74728, "x":2.85799, "y":3.5002, "heading":1.62548, "vx":0.8902, "vy":0.71288, "omega":-0.50796, "ax":-5.01245, "ay":-4.01333, "alpha":2.76705, "fx":[-84.3152,-75.90124,-87.26842,-93.55674], "fy":[-70.19332,-79.1856,-66.41171,-57.27188]}, + {"t":3.7828, "x":2.88645, "y":3.523, "heading":1.60743, "vx":0.71214, "vy":0.57031, "omega":-0.40966, "ax":-5.01204, "ay":-4.01339, "alpha":2.8202, "fx":[-84.44622,-75.69313,-87.13253,-93.74153], "fy":[-70.05475,-79.40378,-66.61414,-56.99347]}, + {"t":3.81833, "x":2.90859, "y":3.54072, "heading":1.59288, "vx":0.53408, "vy":0.42773, "omega":-0.30948, "ax":-5.01167, "ay":-4.01342, "alpha":2.8658, "fx":[-84.55504,-75.51757,-87.01825,-93.89729], "fy":[-69.93965,-79.58711,-66.78418,-56.75764]}, + {"t":3.85385, "x":2.9244, "y":3.55339, "heading":1.58188, "vx":0.35604, "vy":0.28516, "omega":-0.20767, "ax":-5.01133, "ay":-4.01344, "alpha":2.90544, "fx":[-84.63857,-75.367,-86.93123,-94.02867], "fy":[-69.85258,-79.74373,-66.91551,-56.55817]}, + {"t":3.88938, "x":2.93388, "y":3.56098, "heading":1.57451, "vx":0.17802, "vy":0.14258, "omega":-0.10445, "ax":-5.01104, "ay":-4.01345, "alpha":2.9403, "fx":[-84.69446,-75.2356,-86.87588,-94.13917], "fy":[-69.79705,-79.87986,-67.00323,-56.39031]}, + {"t":3.9249, "x":2.93704, "y":3.56352, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3.traj new file mode 100644 index 00000000..bd2c3368 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3.traj @@ -0,0 +1,121 @@ +{ + "name":"E_RIGHT_PATH_3", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.6728932857513423, "y":2.9787795543670654, "heading":2.618314122167914, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9501848220825195, "y":2.551724433898926, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.232835292816162, "y":3.684377908706665, "heading":1.5707963267948966, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2258362770080566, "y":4.026000022888184, "heading":1.5707963267948966, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8381152153015137, "y":3.927910327911377, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.6728932857513428 m", "val":3.6728932857513423}, "y":{"exp":"2.9787795543670654 m", "val":2.9787795543670654}, "heading":{"exp":"2.6183141221679134 rad", "val":2.618314122167914}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9501848220825195 m", "val":2.9501848220825195}, "y":{"exp":"2.551724433898926 m", "val":2.551724433898926}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.232835292816162 m", "val":2.232835292816162}, "y":{"exp":"3.684377908706665 m", "val":3.684377908706665}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2258362770080566 m", "val":1.2258362770080566}, "y":{"exp":"4.026000022888184 m", "val":4.026000022888184}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8381152153015137 m", "val":1.8381152153015137}, "y":{"exp":"3.927910327911377 m", "val":3.927910327911377}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.66638,1.23554,1.85262,2.29133], + "samples":[ + {"t":0.0, "x":3.67289, "y":2.97878, "heading":2.61831, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.42941, "ay":-5.44705, "alpha":-1.35023, "fx":[-64.79624,-63.66172,-52.59453,-52.28035], "fy":[-88.51856,-89.36133,-96.29093,-96.43991]}, + {"t":0.03029, "x":3.67132, "y":2.97628, "heading":2.61831, "vx":-0.10388, "vy":-0.16499, "omega":-0.0409, "ax":-3.49791, "ay":-5.40047, "alpha":-1.42396, "fx":[-66.3419,-64.97455,-53.44354,-53.23399], "fy":[-87.34485,-88.39407,-95.80597,-95.89684]}, + {"t":0.06058, "x":3.66657, "y":2.96881, "heading":2.61708, "vx":-0.20983, "vy":-0.32857, "omega":-0.08403, "ax":-3.58119, "ay":-5.34196, "alpha":-1.51443, "fx":[-68.2147,-66.56126,-54.48318,-54.40123], "fy":[-85.86353,-87.1842,-95.19852,-95.21462]}, + {"t":0.09087, "x":3.65857, "y":2.9564, "heading":2.61453, "vx":-0.3183, "vy":-0.49038, "omega":-0.1299, "ax":-3.68449, "ay":-5.26641, "alpha":-1.62792, "fx":[-70.5302,-68.51094,-55.78057,-55.86651], "fy":[-83.93758,-85.6337,-94.41897,-94.33041]}, + {"t":0.12116, "x":3.64724, "y":2.93913, "heading":2.6106, "vx":-0.42991, "vy":-0.6499, "omega":-0.17921, "ax":-3.81573, "ay":-5.16543, "alpha":-1.77427, "fx":[-73.46091,-70.95418,-57.43851,-57.76412], "fy":[-81.33917,-83.58552,-93.38701,-93.13804]}, + {"t":0.15145, "x":3.63247, "y":2.91708, "heading":2.60517, "vx":-0.54548, "vy":-0.80636, "omega":-0.23295, "ax":-3.98745, "ay":-5.02428, "alpha":-1.96953, "fx":[-77.27088,-74.08785,-59.6224,-60.32048], "fy":[-77.66462,-80.77371,-91.96417,-91.44408]}, + {"t":0.18174, "x":3.61411, "y":2.89035, "heading":2.59811, "vx":-0.66626, "vy":-0.95854, "omega":-0.29261, "ax":-4.22028, "ay":-4.81501, "alpha":-2.24124, "fx":[-82.36698,-78.21564,-62.61445,-63.94576], "fy":[-72.14111,-76.71622,-89.89153,-88.85885]}, + {"t":0.21203, "x":3.592, "y":2.85911, "heading":2.58925, "vx":-0.7941, "vy":-1.10439, "omega":-0.3605, "ax":-4.54919, "ay":-4.47868, "alpha":-2.63873, "fx":[-89.33379,-83.80438,-66.93203,-69.45135], "fy":[-63.14892,-70.46471,-86.62984,-84.48109]}, + {"t":0.24232, "x":3.56586, "y":2.8236, "heading":2.57833, "vx":-0.93189, "vy":-1.24005, "omega":-0.44043, "ax":-5.03131, "ay":-3.8743, "alpha":-3.2464, "fx":[-98.63882,-91.48947,-73.60315,-78.59344], "fy":[-46.98695,-59.97045,-80.8763,-75.76924]}, + {"t":0.27261, "x":3.53532, "y":2.78426, "heading":2.56499, "vx":-1.08429, "vy":-1.3574, "omega":-0.53876, "ax":-5.71771, "ay":-2.61483, "alpha":-4.1419, "fx":[-107.95762,-101.47056,-84.75928,-94.83875], "fy":[-15.41923,-40.46546,-68.75595,-53.26968]}, + {"t":0.3029, "x":3.49986, "y":2.74195, "heading":2.54867, "vx":-1.25748, "vy":-1.43661, "omega":-0.66422, "ax":-6.16992, "ay":0.30125, "alpha":-5.42545, "fx":[-101.68578,-109.01504,-102.7199,-106.37336], "fy":[38.92114,-2.17589,-35.60891,19.36041]}, + {"t":0.33319, "x":3.45894, "y":2.69857, "heading":2.52855, "vx":-1.44436, "vy":-1.42748, "omega":-0.82855, "ax":-4.45087, "ay":4.27336, "alpha":-5.64126, "fx":[-65.34357,-93.61414,-95.7833,-48.09107], "fy":[87.2115,55.70171,50.58593,97.25558]}, + {"t":0.36348, "x":3.41315, "y":2.65729, "heading":2.50345, "vx":-1.57918, "vy":-1.29804, "omega":-0.99943, "ax":-1.87443, "ay":6.04455, "alpha":-3.35688, "fx":[-29.31581,-53.74834,-36.81734,-7.65291], "fy":[105.18388,94.92752,102.38677,108.76594]}, + {"t":0.39377, "x":3.36445, "y":2.62075, "heading":2.47318, "vx":-1.63596, "vy":-1.11495, "omega":-1.10111, "ax":-0.3601, "ay":6.38063, "alpha":-2.16507, "fx":[-7.00842,-21.98337,-5.02653,9.5173], "fy":[109.13189,107.05047,109.06922,108.87907]}, + {"t":0.42406, "x":3.31473, "y":2.5899, "heading":2.43983, "vx":-1.64687, "vy":-0.92168, "omega":-1.16669, "ax":0.46246, "ay":6.40074, "alpha":-1.47365, "fx":[6.3608,-2.92546,9.7326,18.29743], "fy":[109.27921,109.38173,108.94462,107.89347]}, + {"t":0.45435, "x":3.26506, "y":2.56492, "heading":2.40449, "vx":-1.63286, "vy":-0.7278, "omega":-1.21132, "ax":0.95492, "ay":6.35921, "alpha":-1.04134, "fx":[14.87867,8.73643,17.84459,23.51202], "fy":[108.5212,109.16123,108.02492,106.96608]}, + {"t":0.48464, "x":3.21604, "y":2.54579, "heading":2.3678, "vx":-1.60393, "vy":-0.53518, "omega":-1.24287, "ax":1.27713, "ay":6.30982, "alpha":-0.75161, "fx":[20.65948,16.38196,22.92351,26.92954], "fy":[107.62234,108.33963,107.13405,106.21659]}, + {"t":0.51493, "x":3.16804, "y":2.53248, "heading":2.33015, "vx":-1.56525, "vy":-0.34406, "omega":-1.26563, "ax":1.50263, "ay":6.2645, "alpha":-0.54573, "fx":[24.78858,21.72016,26.40311,29.32557], "fy":[106.78543,107.44122,106.37974,105.62293]}, + {"t":0.54522, "x":3.12132, "y":2.52493, "heading":2.29182, "vx":-1.51973, "vy":-0.15431, "omega":-1.28216, "ax":1.66864, "ay":6.22534, "alpha":-0.39255, "fx":[27.85739,25.64004,28.94586,31.08927], "fy":[106.05588,106.60711,105.75263,105.14953]}, + {"t":0.57551, "x":3.07606, "y":2.52311, "heading":2.25298, "vx":-1.46919, "vy":0.03426, "omega":-1.29405, "ax":1.79569, "ay":6.19199, "alpha":-0.27438, "fx":[30.20951,28.63475,30.89586,32.43626], "fy":[105.43344,105.86792,105.22728,104.767]}, + {"t":0.6058, "x":3.03238, "y":2.52699, "heading":2.21378, "vx":-1.4148, "vy":0.22181, "omega":-1.30236, "ax":1.89591, "ay":6.16356, "alpha":-0.18057, "fx":[32.05629,30.99547,32.44834,33.49535], "fy":[104.9057,105.22162,104.78089,104.45338]}, + {"t":0.63609, "x":2.99039, "y":2.53653, "heading":2.17433, "vx":-1.35737, "vy":0.40851, "omega":-1.30783, "ax":1.97693, "ay":6.1392, "alpha":-0.10435, "fx":[33.53413,32.90369,33.72184,34.3481], "fy":[104.45837,104.65743,104.39571,104.1926]}, + {"t":0.66638, "x":2.95018, "y":2.55172, "heading":2.13472, "vx":-1.29749, "vy":0.59446, "omega":-1.31099, "ax":1.9906, "ay":6.13141, "alpha":-0.06082, "fx":[33.82092,33.4371,33.89867,34.28131], "fy":[104.30739,104.4301,104.28038,104.15619]}, + {"t":0.68827, "x":2.92226, "y":2.56621, "heading":2.10602, "vx":-1.25392, "vy":0.72868, "omega":-1.31233, "ax":1.94601, "ay":6.14488, "alpha":-0.02013, "fx":[33.09319,32.96063,33.1089,33.24136], "fy":[104.52545,104.56699,104.51985,104.47813]}, + {"t":0.71016, "x":2.89528, "y":2.58363, "heading":2.07729, "vx":-1.21132, "vy":0.8632, "omega":-1.31277, "ax":1.89494, "ay":6.15984, "alpha":0.02635, "fx":[32.23584,32.4168,32.22888,32.0478], "fy":[104.77573,104.72034,104.77875,104.83383]}, + {"t":0.73205, "x":2.86921, "y":2.604, "heading":2.04856, "vx":-1.16984, "vy":0.99804, "omega":-1.31219, "ax":1.83588, "ay":6.17654, "alpha":0.07993, "fx":[31.21704,31.78874,31.23883,30.66647], "fy":[105.06378,104.89363,105.06015,105.22726]}, + {"t":0.75394, "x":2.84404, "y":2.62733, "heading":2.01983, "vx":-1.12965, "vy":1.13325, "omega":-1.31044, "ax":1.76682, "ay":6.19525, "alpha":0.14235, "fx":[29.9939,31.05384,30.11268,29.05228], "fy":[105.39611,105.09131,105.36771,105.66247]}, + {"t":0.77583, "x":2.81974, "y":2.65362, "heading":1.99114, "vx":-1.09097, "vy":1.26887, "omega":-1.30732, "ax":1.68504, "ay":6.21627, "alpha":0.21596, "fx":[28.50723,30.18111,28.81536,27.14443], "fy":[105.78023,105.31902,105.7057,106.14314]}, + {"t":0.79772, "x":2.79626, "y":2.68289, "heading":1.96253, "vx":-1.05408, "vy":1.40495, "omega":-1.3026, "ax":1.58671, "ay":6.23994, "alpha":0.30401, "fx":[26.67318,29.12689,27.29864,24.85928], "fy":[106.22428,105.58396,106.07905,106.67141]}, + {"t":0.81961, "x":2.77357, "y":2.71514, "heading":1.93401, "vx":-1.01935, "vy":1.54154, "omega":-1.29594, "ax":1.46637, "ay":6.26656, "alpha":0.41112, "fx":[24.36912,27.82777,25.49453,22.07882], "fy":[106.73578,105.89538,106.493,107.24545]}, + {"t":0.8415, "x":2.7516, "y":2.75039, "heading":1.90564, "vx":-0.98725, "vy":1.67872, "omega":-1.28694, "ax":1.31591, "ay":6.29624, "alpha":0.54412, "fx":[21.40905,26.18805,23.30422,18.63202], "fy":[107.31793,106.26489,106.95224,107.85396]}, + {"t":0.8634, "x":2.73031, "y":2.78864, "heading":1.87747, "vx":-0.95844, "vy":1.81655, "omega":-1.27503, "ax":1.12285, "ay":6.32856, "alpha":0.71343, "fx":[17.49852,24.05664,20.57893,14.26341], "fy":[107.95945,106.70629,107.45842,108.46393]}, + {"t":0.88529, "x":2.7096, "y":2.82992, "heading":1.84956, "vx":-0.93386, "vy":1.95509, "omega":-1.25941, "ax":0.86704, "ay":6.36157, "alpha":0.9358, "fx":[12.14763,21.18084,17.08506,8.57916], "fy":[108.60567,107.23291,108.0034,108.99214]}, + {"t":0.90718, "x":2.68936, "y":2.87425, "heading":1.82199, "vx":-0.91488, "vy":2.09435, "omega":-1.23893, "ax":0.5143, "ay":6.38905, "alpha":1.23978, "fx":[4.49468,17.1066,12.43702,0.95377], "fy":[109.07215,107.84529,108.55053,109.23577]}, + {"t":0.92907, "x":2.66946, "y":2.92162, "heading":1.79487, "vx":-0.90362, "vy":2.23421, "omega":-1.21179, "ax":0.00322, "ay":6.39223, "alpha":1.67741, "fx":[-7.05713,10.93837,5.95977,-9.6222], "fy":[108.76556,108.47877,108.9799,108.6958]}, + {"t":0.95096, "x":2.64968, "y":2.97206, "heading":1.76834, "vx":-0.90355, "vy":2.37414, "omega":-1.17507, "ax":-0.7809, "ay":6.31265, "alpha":2.34766, "fx":[-25.48647,0.6848,-3.60757,-24.72217], "fy":[105.71227,108.76126,108.91287,106.11928]}, + {"t":0.97285, "x":2.62971, "y":3.02555, "heading":1.74262, "vx":-0.92065, "vy":2.51233, "omega":-1.12368, "ax":-2.03968, "ay":5.96046, "alpha":3.39656, "fx":[-55.02758,-18.72957,-18.74462,-46.27568], "fy":[93.38113,106.65596,107.08671,98.41909]}, + {"t":0.99474, "x":2.60907, "y":3.08197, "heading":1.71802, "vx":-0.9653, "vy":2.6428, "omega":-1.04932, "ax":-3.95677, "ay":4.77902, "alpha":4.58691, "fx":[-91.5817,-59.59896,-44.00545,-74.02779], "fy":[57.43055,89.36667,98.97956,79.38232]}, + {"t":1.01663, "x":2.58699, "y":3.14097, "heading":1.69505, "vx":-1.05191, "vy":2.74742, "omega":-0.94891, "ax":-5.79941, "ay":1.96933, "alpha":5.5506, "fx":[-108.19438,-105.78524,-81.62452,-98.98099], "fy":[3.12001,16.03308,70.50213,44.33554]}, + {"t":1.03852, "x":2.56257, "y":3.20158, "heading":1.67428, "vx":-1.17887, "vy":2.79053, "omega":-0.82741, "ax":-6.08639, "ay":-1.0138, "alpha":5.30411, "fx":[-102.41179,-96.07958,-107.10233,-108.517], "fy":[-36.15216,-49.16241,12.57479,3.76177]}, + {"t":1.06041, "x":2.53531, "y":3.26243, "heading":1.65617, "vx":-1.3121, "vy":2.76834, "omega":-0.71129, "ax":-5.59492, "ay":-2.8384, "alpha":3.99685, "fx":[-92.63835,-80.46448,-102.24913,-105.31997], "fy":[-57.24897,-72.89496,-35.67902,-27.29857]}, + {"t":1.0823, "x":2.50524, "y":3.32235, "heading":1.6406, "vx":-1.43458, "vy":2.7062, "omega":-0.6238, "ax":-5.05927, "ay":-3.8062, "alpha":3.18088, "fx":[-84.66308,-70.86171,-90.39607,-98.30588], "fy":[-68.8117,-82.71714,-60.36365,-47.07733]}, + {"t":1.10419, "x":2.47263, "y":3.38068, "heading":1.62694, "vx":-1.54533, "vy":2.62288, "omega":-0.55417, "ax":-4.64625, "ay":-4.34761, "alpha":2.68253, "fx":[-78.72257,-64.84818,-81.0115,-91.54304], "fy":[-75.73293,-87.76599,-72.86883,-59.43885]}, + {"t":1.12608, "x":2.43769, "y":3.43705, "heading":1.61481, "vx":-1.64704, "vy":2.52771, "omega":-0.49545, "ax":-4.34064, "ay":-4.67977, "alpha":2.34627, "fx":[-74.2808,-60.80938,-74.31369,-85.92829], "fy":[-80.22674,-90.77154,-79.92596,-67.48217]}, + {"t":1.14797, "x":2.40059, "y":3.49127, "heading":1.60396, "vx":-1.74206, "vy":2.42527, "omega":-0.44409, "ax":-4.11094, "ay":-4.90006, "alpha":2.10395, "fx":[-70.88267,-57.9308,-69.4718,-81.41841], "fy":[-83.33882,-92.74493,-84.32451,-72.98597]}, + {"t":1.16986, "x":2.36147, "y":3.54318, "heading":1.59424, "vx":-1.83205, "vy":2.318, "omega":-0.39803, "ax":-3.93385, "ay":-5.0552, "alpha":1.92139, "fx":[-68.21731,-55.78219,-65.86071,-77.79449], "fy":[-85.60481,-94.13251,-87.28334,-76.92919]}, + {"t":1.19176, "x":2.32042, "y":3.59271, "heading":1.58553, "vx":-1.91816, "vy":2.20734, "omega":-0.35597, "ax":-3.7939, "ay":-5.16965, "alpha":1.77925, "fx":[-66.07844,-54.11972,-63.08363,-74.85092], "fy":[-87.32095,-95.15836,-89.39159,-79.86613]}, + {"t":1.21365, "x":2.27753, "y":3.63979, "heading":1.57774, "vx":-2.00122, "vy":2.09417, "omega":-0.31702, "ax":-3.68086, "ay":-5.25721, "alpha":1.66566, "fx":[-64.32743,-52.79617,-60.89037,-72.42751], "fy":[-88.66217,-95.94625,-90.96139,-82.12501]}, + {"t":1.23554, "x":2.23284, "y":3.68438, "heading":1.5708, "vx":-2.08179, "vy":1.97909, "omega":-0.28056, "ax":-3.41832, "ay":-5.44517, "alpha":1.52191, "fx":[-60.21633,-49.04289,-55.92445,-67.39499], "fy":[-91.65165,-98.07031,-94.28111,-86.47989]}, + {"t":1.27184, "x":2.15502, "y":3.75263, "heading":1.56061, "vx":-2.20587, "vy":1.78144, "omega":-0.22531, "ax":-2.86207, "ay":-5.75435, "alpha":1.55044, "fx":[-51.84052,-39.3517,-45.07859,-58.46143], "fy":[-96.60484,-102.31585,-99.88568,-92.71306]}, + {"t":1.30813, "x":2.07306, "y":3.8135, "heading":1.55243, "vx":-2.30976, "vy":1.57256, "omega":-0.16903, "ax":-2.0951, "ay":-6.07318, "alpha":1.56566, "fx":[-40.19928,-26.45056,-30.28032,-45.61805], "fy":[-101.96355,-106.35142,-105.28085,-99.61642]}, + {"t":1.34443, "x":1.98784, "y":3.86658, "heading":1.5463, "vx":-2.38581, "vy":1.35211, "omega":-0.1122, "ax":-1.04255, "ay":-6.33741, "alpha":1.54409, "fx":[-23.92081,-9.43049,-10.48532,-27.09737], "fy":[-106.91952,-109.15087,-109.00676,-106.11268]}, + {"t":1.38073, "x":1.90055, "y":3.91149, "heading":1.54223, "vx":-2.42366, "vy":1.12207, "omega":-0.05615, "ax":0.33713, "ay":-6.41366, "alpha":1.44603, "fx":[-1.77997,12.06596,14.23385,-1.58208], "fy":[-109.50824,-108.85928,-108.54893,-109.46191]}, + {"t":1.41703, "x":1.8128, "y":3.94799, "heading":1.54019, "vx":-2.41142, "vy":0.88926, "omega":-0.00366, "ax":1.95061, "ay":-6.12196, "alpha":1.23803, "fx":[25.67076,36.68762,41.16918,29.18981], "fy":[-106.44098,-103.17675,-101.42905,-105.48403]}, + {"t":1.45333, "x":1.72655, "y":3.97624, "heading":1.54005, "vx":-2.34061, "vy":0.66704, "omega":0.04127, "ax":3.51306, "ay":-5.38563, "alpha":0.93655, "fx":[54.12073,60.77545,65.40672,58.72135], "fy":[-95.17546,-91.09077,-87.79276,-92.3728]}, + {"t":1.48963, "x":1.6439, "y":3.9969, "heading":1.54155, "vx":-2.21309, "vy":0.47155, "omega":0.07527, "ax":4.72813, "ay":-4.36573, "alpha":0.60383, "fx":[77.39193,80.26766,83.37553,80.66163], "fy":[-77.47705,-74.51467,-71.00107,-74.04616]}, + {"t":1.52593, "x":1.56668, "y":4.01114, "heading":1.54428, "vx":-2.04147, "vy":0.31308, "omega":0.09719, "ax":5.51629, "ay":-3.32265, "alpha":0.30131, "fx":[92.71807,93.54631,94.91516,94.14276], "fy":[-58.34231,-57.01942,-54.70204,-56.00543]}, + {"t":1.56223, "x":1.49622, "y":4.02032, "heading":1.54781, "vx":-1.84123, "vy":0.19247, "omega":0.10813, "ax":5.97306, "ay":-2.41447, "alpha":0.05679, "fx":[101.45718,101.53727,101.7421,101.66385], "fy":[-41.42126,-41.22816,-40.71869,-40.90998]}, + {"t":1.59853, "x":1.43332, "y":4.02572, "heading":1.55174, "vx":-1.62442, "vy":0.10483, "omega":0.11019, "ax":6.22206, "ay":-1.67867, "alpha":-0.13121, "fx":[106.04926,105.96459,105.61788,105.71022], "fy":[-27.76176,-28.07196,-29.35221,-29.0287]}, + {"t":1.63482, "x":1.37845, "y":4.02841, "heading":1.55574, "vx":-1.39857, "vy":0.0439, "omega":0.10542, "ax":6.3519, "ay":-1.09627, "alpha":-0.27446, "fx":[108.31717,108.24788,107.75934,107.85162], "fy":[-17.06276,-17.46298,-20.26519,-19.79784]}, + {"t":1.67112, "x":1.33187, "y":4.02929, "heading":1.55956, "vx":-1.168, "vy":0.0041, "omega":0.09546, "ax":6.4153, "ay":-0.63533, "alpha":-0.38464, "fx":[109.33232,109.30329,108.89921,108.95475], "fy":[-8.6935,-8.96871,-12.9902,-12.5746]}, + {"t":1.70742, "x":1.2937, "y":4.02902, "heading":1.56303, "vx":-0.93513, "vy":-0.01896, "omega":0.0815, "ax":6.44177, "ay":-0.26691, "alpha":-0.47077, "fx":[109.6778,109.6693,109.45954,109.48422], "fy":[-2.07654,-2.1321,-7.11127,-6.84022]}, + {"t":1.74372, "x":1.264, "y":4.02815, "heading":1.56599, "vx":-0.7013, "vy":-0.02865, "omega":0.06441, "ax":6.44768, "ay":0.03154, "alpha":-0.5393, "fx":[109.66632,109.65269,109.68205,109.69155], "fy":[3.23258,3.4286,-2.30151,-2.2136]}, + {"t":1.78002, "x":1.24279, "y":4.02713, "heading":1.56833, "vx":-0.46726, "vy":-0.0275, "omega":0.04484, "ax":6.44216, "ay":0.27674, "alpha":-0.59478, "fx":[109.46704,109.42737,109.70666,109.71598], "fy":[7.55809,8.00786,1.68511,1.5783]}, + {"t":1.81632, "x":1.23007, "y":4.02632, "heading":1.56995, "vx":-0.23342, "vy":-0.01746, "omega":0.02325, "ax":6.43041, "ay":0.48095, "alpha":-0.6404, "fx":[109.17291,109.09245,109.61558,109.6366], "fy":[11.13363,11.82662,5.03169,4.73112]}, + {"t":1.85262, "x":1.22584, "y":4.026, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.36956, "ay":-1.02043, "alpha":0.0, "fx":[108.34442,108.34442,108.34442,108.34442], "fy":[-17.35724,-17.35724,-17.35724,-17.35724]}, + {"t":1.88636, "x":1.22946, "y":4.02542, "heading":1.5708, "vx":0.21495, "vy":-0.03444, "omega":0.0, "ax":6.36888, "ay":-1.02032, "alpha":0.0, "fx":[108.33282,108.33282,108.33282,108.33282], "fy":[-17.35538,-17.35538,-17.35538,-17.35538]}, + {"t":1.92011, "x":1.24034, "y":4.02368, "heading":1.5708, "vx":0.42989, "vy":-0.06887, "omega":0.0, "ax":6.36807, "ay":-1.02019, "alpha":0.0, "fx":[108.31901,108.31901,108.31901,108.31901], "fy":[-17.35317,-17.35317,-17.35317,-17.35317]}, + {"t":1.95386, "x":1.25848, "y":4.02077, "heading":1.5708, "vx":0.64479, "vy":-0.1033, "omega":0.0, "ax":6.36709, "ay":-1.02003, "alpha":0.0, "fx":[108.30229,108.30229,108.30229,108.30229], "fy":[-17.35049,-17.35049,-17.35049,-17.35049]}, + {"t":1.98761, "x":1.28386, "y":4.0167, "heading":1.5708, "vx":0.85966, "vy":-0.13772, "omega":0.0, "ax":6.36587, "ay":-1.01984, "alpha":0.0, "fx":[108.28165,108.28165,108.28165,108.28165], "fy":[-17.34718,-17.34718,-17.34718,-17.34718]}, + {"t":2.02135, "x":1.3165, "y":4.01148, "heading":1.5708, "vx":1.07449, "vy":-0.17214, "omega":0.0, "ax":6.36433, "ay":-1.01959, "alpha":0.0, "fx":[108.2555,108.2555,108.2555,108.2555], "fy":[-17.34299,-17.34299,-17.34299,-17.34299]}, + {"t":2.0551, "x":1.35638, "y":4.00509, "heading":1.5708, "vx":1.28927, "vy":-0.20655, "omega":0.0, "ax":6.36232, "ay":-1.01927, "alpha":0.0, "fx":[108.22131,108.22131,108.22131,108.22131], "fy":[-17.33752,-17.33752,-17.33752,-17.33752]}, + {"t":2.08885, "x":1.40352, "y":3.99753, "heading":1.5708, "vx":1.50398, "vy":-0.24094, "omega":0.0, "ax":6.35958, "ay":-1.01883, "alpha":0.0, "fx":[108.1747,108.1747,108.1747,108.1747], "fy":[-17.33005,-17.33005,-17.33005,-17.33005]}, + {"t":2.12259, "x":1.45789, "y":3.98882, "heading":1.5708, "vx":1.7186, "vy":-0.27533, "omega":0.0, "ax":6.35563, "ay":-1.0182, "alpha":0.0, "fx":[108.10741,108.10741,108.10741,108.10741], "fy":[-17.31927,-17.31927,-17.31927,-17.31927]}, + {"t":2.15634, "x":1.51951, "y":3.97895, "heading":1.5708, "vx":1.93308, "vy":-0.30969, "omega":0.0, "ax":6.34941, "ay":-1.0172, "alpha":0.0, "fx":[108.00172,108.00172,108.00172,108.00172], "fy":[-17.30234,-17.30234,-17.30234,-17.30234]}, + {"t":2.19009, "x":1.58836, "y":3.96792, "heading":1.5708, "vx":2.14736, "vy":-0.34402, "omega":0.0, "ax":6.33824, "ay":-1.01541, "alpha":0.0, "fx":[107.81168,107.81168,107.81168,107.81168], "fy":[-17.27189,-17.27189,-17.27189,-17.27189]}, + {"t":2.22384, "x":1.66444, "y":3.95573, "heading":1.5708, "vx":2.36126, "vy":-0.37828, "omega":0.0, "ax":6.31224, "ay":-1.01125, "alpha":0.0, "fx":[107.36932,107.36932,107.36932,107.36932], "fy":[-17.20102,-17.20102,-17.20102,-17.20102]}, + {"t":2.25758, "x":1.74772, "y":3.94239, "heading":1.5708, "vx":2.57428, "vy":-0.41241, "omega":0.0, "ax":6.18355, "ay":-0.99063, "alpha":0.0, "fx":[105.18048,105.18048,105.18048,105.18048], "fy":[-16.85036,-16.85036,-16.85036,-16.85036]}, + {"t":2.29133, "x":1.83812, "y":3.92791, "heading":1.5708, "vx":2.78296, "vy":-0.44584, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3_BACK.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3_BACK.traj new file mode 100644 index 00000000..69b0191a --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_3_BACK.traj @@ -0,0 +1,104 @@ +{ + "name":"E_RIGHT_PATH_3_BACK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.199847936630249, "y":3.813179254531861, "heading":1.5707963267948966, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2258362770080566, "y":4.026000022888184, "heading":1.5707963267948966, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.785933017730713, "y":4.141683101654053, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.199847936630249 m", "val":3.199847936630249}, "y":{"exp":"3.8131792545318604 m", "val":3.813179254531861}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2258362770080566 m", "val":1.2258362770080566}, "y":{"exp":"4.026000022888184 m", "val":4.026000022888184}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.785933017730713 m", "val":2.785933017730713}, "y":{"exp":"4.141683101654053 m", "val":4.141683101654053}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,2.14243,3.12753], + "samples":[ + {"t":0.0, "x":3.19985, "y":3.81318, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.41608, "ay":0.69173, "alpha":0.0, "fx":[-109.13562,-109.13562,-109.13562,-109.13562], "fy":[11.76605,11.76605,11.76605,11.76605]}, + {"t":0.04982, "x":3.19188, "y":3.81404, "heading":1.5708, "vx":-0.31967, "vy":0.03446, "omega":0.0, "ax":-6.41335, "ay":0.69143, "alpha":0.0, "fx":[-109.08931,-109.08931,-109.08931,-109.08931], "fy":[11.76106,11.76106,11.76106,11.76106]}, + {"t":0.09965, "x":3.168, "y":3.81661, "heading":1.5708, "vx":-0.63921, "vy":0.06891, "omega":0.0, "ax":-6.40517, "ay":0.69055, "alpha":0.0, "fx":[-108.95009,-108.95009,-108.95009,-108.95009], "fy":[11.74605,11.74605,11.74605,11.74605]}, + {"t":0.14947, "x":3.1282, "y":3.8209, "heading":1.5708, "vx":-0.95834, "vy":0.10332, "omega":0.0, "ax":-0.71637, "ay":0.07723, "alpha":0.0, "fx":[-12.18522,-12.18522,-12.18522,-12.18522], "fy":[1.3137,1.3137,1.3137,1.3137]}, + {"t":0.1993, "x":3.07956, "y":3.82615, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00004,-0.00004,-0.00004,-0.00004], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.24912, "x":3.03003, "y":3.83149, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.29894, "x":2.98051, "y":3.83683, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.34877, "x":2.93098, "y":3.84217, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.39859, "x":2.88145, "y":3.84751, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.44842, "x":2.83193, "y":3.85285, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.49824, "x":2.7824, "y":3.85818, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.54806, "x":2.73287, "y":3.86352, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.59789, "x":2.68335, "y":3.86886, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.64771, "x":2.63382, "y":3.8742, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.69754, "x":2.58429, "y":3.87954, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.74736, "x":2.53477, "y":3.88488, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.79718, "x":2.48524, "y":3.89022, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.84701, "x":2.43571, "y":3.89556, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.89683, "x":2.38619, "y":3.9009, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.94665, "x":2.33666, "y":3.90624, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.99648, "x":2.28713, "y":3.91158, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.0463, "x":2.23761, "y":3.91692, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.09613, "x":2.18808, "y":3.92226, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.14595, "x":2.13855, "y":3.9276, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.19577, "x":2.08903, "y":3.93294, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.2456, "x":2.0395, "y":3.93828, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.29542, "x":1.98997, "y":3.94362, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.34525, "x":1.94044, "y":3.94896, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.39507, "x":1.89092, "y":3.9543, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.44489, "x":1.84139, "y":3.95964, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.49472, "x":1.79186, "y":3.96498, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.54454, "x":1.74234, "y":3.97032, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.59437, "x":1.69281, "y":3.97565, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.64419, "x":1.64328, "y":3.98099, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.69401, "x":1.59376, "y":3.98633, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.74384, "x":1.54423, "y":3.99167, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.79366, "x":1.4947, "y":3.99701, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.84349, "x":1.44518, "y":4.00235, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.89331, "x":1.39565, "y":4.00769, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00004,0.00004,0.00004,0.00004], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.94313, "x":1.34612, "y":4.01303, "heading":1.5708, "vx":-0.99404, "vy":0.10717, "omega":0.0, "ax":0.71637, "ay":-0.07723, "alpha":0.0, "fx":[12.18522,12.18522,12.18522,12.18522], "fy":[-1.3137,-1.3137,-1.3137,-1.3137]}, + {"t":1.99296, "x":1.29749, "y":4.01828, "heading":1.5708, "vx":-0.95834, "vy":0.10332, "omega":0.0, "ax":6.40517, "ay":-0.69055, "alpha":0.0, "fx":[108.95009,108.95009,108.95009,108.95009], "fy":[-11.74605,-11.74605,-11.74605,-11.74605]}, + {"t":2.04278, "x":1.25769, "y":4.02257, "heading":1.5708, "vx":-0.63921, "vy":0.06891, "omega":0.0, "ax":6.41335, "ay":-0.69143, "alpha":0.0, "fx":[109.08931,109.08931,109.08931,109.08931], "fy":[-11.76106,-11.76106,-11.76106,-11.76106]}, + {"t":2.09261, "x":1.2338, "y":4.02514, "heading":1.5708, "vx":-0.31967, "vy":0.03446, "omega":0.0, "ax":6.41608, "ay":-0.69173, "alpha":0.0, "fx":[109.13562,109.13562,109.13562,109.13562], "fy":[-11.76605,-11.76605,-11.76605,-11.76605]}, + {"t":2.14243, "x":1.22584, "y":4.026, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.43554, "ay":0.4772, "alpha":0.0, "fx":[109.46674,109.46674,109.46674,109.46674], "fy":[8.11709,8.11709,8.11709,8.11709]}, + {"t":2.19168, "x":1.23364, "y":4.02658, "heading":1.5708, "vx":0.31698, "vy":0.0235, "omega":0.0, "ax":6.4349, "ay":0.47716, "alpha":0.0, "fx":[109.45574,109.45574,109.45574,109.45574], "fy":[8.11628,8.11628,8.11628,8.11628]}, + {"t":2.24094, "x":1.25706, "y":4.02832, "heading":1.5708, "vx":0.63393, "vy":0.04701, "omega":0.0, "ax":6.43408, "ay":0.47709, "alpha":0.0, "fx":[109.4418,109.4418,109.4418,109.4418], "fy":[8.11524,8.11524,8.11524,8.11524]}, + {"t":2.29019, "x":1.29609, "y":4.03121, "heading":1.5708, "vx":0.95084, "vy":0.07051, "omega":0.0, "ax":6.43301, "ay":0.47702, "alpha":0.0, "fx":[109.42358,109.42358,109.42358,109.42358], "fy":[8.11389,8.11389,8.11389,8.11389]}, + {"t":2.33945, "x":1.35073, "y":4.03526, "heading":1.5708, "vx":1.2677, "vy":0.094, "omega":0.0, "ax":6.43154, "ay":0.47691, "alpha":0.0, "fx":[109.39873,109.39873,109.39873,109.39873], "fy":[8.11205,8.11205,8.11205,8.11205]}, + {"t":2.3887, "x":1.42097, "y":4.04047, "heading":1.5708, "vx":1.58449, "vy":0.11749, "omega":0.0, "ax":6.42943, "ay":0.47675, "alpha":0.0, "fx":[109.36284,109.36284,109.36284,109.36284], "fy":[8.10939,8.10939,8.10939,8.10939]}, + {"t":2.43796, "x":1.50681, "y":4.04683, "heading":1.5708, "vx":1.90117, "vy":0.14097, "omega":0.0, "ax":6.42612, "ay":0.4765, "alpha":0.0, "fx":[109.30646,109.30646,109.30646,109.30646], "fy":[8.10521,8.10521,8.10521,8.10521]}, + {"t":2.48721, "x":1.60825, "y":4.05436, "heading":1.5708, "vx":2.21769, "vy":0.16444, "omega":0.0, "ax":6.42016, "ay":0.47606, "alpha":0.0, "fx":[109.20503,109.20503,109.20503,109.20503], "fy":[8.09769,8.09769,8.09769,8.09769]}, + {"t":2.53647, "x":1.72527, "y":4.06303, "heading":1.5708, "vx":2.53391, "vy":0.18789, "omega":0.0, "ax":6.40626, "ay":0.47503, "alpha":0.0, "fx":[108.96867,108.96867,108.96867,108.96867], "fy":[8.08016,8.08016,8.08016,8.08016]}, + {"t":2.58572, "x":1.85785, "y":4.07286, "heading":1.5708, "vx":2.84945, "vy":0.21129, "omega":0.0, "ax":6.33721, "ay":0.46991, "alpha":0.0, "fx":[107.79411,107.79411,107.79411,107.79411], "fy":[7.99307,7.99307,7.99307,7.99307]}, + {"t":2.63498, "x":2.00588, "y":4.08384, "heading":1.5708, "vx":3.16159, "vy":0.23444, "omega":0.0, "ax":-6.33721, "ay":-0.46991, "alpha":0.0, "fx":[-107.79411,-107.79411,-107.79411,-107.79411], "fy":[-7.99307,-7.99307,-7.99307,-7.99307]}, + {"t":2.68423, "x":2.15392, "y":4.09482, "heading":1.5708, "vx":2.84945, "vy":0.21129, "omega":0.0, "ax":-6.40626, "ay":-0.47503, "alpha":0.0, "fx":[-108.96867,-108.96867,-108.96867,-108.96867], "fy":[-8.08016,-8.08016,-8.08016,-8.08016]}, + {"t":2.73349, "x":2.2865, "y":4.10465, "heading":1.5708, "vx":2.53391, "vy":0.18789, "omega":0.0, "ax":-6.42016, "ay":-0.47606, "alpha":0.0, "fx":[-109.20503,-109.20503,-109.20503,-109.20503], "fy":[-8.09769,-8.09769,-8.09769,-8.09769]}, + {"t":2.78274, "x":2.40352, "y":4.11333, "heading":1.5708, "vx":2.21769, "vy":0.16444, "omega":0.0, "ax":-6.42612, "ay":-0.4765, "alpha":0.0, "fx":[-109.30646,-109.30646,-109.30646,-109.30646], "fy":[-8.10521,-8.10521,-8.10521,-8.10521]}, + {"t":2.832, "x":2.50496, "y":4.12085, "heading":1.5708, "vx":1.90117, "vy":0.14097, "omega":0.0, "ax":-6.42943, "ay":-0.47675, "alpha":0.0, "fx":[-109.36284,-109.36284,-109.36284,-109.36284], "fy":[-8.10939,-8.10939,-8.10939,-8.10939]}, + {"t":2.88125, "x":2.5908, "y":4.12721, "heading":1.5708, "vx":1.58449, "vy":0.11749, "omega":0.0, "ax":-6.43154, "ay":-0.47691, "alpha":0.0, "fx":[-109.39873,-109.39873,-109.39873,-109.39873], "fy":[-8.11205,-8.11205,-8.11205,-8.11205]}, + {"t":2.93051, "x":2.66104, "y":4.13242, "heading":1.5708, "vx":1.2677, "vy":0.094, "omega":0.0, "ax":-6.43301, "ay":-0.47702, "alpha":0.0, "fx":[-109.42358,-109.42358,-109.42358,-109.42358], "fy":[-8.11389,-8.11389,-8.11389,-8.11389]}, + {"t":2.97976, "x":2.71568, "y":4.13647, "heading":1.5708, "vx":0.95084, "vy":0.07051, "omega":0.0, "ax":-6.43408, "ay":-0.47709, "alpha":0.0, "fx":[-109.4418,-109.4418,-109.4418,-109.4418], "fy":[-8.11524,-8.11524,-8.11524,-8.11524]}, + {"t":3.02902, "x":2.75471, "y":4.13937, "heading":1.5708, "vx":0.63393, "vy":0.04701, "omega":0.0, "ax":-6.4349, "ay":-0.47716, "alpha":0.0, "fx":[-109.45574,-109.45574,-109.45574,-109.45574], "fy":[-8.11628,-8.11628,-8.11628,-8.11628]}, + {"t":3.07827, "x":2.77813, "y":4.1411, "heading":1.5708, "vx":0.31698, "vy":0.0235, "omega":0.0, "ax":-6.43554, "ay":-0.4772, "alpha":0.0, "fx":[-109.46674,-109.46674,-109.46674,-109.46674], "fy":[-8.11709,-8.11709,-8.11709,-8.11709]}, + {"t":3.12753, "x":2.78593, "y":4.14168, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4.traj new file mode 100644 index 00000000..c7c6caab --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4.traj @@ -0,0 +1,134 @@ +{ + "name":"E_RIGHT_PATH_4", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.2129878997802734, "y":3.852599859237671, "heading":1.5707963267948966, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.9559978246688845, "y":5.059605598449707, "heading":0.7626747895755805, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2163536548614502, "y":5.85614538192749, "heading":0.7626747895755805, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3943861722946167, "y":4.593503475189209, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.108211040496826, "y":4.333054065704346, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.2129878997802734 m", "val":3.2129878997802734}, "y":{"exp":"3.852599859237671 m", "val":3.852599859237671}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.9559978246688843 m", "val":1.9559978246688845}, "y":{"exp":"5.059605598449707 m", "val":5.059605598449707}, "heading":{"exp":"0.7626747895755805 rad", "val":0.7626747895755805}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2163536548614502 m", "val":1.2163536548614502}, "y":{"exp":"5.85614538192749 m", "val":5.85614538192749}, "heading":{"exp":"0.7626747895755805 rad", "val":0.7626747895755805}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3943861722946167 m", "val":1.3943861722946167}, "y":{"exp":"4.593503475189209 m", "val":4.593503475189209}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.108211040496826 m", "val":2.108211040496826}, "y":{"exp":"4.333054065704346 m", "val":4.333054065704346}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.75905,1.33983,2.09545,2.38656], + "samples":[ + {"t":0.0, "x":3.21299, "y":3.8526, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.50364, "ay":4.27131, "alpha":-5.67171, "fx":[-44.63767,-78.98624,-102.05734,-80.74092], "fy":[100.19567,76.17034,40.20774,74.04096]}, + {"t":0.0253, "x":3.21155, "y":3.85397, "heading":1.5708, "vx":-0.11395, "vy":0.10807, "omega":-0.1435, "ax":-4.50731, "ay":4.27482, "alpha":-5.60813, "fx":[-45.10442,-78.98909,-101.86946,-80.70915], "fy":[99.97747,76.15804,40.6597,74.05891]}, + {"t":0.0506, "x":3.20722, "y":3.85807, "heading":1.56717, "vx":-0.22799, "vy":0.21623, "omega":-0.2854, "ax":-4.51174, "ay":4.27807, "alpha":-5.53919, "fx":[-45.56777,-78.9135,-101.63991,-80.85234], "fy":[99.75701,76.22607,41.20726,73.88462]}, + {"t":0.0759, "x":3.20001, "y":3.86491, "heading":1.55994, "vx":-0.34214, "vy":0.32447, "omega":-0.42555, "ax":-4.51686, "ay":4.28117, "alpha":-5.46412, "fx":[-46.04013,-78.76291,-101.36584,-81.15294], "fy":[99.52841,76.37024,41.85259,73.53496]}, + {"t":0.10121, "x":3.18991, "y":3.87449, "heading":1.54918, "vx":-0.45643, "vy":0.43279, "omega":-0.5638, "ax":-4.52259, "ay":4.28428, "alpha":-5.38215, "fx":[-46.53688,-78.54124,-101.04341,-81.59008], "fy":[99.28409,76.58548,42.59915,73.02862]}, + {"t":0.12651, "x":3.17691, "y":3.88681, "heading":1.53491, "vx":-0.57086, "vy":0.54119, "omega":-0.69997, "ax":-4.52884, "ay":4.28753, "alpha":-5.29233, "fx":[-47.07691,-78.25313,-100.66762,-82.13959], "fy":[99.01431,76.86561,43.45187,72.38714]}, + {"t":0.15181, "x":3.16102, "y":3.90188, "heading":1.5172, "vx":-0.68544, "vy":0.64967, "omega":-0.83388, "ax":-4.53555, "ay":4.29112, "alpha":-5.19328, "fx":[-47.68331,-77.90422,-100.23202,-82.77387], "fy":[98.70671,77.20317,44.41732,71.63602]}, + {"t":0.17711, "x":3.14222, "y":3.91969, "heading":1.4961, "vx":-0.8002, "vy":0.75824, "omega":-0.96528, "ax":-4.54264, "ay":4.29526, "alpha":-5.08291, "fx":[-48.38424,-77.50164,-99.72838,-83.46181], "fy":[98.34548,77.58905,45.50397,70.80595]}, + {"t":0.20241, "x":3.12052, "y":3.94025, "heading":1.47168, "vx":-0.91513, "vy":0.86692, "omega":-1.09388, "ax":-4.5501, "ay":4.30018, "alpha":-4.95804, "fx":[-49.21398,-77.05466,-99.14621,-84.16868], "fy":[97.91026,78.01206,46.72264,69.93406]}, + {"t":0.22771, "x":3.09591, "y":3.96356, "heading":1.444, "vx":-1.03026, "vy":0.97572, "omega":-1.21933, "ax":-4.55795, "ay":4.30615, "alpha":-4.81399, "fx":[-50.21432,-76.57554,-98.47196,-84.8559], "fy":[97.37467,78.45818,48.08715,69.06524]}, + {"t":0.25302, "x":3.06838, "y":3.98962, "heading":1.41315, "vx":-1.14558, "vy":1.08467, "omega":-1.34113, "ax":-4.5663, "ay":4.31345, "alpha":-4.64404, "fx":[-51.43611,-76.08096,-97.68788,-85.48056], "fy":[96.70393,78.90944,49.61531,68.25341]}, + {"t":0.27832, "x":3.03794, "y":4.01845, "heading":1.37922, "vx":-1.26112, "vy":1.19381, "omega":-1.45863, "ax":-4.57532, "ay":4.32235, "alpha":-4.43879, "fx":[-52.94113,-75.59402,-96.77013,-85.99457], "fy":[95.85169,79.34219,51.33061,67.56292]}, + {"t":0.30362, "x":3.00457, "y":4.05004, "heading":1.34231, "vx":-1.37688, "vy":1.30317, "omega":-1.57094, "ax":-4.58532, "ay":4.33303, "alpha":-4.18538, "fx":[-54.80406,-75.14754,-95.68575,-86.34281], "fy":[94.75519,79.72416,53.26484,67.07007]}, + {"t":0.32892, "x":2.96826, "y":4.0844, "heading":1.30257, "vx":-1.49289, "vy":1.4128, "omega":-1.67683, "ax":-4.59665, "ay":4.34554, "alpha":-3.86642, "fx":[-57.11417,-74.78934,-94.38729,-86.45983], "fy":[93.32824,80.00946,55.46269,66.86515]}, + {"t":0.35422, "x":2.92902, "y":4.12153, "heading":1.26014, "vx":-1.6092, "vy":1.52275, "omega":-1.77466, "ax":-4.60964, "ay":4.35965, "alpha":-3.45846, "fx":[-59.97604,-74.59139,-92.80305,-86.26396], "fy":[91.451,80.12939,57.98963,67.05531]}, + {"t":0.37952, "x":2.88683, "y":4.16146, "heading":1.21524, "vx":-1.72583, "vy":1.63306, "omega":-1.86216, "ax":-4.6244, "ay":4.37463, "alpha":-2.9295, "fx":[-63.50747,-74.66632,-90.818,-85.64696], "fy":[88.95522,79.97442,60.94657,67.76882]}, + {"t":0.40482, "x":2.84168, "y":4.20418, "heading":1.16812, "vx":-1.84283, "vy":1.74374, "omega":-1.93629, "ax":-4.6403, "ay":4.38894, "alpha":-2.23423, "fx":[-67.8313,-75.19966,-88.23341,-84.45616], "fy":[85.60372,79.35569,64.49787,69.16135]}, + {"t":0.43013, "x":2.79357, "y":4.2497, "heading":1.11913, "vx":-1.96024, "vy":1.85479, "omega":-1.99282, "ax":-4.65481, "ay":4.39939, "alpha":-1.30317, "fx":[-73.05417,-76.51779,-84.67262,-82.46306], "fy":[81.0642,77.91282,68.92853,71.42391]}, + {"t":0.45543, "x":2.74248, "y":4.29804, "heading":1.06871, "vx":-2.07801, "vy":1.9661, "omega":-2.02579, "ax":-4.66063, "ay":4.39891, "alpha":-0.01295, "fx":[-79.21978,-79.24363,-79.33203,-79.30831], "fy":[74.8831,74.85932,74.76525,74.78895]}, + {"t":0.48073, "x":2.68841, "y":4.34919, "heading":1.01746, "vx":-2.19593, "vy":2.0774, "omega":-2.02612, "ax":-4.63712, "ay":4.36741, "alpha":1.91326, "fx":[-86.21377,-84.66916,-70.21149,-74.40983], "fy":[66.48699,68.13232,83.01784,79.51659]}, + {"t":0.50603, "x":2.63137, "y":4.40315, "heading":0.96619, "vx":-2.31326, "vy":2.1879, "omega":-1.97771, "ax":-4.51329, "ay":4.22147, "alpha":5.30616, "fx":[-93.57007,-95.27792,-51.3687,-66.86269], "fy":[55.34414,50.87141,95.22652,85.78187]}, + {"t":0.53133, "x":2.57139, "y":4.45986, "heading":0.91615, "vx":-2.42745, "vy":2.29471, "omega":-1.84345, "ax":-4.01954, "ay":3.65258, "alpha":11.96905, "fx":[-99.509,-106.58796,-10.87232,-56.51561], "fy":[43.28554,5.82126,106.71307,92.69778]}, + {"t":0.55663, "x":2.50869, "y":4.51909, "heading":0.86951, "vx":-2.52915, "vy":2.38713, "omega":-1.54062, "ax":-3.68968, "ay":3.50021, "alpha":14.02274, "fx":[-99.03489,-105.50088,4.56931,-51.07515], "fy":[43.46178,-6.9442,106.24166,95.39097]}, + {"t":0.58194, "x":2.44352, "y":4.58061, "heading":0.83053, "vx":-2.62251, "vy":2.47569, "omega":-1.18582, "ax":-3.39874, "ay":3.41377, "alpha":15.24983, "fx":[-97.73372,-103.21354,15.88278,-46.18168], "fy":[44.77698,-13.31198,103.65582,97.14842]}, + {"t":0.60724, "x":2.37608, "y":4.64434, "heading":0.80053, "vx":-2.7085, "vy":2.56206, "omega":-0.79998, "ax":-3.23418, "ay":3.33612, "alpha":15.45209, "fx":[-95.9605,-99.74131,19.16599,-43.51435], "fy":[45.26911,-14.98512,99.86072,96.84114]}, + {"t":0.63254, "x":2.30651, "y":4.71023, "heading":0.78029, "vx":-2.79033, "vy":2.64647, "omega":-0.40902, "ax":-2.8929, "ay":3.22849, "alpha":14.43973, "fx":[-89.71819,-88.33191,19.81289,-38.59243], "fy":[46.06867,-9.0503,89.22075,93.42397]}, + {"t":0.65784, "x":2.23499, "y":4.77822, "heading":0.76994, "vx":-2.86353, "vy":2.72816, "omega":-0.04367, "ax":1.05939, "ay":1.68317, "alpha":1.3475, "fx":[12.82141,18.40826,23.13025,17.71975], "fy":[29.18184,23.83731,28.1888,33.31307]}, + {"t":0.68314, "x":2.16287, "y":4.84779, "heading":0.76883, "vx":-2.83672, "vy":2.77074, "omega":-0.00957, "ax":1.92352, "ay":1.86992, "alpha":-0.14708, "fx":[33.2563,32.63514,32.17878,32.80354], "fy":[31.70565,32.34682,31.90982,31.26494]}, + {"t":0.70844, "x":2.09172, "y":4.91849, "heading":0.76859, "vx":-2.78805, "vy":2.81806, "omega":-0.0133, "ax":4.01755, "ay":-0.28008, "alpha":-8.19216, "fx":[87.08798,70.70732,48.33855,67.21551], "fy":[-4.22615,36.75982,-6.80226,-44.78787]}, + {"t":0.73374, "x":2.02246, "y":4.9897, "heading":0.76826, "vx":-2.6864, "vy":2.81097, "omega":-0.22057, "ax":4.71315, "ay":-3.8059, "alpha":4.35366, "fx":[65.22046,68.84746,91.09306,95.51614], "fy":[-81.25402,-80.63349,-54.49669,-42.56499]}, + {"t":0.75905, "x":1.956, "y":5.05961, "heading":0.76267, "vx":-2.56715, "vy":2.71468, "omega":-0.11042, "ax":4.65411, "ay":-4.32127, "alpha":1.62441, "fx":[73.36452,73.72146,84.18313,85.39107], "fy":[-79.56035,-79.54449,-68.39403,-66.51522]}, + {"t":0.79321, "x":1.87101, "y":5.14983, "heading":0.7589, "vx":-2.40815, "vy":2.56704, "omega":-0.05492, "ax":4.53713, "ay":-4.51305, "alpha":0.8577, "fx":[74.0051,74.12854,80.1042,80.46347], "fy":[-79.8963,-79.88287,-73.89213,-73.39164]}, + {"t":0.82737, "x":1.79139, "y":5.23489, "heading":0.75703, "vx":-2.25314, "vy":2.41286, "omega":-0.02562, "ax":4.48212, "ay":-4.59365, "alpha":0.52913, "fx":[74.25739,74.31532,78.12622,78.25907], "fy":[-80.04657,-80.03755,-76.32285,-76.1397]}, + {"t":0.86154, "x":1.71702, "y":5.31465, "heading":0.75615, "vx":-2.10002, "vy":2.25592, "omega":-0.00754, "ax":4.45036, "ay":-4.63788, "alpha":0.34652, "fx":[74.39175,74.42349,76.96514,77.01686], "fy":[-80.13163,-80.12501,-77.687,-77.61215]}, + {"t":0.8957, "x":1.64788, "y":5.38901, "heading":0.75589, "vx":-1.94798, "vy":2.09747, "omega":0.0043, "ax":4.42972, "ay":-4.66579, "alpha":0.23029, "fx":[74.47519,74.49417,76.20284,76.22117], "fy":[-80.18621,-80.18102,-78.55898,-78.52849]}, + {"t":0.92987, "x":1.58391, "y":5.45795, "heading":0.75604, "vx":-1.79664, "vy":1.93807, "omega":0.01217, "ax":4.41525, "ay":-4.68499, "alpha":0.14981, "fx":[74.53216,74.54393,75.66428,75.66844], "fy":[-80.22407,-80.21999,-79.16413,-79.15321]}, + {"t":0.96403, "x":1.52511, "y":5.52142, "heading":0.75646, "vx":-1.6458, "vy":1.77801, "omega":0.01728, "ax":4.40455, "ay":-4.69901, "alpha":0.09079, "fx":[74.57365,74.58074,75.26361,75.26241], "fy":[-80.25172,-80.24874,-79.60863,-79.60613]}, + {"t":0.99819, "x":1.47145, "y":5.57943, "heading":0.75705, "vx":-1.49532, "vy":1.61748, "omega":0.02039, "ax":4.39631, "ay":-4.70969, "alpha":0.04565, "fx":[74.60534,74.60898,74.95385,74.95171], "fy":[-80.2727,-80.27092,-79.94897,-79.94938]}, + {"t":1.03236, "x":1.42293, "y":5.63194, "heading":0.75774, "vx":-1.34512, "vy":1.45658, "omega":0.02195, "ax":4.38977, "ay":-4.7181, "alpha":0.01002, "fx":[74.63042,74.63124,74.70717,74.70641], "fy":[-80.28908,-80.28862,-80.21797,-80.21837]}, + {"t":1.06652, "x":1.37954, "y":5.67895, "heading":0.75849, "vx":-1.19515, "vy":1.29539, "omega":0.02229, "ax":4.38446, "ay":-4.72489, "alpha":-0.01883, "fx":[74.65082,74.64919,74.50603,74.50792], "fy":[-80.30215,-80.30313,-80.43597,-80.43476]}, + {"t":1.10069, "x":1.34127, "y":5.72044, "heading":0.75925, "vx":-1.04536, "vy":1.13396, "omega":0.02164, "ax":4.38007, "ay":-4.73049, "alpha":-0.04266, "fx":[74.66779,74.66391,74.33887,74.34407], "fy":[-80.31278,-80.31529,-80.61625,-80.61255]}, + {"t":1.13485, "x":1.30811, "y":5.75642, "heading":0.75999, "vx":-0.89572, "vy":0.97235, "omega":0.02019, "ax":4.37636, "ay":-4.73518, "alpha":-0.06267, "fx":[74.68215,74.67617,74.19771,74.20655], "fy":[-80.32157,-80.32563,-80.76782,-80.76119]}, + {"t":1.16901, "x":1.28006, "y":5.78688, "heading":0.76068, "vx":-0.74621, "vy":0.81058, "omega":0.01805, "ax":4.3732, "ay":-4.73917, "alpha":-0.07972, "fx":[74.69448,74.68654,74.07694,74.0895], "fy":[-80.32895,-80.33456,-80.89705,-80.88729]}, + {"t":1.20318, "x":1.25712, "y":5.81181, "heading":0.7613, "vx":-0.5968, "vy":0.64867, "omega":0.01532, "ax":4.37047, "ay":-4.74261, "alpha":-0.09441, "fx":[74.70517,74.69542,73.97242,73.98869], "fy":[-80.33522,-80.34234,-81.00854,-80.99561]}, + {"t":1.23734, "x":1.23928, "y":5.8312, "heading":0.76182, "vx":-0.44749, "vy":0.48664, "omega":0.0121, "ax":4.36809, "ay":-4.7456, "alpha":-0.10721, "fx":[74.71452,74.70311,73.88111,73.90093], "fy":[-80.34062,-80.34918,-81.10568,-81.08966]}, + {"t":1.27151, "x":1.22654, "y":5.84506, "heading":0.76224, "vx":-0.29826, "vy":0.32452, "omega":0.00843, "ax":4.36599, "ay":-4.74822, "alpha":-0.11846, "fx":[74.72276,74.70986,73.80066,73.82384], "fy":[-80.34535,-80.3552,-81.19107,-81.17212]}, + {"t":1.30567, "x":1.2189, "y":5.85337, "heading":0.76252, "vx":-0.1491, "vy":0.1623, "omega":0.00439, "ax":4.36413, "ay":-4.75055, "alpha":-0.12842, "fx":[74.73004,74.71585,73.72927,73.75555], "fy":[-80.34953,-80.36054,-81.2667,-81.24501]}, + {"t":1.33983, "x":1.21635, "y":5.85615, "heading":0.76267, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.39275, "ay":-5.98084, "alpha":1.06363, "fx":[-47.26881,-38.36626,-33.71734,-43.44735], "fy":[-99.00968,-102.79341,-104.39714,-100.7292]}, + {"t":1.37006, "x":1.21526, "y":5.85341, "heading":0.76267, "vx":-0.07232, "vy":-0.18077, "omega":0.03215, "ax":-2.29039, "ay":-6.01918, "alpha":1.11341, "fx":[-45.92217,-36.62239,-31.55577,-41.73526], "fy":[-99.6284,-103.41598,-105.05777,-101.43574]}, + {"t":1.40028, "x":1.21203, "y":5.8452, "heading":0.76365, "vx":-0.14155, "vy":-0.3627, "omega":0.0658, "ax":-2.16927, "ay":-6.06197, "alpha":1.17147, "fx":[-44.32655,-34.56732,-29.00209,-39.69883], "fy":[-100.33337,-104.10756,-105.77618,-102.23256]}, + {"t":1.43051, "x":1.20676, "y":5.83147, "heading":0.76564, "vx":-0.20711, "vy":-0.54592, "omega":0.10121, "ax":-2.02392, "ay":-6.10973, "alpha":1.23994, "fx":[-42.40915,-32.11544,-25.9438,-37.23698], "fy":[-101.14097,-104.8743,-106.54992,-103.13419]}, + {"t":1.46073, "x":1.19958, "y":5.81218, "heading":0.76869, "vx":-0.26828, "vy":-0.73058, "omega":0.13868, "ax":-1.84663, "ay":-6.16281, "alpha":1.3217, "fx":[-40.06653,-29.14814,-22.22407,-34.20381], "fy":[-102.07044,-105.71908,-107.36651,-104.15469]}, + {"t":1.49096, "x":1.19062, "y":5.78728, "heading":0.77289, "vx":-0.3241, "vy":-0.91685, "omega":0.17863, "ax":-1.62623, "ay":-6.22107, "alpha":1.42069, "fx":[-37.14733,-25.49729,-17.61999,-30.38228], "fy":[-103.14301,-106.63652,-108.1923,-105.30262]}, + {"t":1.52118, "x":1.18008, "y":5.75673, "heading":0.77829, "vx":-0.37325, "vy":-1.10488, "omega":0.22157, "ax":-1.34613, "ay":-6.28311, "alpha":1.54233, "fx":[-33.42245,-20.91905,-11.80893,-25.43852], "fy":[-104.37783,-107.60171,-108.9491,-106.56713]}, + {"t":1.55141, "x":1.16819, "y":5.72046, "heading":0.78498, "vx":-0.41394, "vy":-1.29479, "omega":0.26819, "ax":-0.98095, "ay":-6.34446, "alpha":1.69414, "fx":[-28.53163,-15.05175,-4.31872,-18.84071], "fy":[-105.77943,-108.54522,-109.46413,-107.8809]}, + {"t":1.58163, "x":1.15523, "y":5.67843, "heading":0.79309, "vx":-0.44359, "vy":-1.48655, "omega":0.31939, "ax":-0.49111, "ay":-6.39306, "alpha":1.88637, "fx":[-21.88482,-7.34991,5.53631,-9.71608], "fy":[-107.30013,-109.29602,-109.36167,-109.01876]}, + {"t":1.61186, "x":1.1416, "y":5.63058, "heading":0.80274, "vx":-0.45843, "vy":-1.67977, "omega":0.37641, "ax":0.18519, "ay":-6.39818, "alpha":2.13236, "fx":[-12.47824,3.00753,18.69429,3.3765], "fy":[-108.7273,-109.44928,-107.83392,-109.31416]}, + {"t":1.64208, "x":1.12783, "y":5.57689, "heading":0.81412, "vx":-0.45283, "vy":-1.87316, "omega":0.44086, "ax":1.13772, "ay":-6.28325, "alpha":2.44753, "fx":[1.42126,17.18871,36.17712,22.62192], "fy":[-109.33796,-108.06577,-103.21483,-106.88653]}, + {"t":1.67231, "x":1.11466, "y":5.5174, "heading":0.82744, "vx":-0.41844, "vy":-2.06307, "omega":0.51483, "ax":2.45728, "ay":-5.87139, "alpha":2.83748, "fx":[22.56228,36.49492,58.16566,49.96743], "fy":[-106.86443,-103.07395,-92.53393,-97.0103]}, + {"t":1.70253, "x":1.10313, "y":5.45236, "heading":0.843, "vx":-0.34417, "vy":-2.24053, "omega":0.6006, "ax":4.08108, "ay":-4.85798, "alpha":3.19807, "fx":[53.26103,60.98699,81.75889,81.66515], "fy":[-95.18537,-90.65764,-72.45336,-72.23474]}, + {"t":1.73275, "x":1.0946, "y":5.38243, "heading":0.86116, "vx":-0.22083, "vy":-2.38736, "omega":0.69726, "ax":5.54338, "ay":-3.08774, "alpha":3.14523, "fx":[87.18131,86.23267,100.12374,103.62775], "fy":[-65.40714,-67.01831,-43.69015,-33.97068]}, + {"t":1.76298, "x":1.09045, "y":5.30886, "heading":0.88223, "vx":-0.05328, "vy":-2.48068, "omega":0.79232, "ax":6.2844, "ay":-1.03281, "alpha":2.65085, "fx":[106.48116,103.48421,108.45805,109.1597], "fy":[-23.56398,-34.96578,-13.47907,1.73788]}, + {"t":1.7932, "x":1.09171, "y":5.23341, "heading":0.90618, "vx":0.13667, "vy":-2.5119, "omega":0.87244, "ax":6.35976, "ay":0.65223, "alpha":2.11451, "fx":[108.66671,109.21968,108.76979,106.05478], "fy":[10.80604,-4.27865,11.41099,26.43853]}, + {"t":1.82343, "x":1.09875, "y":5.15779, "heading":0.93255, "vx":0.32889, "vy":-2.49219, "omega":0.93635, "ax":6.14817, "ay":1.81712, "alpha":1.6653, "fx":[104.28995,107.71053,105.37731,100.93677], "fy":[32.82393,19.07901,29.53628,42.19572]}, + {"t":1.85365, "x":1.1115, "y":5.08329, "heading":0.96085, "vx":0.51471, "vy":-2.43726, "omega":0.98669, "ax":5.87556, "ay":2.59498, "alpha":1.31592, "fx":[99.06293,103.60041,101.00111,96.10179], "fy":[46.49368,35.33542,42.29321,52.43714]}, + {"t":1.88388, "x":1.12974, "y":5.01081, "heading":0.99067, "vx":0.6923, "vy":-2.35883, "omega":1.02646, "ax":5.6204, "ay":3.12562, "alpha":1.05078, "fx":[94.48237,99.12861,96.76343,92.03084], "fy":[55.35084,46.56193,51.35512,59.39585]}, + {"t":1.9141, "x":1.15323, "y":4.94094, "heading":1.0217, "vx":0.86218, "vy":-2.26436, "omega":1.05822, "ax":5.40098, "ay":3.50104, "alpha":0.84852, "fx":[90.7251,95.0468,93.00748,88.69731], "fy":[61.40905,54.50388,57.95292,64.34083]}, + {"t":1.94433, "x":1.18176, "y":4.8741, "heading":1.05368, "vx":1.02542, "vy":-2.15854, "omega":1.08386, "ax":5.21675, "ay":3.77656, "alpha":0.69153, "fx":[87.67247,91.51852,89.77931,85.97132], "fy":[65.76004,60.30638,62.89173,67.99441]}, + {"t":1.97455, "x":1.21513, "y":4.81059, "heading":1.08644, "vx":1.18309, "vy":-2.0444, "omega":1.10477, "ax":5.06247, "ay":3.98547, "alpha":0.56719, "fx":[85.17388,88.51606,87.02905,83.72536], "fy":[69.015,64.68183,66.68686,70.78309]}, + {"t":2.00478, "x":1.2532, "y":4.75062, "heading":1.11983, "vx":1.3361, "vy":-1.92394, "omega":1.12191, "ax":4.93258, "ay":4.14837, "alpha":0.46679, "fx":[83.10303,85.96312,84.68506,81.85585], "fy":[71.53316,68.07575,69.67186,72.96975]}, + {"t":2.035, "x":1.29584, "y":4.69436, "heading":1.15374, "vx":1.48519, "vy":-1.79856, "omega":1.13602, "ax":4.82234, "ay":4.27845, "alpha":0.38428, "fx":[81.36331,83.78125,82.67878,80.28301], "fy":[73.53612,70.77357,72.06756,74.72327]}, + {"t":2.06523, "x":1.34293, "y":4.64195, "heading":1.18808, "vx":1.63094, "vy":-1.66924, "omega":1.14763, "ax":4.72793, "ay":4.38441, "alpha":0.31542, "fx":[79.88253,81.90271,80.95164,78.94616], "fy":[75.16659,72.96352,74.02399,76.15619]}, + {"t":2.09545, "x":1.39439, "y":4.5935, "heading":1.22276, "vx":1.77384, "vy":-1.53672, "omega":1.15717, "ax":4.67696, "ay":4.42224, "alpha":0.29427, "fx":[79.08057,80.95866,80.02182,78.15394], "fy":[75.72361,73.71927,74.7482,76.6928]}, + {"t":2.10931, "x":1.41943, "y":4.57263, "heading":1.2388, "vx":1.83868, "vy":-1.47542, "omega":1.16125, "ax":4.67594, "ay":4.42172, "alpha":0.29284, "fx":[79.08821,80.94101,79.9805,78.13577], "fy":[75.68802,73.71085,74.76547,76.68456]}, + {"t":2.12318, "x":1.44536, "y":4.5526, "heading":1.2549, "vx":1.9035, "vy":-1.41413, "omega":1.1653, "ax":4.67481, "ay":4.42115, "alpha":0.29127, "fx":[79.09419,80.92053,79.93727,78.11688], "fy":[75.65118,73.70253,74.78186,76.67416]}, + {"t":2.13704, "x":1.4722, "y":4.53342, "heading":1.27105, "vx":1.9683, "vy":-1.35284, "omega":1.16934, "ax":4.67356, "ay":4.4205, "alpha":0.28956, "fx":[79.09821,80.89684,79.89186,78.09709], "fy":[75.61288,73.69422,74.79713,76.66129]}, + {"t":2.1509, "x":1.49993, "y":4.51509, "heading":1.28726, "vx":2.03309, "vy":-1.29156, "omega":1.17336, "ax":4.67218, "ay":4.41976, "alpha":0.28768, "fx":[79.09991,80.8695,79.84395,78.07616], "fy":[75.5729,73.68577,74.81103,76.64558]}, + {"t":2.16476, "x":1.52856, "y":4.49761, "heading":1.30353, "vx":2.09785, "vy":-1.23029, "omega":1.17734, "ax":4.67062, "ay":4.41891, "alpha":0.28561, "fx":[79.09881,80.83792,79.79311,78.0538], "fy":[75.53091,73.677,74.8232,76.62655]}, + {"t":2.17862, "x":1.55809, "y":4.48098, "heading":1.31985, "vx":2.1626, "vy":-1.16904, "omega":1.1813, "ax":4.66887, "ay":4.41793, "alpha":0.28331, "fx":[79.09435,80.80141,79.73884,78.02963], "fy":[75.48651,73.66767,74.83321,76.60361]}, + {"t":2.19249, "x":1.58852, "y":4.4652, "heading":1.33623, "vx":2.22732, "vy":-1.10779, "omega":1.18523, "ax":4.66687, "ay":4.41679, "alpha":0.28075, "fx":[79.08578,80.75905,79.68045,78.00316], "fy":[75.43917,73.65744,74.84049,76.57599]}, + {"t":2.20635, "x":1.61985, "y":4.45027, "heading":1.35266, "vx":2.29201, "vy":-1.04657, "omega":1.18912, "ax":4.66458, "ay":4.41543, "alpha":0.27788, "fx":[79.07212,80.70966,79.61705,77.97371], "fy":[75.38818,73.64586,74.84428,76.54268]}, + {"t":2.22021, "x":1.65207, "y":4.43618, "heading":1.36914, "vx":2.35668, "vy":-0.98536, "omega":1.19297, "ax":4.66192, "ay":4.41381, "alpha":0.27464, "fx":[79.05209,80.65165,79.54743,77.94039], "fy":[75.33256,73.63232,74.84357,76.50232]}, + {"t":2.23407, "x":1.68518, "y":4.42295, "heading":1.38568, "vx":2.4213, "vy":-0.92417, "omega":1.19678, "ax":4.65879, "ay":4.41184, "alpha":0.27093, "fx":[79.02391,80.58288,79.46995,77.90195], "fy":[75.27096,73.61589,74.83697,76.45306]}, + {"t":2.24794, "x":1.7192, "y":4.41056, "heading":1.40227, "vx":2.48588, "vy":-0.86302, "omega":1.20054, "ax":4.65505, "ay":4.40941, "alpha":0.26665, "fx":[78.98509,80.50033,79.38226,77.85659], "fy":[75.20142,73.59528,74.82246,76.39228]}, + {"t":2.2618, "x":1.7541, "y":4.39902, "heading":1.41891, "vx":2.55041, "vy":-0.80189, "omega":1.20423, "ax":4.65049, "ay":4.40635, "alpha":0.26163, "fx":[78.93202,80.39961,79.2809,77.80168], "fy":[75.12106,73.5685,74.79712,76.31614]}, + {"t":2.27566, "x":1.7899, "y":4.38833, "heading":1.4356, "vx":2.61488, "vy":-0.74081, "omega":1.20786, "ax":4.6448, "ay":4.40238, "alpha":0.25562, "fx":[78.85918,80.27409,79.16062,77.73313], "fy":[75.0254,73.53242,74.75642,76.2188]}, + {"t":2.28952, "x":1.8266, "y":4.37848, "heading":1.45235, "vx":2.67927, "vy":-0.67978, "omega":1.2114, "ax":4.63747, "ay":4.39709, "alpha":0.24823, "fx":[78.75777,80.11315,79.01295,77.64434], "fy":[74.90719,73.4819,74.69304,76.09087]}, + {"t":2.30339, "x":1.86419, "y":4.36948, "heading":1.46914, "vx":2.74355, "vy":-0.61883, "omega":1.21485, "ax":4.62763, "ay":4.38973, "alpha":0.23884, "fx":[78.61273,79.8987,78.82328,77.52386], "fy":[74.75383,73.40788,74.59433,75.91622]}, + {"t":2.31725, "x":1.90266, "y":4.36133, "heading":1.48598, "vx":2.8077, "vy":-0.55798, "omega":1.21816, "ax":4.61364, "ay":4.3789, "alpha":0.22625, "fx":[78.39586,79.59689,78.56416,77.35005], "fy":[74.54152,73.29302,74.4364,75.66461]}, + {"t":2.33111, "x":1.94203, "y":4.35401, "heading":1.50287, "vx":2.87166, "vy":-0.49727, "omega":1.22129, "ax":4.59204, "ay":4.36161, "alpha":0.20801, "fx":[78.04743,79.13606,78.17696,77.07654], "fy":[74.21938,73.09976,74.16805,75.27171]}, + {"t":2.34497, "x":1.98228, "y":4.34754, "heading":1.5198, "vx":2.93531, "vy":-0.43681, "omega":1.22418, "ax":4.55387, "ay":4.3301, "alpha":0.17779, "fx":[77.41448,78.33234,77.51013,76.58327], "fy":[73.65616,72.73,73.65658,74.57209]}, + {"t":2.35883, "x":2.0234, "y":4.3419, "heading":1.53677, "vx":2.99844, "vy":-0.37679, "omega":1.22664, "ax":4.46692, "ay":4.25632, "alpha":0.11265, "fx":[75.95031,76.5254,76.01341,75.43474], "fy":[72.38044,71.82501,72.41896,72.97067]}, + {"t":2.3727, "x":2.0654, "y":4.33708, "heading":1.55377, "vx":3.06036, "vy":-0.31779, "omega":1.2282, "ax":4.05866, "ay":3.90166, "alpha":-0.19203, "fx":[69.15819,68.17478,68.91872,69.89494], "fy":[66.48761,67.25855,66.24813,65.47049]}, + {"t":2.38656, "x":2.10821, "y":4.33305, "heading":1.5708, "vx":3.11662, "vy":-0.2637, "omega":1.22554, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4_BACK.traj b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4_BACK.traj new file mode 100644 index 00000000..8c1b69d1 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/E_RIGHT_PATH_4_BACK.traj @@ -0,0 +1,143 @@ +{ + "name":"E_RIGHT_PATH_4_BACK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.1932778358459473, "y":4.194243907928467, "heading":1.5707963267948966, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.9559978246688845, "y":5.059605598449707, "heading":0.7626747895755805, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2163536548614502, "y":5.85614538192749, "heading":0.7626747895755805, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.812213182449341, "y":4.470187187194824, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.1932778358459473 m", "val":3.1932778358459473}, "y":{"exp":"4.194243907928467 m", "val":4.194243907928467}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.9559978246688843 m", "val":1.9559978246688845}, "y":{"exp":"5.059605598449707 m", "val":5.059605598449707}, "heading":{"exp":"0.7626747895755805 rad", "val":0.7626747895755805}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2163536548614502 m", "val":1.2163536548614502}, "y":{"exp":"5.85614538192749 m", "val":5.85614538192749}, "heading":{"exp":"0.7626747895755805 rad", "val":0.7626747895755805}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.812213182449341 m", "val":2.812213182449341}, "y":{"exp":"4.470187187194824 m", "val":4.470187187194824}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":2, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.58865,2.75503,3.90263], + "samples":[ + {"t":0.0, "x":3.19328, "y":4.19424, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.29244, "ay":3.68917, "alpha":-0.00642, "fx":[-90.00419,-90.01958,-90.04156,-90.02618], "fy":[62.77854,62.75655,62.72496,62.74695]}, + {"t":0.03695, "x":3.18967, "y":4.19676, "heading":1.5708, "vx":-0.19553, "vy":0.1363, "omega":-0.00024, "ax":-5.29042, "ay":3.68776, "alpha":-0.00765, "fx":[-89.9663,-89.98464,-90.0108,-89.99248], "fy":[62.75973,62.73355,62.69592,62.72209]}, + {"t":0.07389, "x":3.17883, "y":4.20431, "heading":1.57079, "vx":-0.39099, "vy":0.27254, "omega":-0.00052, "ax":-5.28638, "ay":3.68495, "alpha":-0.01011, "fx":[-89.89049,-89.91475,-89.94927,-89.92504], "fy":[62.72209,62.68755,62.63783,62.67237]}, + {"t":0.11084, "x":3.16078, "y":4.2169, "heading":1.57077, "vx":-0.58629, "vy":0.40868, "omega":-0.00089, "ax":-5.27427, "ay":3.67651, "alpha":-0.01751, "fx":[-89.66306,-89.70517,-89.76465,-89.72262], "fy":[62.6091,62.54958,62.46361,62.52313]}, + {"t":0.14778, "x":3.13552, "y":4.23451, "heading":1.57074, "vx":-0.78115, "vy":0.54451, "omega":-0.00154, "ax":-1.05522, "ay":0.73555, "alpha":-2.9612, "fx":[-12.46081,-23.03856,-23.53808,-12.7582], "fy":[18.28038,17.72011,6.90636,7.13933]}, + {"t":0.18473, "x":3.10594, "y":4.25513, "heading":1.57068, "vx":-0.82014, "vy":0.57169, "omega":-0.11094, "ax":-0.00001, "ay":0.00001, "alpha":-2.70066, "fx":[4.93808,-4.93731,-4.93848,4.93692], "fy":[4.93726,4.93842,-4.93698,-4.93814]}, + {"t":0.22167, "x":3.07564, "y":4.27625, "heading":1.56658, "vx":-0.82014, "vy":0.57169, "omega":-0.21072, "ax":0.0, "ay":0.0, "alpha":-2.27414, "fx":[4.17538,-4.14032,-4.17538,4.14032], "fy":[4.14032,4.17538,-4.14032,-4.17538]}, + {"t":0.25862, "x":3.04534, "y":4.29737, "heading":1.55879, "vx":-0.82014, "vy":0.57169, "omega":-0.29474, "ax":0.0, "ay":0.0, "alpha":-1.9136, "fx":[3.54042,-3.45644,-3.54042,3.45644], "fy":[3.45644,3.54042,-3.45644,-3.54042]}, + {"t":0.29556, "x":3.01504, "y":4.31849, "heading":1.54791, "vx":-0.82014, "vy":0.57169, "omega":-0.36544, "ax":0.0, "ay":0.0, "alpha":-1.60909, "fx":[3.00852,-2.87384,-3.00852,2.87384], "fy":[2.87384,3.00852,-2.87384,-3.00852]}, + {"t":0.33251, "x":2.98474, "y":4.33961, "heading":1.5344, "vx":-0.82014, "vy":0.57169, "omega":-0.42488, "ax":0.0, "ay":0.0, "alpha":-1.35206, "fx":[2.56032,-2.38044,-2.56032,2.38044], "fy":[2.38044,2.56032,-2.38044,-2.56032]}, + {"t":0.36945, "x":2.95444, "y":4.36073, "heading":1.51871, "vx":-0.82014, "vy":0.57169, "omega":-0.47484, "ax":0.0, "ay":0.0, "alpha":-1.13516, "fx":[2.18069,-1.96457,-2.18069,1.96457], "fy":[1.96457,2.18069,-1.96457,-2.18069]}, + {"t":0.4064, "x":2.92414, "y":4.38185, "heading":1.50116, "vx":-0.82014, "vy":0.57169, "omega":-0.51678, "ax":0.0, "ay":0.0, "alpha":-0.9521, "fx":[1.85764,-1.61541,-1.85764,1.61541], "fy":[1.61541,1.85764,-1.61541,-1.85764]}, + {"t":0.44334, "x":2.89384, "y":4.40297, "heading":1.48207, "vx":-0.82014, "vy":0.57169, "omega":-0.55195, "ax":0.0, "ay":0.0, "alpha":-0.79752, "fx":[1.58159,-1.32318,-1.58159,1.32318], "fy":[1.32318,1.58159,-1.32318,-1.58159]}, + {"t":0.48029, "x":2.86354, "y":4.4241, "heading":1.46168, "vx":-0.82014, "vy":0.57169, "omega":-0.58142, "ax":0.0, "ay":0.0, "alpha":-0.66686, "fx":[1.34477,-1.07922,-1.34477,1.07922], "fy":[1.07922,1.34477,-1.07922,-1.34477]}, + {"t":0.51723, "x":2.83324, "y":4.44522, "heading":1.4402, "vx":-0.82014, "vy":0.57169, "omega":-0.60605, "ax":0.0, "ay":0.0, "alpha":-0.55626, "fx":[1.14082,-0.87593,-1.14082,0.87593], "fy":[0.87593,1.14082,-0.87593,-1.14082]}, + {"t":0.55418, "x":2.80294, "y":4.46634, "heading":1.41781, "vx":-0.82014, "vy":0.57169, "omega":-0.6266, "ax":0.0, "ay":0.0, "alpha":-0.46242, "fx":[0.96443,-0.70674,-0.96443,0.70674], "fy":[0.70674,0.96443,-0.70674,-0.96443]}, + {"t":0.59112, "x":2.77264, "y":4.48746, "heading":1.39466, "vx":-0.82014, "vy":0.57169, "omega":-0.64369, "ax":0.0, "ay":0.0, "alpha":-0.38253, "fx":[0.81112,-0.56601,-0.81112,0.56601], "fy":[0.56601,0.81112,-0.56601,-0.81112]}, + {"t":0.62807, "x":2.74234, "y":4.50858, "heading":1.37088, "vx":-0.82014, "vy":0.57169, "omega":-0.65782, "ax":0.0, "ay":0.0, "alpha":-0.31419, "fx":[0.67708,-0.44892,-0.67708,0.44892], "fy":[0.44892,0.67708,-0.44892,-0.67708]}, + {"t":0.66501, "x":2.71204, "y":4.5297, "heading":1.34657, "vx":-0.82014, "vy":0.57169, "omega":-0.66943, "ax":0.0, "ay":0.0, "alpha":-0.25535, "fx":[0.55898,-0.35137,-0.55898,0.35137], "fy":[0.35137,0.55898,-0.35137,-0.55898]}, + {"t":0.70196, "x":2.68173, "y":4.55082, "heading":1.32184, "vx":-0.82014, "vy":0.57169, "omega":-0.67886, "ax":0.0, "ay":0.0, "alpha":-0.20423, "fx":[0.45389,-0.26988,-0.45389,0.26988], "fy":[0.26988,0.45389,-0.26988,-0.45389]}, + {"t":0.73891, "x":2.65143, "y":4.57194, "heading":1.29676, "vx":-0.82014, "vy":0.57169, "omega":-0.68641, "ax":0.0, "ay":0.0, "alpha":-0.15929, "fx":[0.35918,-0.20156,-0.35918,0.20156], "fy":[0.20156,0.35918,-0.20156,-0.35918]}, + {"t":0.77585, "x":2.62113, "y":4.59306, "heading":1.2714, "vx":-0.82014, "vy":0.57169, "omega":-0.69229, "ax":0.0, "ay":0.0, "alpha":-0.11918, "fx":[0.27247,-0.14394,-0.27247,0.14394], "fy":[0.14394,0.27247,-0.14394,-0.27247]}, + {"t":0.8128, "x":2.59083, "y":4.61419, "heading":1.24582, "vx":-0.82014, "vy":0.57169, "omega":-0.6967, "ax":0.0, "ay":0.0, "alpha":-0.08267, "fx":[0.1915,-0.09498,-0.1915,0.09498], "fy":[0.09498,0.1915,-0.09498,-0.1915]}, + {"t":0.84974, "x":2.56053, "y":4.63531, "heading":1.22008, "vx":-0.82014, "vy":0.57169, "omega":-0.69975, "ax":0.0, "ay":0.0, "alpha":-0.04867, "fx":[0.11415,-0.053,-0.11415,0.053], "fy":[0.053,0.11415,-0.053,-0.11415]}, + {"t":0.88669, "x":2.53023, "y":4.65643, "heading":1.19423, "vx":-0.82014, "vy":0.57169, "omega":-0.70155, "ax":0.0, "ay":0.0, "alpha":-0.01615, "fx":[0.03831,-0.0166,-0.03831,0.0166], "fy":[0.0166,0.03831,-0.0166,-0.03831]}, + {"t":0.92363, "x":2.49993, "y":4.67755, "heading":1.16831, "vx":-0.82014, "vy":0.57169, "omega":-0.70215, "ax":0.0, "ay":0.0, "alpha":0.01589, "fx":[-0.03811,0.01535,0.03811,-0.01535], "fy":[-0.01535,-0.03811,0.01535,0.03811]}, + {"t":0.96058, "x":2.46963, "y":4.69867, "heading":1.14237, "vx":-0.82014, "vy":0.57169, "omega":-0.70156, "ax":0.0, "ay":0.0, "alpha":0.04841, "fx":[-0.11727,0.04374,0.11727,-0.04374], "fy":[-0.04374,-0.11727,0.04374,0.11727]}, + {"t":0.99752, "x":2.43933, "y":4.71979, "heading":1.11645, "vx":-0.82014, "vy":0.57169, "omega":-0.69977, "ax":0.0, "ay":0.0, "alpha":0.08239, "fx":[-0.20147,0.06925,0.20147,-0.06925], "fy":[-0.06925,-0.20147,0.06925,0.20147]}, + {"t":1.03447, "x":2.40903, "y":4.74091, "heading":1.0906, "vx":-0.82014, "vy":0.57169, "omega":-0.69673, "ax":0.0, "ay":0.0, "alpha":0.11888, "fx":[-0.29317,0.09236,0.29317,-0.09236], "fy":[-0.09236,-0.29317,0.09236,0.29317]}, + {"t":1.07141, "x":2.37873, "y":4.76203, "heading":1.06486, "vx":-0.82014, "vy":0.57169, "omega":-0.69233, "ax":0.0, "ay":0.0, "alpha":0.15896, "fx":[-0.39508,0.11338,0.39508,-0.11338], "fy":[-0.11338,-0.39508,0.11338,0.39508]}, + {"t":1.10836, "x":2.34843, "y":4.78316, "heading":1.03928, "vx":-0.82014, "vy":0.57169, "omega":-0.68646, "ax":0.0, "ay":0.0, "alpha":0.20386, "fx":[-0.51022,0.13239,0.51022,-0.13239], "fy":[-0.13239,-0.51022,0.13239,0.51022]}, + {"t":1.1453, "x":2.31813, "y":4.80428, "heading":1.01392, "vx":-0.82014, "vy":0.57169, "omega":-0.67893, "ax":0.0, "ay":0.0, "alpha":0.25494, "fx":[-0.64204,0.14933,0.64204,-0.14933], "fy":[-0.14933,-0.64204,0.14933,0.64204]}, + {"t":1.18225, "x":2.28783, "y":4.8254, "heading":0.98884, "vx":-0.82014, "vy":0.57169, "omega":-0.66951, "ax":0.0, "ay":0.0, "alpha":0.31372, "fx":[-0.79444,0.16389,0.79444,-0.16389], "fy":[-0.16389,-0.79444,0.16389,0.79444]}, + {"t":1.21919, "x":2.25753, "y":4.84652, "heading":0.9641, "vx":-0.82014, "vy":0.57169, "omega":-0.65792, "ax":0.0, "ay":0.0, "alpha":0.38199, "fx":[-0.97196,0.17556,0.97196,-0.17556], "fy":[-0.17556,-0.97196,0.17556,0.97196]}, + {"t":1.25614, "x":2.22723, "y":4.86764, "heading":0.93979, "vx":-0.82014, "vy":0.57169, "omega":-0.64381, "ax":0.0, "ay":0.0, "alpha":0.46179, "fx":[-1.17983,0.18362,1.17983,-0.18362], "fy":[-0.18362,-1.17983,0.18362,1.17983]}, + {"t":1.29308, "x":2.19693, "y":4.88876, "heading":0.91601, "vx":-0.82014, "vy":0.57169, "omega":-0.62675, "ax":0.0, "ay":0.0, "alpha":0.55553, "fx":[-1.42416,0.18707,1.42416,-0.18707], "fy":[-0.18707,-1.42416,0.18707,1.42416]}, + {"t":1.33003, "x":2.16663, "y":4.90988, "heading":0.89285, "vx":-0.82014, "vy":0.57169, "omega":-0.60622, "ax":0.0, "ay":0.0, "alpha":0.666, "fx":[-1.7121,0.18468,1.7121,-0.18468], "fy":[-0.18468,-1.7121,0.18468,1.7121]}, + {"t":1.36697, "x":2.13633, "y":4.931, "heading":0.87046, "vx":-0.82014, "vy":0.57169, "omega":-0.58162, "ax":0.0, "ay":0.0, "alpha":0.79649, "fx":[-2.052,0.17496,2.052,-0.17496], "fy":[-0.17496,-2.052,0.17496,2.052]}, + {"t":1.40392, "x":2.10603, "y":4.95213, "heading":0.84897, "vx":-0.82014, "vy":0.57169, "omega":-0.55219, "ax":0.0, "ay":0.00001, "alpha":0.95088, "fx":[-2.45362,0.15625,2.45374,-0.15613], "fy":[-0.1561,-2.45359,0.15628,2.45377]}, + {"t":1.44087, "x":2.07573, "y":4.97325, "heading":0.82857, "vx":-0.82014, "vy":0.57169, "omega":-0.51706, "ax":0.0001, "ay":0.00015, "alpha":1.13372, "fx":[-2.92695,0.12823,2.93039,-0.12478], "fy":[-0.12403,-2.9262,0.12898,2.93114]}, + {"t":1.47781, "x":2.04543, "y":4.99437, "heading":0.80946, "vx":-0.82013, "vy":0.57169, "omega":-0.47517, "ax":0.00284, "ay":0.00408, "alpha":1.35035, "fx":[-3.44223,0.13242,3.53882,-0.03562], "fy":[-0.0146,-3.42124,0.15343,3.5598]}, + {"t":1.51476, "x":2.01513, "y":5.01549, "heading":0.79191, "vx":-0.82003, "vy":0.57185, "omega":-0.42529, "ax":0.07997, "ay":0.11405, "alpha":1.60557, "fx":[-2.79363,1.39117,5.51035,1.33347], "fy":[1.91754,-2.21345,1.96794,6.08795]}, + {"t":1.5517, "x":1.98489, "y":5.0367, "heading":0.7762, "vx":-0.81707, "vy":0.57606, "omega":-0.36597, "ax":1.89652, "ay":2.38269, "alpha":1.21694, "fx":[28.91325,32.9479,35.53191,31.64428], "fy":[41.27276,37.51055,39.85426,43.47765]}, + {"t":1.58865, "x":1.956, "y":5.05961, "heading":0.76267, "vx":-0.74701, "vy":0.66409, "omega":-0.32101, "ax":1.43275, "ay":1.47208, "alpha":1.59531, "fx":[20.1892,24.73117,28.4782,24.08413], "fy":[25.59005,20.89687,24.56827,29.10325]}, + {"t":1.6353, "x":1.92271, "y":5.09219, "heading":0.7477, "vx":-0.68016, "vy":0.73277, "omega":-0.24658, "ax":0.03069, "ay":0.02843, "alpha":1.57059, "fx":[-3.53679,0.36974,4.57941,0.67553], "fy":[0.63751,-3.57526,0.33104,4.54096]}, + {"t":1.68196, "x":1.89101, "y":5.12641, "heading":0.73619, "vx":-0.67873, "vy":0.73409, "omega":-0.1733, "ax":0.00056, "ay":0.00052, "alpha":1.26149, "fx":[-3.24825,-0.15085,3.26738,0.17001], "fy":[0.16929,-3.24897,-0.15157,3.26666]}, + {"t":1.72861, "x":1.85934, "y":5.16066, "heading":0.72811, "vx":-0.6787, "vy":0.73412, "omega":-0.11445, "ax":0.00001, "ay":0.00001, "alpha":1.01158, "fx":[-2.61112,-0.14959,2.61147,0.14994], "fy":[0.14993,-2.61113,-0.1496,2.61146]}, + {"t":1.77527, "x":1.82767, "y":5.19491, "heading":0.72277, "vx":-0.6787, "vy":0.73412, "omega":-0.06725, "ax":0.0, "ay":0.0, "alpha":0.80936, "fx":[-2.08861,-0.13098,2.08862,0.13098], "fy":[0.13098,-2.08861,-0.13098,2.08862]}, + {"t":1.82192, "x":1.79601, "y":5.22916, "heading":0.71963, "vx":-0.6787, "vy":0.73412, "omega":-0.02949, "ax":0.0, "ay":0.0, "alpha":0.64535, "fx":[-1.66505,-0.10966,1.66505,0.10966], "fy":[0.10966,-1.66505,-0.10966,1.66505]}, + {"t":1.86858, "x":1.76434, "y":5.26341, "heading":0.71826, "vx":-0.6787, "vy":0.73412, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.51186, "fx":[-1.32051,-0.0888,1.32051,0.0888], "fy":[0.0888,-1.32051,-0.0888,1.32051]}, + {"t":1.91523, "x":1.73268, "y":5.29766, "heading":0.71828, "vx":-0.6787, "vy":0.73412, "omega":0.0245, "ax":0.0, "ay":0.0, "alpha":0.40258, "fx":[-1.03859,-0.06981,1.03859,0.06981], "fy":[0.06981,-1.03859,-0.06981,1.03859]}, + {"t":1.96189, "x":1.70101, "y":5.33191, "heading":0.71943, "vx":-0.6787, "vy":0.73412, "omega":0.04328, "ax":0.0, "ay":0.0, "alpha":0.31236, "fx":[-0.80589,-0.05324,0.80589,0.05324], "fy":[0.05324,-0.80589,-0.05324,0.80589]}, + {"t":2.00854, "x":1.66935, "y":5.36616, "heading":0.72145, "vx":-0.6787, "vy":0.73412, "omega":0.05786, "ax":0.0, "ay":0.0, "alpha":0.23693, "fx":[-0.61136,-0.03915,0.61136,0.03915], "fy":[0.03915,-0.61136,-0.03915,0.61136]}, + {"t":2.0552, "x":1.63768, "y":5.40041, "heading":0.72415, "vx":-0.6787, "vy":0.73412, "omega":0.06891, "ax":0.0, "ay":0.0, "alpha":0.17272, "fx":[-0.44576,-0.02734,0.44576,0.02734], "fy":[0.02734,-0.44576,-0.02734,0.44576]}, + {"t":2.10185, "x":1.60602, "y":5.43466, "heading":0.72736, "vx":-0.6787, "vy":0.73412, "omega":0.07697, "ax":0.0, "ay":0.0, "alpha":0.1167, "fx":[-0.30123,-0.0175,0.30123,0.0175], "fy":[0.0175,-0.30123,-0.0175,0.30123]}, + {"t":2.14851, "x":1.57435, "y":5.46891, "heading":0.73095, "vx":-0.6787, "vy":0.73412, "omega":0.08241, "ax":0.0, "ay":0.0, "alpha":0.0662, "fx":[-0.17092,-0.00932,0.17092,0.00932], "fy":[0.00932,-0.17092,-0.00932,0.17092]}, + {"t":2.19517, "x":1.54269, "y":5.50317, "heading":0.7348, "vx":-0.6787, "vy":0.73412, "omega":0.0855, "ax":0.0, "ay":0.0, "alpha":0.01884, "fx":[-0.04866,-0.00246,0.04866,0.00246], "fy":[0.00246,-0.04866,-0.00246,0.04866]}, + {"t":2.24182, "x":1.51102, "y":5.53742, "heading":0.73879, "vx":-0.6787, "vy":0.73412, "omega":0.08638, "ax":0.0, "ay":0.0, "alpha":-0.02762, "fx":[0.07134,0.00333,-0.07134,-0.00333], "fy":[-0.00333,0.07134,0.00333,-0.07134]}, + {"t":2.28848, "x":1.47936, "y":5.57167, "heading":0.74282, "vx":-0.6787, "vy":0.73412, "omega":0.08509, "ax":0.0, "ay":0.0, "alpha":-0.0754, "fx":[0.19477,0.0083,-0.19477,-0.0083], "fy":[-0.0083,0.19477,0.0083,-0.19477]}, + {"t":2.33513, "x":1.44769, "y":5.60592, "heading":0.74679, "vx":-0.6787, "vy":0.73412, "omega":0.08157, "ax":0.0, "ay":0.0, "alpha":-0.12674, "fx":[0.32747,0.01265,-0.32747,-0.01265], "fy":[-0.01265,0.32747,0.01265,-0.32747]}, + {"t":2.38179, "x":1.41603, "y":5.64017, "heading":0.75059, "vx":-0.6787, "vy":0.73412, "omega":0.07566, "ax":0.0, "ay":0.0, "alpha":-0.1841, "fx":[0.47572,0.01656,-0.47572,-0.01656], "fy":[-0.01656,0.47572,0.01656,-0.47572]}, + {"t":2.42844, "x":1.38436, "y":5.67442, "heading":0.75412, "vx":-0.6787, "vy":0.73412, "omega":0.06707, "ax":0.0, "ay":0.0, "alpha":-0.25017, "fx":[0.64653,0.02023,-0.64653,-0.02023], "fy":[-0.02023,0.64653,0.02023,-0.64653]}, + {"t":2.4751, "x":1.3527, "y":5.70867, "heading":0.75725, "vx":-0.6787, "vy":0.73412, "omega":0.0554, "ax":0.0, "ay":0.0, "alpha":-0.32809, "fx":[0.84799,0.02387,-0.84799,-0.02387], "fy":[-0.02387,0.84799,0.02387,-0.84799]}, + {"t":2.52175, "x":1.32103, "y":5.74292, "heading":0.75984, "vx":-0.6787, "vy":0.73412, "omega":0.04009, "ax":0.00001, "ay":-0.00001, "alpha":-0.42155, "fx":[1.08974,0.02797,-1.08952,-0.02775], "fy":[-0.02798,1.08951,0.02774,-1.08975]}, + {"t":2.56841, "x":1.28937, "y":5.77717, "heading":0.76171, "vx":-0.6787, "vy":0.73412, "omega":0.02043, "ax":1.41661, "ay":-1.53228, "alpha":-0.43384, "fx":[25.22113,24.24267,22.96541,23.95565], "fy":[-25.97614,-24.94579,-26.15687,-27.17589]}, + {"t":2.61506, "x":1.25924, "y":5.80975, "heading":0.76266, "vx":-0.61261, "vy":0.66263, "omega":0.00018, "ax":4.37199, "ay":-4.72896, "alpha":-0.00191, "fx":[74.37151,74.37132,74.36108,74.36128], "fy":[-80.43351,-80.43364,-80.4431,-80.44297]}, + {"t":2.66172, "x":1.23542, "y":5.83552, "heading":0.76267, "vx":-0.40863, "vy":0.442, "omega":0.0001, "ax":4.37824, "ay":-4.73572, "alpha":-0.00115, "fx":[74.47573,74.47562,74.46947,74.46958], "fy":[-80.5504,-80.55049,-80.55618,-80.55609]}, + {"t":2.70837, "x":1.22112, "y":5.85099, "heading":0.76267, "vx":-0.20437, "vy":0.22105, "omega":0.00004, "ax":4.38033, "ay":-4.73798, "alpha":-0.00089, "fx":[74.51052,74.51044,74.50565,74.50573], "fy":[-80.58942,-80.58949,-80.59392,-80.59385]}, + {"t":2.75503, "x":1.21635, "y":5.85615, "heading":0.76267, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.84902, "ay":-4.21373, "alpha":2.76554, "fx":[75.9427,75.93531,87.97747,90.06626], "fy":[-79.19187,-79.23148,-65.60581,-62.66841]}, + {"t":2.79089, "x":1.21947, "y":5.85344, "heading":0.76267, "vx":0.1739, "vy":-0.15111, "omega":0.09918, "ax":4.84906, "ay":-4.21361, "alpha":2.74115, "fx":[76.006,75.98818,87.93369,89.99689], "fy":[-79.11776,-79.16937,-65.6509,-62.75143]}, + {"t":2.82675, "x":1.22883, "y":5.84531, "heading":0.76623, "vx":0.3478, "vy":-0.30223, "omega":0.19748, "ax":4.84911, "ay":-4.21347, "alpha":2.71337, "fx":[76.05056,76.07191,87.90519,89.90038], "fy":[-79.05965,-79.07584,-65.67337,-62.87052]}, + {"t":2.86262, "x":1.24442, "y":5.83176, "heading":0.77331, "vx":0.5217, "vy":-0.45333, "omega":0.29479, "ax":4.84916, "ay":-4.21328, "alpha":2.6813, "fx":[76.08015,76.18748,87.88934,89.77455], "fy":[-79.01355,-78.9493,-65.67627,-63.02786]}, + {"t":2.89848, "x":1.26625, "y":5.81279, "heading":0.78389, "vx":0.6956, "vy":-0.60443, "omega":0.39095, "ax":4.84922, "ay":-4.21306, "alpha":2.64376, "fx":[76.09991,76.33639,87.88253,89.61627], "fy":[-78.97392,-78.78745,-65.66376,-63.22662]}, + {"t":2.93434, "x":1.29431, "y":5.78841, "heading":0.79791, "vx":0.86951, "vy":-0.75552, "omega":0.48576, "ax":4.84927, "ay":-4.21278, "alpha":2.59911, "fx":[76.11681,76.52076,87.87977,89.42119], "fy":[-78.93319,-78.58701,-65.64149,-63.47121]}, + {"t":2.9702, "x":1.32861, "y":5.7586, "heading":0.81533, "vx":1.04341, "vy":-0.9066, "omega":0.57897, "ax":4.84931, "ay":-4.21244, "alpha":2.54504, "fx":[76.14041,76.74363,87.87414,89.18328], "fy":[-78.88097,-78.34334,-65.61738,-63.7676]}, + {"t":3.00607, "x":1.36915, "y":5.72338, "heading":0.83609, "vx":1.21732, "vy":-1.05767, "omega":0.67024, "ax":4.84933, "ay":-4.21199, "alpha":2.4782, "fx":[76.18402,77.00936,87.85561,88.89414], "fy":[-78.80255,-78.04971,-65.60273,-64.12399]}, + {"t":3.04193, "x":1.41592, "y":5.68274, "heading":0.86013, "vx":1.39123, "vy":-1.20872, "omega":0.75912, "ax":4.84932, "ay":-4.2114, "alpha":2.39352, "fx":[76.26687,77.32444,87.80918,88.54163], "fy":[-78.67647,-77.69599,-65.61441,-64.55193]}, + {"t":3.07779, "x":1.46893, "y":5.63669, "heading":0.88735, "vx":1.56514, "vy":-1.35975, "omega":0.84496, "ax":4.84922, "ay":-4.21058, "alpha":2.28288, "fx":[76.41819,77.69913,87.71103,88.10726], "fy":[-78.4695,-77.26606,-65.67891,-65.06853]}, + {"t":3.11365, "x":1.52818, "y":5.58521, "heading":0.91765, "vx":1.73904, "vy":-1.51075, "omega":0.92683, "ax":4.84896, "ay":-4.20937, "alpha":2.13238, "fx":[76.68532,78.15099,87.52038,87.56086], "fy":[-78.12648,-76.73208,-65.84081,-65.70106]}, + {"t":3.14952, "x":1.59367, "y":5.52833, "heading":0.95089, "vx":1.91294, "vy":-1.66171, "omega":1.0033, "ax":4.84831, "ay":-4.2074, "alpha":1.91593, "fx":[77.15239,78.7133,87.15998,86.84808], "fy":[-77.54622,-76.04072,-66.18264,-66.49741]}, + {"t":3.18538, "x":1.66539, "y":5.46603, "heading":0.98687, "vx":2.08681, "vy":-1.8126, "omega":1.07201, "ax":4.84671, "ay":-4.20381, "alpha":1.57766, "fx":[77.98836,79.45867,86.46061,85.8572], "fy":[-76.51622,-75.07359,-66.8791,-67.55332]}, + {"t":3.22124, "x":1.74334, "y":5.39832, "heading":1.02532, "vx":2.26063, "vy":-1.96336, "omega":1.12859, "ax":4.84209, "ay":-4.19574, "alpha":0.97154, "fx":[79.59931,80.58275,84.95874,84.3097], "fy":[-74.49017,-73.50127,-68.38327,-69.09836]}, + {"t":3.2571, "x":1.82753, "y":5.32521, "heading":1.06579, "vx":2.43428, "vy":-2.11383, "omega":1.16343, "ax":4.82263, "ay":-4.16901, "alpha":-0.45123, "fx":[83.288,82.85306,80.73917,81.24623], "fy":[-69.47852,-69.93201,-72.37592,-71.86803]}, + {"t":3.29297, "x":1.91793, "y":5.24672, "heading":1.10751, "vx":2.60723, "vy":-2.26334, "omega":1.14725, "ax":4.51007, "ay":-3.80741, "alpha":-8.75857, "fx":[95.96788,94.12463,47.1588,69.6089], "fy":[-44.68141,-41.35847,-92.86252,-80.14937]}, + {"t":3.32883, "x":2.01433, "y":5.16311, "heading":1.14866, "vx":2.76897, "vy":-2.39988, "omega":0.83314, "ax":-4.51628, "ay":3.8128, "alpha":8.95423, "fx":[-96.8748,-94.16898,-45.8514,-70.38728], "fy":[43.38282,42.06747,94.0808,79.88766]}, + {"t":3.36469, "x":2.11073, "y":5.07949, "heading":1.17854, "vx":2.60701, "vy":-2.26315, "omega":1.15426, "ax":-4.82259, "ay":4.17052, "alpha":0.65468, "fx":[-83.96236,-83.01994,-80.02382,-81.11759], "fy":[68.71408,69.75957,73.20694,72.077]}, + {"t":3.40055, "x":2.20112, "y":5.00101, "heading":1.21993, "vx":2.43406, "vy":-2.11358, "omega":1.17774, "ax":-4.84258, "ay":4.19691, "alpha":-0.89096, "fx":[-79.55805,-81.23673,-85.04356,-83.64505], "fy":[74.56095,72.79341,68.28219,69.9163]}, + {"t":3.43642, "x":2.2853, "y":4.92791, "heading":1.26217, "vx":2.26039, "vy":-1.96307, "omega":1.14579, "ax":-4.8469, "ay":4.20443, "alpha":-1.55404, "fx":[-77.35734,-80.73954,-87.11283,-84.56796], "fy":[77.1777,73.70783,66.02003,69.15875]}, + {"t":3.47228, "x":2.36324, "y":4.86022, "heading":1.30326, "vx":2.08657, "vy":-1.81229, "omega":1.09006, "ax":-4.84823, "ay":4.20759, "alpha":-1.92202, "fx":[-76.02232,-80.64695,-88.28751,-84.9113], "fy":[78.67777,73.99946,64.65799,68.94411]}, + {"t":3.50814, "x":2.43496, "y":4.79793, "heading":1.34235, "vx":1.9127, "vy":-1.66139, "omega":1.02113, "ax":-4.84869, "ay":4.20922, "alpha":-2.15512, "fx":[-75.12345,-80.72308,-89.06572,-84.9869], "fy":[79.65301,74.03356,63.71888,68.98515]}, + {"t":3.544, "x":2.50043, "y":4.74105, "heading":1.37897, "vx":1.73881, "vy":-1.51044, "omega":0.94384, "ax":-4.84883, "ay":4.21019, "alpha":-2.31547, "fx":[-74.4798,-80.87662,-89.62815,-84.92416], "fy":[80.33523,73.94522,63.01988,69.15628]}, + {"t":3.57987, "x":2.55967, "y":4.68959, "heading":1.41282, "vx":1.56492, "vy":-1.35945, "omega":0.8608, "ax":-4.84884, "ay":4.21083, "alpha":-2.43227, "fx":[-74.00062,-81.06417,-90.05675,-84.78785], "fy":[80.83515,73.79697,62.47514,69.39267]}, + {"t":3.61573, "x":2.61268, "y":4.64355, "heading":1.44369, "vx":1.39103, "vy":-1.20844, "omega":0.77358, "ax":-4.84879, "ay":4.21128, "alpha":-2.52108, "fx":[-73.63433,-81.26224,-90.39465,-84.61482], "fy":[81.21323,73.62226,62.03824,69.65682]}, + {"t":3.65159, "x":2.65944, "y":4.60292, "heading":1.47143, "vx":1.21714, "vy":-1.05742, "omega":0.68316, "ax":-4.84871, "ay":4.21161, "alpha":-2.59091, "fx":[-73.34881,-81.4569,-90.6671,-84.42801], "fy":[81.50593,73.44087,61.68131,69.92532]}, + {"t":3.68745, "x":2.69997, "y":4.56771, "heading":1.49593, "vx":1.04325, "vy":-0.90638, "omega":0.59025, "ax":-4.84862, "ay":4.21188, "alpha":-2.64734, "fx":[-73.12252,-81.63931,-90.89015,-84.24274], "fy":[81.73693,73.26543,61.3862,70.18265]}, + {"t":3.72332, "x":2.73427, "y":4.53791, "heading":1.5171, "vx":0.86937, "vy":-0.75533, "omega":0.49531, "ax":-4.84853, "ay":4.21208, "alpha":-2.69398, "fx":[-72.94029,-81.80361,-91.0747,-84.06973], "fy":[81.92249,73.10446,61.14029,70.41814]}, + {"t":3.75918, "x":2.76233, "y":4.51353, "heading":1.53486, "vx":0.69549, "vy":-0.60427, "omega":0.39869, "ax":-4.84843, "ay":4.21225, "alpha":-2.73324, "fx":[-72.79098,-81.94575,-91.22848,-83.91676], "fy":[82.07429,72.96393,60.93442,70.62422]}, + {"t":3.79504, "x":2.78415, "y":4.49457, "heading":1.54916, "vx":0.52161, "vy":-0.45321, "omega":0.30067, "ax":-4.84835, "ay":4.21239, "alpha":-2.76681, "fx":[-72.66621,-82.06284,-91.35718,-83.78962], "fy":[82.20092,72.84821,60.76169,70.79536]}, + {"t":3.8309, "x":2.79974, "y":4.48102, "heading":1.55994, "vx":0.34774, "vy":-0.30214, "omega":0.20145, "ax":-4.84826, "ay":4.2125, "alpha":-2.79589, "fx":[-72.5596,-82.15274,-91.46506,-83.69273], "fy":[82.30885,72.76056,60.61688,70.92744]}, + {"t":3.86677, "x":2.8091, "y":4.4729, "heading":1.56717, "vx":0.17387, "vy":-0.15107, "omega":0.10118, "ax":-4.84818, "ay":4.21259, "alpha":-2.82137, "fx":[-72.46628,-82.21383,-91.5553,-83.62948], "fy":[82.40295,72.70346,60.49602,71.01734]}, + {"t":3.90263, "x":2.81221, "y":4.47019, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/F_PATH_1.traj b/src/main/deploy/v3_poot/choreo/F_PATH_1.traj new file mode 100644 index 00000000..30ab8e58 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/F_PATH_1.traj @@ -0,0 +1,53 @@ +{ + "name":"F_PATH_1", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.152472496032715, "y":4.007034778594971, "heading":4.71238898038469, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.881801605224609, "y":4.026000022888184, "heading":4.71238898038469, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.152472496032715 m", "val":7.152472496032715}, "y":{"exp":"4.007034778594971 m", "val":4.007034778594971}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.881801605224609 m", "val":5.881801605224609}, "y":{"exp":"4.026000022888184 m", "val":4.026000022888184}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.88787], + "samples":[ + {"t":0.0, "x":7.15247, "y":4.00703, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.45247, "ay":0.09631, "alpha":0.0, "fx":[-109.75464,-109.75464,-109.75464,-109.75464], "fy":[1.63813,1.63813,1.63813,1.63813]}, + {"t":0.04933, "x":7.14462, "y":4.00715, "heading":-1.5708, "vx":-0.31827, "vy":0.00475, "omega":0.0, "ax":-6.45173, "ay":0.09629, "alpha":0.0, "fx":[-109.74209,-109.74209,-109.74209,-109.74209], "fy":[1.63794,1.63794,1.63794,1.63794]}, + {"t":0.09865, "x":7.12107, "y":4.0075, "heading":-1.5708, "vx":-0.63651, "vy":0.0095, "omega":0.0, "ax":-6.45077, "ay":0.09628, "alpha":0.0, "fx":[-109.72567,-109.72567,-109.72567,-109.72567], "fy":[1.6377,1.6377,1.6377,1.6377]}, + {"t":0.14798, "x":7.08183, "y":4.00809, "heading":-1.5708, "vx":-0.9547, "vy":0.01425, "omega":0.0, "ax":-6.44945, "ay":0.09626, "alpha":0.0, "fx":[-109.70329,-109.70329,-109.70329,-109.70329], "fy":[1.63736,1.63736,1.63736,1.63736]}, + {"t":0.1973, "x":7.02689, "y":4.00891, "heading":-1.5708, "vx":-1.27283, "vy":0.019, "omega":0.0, "ax":-6.44755, "ay":0.09623, "alpha":0.0, "fx":[-109.67096,-109.67096,-109.67096,-109.67096], "fy":[1.63688,1.63688,1.63688,1.63688]}, + {"t":0.24663, "x":6.95627, "y":4.00996, "heading":-1.5708, "vx":-1.59086, "vy":0.02374, "omega":0.0, "ax":-6.44456, "ay":0.09619, "alpha":0.0, "fx":[-109.62018,-109.62018,-109.62018,-109.62018], "fy":[1.63612,1.63612,1.63612,1.63612]}, + {"t":0.29596, "x":6.86995, "y":4.01125, "heading":-1.5708, "vx":-1.90875, "vy":0.02849, "omega":0.0, "ax":-6.43919, "ay":0.09611, "alpha":0.0, "fx":[-109.52883,-109.52883,-109.52883,-109.52883], "fy":[1.63476,1.63476,1.63476,1.63476]}, + {"t":0.34528, "x":6.76797, "y":4.01277, "heading":-1.5708, "vx":-2.22637, "vy":0.03323, "omega":0.0, "ax":-6.42668, "ay":0.09592, "alpha":0.0, "fx":[-109.31594,-109.31594,-109.31594,-109.31594], "fy":[1.63158,1.63158,1.63158,1.63158]}, + {"t":0.39461, "x":6.65033, "y":4.01453, "heading":-1.5708, "vx":-2.54337, "vy":0.03796, "omega":0.0, "ax":-6.36445, "ay":0.09499, "alpha":0.0, "fx":[-108.25753,-108.25753,-108.25753,-108.25753], "fy":[1.61578,1.61578,1.61578,1.61578]}, + {"t":0.44393, "x":6.51714, "y":4.01652, "heading":-1.5708, "vx":-2.8573, "vy":0.04265, "omega":0.0, "ax":6.36445, "ay":-0.09499, "alpha":0.0, "fx":[108.25753,108.25753,108.25753,108.25753], "fy":[-1.61578,-1.61578,-1.61578,-1.61578]}, + {"t":0.49326, "x":6.38394, "y":4.01851, "heading":-1.5708, "vx":-2.54337, "vy":0.03796, "omega":0.0, "ax":6.42668, "ay":-0.09592, "alpha":0.0, "fx":[109.31594,109.31594,109.31594,109.31594], "fy":[-1.63158,-1.63158,-1.63158,-1.63158]}, + {"t":0.54259, "x":6.2663, "y":4.02026, "heading":-1.5708, "vx":-2.22637, "vy":0.03323, "omega":0.0, "ax":6.43919, "ay":-0.09611, "alpha":0.0, "fx":[109.52883,109.52883,109.52883,109.52883], "fy":[-1.63476,-1.63476,-1.63476,-1.63476]}, + {"t":0.59191, "x":6.16432, "y":4.02178, "heading":-1.5708, "vx":-1.90875, "vy":0.02849, "omega":0.0, "ax":6.44456, "ay":-0.09619, "alpha":0.0, "fx":[109.62018,109.62018,109.62018,109.62018], "fy":[-1.63612,-1.63612,-1.63612,-1.63612]}, + {"t":0.64124, "x":6.07801, "y":4.02307, "heading":-1.5708, "vx":-1.59086, "vy":0.02374, "omega":0.0, "ax":6.44755, "ay":-0.09623, "alpha":0.0, "fx":[109.67096,109.67096,109.67096,109.67096], "fy":[-1.63688,-1.63688,-1.63688,-1.63688]}, + {"t":0.69056, "x":6.00738, "y":4.02413, "heading":-1.5708, "vx":-1.27283, "vy":0.019, "omega":0.0, "ax":6.44945, "ay":-0.09626, "alpha":0.0, "fx":[109.70329,109.70329,109.70329,109.70329], "fy":[-1.63736,-1.63736,-1.63736,-1.63736]}, + {"t":0.73989, "x":5.95244, "y":4.02495, "heading":-1.5708, "vx":-0.9547, "vy":0.01425, "omega":0.0, "ax":6.45077, "ay":-0.09628, "alpha":0.0, "fx":[109.72567,109.72567,109.72567,109.72567], "fy":[-1.6377,-1.6377,-1.6377,-1.6377]}, + {"t":0.78922, "x":5.9132, "y":4.02553, "heading":-1.5708, "vx":-0.63651, "vy":0.0095, "omega":0.0, "ax":6.45173, "ay":-0.09629, "alpha":0.0, "fx":[109.74209,109.74209,109.74209,109.74209], "fy":[-1.63794,-1.63794,-1.63794,-1.63794]}, + {"t":0.83854, "x":5.88965, "y":4.02588, "heading":-1.5708, "vx":-0.31827, "vy":0.00475, "omega":0.0, "ax":6.45247, "ay":-0.09631, "alpha":0.0, "fx":[109.75464,109.75464,109.75464,109.75464], "fy":[-1.63813,-1.63813,-1.63813,-1.63813]}, + {"t":0.88787, "x":5.8818, "y":4.026, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/F_PATH_2.traj b/src/main/deploy/v3_poot/choreo/F_PATH_2.traj new file mode 100644 index 00000000..e45795d3 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/F_PATH_2.traj @@ -0,0 +1,106 @@ +{ + "name":"F_PATH_2", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.976627826690674, "y":4.168239593505859, "heading":4.71238898038469, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.811098575592041, "y":4.139791488647461, "heading":4.71238898038469, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.111105918884277, "y":6.0616984367370605, "heading":-0.5317242496599526, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.21057653427124, "y":5.295576572418213, "heading":-0.5176488456806968, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.976627826690674 m", "val":5.976627826690674}, "y":{"exp":"4.168239593505859 m", "val":4.168239593505859}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.811098575592041 m", "val":6.811098575592041}, "y":{"exp":"4.139791488647461 m", "val":4.139791488647461}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.111105918884277 m", "val":6.111105918884277}, "y":{"exp":"6.0616984367370605 m", "val":6.0616984367370605}, "heading":{"exp":"-0.5317242496599526 rad", "val":-0.5317242496599526}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.21057653427124 m", "val":5.21057653427124}, "y":{"exp":"5.295576572418213 m", "val":5.295576572418213}, "heading":{"exp":"-0.5176488456806968 rad", "val":-0.5176488456806968}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.60216,1.60646,2.33334], + "samples":[ + {"t":0.0, "x":5.97663, "y":4.16824, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.58714, "ay":-3.20259, "alpha":-1.28759, "fx":[90.24007,94.06194,99.32466,96.51581], "fy":[-62.45144,-56.55315,-46.68191,-52.21439]}, + {"t":0.04014, "x":5.98113, "y":4.16566, "heading":-1.5708, "vx":0.22429, "vy":-0.12856, "omega":-0.05169, "ax":5.68126, "ay":-3.03343, "alpha":-1.20977, "fx":[92.40474,95.65173,100.4199,98.07034], "fy":[-59.17022,-53.78846,-44.23741,-49.19491]}, + {"t":0.08029, "x":5.99471, "y":4.15805, "heading":-1.57287, "vx":0.45236, "vy":-0.25034, "omega":-0.10025, "ax":5.7981, "ay":-2.80478, "alpha":-1.10462, "fx":[95.10111,97.65195,101.77648,99.96684], "fy":[-54.68696,-50.01944,-40.96425,-45.16367]}, + {"t":0.12043, "x":6.01754, "y":4.14574, "heading":-1.5769, "vx":0.68512, "vy":-0.36293, "omega":-0.1446, "ax":5.94446, "ay":-2.48084, "alpha":-0.95568, "fx":[98.48296,100.21632,103.47819,102.27655], "fy":[-48.26359,-44.58857,-36.36622,-39.57544]}, + {"t":0.16058, "x":6.04984, "y":4.12918, "heading":-1.5827, "vx":0.92375, "vy":-0.46252, "omega":-0.18296, "ax":6.12555, "ay":-1.99301, "alpha":-0.73175, "fx":[102.64886,103.5032,105.60231,105.02134], "fy":[-38.4985,-36.18238,-29.48402,-31.43755]}, + {"t":0.20072, "x":6.09185, "y":4.109, "heading":-1.59005, "vx":1.16965, "vy":-0.54253, "omega":-0.21234, "ax":6.32815, "ay":-1.19934, "alpha":-0.36946, "fx":[107.21359,107.36584,108.0442,107.9368], "fy":[-22.59615,-21.91039,-18.26448,-18.83082]}, + {"t":0.24086, "x":6.14391, "y":4.08626, "heading":-1.59857, "vx":1.42369, "vy":-0.59068, "omega":-0.22717, "ax":6.43246, "ay":0.20158, "alpha":0.26295, "fx":[109.38101,109.36303,109.45057,109.46251], "fy":[4.68924,4.88282,2.13514,2.00828]}, + {"t":0.28101, "x":6.20624, "y":4.06271, "heading":-1.60769, "vx":1.68192, "vy":-0.58259, "omega":-0.21661, "ax":5.84317, "ay":2.64899, "alpha":1.35097, "fx":[98.40392,95.23129,100.91557,103.01202], "fy":[47.78278,53.72036,41.99294,36.73838]}, + {"t":0.32115, "x":6.27847, "y":4.04145, "heading":-1.61638, "vx":1.91648, "vy":-0.47625, "omega":-0.16238, "ax":3.40939, "ay":5.39305, "alpha":2.52473, "fx":[61.98209,43.45446,53.36349,73.17091], "fy":[90.13451,100.33598,95.27983,81.18681]}, + {"t":0.3613, "x":6.35815, "y":4.02668, "heading":-1.6229, "vx":2.05335, "vy":-0.25975, "omega":-0.06103, "ax":0.4474, "ay":6.3541, "alpha":2.97688, "fx":[20.43438,-5.68444,-9.24168,24.93255], "fy":[107.55338,109.32823,108.96475,106.47916]}, + {"t":0.40144, "x":6.44094, "y":4.02137, "heading":-1.62535, "vx":2.07131, "vy":-0.00467, "omega":0.05848, "ax":-1.35851, "ay":6.23593, "alpha":2.83009, "fx":[-7.37873,-31.73641,-41.79255,-11.52408], "fy":[109.31663,104.88472,101.21869,108.8657]}, + {"t":0.44158, "x":6.523, "y":4.02621, "heading":-1.62301, "vx":2.01677, "vy":0.24567, "omega":0.17209, "ax":-2.34159, "ay":5.94988, "alpha":2.63267, "fx":[-24.07606,-45.51333,-57.17701,-32.55259], "fy":[106.95183,99.75423,93.50208,104.61496]}, + {"t":0.48173, "x":6.60207, "y":4.04087, "heading":-1.6161, "vx":1.92277, "vy":0.48452, "omega":0.27777, "ax":-2.91692, "ay":5.6992, "alpha":2.48595, "fx":[-34.5241,-53.65815,-65.49644,-44.78498], "fy":[104.09583,95.66833,87.94516,100.05739]}, + {"t":0.52187, "x":6.67691, "y":4.06491, "heading":-1.60495, "vx":1.80568, "vy":0.71331, "omega":0.37757, "ax":-3.28396, "ay":5.5025, "alpha":2.38031, "fx":[-41.49446,-58.97646,-70.57441,-52.39162], "fy":[101.55204,92.51884,83.9731,96.34013]}, + {"t":0.56202, "x":6.74675, "y":4.09798, "heading":-1.58979, "vx":1.67385, "vy":0.9342, "omega":0.47312, "ax":-3.53516, "ay":5.34948, "alpha":2.30208, "fx":[-46.41892,-62.72816,-73.96007,-57.42091], "fy":[99.42268,90.04277,81.04202,93.46493]}, + {"t":0.60216, "x":6.8111, "y":4.13979, "heading":-1.5708, "vx":1.53193, "vy":1.14895, "omega":0.56554, "ax":-3.68243, "ay":5.25537, "alpha":2.07425, "fx":[-50.44338,-64.85532,-74.85927,-60.39059], "fy":[97.40131,88.48055,80.16109,91.52625]}, + {"t":0.63259, "x":6.85601, "y":4.17719, "heading":-1.55359, "vx":1.41986, "vy":1.30889, "omega":0.62866, "ax":-3.78506, "ay":5.18171, "alpha":2.05951, "fx":[-52.42446,-66.54083,-76.33651,-62.22935], "fy":[96.3376,87.20807,78.74063,90.27138]}, + {"t":0.66303, "x":6.89747, "y":4.21942, "heading":-1.53445, "vx":1.30467, "vy":1.46658, "omega":0.69134, "ax":-3.89896, "ay":5.09633, "alpha":2.04202, "fx":[-54.63476,-68.40739,-77.96446,-64.27447], "fy":[95.08802,85.73806,77.11196,88.81012]}, + {"t":0.69346, "x":6.93537, "y":4.26642, "heading":-1.51341, "vx":1.18601, "vy":1.62168, "omega":0.75349, "ax":-4.02586, "ay":4.99647, "alpha":2.02104, "fx":[-57.11061,-70.47843,-79.76343,-66.56212], "fy":[93.60647,84.02817,75.22998,87.08964]}, + {"t":0.72389, "x":6.9696, "y":4.31808, "heading":-1.49048, "vx":1.06349, "vy":1.77374, "omega":0.81499, "ax":-4.16771, "ay":4.87858, "alpha":1.99558, "fx":[-59.89527,-72.78124,-81.75594,-69.13365], "fy":[91.83171,82.0234,73.03693,85.04072]}, + {"t":0.75433, "x":7.00004, "y":4.37432, "heading":-1.46568, "vx":0.93666, "vy":1.92221, "omega":0.87573, "ax":-4.32676, "ay":4.73792, "alpha":1.96432, "fx":[-63.03977,-75.34705,-83.96605,-72.03513], "fy":[89.68133,79.65144,70.45827,82.57185]}, + {"t":0.78476, "x":7.02654, "y":4.43502, "heading":-1.43903, "vx":0.80498, "vy":2.0664, "omega":0.93551, "ax":-4.50547, "ay":4.56823, "alpha":1.92549, "fx":[-66.60309,-78.21054,-86.41772,-75.31552], "fy":[87.04293,76.81614,67.39719,79.56111]}, + {"t":0.81519, "x":7.04895, "y":4.50002, "heading":-1.41056, "vx":0.66786, "vy":2.20543, "omega":0.99411, "ax":-4.70631, "ay":4.36107, "alpha":1.87664, "fx":[-70.65063,-81.40799,-89.13127,-79.02185], "fy":[83.76128,73.38818,63.72737,75.84548]}, + {"t":0.84563, "x":7.0671, "y":4.56916, "heading":-1.3803, "vx":0.52464, "vy":2.33815, "omega":1.05122, "ax":-4.93139, "ay":4.10506, "alpha":1.81442, "fx":[-75.24846,-84.97231,-92.1165,-83.18887], "fy":[79.62007,69.19212,59.28412,71.20731]}, + {"t":0.87606, "x":7.08078, "y":4.64222, "heading":-1.34831, "vx":0.37456, "vy":2.46308, "omega":1.10644, "ax":-5.18158, "ay":3.7849, "alpha":1.73414, "fx":[-80.4486,-88.92217,-95.35989,-87.81841], "fy":[74.31689,63.98902,53.85443,65.35985]}, + {"t":0.90649, "x":7.08978, "y":4.71893, "heading":-1.31464, "vx":0.21687, "vy":2.57826, "omega":1.15921, "ax":-5.45481, "ay":3.38034, "alpha":1.62935, "fx":[-86.25559,-93.23935,-98.80211,-92.84166], "fy":[67.43293,57.45513,47.16901,57.93771]}, + {"t":0.93692, "x":7.09385, "y":4.79896, "heading":-1.27936, "vx":0.05086, "vy":2.68114, "omega":1.2088, "ax":-5.74289, "ay":2.86574, "alpha":1.49124, "fx":[-92.55764,-97.82437,-102.30089,-98.05654], "fy":[58.40798,49.16197,38.90366,48.50824]}, + {"t":0.96736, "x":7.09274, "y":4.88188, "heading":-1.24257, "vx":-0.12392, "vy":2.76835, "omega":1.25418, "ax":-6.0265, "ay":2.21162, "alpha":1.30852, "fx":[-99.0012,-102.41664,-105.57685,-103.04128], "fy":[46.55637,38.57614,28.70686,36.63662]}, + {"t":0.99779, "x":7.08618, "y":4.96716, "heading":-1.2044, "vx":-0.30732, "vy":2.83566, "omega":1.294, "ax":-6.26861, "ay":1.39139, "alpha":1.06863, "fx":[-104.80862,-106.47078,-108.15196,-107.07803], "fy":[31.20936,25.1252,16.28201,22.05172]}, + {"t":1.02822, "x":7.07392, "y":5.0541, "heading":-1.16502, "vx":-0.4981, "vy":2.878, "omega":1.32653, "ax":-6.41073, "ay":0.39702, "alpha":0.76264, "fx":[-108.64398,-109.02537,-109.32292,-109.18665], "fy":[12.11168,8.41649,1.55527,4.92959]}, + {"t":1.05866, "x":7.0558, "y":5.14187, "heading":-1.12465, "vx":-0.6932, "vy":2.89009, "omega":1.34974, "ax":-6.38205, "ay":-0.7375, "alpha":0.39466, "fx":[-108.83899,-108.7237,-108.25877,-108.40605], "fy":[-9.94047,-11.32004,-15.08694,-13.8311]}, + {"t":1.08909, "x":7.03174, "y":5.22948, "heading":-1.08358, "vx":-0.88742, "vy":2.86764, "omega":1.36175, "ax":-6.13084, "ay":-1.92221, "alpha":-0.01071, "fx":[-104.26482,-104.2703,-104.30294,-104.29744], "fy":[-32.7577,-32.73834,-32.63456,-32.65403]}, + {"t":1.11952, "x":7.0019, "y":5.31586, "heading":-1.04213, "vx":-1.074, "vy":2.80914, "omega":1.36142, "ax":-5.66299, "ay":-3.03402, "alpha":-0.41416, "fx":[-95.30775,-95.3248,-97.38036,-97.29028], "fy":[-53.5288,-53.45368,-49.61214,-49.8364]}, + {"t":1.14996, "x":6.96659, "y":5.39995, "heading":-1.0007, "vx":-1.24635, "vy":2.71681, "omega":1.34882, "ax":-5.04736, "ay":-3.9724, "alpha":-0.77838, "fx":[-83.74086,-83.02459,-88.16714,-88.48382], "fy":[-70.31911,-71.1044,-64.61081,-64.24316]}, + {"t":1.18039, "x":6.92632, "y":5.48079, "heading":-0.95965, "vx":-1.39995, "vy":2.59592, "omega":1.32513, "ax":-4.37702, "ay":-4.69833, "alpha":-1.08131, "fx":[-71.55776,-69.365,-77.80071,-79.08385], "fy":[-82.74573,-84.53088,-76.82037,-75.57211]}, + {"t":1.21082, "x":6.88169, "y":5.55762, "heading":-0.91932, "vx":-1.53316, "vy":2.45293, "omega":1.29222, "ax":-3.72636, "ay":-5.22739, "alpha":-1.31898, "fx":[-60.09073,-56.08927,-67.37873,-69.97846], "fy":[-91.46902,-93.91695,-86.15204,-84.12761]}, + {"t":1.24126, "x":6.83331, "y":5.62985, "heading":-0.88, "vx":-1.64657, "vy":2.29384, "omega":1.25208, "ax":-3.13604, "ay":-5.60002, "alpha":-1.49957, "fx":[-49.92073,-44.16131,-57.64417,-61.64611], "fy":[-97.44078,-100.12794,-92.99129,-90.45866]}, + {"t":1.27169, "x":6.78174, "y":5.69706, "heading":-0.84189, "vx":-1.74201, "vy":2.12342, "omega":1.20644, "ax":-2.61977, "ay":-5.85836, "alpha":-1.63537, "fx":[-41.15226,-33.88225,-48.95438,-54.25748], "fy":[-101.49621,-104.09864,-97.88578,-95.11536]}, + {"t":1.30212, "x":6.72752, "y":5.75897, "heading":-0.80518, "vx":-1.82173, "vy":1.94513, "omega":1.15667, "ax":-2.17617, "ay":-6.03662, "alpha":-1.73785, "fx":[-33.67203,-25.18996,-41.39457,-47.80778], "fy":[-104.25103,-106.57266,-101.35084,-98.5502]}, + {"t":1.33256, "x":6.67107, "y":5.81537, "heading":-0.76998, "vx":-1.88796, "vy":1.76141, "omega":1.10378, "ax":-1.79742, "ay":-6.1597, "alpha":-1.81605, "fx":[-27.29788,-17.88134,-34.90255,-42.21237], "fy":[-106.12725,-108.07066,-103.79765,-101.10352]}, + {"t":1.36299, "x":6.61278, "y":5.86613, "heading":-0.73638, "vx":-1.94266, "vy":1.57395, "omega":1.04852, "ax":-1.47399, "ay":-6.24487, "alpha":-1.87659, "fx":[-21.84598,-11.72708,-29.35466,-37.36092], "fy":[-107.40644,-108.93726,-105.52897,-103.02081]}, + {"t":1.39342, "x":6.55297, "y":5.91114, "heading":-0.70447, "vx":-1.98752, "vy":1.3839, "omega":0.99141, "ax":-1.19681, "ay":-6.30385, "alpha":-1.92418, "fx":[-17.155,-6.51809,-24.61307,-33.14362], "fy":[-108.27555,-109.39571,-106.75909,-104.47622]}, + {"t":1.42386, "x":6.49193, "y":5.95033, "heading":-0.6743, "vx":-2.02394, "vy":1.19206, "omega":0.93285, "ax":-0.95798, "ay":-6.3446, "alpha":-1.96215, "fx":[-13.09132,-2.07882,-20.54788,-29.46187], "fy":[-108.85953,-109.59006,-107.63706,-105.59288]}, + {"t":1.45429, "x":6.42989, "y":5.98367, "heading":-0.64591, "vx":-2.0531, "vy":0.99897, "omega":0.87313, "ax":-0.75091, "ay":-6.37255, "alpha":-1.99288, "fx":[-9.54705,1.73293,-17.04525,-26.23138], "fy":[-109.24295,-109.61357,-108.26602,-106.45825]}, + {"t":1.48472, "x":6.36706, "y":6.01112, "heading":-0.61934, "vx":-2.07595, "vy":0.80503, "omega":0.81248, "ax":-0.5702, "ay":-6.3914, "alpha":-2.01807, "fx":[-6.43586,5.03067,-14.00896,-23.38174], "fy":[-109.48387,-109.52716,-108.71743,-107.135]}, + {"t":1.51516, "x":6.30362, "y":6.03266, "heading":-0.59461, "vx":-2.0933, "vy":0.61052, "omega":0.75107, "ax":-0.4115, "ay":-6.40374, "alpha":-2.03896, "fx":[-3.68882,7.90473,-11.35911,-20.85489], "fy":[-109.62275,-109.37091,-109.04111,-107.66846]}, + {"t":1.54559, "x":6.23972, "y":6.04828, "heading":-0.57176, "vx":-2.10583, "vy":0.41564, "omega":0.68901, "ax":-0.27127, "ay":-6.41139, "alpha":-2.05645, "fx":[-1.2507,10.42706,-9.02992,-18.60313], "fy":[-109.68825,-109.17143,-109.27201,-108.09186]}, + {"t":1.57602, "x":6.17551, "y":6.05796, "heading":-0.55079, "vx":-2.11408, "vy":0.22052, "omega":0.62643, "ax":-0.14663, "ay":-6.41563, "alpha":-2.07123, "fx":[0.92293,12.65534,-6.96732,-16.58729], "fy":[-109.70093,-108.94646,-109.43487,-108.42983]}, + {"t":1.60646, "x":6.11111, "y":6.0617, "heading":-0.53172, "vx":-2.11855, "vy":0.02527, "omega":0.5634, "ax":-0.01125, "ay":-6.42277, "alpha":-1.94784, "fx":[3.02898,14.03522,-4.32291,-13.50645], "fy":[-109.69032,-108.81066,-109.60971,-108.88699]}, + {"t":1.64471, "x":6.03005, "y":6.05796, "heading":-0.51017, "vx":-2.11898, "vy":-0.22045, "omega":0.48888, "ax":0.17595, "ay":-6.41788, "alpha":-1.99504, "fx":[6.19925,17.56809,-1.1188,-10.67715], "fy":[-109.5402,-108.2765,-109.66647,-109.18218]}, + {"t":1.68297, "x":5.94911, "y":6.04483, "heading":-0.49147, "vx":-2.11224, "vy":-0.46598, "omega":0.41255, "ax":0.41967, "ay":-6.40338, "alpha":-2.05319, "fx":[10.19207,22.13943,3.23298,-7.01065], "fy":[-109.2196,-107.40943,-109.59477,-109.45462]}, + {"t":1.72123, "x":5.86861, "y":6.02232, "heading":-0.47568, "vx":-2.09619, "vy":-0.71096, "omega":0.334, "ax":0.74832, "ay":-6.36912, "alpha":-2.12617, "fx":[15.41762,28.22482,9.32687,-2.05475], "fy":[-108.57666,-105.93903,-109.20465,-109.62706]}, + {"t":1.75948, "x":5.78896, "y":5.99046, "heading":-0.46291, "vx":-2.06756, "vy":-0.95462, "omega":0.25266, "ax":1.21094, "ay":-6.29176, "alpha":-2.21926, "fx":[22.57461,36.59078,18.21401,5.01184], "fy":[-107.27745,-103.30081,-108.02071,-109.4855]}, + {"t":1.79774, "x":5.71075, "y":5.94934, "heading":-0.45324, "vx":-2.02123, "vy":-1.19533, "omega":0.16776, "ax":1.89548, "ay":-6.11214, "alpha":-2.3386, "fx":[32.90645,48.46001,31.80775,15.79229], "fy":[-104.51309,-98.22406,-104.73909,-108.38655]}, + {"t":1.836, "x":5.63481, "y":5.89913, "heading":-0.44682, "vx":-1.94872, "vy":-1.42916, "omega":0.07829, "ax":2.95336, "ay":-5.66468, "alpha":-2.48512, "fx":[48.61696,65.48288,53.27488,33.56842], "fy":[-98.10169,-87.68906,-95.48227,-104.14511]}, + {"t":1.87426, "x":5.56242, "y":5.84031, "heading":-0.44383, "vx":-1.83573, "vy":-1.64588, "omega":-0.01679, "ax":4.53378, "ay":-4.48422, "alpha":-2.59223, "fx":[72.61436,87.88898,84.06431,63.90546], "fy":[-81.78069,-65.03336,-69.6768,-88.61053]}, + {"t":1.91251, "x":5.49551, "y":5.77406, "heading":-0.44447, "vx":-1.66228, "vy":-1.81743, "omega":-0.11596, "ax":6.1239, "ay":-1.80543, "alpha":-2.19601, "fx":[100.86019,106.84401,107.75082,101.20786], "fy":[-41.96563,-22.84197,-17.29122,-40.74072]}, + {"t":1.95077, "x":5.43639, "y":5.70321, "heading":-0.44891, "vx":-1.428, "vy":-1.8865, "omega":-0.19997, "ax":6.22965, "ay":1.52321, "alpha":-1.16836, "fx":[107.81867,105.98524,103.76762,106.28669], "fy":[17.68942,26.70391,34.17719,25.06723]}, + {"t":1.98903, "x":5.38632, "y":5.63215, "heading":-0.45656, "vx":-1.18967, "vy":-1.82823, "omega":-0.24467, "ax":5.32981, "ay":3.59813, "alpha":-0.30191, "fx":[91.84654,90.99123,89.46731,90.32896], "fy":[59.42164,60.74356,62.95549,61.69196]}, + {"t":2.02729, "x":5.34471, "y":5.56484, "heading":-0.46592, "vx":-0.98576, "vy":-1.69057, "omega":-0.25622, "ax":4.49134, "ay":4.6117, "alpha":0.22561, "fx":[75.35357,75.84295,77.44896,76.94039], "fy":[79.45815,78.98105,77.41016,77.92575]}, + {"t":2.06554, "x":5.31028, "y":5.50354, "heading":-0.47572, "vx":-0.81394, "vy":-1.51414, "omega":-0.24759, "ax":3.88986, "ay":5.13247, "alpha":0.54139, "fx":[63.63334,64.29673,68.79015,67.94165], "fy":[89.21574,88.72103,85.28771,85.98307]}, + {"t":2.1038, "x":5.28199, "y":5.44937, "heading":-0.48519, "vx":-0.66512, "vy":-1.31779, "omega":-0.22688, "ax":3.4654, "ay":5.4294, "alpha":0.74336, "fx":[55.55921,55.86879,62.54341,61.81048], "fy":[94.5156,94.31392,90.02825,90.5525]}, + {"t":2.14206, "x":5.25908, "y":5.40293, "heading":-0.49387, "vx":-0.53254, "vy":-1.11007, "omega":-0.19844, "ax":3.15713, "ay":5.61484, "alpha":0.88122, "fx":[49.82663,49.6307,57.90247,57.44766], "fy":[97.70012,97.78147,93.12251,93.42286]}, + {"t":2.18031, "x":5.24102, "y":5.36457, "heading":-0.50146, "vx":-0.41176, "vy":-0.89526, "omega":-0.16472, "ax":2.92554, "ay":5.73918, "alpha":0.98038, "fx":[45.60045,44.89244,54.34599,54.21126], "fy":[99.77199,100.07546,95.27237,95.36719]}, + {"t":2.21857, "x":5.2274, "y":5.33452, "heading":-0.50776, "vx":-0.29984, "vy":-0.6757, "omega":-0.12722, "ax":2.74615, "ay":5.82727, "alpha":1.05473, "fx":[42.37574,41.19923,51.54612,51.72407], "fy":[101.20568,101.67449,96.83915,96.76116]}, + {"t":2.25683, "x":5.21794, "y":5.31293, "heading":-0.51263, "vx":-0.19478, "vy":-0.45276, "omega":-0.08687, "ax":2.60355, "ay":5.89242, "alpha":1.11235, "fx":[39.84085,38.25413,49.29251,49.75534], "fy":[102.24712,102.83671,98.02383,97.80553]}, + {"t":2.29509, "x":5.2124, "y":5.29993, "heading":-0.51595, "vx":-0.09517, "vy":-0.22734, "omega":-0.04431, "ax":2.48769, "ay":5.94228, "alpha":1.15822, "fx":[37.79648,35.85962,47.44601,48.15762], "fy":[103.03389,103.7104,98.9456,98.61596]}, + {"t":2.33334, "x":5.21058, "y":5.29558, "heading":-0.51765, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/F_PATH_3.traj b/src/main/deploy/v3_poot/choreo/F_PATH_3.traj new file mode 100644 index 00000000..e38300bb --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/F_PATH_3.traj @@ -0,0 +1,94 @@ +{ + "name":"F_PATH_3", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.21057653427124, "y":5.295576572418213, "heading":-0.5176488456806968, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.010232925415039, "y":6.064763069152832, "heading":1.5707963267948966, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.21057653427124 m", "val":5.21057653427124}, "y":{"exp":"5.295576572418213 m", "val":5.295576572418213}, "heading":{"exp":"-0.5176488456806968 rad", "val":-0.5176488456806968}, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.010232925415039 m", "val":7.010232925415039}, "y":{"exp":"6.064763069152832 m", "val":6.064763069152832}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,2.11406], + "samples":[ + {"t":0.0, "x":5.21058, "y":5.29558, "heading":-0.51765, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.93225, "ay":2.53549, "alpha":0.00947, "fx":[100.8788,100.90207,100.93285,100.90962], "fy":[43.19115,43.13655,43.06463,43.11927]}, + {"t":0.03709, "x":5.21466, "y":5.29732, "heading":-0.51765, "vx":0.22002, "vy":0.09404, "omega":0.00035, "ax":5.92999, "ay":2.53452, "alpha":0.01155, "fx":[100.83453,100.86287,100.90044,100.87214], "fy":[43.18865,43.12205,43.03434,43.10101]}, + {"t":0.07418, "x":5.2269, "y":5.30255, "heading":-0.51764, "vx":0.43996, "vy":0.18804, "omega":0.00078, "ax":5.92548, "ay":2.5326, "alpha":0.01572, "fx":[100.74595,100.78443,100.83558,100.79719], "fy":[43.18364,43.09302,42.97376,43.06448]}, + {"t":0.11127, "x":5.24729, "y":5.31127, "heading":-0.51761, "vx":0.65973, "vy":0.28197, "omega":0.00136, "ax":5.91195, "ay":2.52681, "alpha":0.02826, "fx":[100.48016,100.54872,100.64078,100.57247], "fy":[43.16844,43.00579,42.792,42.95498]}, + {"t":0.14836, "x":5.27582, "y":5.32346, "heading":-0.51756, "vx":0.87899, "vy":0.37569, "omega":0.00241, "ax":1.08629, "ay":0.46429, "alpha":4.97423, "fx":[13.84618,0.84061,24.12154,35.1013], "fy":[26.03606,3.32343,-10.14314,12.3735]}, + {"t":0.18544, "x":5.30917, "y":5.33772, "heading":-0.51747, "vx":0.91928, "vy":0.39291, "omega":0.1869, "ax":0.00001, "ay":0.0, "alpha":4.72555, "fx":[-4.59236,-16.72858,4.59283,16.72903], "fy":[16.72885,-4.59255,-16.72876,4.59264]}, + {"t":0.22253, "x":5.34327, "y":5.35229, "heading":-0.51054, "vx":0.91928, "vy":0.39291, "omega":0.36216, "ax":0.0, "ay":0.0, "alpha":4.19747, "fx":[-4.18228,-14.83074,4.18228,14.83074], "fy":[14.83074,-4.18228,-14.83074,4.18228]}, + {"t":0.25962, "x":5.37736, "y":5.36686, "heading":-0.4971, "vx":0.91928, "vy":0.39291, "omega":0.51784, "ax":0.0, "ay":0.0, "alpha":3.72142, "fx":[-3.88423,-13.09774,3.88423,13.09774], "fy":[13.09774,-3.88423,-13.09774,3.88423]}, + {"t":0.29671, "x":5.41146, "y":5.38143, "heading":-0.4779, "vx":0.91928, "vy":0.39291, "omega":0.65587, "ax":0.0, "ay":0.0, "alpha":3.29372, "fx":[-3.65981,-11.52425,3.65981,11.52425], "fy":[11.52425,-3.65981,-11.52425,3.65981]}, + {"t":0.3338, "x":5.44555, "y":5.39601, "heading":-0.45357, "vx":0.91928, "vy":0.39291, "omega":0.77803, "ax":0.0, "ay":0.0, "alpha":2.91051, "fx":[-3.48075,-10.1018,3.48075,10.1018], "fy":[10.1018,-3.48075,-10.1018,3.48075]}, + {"t":0.37089, "x":5.47965, "y":5.41058, "heading":-0.42472, "vx":0.91928, "vy":0.39291, "omega":0.88597, "ax":0.0, "ay":0.0, "alpha":2.56794, "fx":[-3.32693,-8.82048,3.32693,8.82048], "fy":[8.82048,-3.32693,-8.82048,3.32693]}, + {"t":0.40798, "x":5.51374, "y":5.42515, "heading":-0.39186, "vx":0.91928, "vy":0.39291, "omega":0.98122, "ax":0.0, "ay":0.0, "alpha":2.2622, "fx":[-3.18453,-7.66984,3.18453,7.66984], "fy":[7.66984,-3.18453,-7.66984,3.18453]}, + {"t":0.44507, "x":5.54784, "y":5.43972, "heading":-0.35546, "vx":0.91928, "vy":0.39291, "omega":1.06512, "ax":0.0, "ay":0.0, "alpha":1.98967, "fx":[-3.04447,-6.63945,3.04447,6.63945], "fy":[6.63945,-3.04447,-6.63946,3.04447]}, + {"t":0.48215, "x":5.58193, "y":5.4543, "heading":-0.31596, "vx":0.91928, "vy":0.39291, "omega":1.13891, "ax":0.0, "ay":0.0, "alpha":1.7469, "fx":[-2.90113,-5.71922,2.90113,5.71922], "fy":[5.71922,-2.90113,-5.71922,2.90113]}, + {"t":0.51924, "x":5.61603, "y":5.46887, "heading":-0.27372, "vx":0.91928, "vy":0.39291, "omega":1.2037, "ax":0.0, "ay":0.0, "alpha":1.53068, "fx":[-2.7514,-4.89951,2.7514,4.89951], "fy":[4.89951,-2.7514,-4.89951,2.7514]}, + {"t":0.55633, "x":5.65012, "y":5.48344, "heading":-0.22907, "vx":0.91928, "vy":0.39291, "omega":1.26047, "ax":0.0, "ay":0.0, "alpha":1.33803, "fx":[-2.59387,-4.17128,2.59387,4.17128], "fy":[4.17128,-2.59387,-4.17128,2.59387]}, + {"t":0.59342, "x":5.68422, "y":5.49801, "heading":-0.18233, "vx":0.91928, "vy":0.39291, "omega":1.3101, "ax":0.0, "ay":0.0, "alpha":1.16623, "fx":[-2.42826,-3.52607,2.42826,3.52607], "fy":[3.52607,-2.42826,-3.52607,2.42826]}, + {"t":0.63051, "x":5.71831, "y":5.51259, "heading":-0.13374, "vx":0.91928, "vy":0.39291, "omega":1.35335, "ax":0.0, "ay":0.0, "alpha":1.01277, "fx":[-2.25496,-2.95605,2.25496,2.95605], "fy":[2.95605,-2.25496,-2.95605,2.25496]}, + {"t":0.6676, "x":5.75241, "y":5.52716, "heading":-0.08354, "vx":0.91928, "vy":0.39291, "omega":1.39092, "ax":0.0, "ay":0.0, "alpha":0.87536, "fx":[-2.07474,-2.45396,2.07474,2.45396], "fy":[2.45396,-2.07474,-2.45396,2.07474]}, + {"t":0.70469, "x":5.7865, "y":5.54173, "heading":-0.03195, "vx":0.91928, "vy":0.39291, "omega":1.42338, "ax":0.0, "ay":0.0, "alpha":0.7519, "fx":[-1.88845,-2.01317,1.88845,2.01317], "fy":[2.01317,-1.88845,-2.01317,1.88845]}, + {"t":0.74178, "x":5.8206, "y":5.5563, "heading":0.02084, "vx":0.91928, "vy":0.39291, "omega":1.45127, "ax":0.0, "ay":0.0, "alpha":0.6405, "fx":[-1.69692,-1.62763,1.69692,1.62763], "fy":[1.62763,-1.69692,-1.62763,1.69692]}, + {"t":0.77886, "x":5.85469, "y":5.57088, "heading":0.07466, "vx":0.91928, "vy":0.39291, "omega":1.47502, "ax":0.0, "ay":0.0, "alpha":0.53942, "fx":[-1.50079,-1.29189,1.50079,1.29189], "fy":[1.29189,-1.50079,-1.29189,1.50079]}, + {"t":0.81595, "x":5.88879, "y":5.58545, "heading":0.12937, "vx":0.91928, "vy":0.39291, "omega":1.49503, "ax":0.0, "ay":0.0, "alpha":0.44706, "fx":[-1.3005,-1.00107,1.3005,1.00107], "fy":[1.00107,-1.3005,-1.00107,1.3005]}, + {"t":0.85304, "x":5.92288, "y":5.60002, "heading":0.18482, "vx":0.91928, "vy":0.39291, "omega":1.51161, "ax":0.0, "ay":0.0, "alpha":0.36194, "fx":[-1.09619,-0.75087,1.09619,0.75087], "fy":[0.75087,-1.09619,-0.75087,1.09619]}, + {"t":0.89013, "x":5.95698, "y":5.61459, "heading":0.24088, "vx":0.91928, "vy":0.39291, "omega":1.52504, "ax":0.0, "ay":0.0, "alpha":0.2827, "fx":[-0.88773,-0.53759,0.88773,0.53759], "fy":[0.53759,-0.88773,-0.53759,0.88773]}, + {"t":0.92722, "x":5.99107, "y":5.62917, "heading":0.29745, "vx":0.91928, "vy":0.39291, "omega":1.53552, "ax":0.0, "ay":0.0, "alpha":0.20807, "fx":[-0.67469,-0.3581,0.67469,0.3581], "fy":[0.3581,-0.67469,-0.3581,0.67469]}, + {"t":0.96431, "x":6.02517, "y":5.64374, "heading":0.3544, "vx":0.91928, "vy":0.39291, "omega":1.54324, "ax":0.0, "ay":0.0, "alpha":0.13683, "fx":[-0.45637,-0.20986,0.45637,0.20985], "fy":[0.20986,-0.45637,-0.20985,0.45637]}, + {"t":1.0014, "x":6.05926, "y":5.65831, "heading":0.41163, "vx":0.91928, "vy":0.39291, "omega":1.54831, "ax":0.0, "ay":0.0, "alpha":0.06782, "fx":[-0.2318,-0.09091,0.2318,0.09091], "fy":[0.09091,-0.2318,-0.09091,0.2318]}, + {"t":1.03849, "x":6.09336, "y":5.67288, "heading":0.46906, "vx":0.91928, "vy":0.39291, "omega":1.55083, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00024,0.00008,-0.00024,-0.00008], "fy":[-0.00008,0.00024,0.00008,-0.00024]}, + {"t":1.07558, "x":6.12745, "y":5.68746, "heading":0.52658, "vx":0.91928, "vy":0.39291, "omega":1.55083, "ax":0.0, "ay":0.0, "alpha":-0.06796, "fx":[0.24117,0.06385,-0.24117,-0.06385], "fy":[-0.06385,0.24117,0.06385,-0.24117]}, + {"t":1.11266, "x":6.16155, "y":5.70203, "heading":0.58409, "vx":0.91928, "vy":0.39291, "omega":1.54831, "ax":0.0, "ay":0.0, "alpha":-0.13695, "fx":[0.4926,0.10052,-0.4926,-0.10052], "fy":[-0.10052,0.4926,0.10053,-0.4926]}, + {"t":1.14975, "x":6.19564, "y":5.7166, "heading":0.64152, "vx":0.91928, "vy":0.39291, "omega":1.54323, "ax":0.0, "ay":0.0, "alpha":-0.20817, "fx":[0.75632,0.10958,-0.75632,-0.10958], "fy":[-0.10958,0.75632,0.10958,-0.75632]}, + {"t":1.18684, "x":6.22974, "y":5.73117, "heading":0.69876, "vx":0.91928, "vy":0.39291, "omega":1.53551, "ax":0.0, "ay":0.0, "alpha":-0.28278, "fx":[1.03422,0.08983,-1.03422,-0.08983], "fy":[-0.08983,1.03422,0.08983,-1.03422]}, + {"t":1.22393, "x":6.26383, "y":5.74575, "heading":0.75571, "vx":0.91928, "vy":0.39291, "omega":1.52502, "ax":0.0, "ay":0.0, "alpha":-0.36199, "fx":[1.32831,0.03945,-1.32831,-0.03945], "fy":[-0.03945,1.32831,0.03945,-1.32831]}, + {"t":1.26102, "x":6.29793, "y":5.76032, "heading":0.81227, "vx":0.91928, "vy":0.39291, "omega":1.51159, "ax":0.0, "ay":0.0, "alpha":-0.44708, "fx":[1.64067,-0.04409,-1.64067,0.04409], "fy":[0.04409,1.64067,-0.04409,-1.64067]}, + {"t":1.29811, "x":6.33202, "y":5.77489, "heading":0.86833, "vx":0.91928, "vy":0.39291, "omega":1.49501, "ax":0.0, "ay":0.0, "alpha":-0.53942, "fx":[1.97342,-0.16403,-1.97342,0.16404], "fy":[0.16403,1.97342,-0.16404,-1.97342]}, + {"t":1.3352, "x":6.36612, "y":5.78946, "heading":0.92378, "vx":0.91928, "vy":0.39291, "omega":1.475, "ax":0.0, "ay":0.0, "alpha":-0.64047, "fx":[2.32874,-0.32432,-2.32874,0.32432], "fy":[0.32432,2.32874,-0.32432,-2.32874]}, + {"t":1.37229, "x":6.40021, "y":5.80404, "heading":0.97848, "vx":0.91928, "vy":0.39291, "omega":1.45125, "ax":0.0, "ay":0.0, "alpha":-0.75185, "fx":[2.70879,-0.52963,-2.70879,0.52963], "fy":[0.52963,2.70879,-0.52963,-2.70879]}, + {"t":1.40937, "x":6.43431, "y":5.81861, "heading":1.03231, "vx":0.91928, "vy":0.39291, "omega":1.42336, "ax":0.0, "ay":0.0, "alpha":-0.87528, "fx":[3.11577,-0.78534,-3.11577,0.78534], "fy":[0.78534,3.11577,-0.78534,-3.11577]}, + {"t":1.44646, "x":6.4684, "y":5.83318, "heading":1.0851, "vx":0.91928, "vy":0.39291, "omega":1.3909, "ax":0.0, "ay":0.0, "alpha":-1.01269, "fx":[3.55192,-1.09758,-3.55192,1.09758], "fy":[1.09758,3.55192,-1.09758,-3.55192]}, + {"t":1.48355, "x":6.5025, "y":5.84775, "heading":1.13669, "vx":0.91928, "vy":0.39291, "omega":1.35334, "ax":0.0, "ay":0.0, "alpha":-1.16614, "fx":[4.01954,-1.47312,-4.01954,1.47312], "fy":[1.47312,4.01954,-1.47312,-4.01954]}, + {"t":1.52064, "x":6.53659, "y":5.86233, "heading":1.18688, "vx":0.91928, "vy":0.39291, "omega":1.31009, "ax":0.0, "ay":0.0, "alpha":-1.33794, "fx":[4.5211,-1.91939,-4.5211,1.91939], "fy":[1.91939,4.5211,-1.91939,-4.5211]}, + {"t":1.55773, "x":6.57069, "y":5.8769, "heading":1.23547, "vx":0.91928, "vy":0.39291, "omega":1.26047, "ax":0.0, "ay":0.0, "alpha":-1.53059, "fx":[5.05932,-2.44438,-5.05932,2.44438], "fy":[2.44438,5.05932,-2.44438,-5.05932]}, + {"t":1.59482, "x":6.60478, "y":5.89147, "heading":1.28222, "vx":0.91928, "vy":0.39291, "omega":1.2037, "ax":0.0, "ay":0.0, "alpha":-1.74681, "fx":[5.63737,-3.05648,-5.63737,3.05648], "fy":[3.05648,5.63737,-3.05648,-5.63737]}, + {"t":1.63191, "x":6.63888, "y":5.90604, "heading":1.32686, "vx":0.91928, "vy":0.39291, "omega":1.13891, "ax":0.0, "ay":0.0, "alpha":-1.98959, "fx":[6.25912,-3.76437,-6.25912,3.76437], "fy":[3.76437,6.25912,-3.76437,-6.25912]}, + {"t":1.669, "x":6.67297, "y":5.92062, "heading":1.3691, "vx":0.91928, "vy":0.39291, "omega":1.06512, "ax":0.0, "ay":0.0, "alpha":-2.26213, "fx":[6.92943,-4.57673,-6.92943,4.57673], "fy":[4.57673,6.92943,-4.57673,-6.92943]}, + {"t":1.70608, "x":6.70707, "y":5.93519, "heading":1.40861, "vx":0.91928, "vy":0.39291, "omega":0.98122, "ax":0.0, "ay":0.0, "alpha":-2.56788, "fx":[7.65467,-5.50191,-7.65467,5.50191], "fy":[5.50191,7.65467,-5.50191,-7.65467]}, + {"t":1.74317, "x":6.74116, "y":5.94976, "heading":1.445, "vx":0.91928, "vy":0.39291, "omega":0.88598, "ax":0.0, "ay":0.0, "alpha":-2.91046, "fx":[8.44324,-6.54746,-8.44324,6.54746], "fy":[6.54746,8.44324,-6.54746,-8.44324]}, + {"t":1.78026, "x":6.77526, "y":5.96433, "heading":1.47786, "vx":0.91928, "vy":0.39291, "omega":0.77804, "ax":0.0, "ay":0.0, "alpha":-3.29366, "fx":[9.30633,-7.71945,-9.30633,7.71945], "fy":[7.71945,9.30633,-7.71945,-9.30633]}, + {"t":1.81735, "x":6.80935, "y":5.97891, "heading":1.50672, "vx":0.91928, "vy":0.39291, "omega":0.65588, "ax":0.0, "ay":0.0, "alpha":-3.72137, "fx":[10.2588,-9.02162,-10.2588,9.02162], "fy":[9.02162,10.2588,-9.02162,-10.2588]}, + {"t":1.85444, "x":6.84345, "y":5.99348, "heading":1.53104, "vx":0.91928, "vy":0.39291, "omega":0.51786, "ax":0.0, "ay":0.0, "alpha":-4.19742, "fx":[11.32021,-10.45414,-11.32021,10.45414], "fy":[10.45414,11.32021,-10.45414,-11.32021]}, + {"t":1.89153, "x":6.87754, "y":6.00805, "heading":1.55025, "vx":0.91928, "vy":0.39291, "omega":0.36218, "ax":-0.00001, "ay":-0.00001, "alpha":-4.72549, "fx":[12.51584,-12.01214,-12.51618,12.01178], "fy":[12.01179,12.51583,-12.01213,-12.51619]}, + {"t":1.92862, "x":6.91164, "y":6.02262, "heading":1.56368, "vx":0.91928, "vy":0.39291, "omega":0.18692, "ax":-1.08629, "ay":-0.46428, "alpha":-4.97472, "fx":[-5.88236,-31.47303,-30.70089,-5.85369], "fy":[5.44312,5.19629,-20.23056,-21.99792]}, + {"t":1.96571, "x":6.94499, "y":6.03688, "heading":1.57061, "vx":0.87899, "vy":0.37569, "omega":0.00241, "ax":-5.91195, "ay":-2.52681, "alpha":-0.02826, "fx":[-100.59004,-100.63607,-100.53123,-100.48473], "fy":[-42.90922,-42.80429,-43.05126,-43.15658]}, + {"t":2.00279, "x":6.97352, "y":6.04907, "heading":1.5707, "vx":0.65973, "vy":0.28197, "omega":0.00136, "ax":-5.92548, "ay":-2.5326, "alpha":-0.01572, "fx":[-100.80744,-100.83285,-100.77419,-100.74864], "fy":[-43.03919,-42.98051,-43.11823,-43.17703]}, + {"t":2.03988, "x":6.99391, "y":6.05779, "heading":1.57075, "vx":0.43996, "vy":0.18804, "omega":0.00078, "ax":-5.92999, "ay":-2.53452, "alpha":-0.01155, "fx":[-100.87979,-100.89841,-100.85523,-100.83654], "fy":[-43.08247,-43.03929,-43.14054,-43.18379]}, + {"t":2.07697, "x":7.00615, "y":6.06302, "heading":1.57078, "vx":0.22002, "vy":0.09404, "omega":0.00035, "ax":-5.93225, "ay":-2.53549, "alpha":-0.00947, "fx":[-100.91594,-100.93117,-100.89575,-100.88047], "fy":[-43.1041,-43.06868,-43.15169,-43.18716]}, + {"t":2.11406, "x":7.01023, "y":6.06476, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/F_PATH_4.traj b/src/main/deploy/v3_poot/choreo/F_PATH_4.traj new file mode 100644 index 00000000..885c0670 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/F_PATH_4.traj @@ -0,0 +1,123 @@ +{ + "name":"F_PATH_4", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.010232925415039, "y":6.064763069152832, "heading":1.5707963267948966, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.7965826988220215, "y":4.032148838043213, "heading":-1.5604875077843534, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.9229350090026855, "y":4.018707752227783, "heading":-1.5707963267948966, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.010232925415039 m", "val":7.010232925415039}, "y":{"exp":"6.064763069152832 m", "val":6.064763069152832}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.7965826988220215 m", "val":6.7965826988220215}, "y":{"exp":"4.032148838043213 m", "val":4.032148838043213}, "heading":{"exp":"-1.5604875077843534 rad", "val":-1.5604875077843534}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.9229350090026855 m", "val":5.9229350090026855}, "y":{"exp":"4.018707752227783 m", "val":4.018707752227783}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":"first", "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,2.13409,3.09778], + "samples":[ + {"t":0.0, "x":7.01023, "y":6.06476, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.53484, "ay":-6.42809, "alpha":-0.01447, "fx":[-9.02906,-9.17816,-9.16568,-9.01678], "fy":[-109.3456,-109.33321,-109.33445,-109.34681]}, + {"t":0.03442, "x":7.00992, "y":6.06096, "heading":1.5708, "vx":-0.01841, "vy":-0.22126, "omega":-0.0005, "ax":-0.53461, "ay":-6.4253, "alpha":-0.01776, "fx":[-9.00957,-9.19257,-9.17723,-8.99454], "fy":[-109.29937,-109.28416,-109.28577,-109.30093]}, + {"t":0.06884, "x":7.00897, "y":6.04953, "heading":1.57078, "vx":-0.03681, "vy":-0.44242, "omega":-0.00111, "ax":-0.53414, "ay":-6.41973, "alpha":-0.02434, "fx":[-8.97068,-9.22132,-9.20027,-8.95019], "fy":[-109.20704,-109.18622,-109.18865,-109.20938]}, + {"t":0.10326, "x":7.00738, "y":6.0305, "heading":1.57074, "vx":-0.0552, "vy":-0.6634, "omega":-0.00195, "ax":-0.53276, "ay":-6.40312, "alpha":-0.044, "fx":[-8.85469,-9.30698,-9.26873,-8.81828], "fy":[-108.93123,-108.89372,-108.89934,-108.93655]}, + {"t":0.13768, "x":7.00517, "y":6.00387, "heading":1.57067, "vx":-0.07353, "vy":-0.8838, "omega":-0.00346, "ax":-0.27181, "ay":-3.26747, "alpha":-4.33748, "fx":[10.51711,-20.63955,-16.89956,8.52813], "fy":[-47.73807,-46.67425,-63.52471,-64.37812]}, + {"t":0.1721, "x":7.00247, "y":5.97152, "heading":1.57055, "vx":-0.08289, "vy":-0.99627, "omega":-0.15276, "ax":-0.00004, "ay":-0.00006, "alpha":-6.46077, "fx":[16.77452,-16.76767,-16.77568,16.76632], "fy":[16.76598,16.77403,-16.76801,-16.77617]}, + {"t":0.20652, "x":6.99962, "y":5.93722, "heading":1.5653, "vx":-0.08289, "vy":-0.99627, "omega":-0.37515, "ax":0.0, "ay":0.0, "alpha":-5.8129, "fx":[15.17202,-15.0061,-15.17207,15.00605], "fy":[15.00608,15.17205,-15.00607,-15.17205]}, + {"t":0.24095, "x":6.99677, "y":5.90293, "heading":1.55238, "vx":-0.08289, "vy":-0.99627, "omega":-0.57523, "ax":0.0, "ay":0.0, "alpha":-5.2166, "fx":[13.78843,-13.28979,-13.78843,13.28979], "fy":[13.28979,13.78843,-13.28979,-13.78843]}, + {"t":0.27537, "x":6.99391, "y":5.86864, "heading":1.53258, "vx":-0.08289, "vy":-0.99627, "omega":-0.75479, "ax":0.0, "ay":0.0, "alpha":-4.67069, "fx":[12.57865,-11.65226,-12.57865,11.65226], "fy":[11.65226,12.57865,-11.65226,-12.57865]}, + {"t":0.30979, "x":6.99106, "y":5.83435, "heading":1.5066, "vx":-0.08289, "vy":-0.99627, "omega":-0.91556, "ax":0.0, "ay":0.0, "alpha":-4.17324, "fx":[11.50565,-10.11578,-11.50565,10.11578], "fy":[10.11578,11.50565,-10.11578,-11.50565]}, + {"t":0.34421, "x":6.98821, "y":5.80006, "heading":1.47509, "vx":-0.08289, "vy":-0.99627, "omega":-1.0592, "ax":0.0, "ay":0.0, "alpha":-3.72181, "fx":[10.5402,-8.69372,-10.5402,8.69372], "fy":[8.69372,10.5402,-8.69372,-10.5402]}, + {"t":0.37863, "x":6.98536, "y":5.76576, "heading":1.43863, "vx":-0.08289, "vy":-0.99627, "omega":-1.18731, "ax":0.0, "ay":0.0, "alpha":-3.31355, "fx":[9.65991,-7.39288,-9.65992,7.39288], "fy":[7.39288,9.65992,-7.39288,-9.65991]}, + {"t":0.41305, "x":6.9825, "y":5.73147, "heading":1.39776, "vx":-0.08289, "vy":-0.99627, "omega":-1.30137, "ax":0.0, "ay":0.0, "alpha":-2.94544, "fx":[8.8481,-6.21527,-8.8481,6.21527], "fy":[6.21527,8.8481,-6.21527,-8.8481]}, + {"t":0.44747, "x":6.97965, "y":5.69718, "heading":1.35297, "vx":-0.08289, "vy":-0.99627, "omega":-1.40275, "ax":0.0, "ay":0.0, "alpha":-2.61433, "fx":[8.0926,-5.15937,-8.0926,5.15937], "fy":[5.15937,8.0926,-5.15937,-8.0926]}, + {"t":0.48189, "x":6.9768, "y":5.66289, "heading":1.30468, "vx":-0.08289, "vy":-0.99627, "omega":-1.49274, "ax":0.0, "ay":0.0, "alpha":-2.31712, "fx":[7.38492,-4.2213,-7.38493,4.2213], "fy":[4.2213,7.38492,-4.2213,-7.38492]}, + {"t":0.51631, "x":6.97394, "y":5.62859, "heading":1.2533, "vx":-0.08289, "vy":-0.99627, "omega":-1.5725, "ax":0.0, "ay":0.0, "alpha":-2.0507, "fx":[6.71909,-3.39535,-6.71909,3.39535], "fy":[3.39535,6.71909,-3.39535,-6.71909]}, + {"t":0.55073, "x":6.97109, "y":5.5943, "heading":1.19918, "vx":-0.08289, "vy":-0.99627, "omega":-1.64308, "ax":0.0, "ay":0.0, "alpha":-1.81222, "fx":[6.09134,-2.67487,-6.09135,2.67487], "fy":[2.67487,6.09135,-2.67487,-6.09135]}, + {"t":0.58515, "x":6.96824, "y":5.56001, "heading":1.14262, "vx":-0.08289, "vy":-0.99627, "omega":-1.70546, "ax":0.0, "ay":0.0, "alpha":-1.59883, "fx":[5.49889,-2.05235,-5.49889,2.05235], "fy":[2.05235,5.49889,-2.05235,-5.49889]}, + {"t":0.61957, "x":6.96538, "y":5.52572, "heading":1.08392, "vx":-0.08289, "vy":-0.99627, "omega":-1.76049, "ax":0.0, "ay":0.0, "alpha":-1.40803, "fx":[4.94036,-1.52019,-4.94036,1.52019], "fy":[1.52019,4.94036,-1.52019,-4.94036]}, + {"t":0.65399, "x":6.96253, "y":5.49142, "heading":1.02332, "vx":-0.08289, "vy":-0.99627, "omega":-1.80896, "ax":0.0, "ay":0.0, "alpha":-1.23729, "fx":[4.41423,-1.07049,-4.41423,1.07049], "fy":[1.07049,4.41423,-1.07049,-4.41423]}, + {"t":0.68842, "x":6.95968, "y":5.45713, "heading":0.96105, "vx":-0.08289, "vy":-0.99627, "omega":-1.85155, "ax":0.0, "ay":0.0, "alpha":-1.08453, "fx":[3.92012,-0.69573,-3.92012,0.69574], "fy":[0.69574,3.92012,-0.69574,-3.92012]}, + {"t":0.72284, "x":6.95682, "y":5.42284, "heading":0.89732, "vx":-0.08289, "vy":-0.99627, "omega":-1.88888, "ax":0.0, "ay":0.0, "alpha":-0.94754, "fx":[3.45671,-0.38849,-3.4567,0.38849], "fy":[0.38849,3.45671,-0.38849,-3.45671]}, + {"t":0.75726, "x":6.95397, "y":5.38855, "heading":0.83231, "vx":-0.08289, "vy":-0.99627, "omega":-1.92149, "ax":0.0, "ay":0.0, "alpha":-0.8246, "fx":[3.02384,-0.14192,-3.02383,0.14192], "fy":[0.14192,3.02384,-0.14192,-3.02384]}, + {"t":0.79168, "x":6.95112, "y":5.35426, "heading":0.76617, "vx":-0.08289, "vy":-0.99627, "omega":-1.94988, "ax":0.0, "ay":0.0, "alpha":-0.71381, "fx":[2.61997,0.05042,-2.61996,-0.05041], "fy":[-0.05042,2.61997,0.05042,-2.61997]}, + {"t":0.8261, "x":6.94826, "y":5.31996, "heading":0.69905, "vx":-0.08289, "vy":-0.99627, "omega":-1.97445, "ax":0.0, "ay":0.0, "alpha":-0.61378, "fx":[2.24483,0.19434,-2.24482,-0.19434], "fy":[-0.19434,2.24482,0.19434,-2.24482]}, + {"t":0.86052, "x":6.94541, "y":5.28567, "heading":0.63109, "vx":-0.08289, "vy":-0.99627, "omega":-1.99557, "ax":0.0, "ay":0.0, "alpha":-0.52285, "fx":[1.8966,0.29503,-1.89659,-0.29502], "fy":[-0.29503,1.8966,0.29503,-1.8966]}, + {"t":0.89494, "x":6.94256, "y":5.25138, "heading":0.5624, "vx":-0.08289, "vy":-0.99627, "omega":-2.01357, "ax":0.0, "ay":0.0, "alpha":-0.43987, "fx":[1.57482,0.35714,-1.57481,-0.35714], "fy":[-0.35714,1.57481,0.35714,-1.57481]}, + {"t":0.92936, "x":6.9397, "y":5.21709, "heading":0.49309, "vx":-0.08289, "vy":-0.99627, "omega":-2.02871, "ax":0.0, "ay":0.0, "alpha":-0.36344, "fx":[1.27761,0.38448,-1.2776,-0.38448], "fy":[-0.38448,1.2776,0.38448,-1.2776]}, + {"t":0.96378, "x":6.93685, "y":5.18279, "heading":0.42326, "vx":-0.08289, "vy":-0.99627, "omega":-2.04122, "ax":0.0, "ay":0.0, "alpha":-0.29255, "fx":[1.00431,0.38049,-1.00431,-0.38049], "fy":[-0.38049,1.00431,0.38049,-1.00431]}, + {"t":0.9982, "x":6.934, "y":5.1485, "heading":0.353, "vx":-0.08289, "vy":-0.99627, "omega":-2.05129, "ax":0.0, "ay":0.0, "alpha":-0.22601, "fx":[0.75333,0.34769,-0.75333,-0.34769], "fy":[-0.34769,0.75333,0.34769,-0.75333]}, + {"t":1.03262, "x":6.93114, "y":5.11421, "heading":0.28239, "vx":-0.08289, "vy":-0.99627, "omega":-2.05907, "ax":0.0, "ay":0.0, "alpha":-0.1629, "fx":[0.52393,0.28828,-0.52393,-0.28828], "fy":[-0.28828,0.52393,0.28828,-0.52393]}, + {"t":1.06704, "x":6.92829, "y":5.07992, "heading":0.21152, "vx":-0.08289, "vy":-0.99627, "omega":-2.06468, "ax":0.0, "ay":0.0, "alpha":-0.10222, "fx":[0.31512,0.20372,-0.31513,-0.20372], "fy":[-0.20372,0.31513,0.20372,-0.31513]}, + {"t":1.10146, "x":6.92544, "y":5.04563, "heading":0.14045, "vx":-0.08289, "vy":-0.99627, "omega":-2.0682, "ax":0.0, "ay":0.0, "alpha":-0.04303, "fx":[0.12624,0.09497,-0.12624,-0.09497], "fy":[-0.09497,0.12624,0.09497,-0.12624]}, + {"t":1.13589, "x":6.92258, "y":5.01133, "heading":0.06926, "vx":-0.08289, "vy":-0.99627, "omega":-2.06968, "ax":0.0, "ay":0.0, "alpha":0.01547, "fx":[-0.04285,-0.0373,0.04285,0.03729], "fy":[0.03729,-0.04285,-0.03729,0.04285]}, + {"t":1.17031, "x":6.91973, "y":4.97704, "heading":-0.00198, "vx":-0.08289, "vy":-0.99627, "omega":-2.06914, "ax":0.0, "ay":0.0, "alpha":0.07433, "fx":[-0.19256,-0.19332,0.19255,0.19332], "fy":[0.19332,-0.19255,-0.19332,0.19255]}, + {"t":1.20473, "x":6.91688, "y":4.94275, "heading":-0.0732, "vx":-0.08289, "vy":-0.99627, "omega":-2.06659, "ax":0.0, "ay":0.0, "alpha":0.1342, "fx":[-0.32194,-0.37291,0.32194,0.3729], "fy":[0.3729,-0.32194,-0.3729,0.32194]}, + {"t":1.23915, "x":6.91403, "y":4.90846, "heading":-0.14433, "vx":-0.08289, "vy":-0.99627, "omega":-2.06197, "ax":0.0, "ay":0.0, "alpha":0.19627, "fx":[-0.4309,-0.57746,0.43089,0.57745], "fy":[0.57746,-0.43089,-0.57746,0.43089]}, + {"t":1.27357, "x":6.91117, "y":4.87416, "heading":-0.21531, "vx":-0.08289, "vy":-0.99627, "omega":-2.05521, "ax":0.0, "ay":0.0, "alpha":0.26112, "fx":[-0.51734,-0.80698,0.51734,0.80698], "fy":[0.80698,-0.51734,-0.80698,0.51734]}, + {"t":1.30799, "x":6.90832, "y":4.83987, "heading":-0.28605, "vx":-0.08289, "vy":-0.99627, "omega":-2.04622, "ax":0.0, "ay":0.0, "alpha":0.33012, "fx":[-0.58031,-1.06391,0.5803,1.06391], "fy":[1.06391,-0.5803,-1.06391,0.5803]}, + {"t":1.34241, "x":6.90547, "y":4.80558, "heading":-0.35648, "vx":-0.08289, "vy":-0.99627, "omega":-2.03486, "ax":0.0, "ay":0.0, "alpha":0.40385, "fx":[-0.61657,-1.34828,0.61656,1.34828], "fy":[1.34828,-0.61657,-1.34828,0.61657]}, + {"t":1.37683, "x":6.90261, "y":4.77129, "heading":-0.42652, "vx":-0.08289, "vy":-0.99627, "omega":-2.02096, "ax":0.0, "ay":0.0, "alpha":0.48391, "fx":[-0.62391,-1.66328,0.62391,1.66328], "fy":[1.66328,-0.62391,-1.66328,0.62391]}, + {"t":1.41125, "x":6.89976, "y":4.73699, "heading":-0.49609, "vx":-0.08289, "vy":-0.99627, "omega":-2.0043, "ax":0.0, "ay":0.0, "alpha":0.57096, "fx":[-0.59796,-2.00891,0.59796,2.00892], "fy":[2.00892,-0.59796,-2.00892,0.59796]}, + {"t":1.44567, "x":6.89691, "y":4.7027, "heading":-0.56508, "vx":-0.08289, "vy":-0.99627, "omega":-1.98465, "ax":0.0, "ay":0.0, "alpha":0.6668, "fx":[-0.53494,-2.38869,0.53495,2.38869], "fy":[2.38869,-0.53494,-2.38869,0.53494]}, + {"t":1.48009, "x":6.89405, "y":4.66841, "heading":-0.63339, "vx":-0.08289, "vy":-0.99627, "omega":-1.9617, "ax":0.0, "ay":0.0, "alpha":0.77233, "fx":[-0.4293,-2.80259,0.42931,2.8026], "fy":[2.80259,-0.42931,-2.80259,0.42931]}, + {"t":1.51451, "x":6.8912, "y":4.63412, "heading":-0.70091, "vx":-0.08289, "vy":-0.99627, "omega":-1.93511, "ax":0.0, "ay":0.0, "alpha":0.88955, "fx":[-0.27554,-3.25395,0.27555,3.25396], "fy":[3.25396,-0.27554,-3.25396,0.27554]}, + {"t":1.54893, "x":6.88835, "y":4.59983, "heading":-0.76752, "vx":-0.08289, "vy":-0.99627, "omega":-1.90449, "ax":0.0, "ay":0.0, "alpha":1.0197, "fx":[-0.06689,-3.74276,0.06689,3.74277], "fy":[3.74276,-0.06689,-3.74276,0.06689]}, + {"t":1.58336, "x":6.88549, "y":4.56553, "heading":-0.83307, "vx":-0.08289, "vy":-0.99627, "omega":-1.8694, "ax":0.0, "ay":0.0, "alpha":1.16495, "fx":[0.20385,-4.27172,-0.20384,4.27173], "fy":[4.27173,0.20384,-4.27173,-0.20384]}, + {"t":1.61778, "x":6.88264, "y":4.53124, "heading":-0.89742, "vx":-0.08289, "vy":-0.99627, "omega":-1.8293, "ax":0.0, "ay":0.0, "alpha":1.32698, "fx":[0.5446,-4.84089,-0.5446,4.84089], "fy":[4.84089,0.5446,-4.84089,-0.5446]}, + {"t":1.6522, "x":6.87979, "y":4.49695, "heading":-0.96039, "vx":-0.08289, "vy":-0.99627, "omega":-1.78362, "ax":0.0, "ay":0.0, "alpha":1.50819, "fx":[0.96394,-5.45208,-0.96394,5.45208], "fy":[5.45208,0.96394,-5.45208,-0.96394]}, + {"t":1.68662, "x":6.87693, "y":4.46266, "heading":-1.02178, "vx":-0.08289, "vy":-0.99627, "omega":-1.73171, "ax":0.0, "ay":0.0, "alpha":1.71073, "fx":[1.47077,-6.10555,-1.47077,6.10555], "fy":[6.10555,1.47077,-6.10555,-1.47077]}, + {"t":1.72104, "x":6.87408, "y":4.42836, "heading":-1.08139, "vx":-0.08289, "vy":-0.99627, "omega":-1.67282, "ax":0.0, "ay":0.0, "alpha":1.93722, "fx":[2.0744,-6.80239,-2.0744,6.80239], "fy":[6.80239,2.0744,-6.80239,-2.0744]}, + {"t":1.75546, "x":6.87123, "y":4.39407, "heading":-1.13897, "vx":-0.08289, "vy":-0.99627, "omega":-1.60614, "ax":0.0, "ay":0.0, "alpha":2.19034, "fx":[2.78416,-7.54347,-2.78416,7.54347], "fy":[7.54347,2.78416,-7.54347,-2.78416]}, + {"t":1.78988, "x":6.86837, "y":4.35978, "heading":-1.19425, "vx":-0.08289, "vy":-0.99627, "omega":-1.53075, "ax":0.0, "ay":0.0, "alpha":2.47288, "fx":[3.60909,-8.32985,-3.6091,8.32984], "fy":[8.32984,3.6091,-8.32984,-3.6091]}, + {"t":1.8243, "x":6.86552, "y":4.32549, "heading":-1.24694, "vx":-0.08289, "vy":-0.99627, "omega":-1.44563, "ax":0.0, "ay":0.0, "alpha":2.78797, "fx":[4.55787,-9.16388,-4.5579,9.16386], "fy":[9.16387,4.55789,-9.16387,-4.55788]}, + {"t":1.85872, "x":6.86267, "y":4.29119, "heading":-1.2967, "vx":-0.08289, "vy":-0.99627, "omega":-1.34967, "ax":-0.00001, "ay":0.0, "alpha":3.13854, "fx":[5.63753,-10.04844,-5.63803,10.04795], "fy":[10.04822,5.6378,-10.04817,-5.63776]}, + {"t":1.89314, "x":6.85981, "y":4.2569, "heading":-1.34316, "vx":-0.08289, "vy":-0.99627, "omega":-1.24164, "ax":-0.00033, "ay":0.00003, "alpha":3.52791, "fx":[6.84926,-10.99384,-6.86051,10.98275], "fy":[10.98883,6.85529,-10.98776,-6.85449]}, + {"t":1.92756, "x":6.85696, "y":4.22261, "heading":-1.3859, "vx":-0.0829, "vy":-0.99627, "omega":-1.1202, "ax":-0.00739, "ay":0.00062, "alpha":3.95898, "fx":[8.08559,-12.11577,-8.33898,11.86626], "fy":[12.00352,8.2208,-11.97859,-8.20382]}, + {"t":1.96198, "x":6.8541, "y":4.18832, "heading":-1.42445, "vx":-0.08316, "vy":-0.99625, "omega":-0.98393, "ax":-0.16485, "ay":0.01424, "alpha":4.42918, "fx":[6.86607,-15.82283,-12.51952,10.26039], "fy":[13.35998,9.88796,-12.76103,-9.51834]}, + {"t":1.9964, "x":6.85114, "y":4.15403, "heading":-1.45832, "vx":-0.08883, "vy":-0.99576, "omega":-0.83148, "ax":-2.93358, "ay":0.41356, "alpha":3.26623, "fx":[-43.44704,-56.90501,-56.43289,-42.81215], "fy":[19.35933,14.87719,-4.00084,-2.09748]}, + {"t":2.03083, "x":6.84635, "y":4.12001, "heading":-1.48694, "vx":-0.18981, "vy":-0.98152, "omega":-0.71905, "ax":-5.89967, "ay":1.80952, "alpha":0.25939, "fx":[-99.82159,-100.22233,-100.86639,-100.4965], "fy":[32.38006,31.47735,29.19683,30.06339]}, + {"t":2.06525, "x":6.83632, "y":4.08729, "heading":-1.51169, "vx":-0.39288, "vy":-0.91924, "omega":-0.71012, "ax":-5.55539, "ay":3.13946, "alpha":0.07628, "fx":[-94.22743,-94.44489,-94.76223,-94.54805], "fy":[53.87187,53.50573,52.93125,53.29662]}, + {"t":2.09967, "x":6.8195, "y":4.05751, "heading":-1.53613, "vx":-0.5841, "vy":-0.81117, "omega":-0.7075, "ax":-4.75473, "ay":4.31877, "alpha":0.03803, "fx":[-80.68982,-80.87543,-81.06322,-80.87815], "fy":[73.66649,73.46496,73.25545,73.45742]}, + {"t":2.13409, "x":6.79658, "y":4.03215, "heading":-1.56049, "vx":-0.74776, "vy":-0.66252, "omega":-0.70619, "ax":-3.55495, "ay":5.3554, "alpha":0.03759, "fx":[-60.24513,-60.51706,-60.69244,-60.42038], "fy":[91.24249,91.06357,90.94491,91.12466]}, + {"t":2.17789, "x":6.76042, "y":4.00827, "heading":-1.59142, "vx":-0.90348, "vy":-0.42793, "omega":-0.70454, "ax":-1.89612, "ay":6.06989, "alpha":0.10732, "fx":[-31.593,-32.58398,-32.91568,-31.91731], "fy":[103.45796,103.15574,103.03351,103.34139]}, + {"t":2.2217, "x":6.71902, "y":3.99534, "heading":-1.62228, "vx":-0.98654, "vy":-0.16204, "omega":-0.69984, "ax":-0.29347, "ay":4.32466, "alpha":2.23305, "fx":[3.55723,-12.00628,-14.68538,3.16713], "fy":[76.93112,76.64097,70.10045,70.57214]}, + {"t":2.2655, "x":6.67553, "y":3.9924, "heading":-1.65294, "vx":-0.9994, "vy":0.0274, "omega":-0.60202, "ax":0.00541, "ay":0.17337, "alpha":4.92359, "fx":[13.80871,-11.52724,-13.7832,11.86969], "fy":[14.63924,16.71521,-8.72007,-10.83838]}, + {"t":2.3093, "x":6.63175, "y":3.99376, "heading":-1.67931, "vx":-0.99916, "vy":0.03499, "omega":-0.38635, "ax":0.00014, "ay":0.00397, "alpha":4.18805, "fx":[11.98598,-9.62648,-11.98384,9.63383], "fy":[9.69787,12.05215,-9.56242,-11.91766]}, + {"t":2.35311, "x":6.58799, "y":3.9953, "heading":-1.69623, "vx":-0.99915, "vy":0.03517, "omega":-0.20289, "ax":0.0, "ay":0.00009, "alpha":3.51333, "fx":[10.18935,-7.90729,-10.18929,7.90744], "fy":[7.90889,10.19084,-7.90584,-10.1878]}, + {"t":2.39691, "x":6.54422, "y":3.99684, "heading":-1.70512, "vx":-0.99915, "vy":0.03517, "omega":-0.049, "ax":0.0, "ay":0.0, "alpha":2.89853, "fx":[8.46395,-6.4487,-8.46395,6.4487], "fy":[6.44873,8.46399,-6.44866,-8.46392]}, + {"t":2.44072, "x":6.50045, "y":3.99838, "heading":-1.70727, "vx":-0.99915, "vy":0.03517, "omega":0.07797, "ax":0.0, "ay":0.0, "alpha":2.3353, "fx":[6.83038,-5.18095,-6.83038,5.18095], "fy":[5.18095,6.83039,-5.18095,-6.83038]}, + {"t":2.48452, "x":6.45668, "y":3.99992, "heading":-1.70385, "vx":-0.99915, "vy":0.03517, "omega":0.18027, "ax":0.0, "ay":0.0, "alpha":1.81495, "fx":[5.29466,-4.04464,-5.29466,4.04464], "fy":[4.04464,5.29466,-4.04464,-5.29466]}, + {"t":2.52833, "x":6.41292, "y":4.00146, "heading":-1.69596, "vx":-0.99915, "vy":0.03517, "omega":0.25977, "ax":0.0, "ay":0.0, "alpha":1.32874, "fx":[3.85278,-2.99164,-3.85278,2.99164], "fy":[2.99164,3.85278,-2.99164,-3.85278]}, + {"t":2.57213, "x":6.36915, "y":4.003, "heading":-1.68458, "vx":-0.99915, "vy":0.03517, "omega":0.31797, "ax":0.0, "ay":0.0, "alpha":0.86798, "fx":[2.49438,-1.98276,-2.49438,1.98276], "fy":[1.98276,2.49438,-1.98276,-2.49438]}, + {"t":2.61593, "x":6.32538, "y":4.00454, "heading":-1.67065, "vx":-0.99915, "vy":0.03517, "omega":0.356, "ax":0.0, "ay":0.0, "alpha":0.42405, "fx":[1.20499,-0.98554,-1.20499,0.98554], "fy":[0.98554,1.20499,-0.98554,-1.20499]}, + {"t":2.65974, "x":6.28161, "y":4.00608, "heading":-1.65505, "vx":-0.99915, "vy":0.03517, "omega":0.37457, "ax":0.0, "ay":0.0, "alpha":-0.01162, "fx":[-0.03259,0.02752,0.03259,-0.02752], "fy":[-0.02752,-0.03259,0.02752,0.03259]}, + {"t":2.70354, "x":6.23785, "y":4.00762, "heading":-1.63865, "vx":-0.99915, "vy":0.03517, "omega":0.37406, "ax":0.0, "ay":0.0, "alpha":-0.44751, "fx":[-1.23775,1.08023,1.23775,-1.08023], "fy":[-1.08023,-1.23775,1.08023,1.23775]}, + {"t":2.74735, "x":6.19408, "y":4.00916, "heading":-1.62226, "vx":-0.99915, "vy":0.03517, "omega":0.35446, "ax":0.0, "ay":0.0, "alpha":-0.89213, "fx":[-2.43189,2.19363,2.43189,-2.19363], "fy":[-2.19363,-2.43189,2.19363,2.43189]}, + {"t":2.79115, "x":6.15031, "y":4.0107, "heading":-1.60673, "vx":-0.99915, "vy":0.03517, "omega":0.31538, "ax":0.0, "ay":0.0, "alpha":-1.35403, "fx":[-3.63886,3.38629,3.63886,-3.38629], "fy":[-3.38629,-3.63886,3.38629,3.63886]}, + {"t":2.83496, "x":6.10655, "y":4.01224, "heading":-1.59292, "vx":-0.99915, "vy":0.03517, "omega":0.25607, "ax":0.0, "ay":0.0, "alpha":-1.84184, "fx":[-4.8857,4.67418,4.8857,-4.67418], "fy":[-4.67418,-4.8857,4.67418,4.8857]}, + {"t":2.87876, "x":6.06278, "y":4.01379, "heading":-1.5817, "vx":-0.99915, "vy":0.03517, "omega":0.17539, "ax":0.00003, "ay":0.0, "alpha":-2.36427, "fx":[-6.20336,6.07037,6.20423,-6.0695], "fy":[-6.06996,-6.20381,6.06991,6.20378]}, + {"t":2.92256, "x":6.01901, "y":4.01533, "heading":-1.57402, "vx":-0.99915, "vy":0.03517, "omega":0.07182, "ax":3.48318, "ay":-0.12261, "alpha":-1.61638, "fx":[56.17178,62.13278,62.31394,56.37276], "fy":[-7.79132,-7.21183,3.17584,3.48534]}, + {"t":2.96637, "x":5.97859, "y":4.01675, "heading":-1.57087, "vx":-0.84658, "vy":0.0298, "omega":0.00102, "ax":6.43412, "ay":-0.22648, "alpha":-0.01142, "fx":[109.44021,109.44074,109.44488,109.44436], "fy":[-3.91344,-3.90925,-3.79128,-3.79535]}, + {"t":3.01017, "x":5.94768, "y":4.01784, "heading":-1.57083, "vx":-0.56473, "vy":0.01988, "omega":0.00052, "ax":6.44439, "ay":-0.22684, "alpha":-0.0067, "fx":[109.61599,109.61619,109.61862,109.61843], "fy":[-3.89441,-3.89195,-3.82258,-3.82499]}, + {"t":3.05398, "x":5.92912, "y":4.01849, "heading":-1.57081, "vx":-0.28244, "vy":0.00994, "omega":0.00022, "ax":6.44783, "ay":-0.22696, "alpha":-0.00513, "fx":[109.67479,109.67491,109.67678,109.67666], "fy":[-3.88803,-3.88615,-3.83306,-3.83492]}, + {"t":3.09778, "x":5.92294, "y":4.01871, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/v3_poot/choreo/F_PATH_5.traj b/src/main/deploy/v3_poot/choreo/F_PATH_5.traj new file mode 100644 index 00000000..b5360132 --- /dev/null +++ b/src/main/deploy/v3_poot/choreo/F_PATH_5.traj @@ -0,0 +1,105 @@ +{ + "name":"F_PATH_5", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.9229350090026855, "y":4.018707752227783, "heading":-1.5707963267948966, "intervals":68, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.010232925415039, "y":6.064763069152832, "heading":1.5707963267948966, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"5.9229350090026855 m", "val":5.9229350090026855}, "y":{"exp":"4.018707752227783 m", "val":4.018707752227783}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":68, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.010232925415039 m", "val":7.010232925415039}, "y":{"exp":"6.064763069152832 m", "val":6.064763069152832}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,2.47438], + "samples":[ + {"t":0.0, "x":5.92294, "y":4.01871, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0273, "ay":5.69671, "alpha":0.0115, "fx":[51.51534,51.4223,51.47166,51.56474], "fy":[96.88791,96.93725,96.91091,96.86149]}, + {"t":0.03639, "x":5.92494, "y":4.02248, "heading":-1.5708, "vx":0.11016, "vy":0.20729, "omega":0.00042, "ax":3.02611, "ay":5.69447, "alpha":0.01406, "fx":[51.49999,51.38632,51.44659,51.56032], "fy":[96.84737,96.90761,96.8754,96.81504]}, + {"t":0.07278, "x":5.93095, "y":4.03379, "heading":-1.57078, "vx":0.22027, "vy":0.4145, "omega":0.00093, "ax":3.02374, "ay":5.69, "alpha":0.01917, "fx":[51.46931,51.31442,51.39646,51.55145], "fy":[96.76637,96.84834,96.80436,96.72216]}, + {"t":0.10916, "x":5.94097, "y":4.05264, "heading":-1.57075, "vx":0.3303, "vy":0.62155, "omega":0.00163, "ax":3.01662, "ay":5.67661, "alpha":0.0345, "fx":[51.37746,51.09924,51.24613,51.52469], "fy":[96.52402,96.67071,96.59124,96.44383]}, + {"t":0.14555, "x":5.95498, "y":4.07902, "heading":-1.57069, "vx":0.44007, "vy":0.82811, "omega":0.00288, "ax":0.79883, "ay":1.50329, "alpha":5.44405, "fx":[26.7412,-1.03873,-1.17995,29.82866], "fy":[38.08307,40.19109,12.45553,11.55248]}, + {"t":0.18194, "x":5.97153, "y":4.11015, "heading":-1.57058, "vx":0.46914, "vy":0.88281, "omega":0.20098, "ax":0.00003, "ay":0.00001, "alpha":5.56926, "fx":[14.45428,-14.45942,-14.45326,14.46046], "fy":[14.46007,14.45393,-14.45981,-14.45361]}, + {"t":0.21833, "x":5.9886, "y":4.14227, "heading":-1.56327, "vx":0.46914, "vy":0.88281, "omega":0.40364, "ax":0.0, "ay":0.0, "alpha":4.96921, "fx":[12.80178,-12.99592,-12.80175,12.99595], "fy":[12.99593,12.80176,-12.99594,-12.80177]}, + {"t":0.25472, "x":6.00567, "y":4.17439, "heading":-1.54858, "vx":0.46914, "vy":0.88281, "omega":0.58446, "ax":0.0, "ay":0.0, "alpha":4.42407, "fx":[11.22621,-11.73639,-11.22621,11.73639], "fy":[11.73639,11.22621,-11.73639,-11.22621]}, + {"t":0.2911, "x":6.02274, "y":4.20652, "heading":-1.52732, "vx":0.46914, "vy":0.88281, "omega":0.74544, "ax":0.0, "ay":0.0, "alpha":3.93112, "fx":[9.7513,-10.63843,-9.75131,10.63843], "fy":[10.63843,9.7513,-10.63843,-9.7513]}, + {"t":0.32749, "x":6.03981, "y":4.23864, "heading":-1.50019, "vx":0.46914, "vy":0.88281, "omega":0.88848, "ax":0.0, "ay":0.0, "alpha":3.48714, "fx":[8.39087,-9.66806,-8.39087,9.66806], "fy":[9.66806,8.39087,-9.66806,-8.39087]}, + {"t":0.36388, "x":6.05688, "y":4.27076, "heading":-1.46786, "vx":0.46914, "vy":0.88281, "omega":1.01537, "ax":0.0, "ay":0.0, "alpha":3.08863, "fx":[7.15129,-8.79897,-7.15129,8.79897], "fy":[8.79897,7.15129,-8.79897,-7.15129]}, + {"t":0.40027, "x":6.07395, "y":4.30289, "heading":-1.43091, "vx":0.46914, "vy":0.88281, "omega":1.12776, "ax":0.0, "ay":0.0, "alpha":2.73198, "fx":[6.03369,-8.01126,-6.03369,8.01126], "fy":[8.01126,6.03369,-8.01126,-6.03369]}, + {"t":0.43666, "x":6.09102, "y":4.33501, "heading":-1.38988, "vx":0.46914, "vy":0.88281, "omega":1.22717, "ax":0.0, "ay":0.0, "alpha":2.41354, "fx":[5.03556,-7.2902,-5.03556,7.2902], "fy":[7.2902,5.03556,-7.2902,-5.03556]}, + {"t":0.47304, "x":6.10809, "y":4.36714, "heading":-1.34522, "vx":0.46914, "vy":0.88281, "omega":1.315, "ax":0.0, "ay":0.0, "alpha":2.12979, "fx":[4.15195,-6.62507,-4.15195,6.62507], "fy":[6.62507,4.15195,-6.62507,-4.15195]}, + {"t":0.50943, "x":6.12516, "y":4.39926, "heading":-1.29737, "vx":0.46914, "vy":0.88281, "omega":1.3925, "ax":0.0, "ay":0.0, "alpha":1.87736, "fx":[3.37632,-6.0082,-3.37632,6.0082], "fy":[6.0082,3.37632,-6.0082,-3.37632]}, + {"t":0.54582, "x":6.14224, "y":4.43138, "heading":-1.2467, "vx":0.46914, "vy":0.88281, "omega":1.46081, "ax":0.0, "ay":0.0, "alpha":1.65305, "fx":[2.70116,-5.43413,-2.70115,5.43413], "fy":[5.43413,2.70115,-5.43413,-2.70115]}, + {"t":0.58221, "x":6.15931, "y":4.46351, "heading":-1.19354, "vx":0.46914, "vy":0.88281, "omega":1.52096, "ax":0.0, "ay":0.0, "alpha":1.45391, "fx":[2.11846,-4.89898,-2.11846,4.89898], "fy":[4.89898,2.11846,-4.89898,-2.11846]}, + {"t":0.6186, "x":6.17638, "y":4.49563, "heading":-1.1382, "vx":0.46914, "vy":0.88281, "omega":1.57387, "ax":0.0, "ay":0.0, "alpha":1.27722, "fx":[1.62009,-4.39994,-1.62009,4.39995], "fy":[4.39994,1.62009,-4.39995,-1.62009]}, + {"t":0.65498, "x":6.19345, "y":4.52775, "heading":-1.08093, "vx":0.46914, "vy":0.88281, "omega":1.62034, "ax":0.0, "ay":0.0, "alpha":1.12045, "fx":[1.19798,-3.93492,-1.19798,3.93492], "fy":[3.93492,1.19798,-3.93492,-1.19798]}, + {"t":0.69137, "x":6.21052, "y":4.55988, "heading":-1.02197, "vx":0.46914, "vy":0.88281, "omega":1.66111, "ax":0.0, "ay":0.0, "alpha":0.98134, "fx":[0.84433,-3.50222,-0.84433,3.50222], "fy":[3.50222,0.84433,-3.50222,-0.84433]}, + {"t":0.72776, "x":6.22759, "y":4.592, "heading":-0.96152, "vx":0.46914, "vy":0.88281, "omega":1.69682, "ax":0.0, "ay":0.0, "alpha":0.85781, "fx":[0.55177,-3.10036,-0.55177,3.10036], "fy":[3.10036,0.55177,-3.10036,-0.55177]}, + {"t":0.76415, "x":6.24466, "y":4.62413, "heading":-0.89978, "vx":0.46914, "vy":0.88281, "omega":1.72804, "ax":0.0, "ay":0.0, "alpha":0.748, "fx":[0.3134,-2.728,-0.3134,2.728], "fy":[2.728,0.3134,-2.728,-0.3134]}, + {"t":0.80054, "x":6.26173, "y":4.65625, "heading":-0.8369, "vx":0.46914, "vy":0.88281, "omega":1.75525, "ax":0.0, "ay":0.0, "alpha":0.6502, "fx":[0.12288,-2.38377,-0.12288,2.38377], "fy":[2.38377,0.12288,-2.38377,-0.12288]}, + {"t":0.83692, "x":6.2788, "y":4.68837, "heading":-0.77303, "vx":0.46914, "vy":0.88281, "omega":1.77891, "ax":0.0, "ay":0.0, "alpha":0.56291, "fx":[-0.02556,-2.06631,0.02556,2.06631], "fy":[2.06631,-0.02556,-2.06631,0.02556]}, + {"t":0.87331, "x":6.29587, "y":4.7205, "heading":-0.7083, "vx":0.46914, "vy":0.88281, "omega":1.7994, "ax":0.0, "ay":0.0, "alpha":0.48474, "fx":[-0.13706,-1.77422,0.13706,1.77422], "fy":[1.77422,-0.13706,-1.77422,0.13706]}, + {"t":0.9097, "x":6.31295, "y":4.75262, "heading":-0.64282, "vx":0.46914, "vy":0.88281, "omega":1.81704, "ax":0.0, "ay":0.0, "alpha":0.41446, "fx":[-0.21619,-1.50606,0.21619,1.50605], "fy":[1.50606,-0.21619,-1.50605,0.21619]}, + {"t":0.94609, "x":6.33002, "y":4.78475, "heading":-0.57671, "vx":0.46914, "vy":0.88281, "omega":1.83212, "ax":0.0, "ay":0.0, "alpha":0.35094, "fx":[-0.26692,-1.26037,0.26691,1.26037], "fy":[1.26037,-0.26692,-1.26037,0.26692]}, + {"t":0.98248, "x":6.34709, "y":4.81687, "heading":-0.51004, "vx":0.46914, "vy":0.88281, "omega":1.84489, "ax":0.0, "ay":0.0, "alpha":0.29317, "fx":[-0.29263,-1.03571,0.29263,1.03571], "fy":[1.03571,-0.29263,-1.03571,0.29263]}, + {"t":1.01886, "x":6.36416, "y":4.84899, "heading":-0.44291, "vx":0.46914, "vy":0.88281, "omega":1.85555, "ax":0.0, "ay":0.0, "alpha":0.24023, "fx":[-0.29617,-0.83067,0.29617,0.83067], "fy":[0.83067,-0.29617,-0.83067,0.29617]}, + {"t":1.05525, "x":6.38123, "y":4.88112, "heading":-0.37539, "vx":0.46914, "vy":0.88281, "omega":1.8643, "ax":0.0, "ay":0.0, "alpha":0.19125, "fx":[-0.27986,-0.64389,0.27986,0.64388], "fy":[0.64388,-0.27986,-0.64388,0.27986]}, + {"t":1.09164, "x":6.3983, "y":4.91324, "heading":-0.30755, "vx":0.46914, "vy":0.88281, "omega":1.87125, "ax":0.0, "ay":0.0, "alpha":0.14544, "fx":[-0.24553,-0.4741,0.24553,0.4741], "fy":[0.4741,-0.24553,-0.4741,0.24553]}, + {"t":1.12803, "x":6.41537, "y":4.94536, "heading":-0.23946, "vx":0.46914, "vy":0.88281, "omega":1.87655, "ax":0.0, "ay":0.0, "alpha":0.10205, "fx":[-0.19453,-0.32019,0.19452,0.32019], "fy":[0.32019,-0.19452,-0.32019,0.19453]}, + {"t":1.16442, "x":6.43244, "y":4.97749, "heading":-0.17117, "vx":0.46914, "vy":0.88281, "omega":1.88026, "ax":0.0, "ay":0.0, "alpha":0.06039, "fx":[-0.12777,-0.18117,0.12777,0.18117], "fy":[0.18117,-0.12777,-0.18117,0.12777]}, + {"t":1.2008, "x":6.44951, "y":5.00961, "heading":-0.10275, "vx":0.46914, "vy":0.88281, "omega":1.88246, "ax":0.0, "ay":0.0, "alpha":0.01976, "fx":[-0.04575,-0.05628,0.04575,0.05628], "fy":[0.05628,-0.04575,-0.05628,0.04575]}, + {"t":1.23719, "x":6.46658, "y":5.04174, "heading":-0.03426, "vx":0.46914, "vy":0.88281, "omega":1.88318, "ax":0.0, "ay":0.0, "alpha":-0.02051, "fx":[0.0514,0.05504,-0.05139,-0.05504], "fy":[-0.05504,0.05139,0.05504,-0.0514]}, + {"t":1.27358, "x":6.48365, "y":5.07386, "heading":0.03427, "vx":0.46914, "vy":0.88281, "omega":1.88243, "ax":0.0, "ay":0.0, "alpha":-0.06109, "fx":[0.16392,0.15306,-0.16392,-0.15305], "fy":[-0.15306,0.16392,0.15305,-0.16392]}, + {"t":1.30997, "x":6.50073, "y":5.10598, "heading":0.10277, "vx":0.46914, "vy":0.88281, "omega":1.88021, "ax":0.0, "ay":0.0, "alpha":-0.10265, "fx":[0.2924,0.23772,-0.29239,-0.23772], "fy":[-0.23772,0.29239,0.23772,-0.29239]}, + {"t":1.34636, "x":6.5178, "y":5.13811, "heading":0.17118, "vx":0.46914, "vy":0.88281, "omega":1.87647, "ax":0.0, "ay":0.0, "alpha":-0.14589, "fx":[0.43768,0.30866,-0.43767,-0.30865], "fy":[-0.30865,0.43767,0.30865,-0.43768]}, + {"t":1.38274, "x":6.53487, "y":5.17023, "heading":0.23947, "vx":0.46914, "vy":0.88281, "omega":1.87116, "ax":0.0, "ay":0.0, "alpha":-0.19152, "fx":[0.60089,0.36506,-0.60089,-0.36505], "fy":[-0.36506,0.60089,0.36506,-0.60089]}, + {"t":1.41913, "x":6.55194, "y":5.20235, "heading":0.30755, "vx":0.46914, "vy":0.88281, "omega":1.86419, "ax":0.0, "ay":0.0, "alpha":-0.24032, "fx":[0.7834,0.4057,-0.7834,-0.4057], "fy":[-0.4057,0.7834,0.4057,-0.7834]}, + {"t":1.45552, "x":6.56901, "y":5.23448, "heading":0.37539, "vx":0.46914, "vy":0.88281, "omega":1.85545, "ax":0.0, "ay":0.0, "alpha":-0.29308, "fx":[0.98673,0.42888,-0.98673,-0.42888], "fy":[-0.42888,0.98673,0.42888,-0.98673]}, + {"t":1.49191, "x":6.58608, "y":5.2666, "heading":0.4429, "vx":0.46914, "vy":0.88281, "omega":1.84479, "ax":0.0, "ay":0.0, "alpha":-0.35068, "fx":[1.21258,0.43234,-1.21258,-0.43234], "fy":[-0.43234,1.21258,0.43234,-1.21258]}, + {"t":1.5283, "x":6.60315, "y":5.29873, "heading":0.51003, "vx":0.46914, "vy":0.88281, "omega":1.83202, "ax":0.0, "ay":0.0, "alpha":-0.41405, "fx":[1.46275,0.41329,-1.46275,-0.41329], "fy":[-0.41329,1.46275,0.41329,-1.46275]}, + {"t":1.56468, "x":6.62022, "y":5.33085, "heading":0.5767, "vx":0.46914, "vy":0.88281, "omega":1.81696, "ax":0.0, "ay":0.0, "alpha":-0.48424, "fx":[1.73908,0.36831,-1.73908,-0.36831], "fy":[-0.36831,1.73908,0.36831,-1.73908]}, + {"t":1.60107, "x":6.63729, "y":5.36297, "heading":0.64281, "vx":0.46914, "vy":0.88281, "omega":1.79934, "ax":0.0, "ay":0.0, "alpha":-0.56235, "fx":[2.04347,0.29336,-2.04347,-0.29336], "fy":[-0.29336,2.04347,0.29336,-2.04347]}, + {"t":1.63746, "x":6.65436, "y":5.3951, "heading":0.70829, "vx":0.46914, "vy":0.88281, "omega":1.77888, "ax":0.0, "ay":0.0, "alpha":-0.64963, "fx":[2.37776,0.18372,-2.37776,-0.18372], "fy":[-0.18372,2.37776,0.18372,-2.37776]}, + {"t":1.67385, "x":6.67144, "y":5.42722, "heading":0.77302, "vx":0.46914, "vy":0.88281, "omega":1.75524, "ax":0.0, "ay":0.0, "alpha":-0.74746, "fx":[2.74376,0.03398,-2.74376,-0.03398], "fy":[-0.03398,2.74376,0.03398,-2.74376]}, + {"t":1.71024, "x":6.68851, "y":5.45934, "heading":0.83688, "vx":0.46914, "vy":0.88281, "omega":1.72804, "ax":0.0, "ay":0.0, "alpha":-0.85734, "fx":[3.14319,-0.16198,-3.14319,0.16197], "fy":[0.16198,3.14319,-0.16197,-3.14319]}, + {"t":1.74662, "x":6.70558, "y":5.49147, "heading":0.89976, "vx":0.46914, "vy":0.88281, "omega":1.69684, "ax":0.0, "ay":0.0, "alpha":-0.98097, "fx":[3.57765,-0.41096,-3.57766,0.41096], "fy":[0.41096,3.57765,-0.41096,-3.57765]}, + {"t":1.78301, "x":6.72265, "y":5.52359, "heading":0.96151, "vx":0.46914, "vy":0.88281, "omega":1.66115, "ax":0.0, "ay":0.0, "alpha":-1.12018, "fx":[4.04865,-0.72048,-4.04865,0.72048], "fy":[0.72048,4.04865,-0.72048,-4.04865]}, + {"t":1.8194, "x":6.73972, "y":5.55572, "heading":1.02195, "vx":0.46914, "vy":0.88281, "omega":1.62038, "ax":0.0, "ay":0.0, "alpha":-1.27706, "fx":[4.55759,-1.0987,-4.5576,1.0987], "fy":[1.0987,4.5576,-1.0987,-4.55759]}, + {"t":1.85579, "x":6.75679, "y":5.58784, "heading":1.08092, "vx":0.46914, "vy":0.88281, "omega":1.57391, "ax":0.0, "ay":0.0, "alpha":-1.45386, "fx":[5.10585,-1.55439,-5.10585,1.55439], "fy":[1.55439,5.10585,-1.55439,-5.10585]}, + {"t":1.89218, "x":6.77386, "y":5.61996, "heading":1.13819, "vx":0.46914, "vy":0.88281, "omega":1.52101, "ax":0.0, "ay":0.0, "alpha":-1.65309, "fx":[5.69482,-2.0968,-5.69482,2.0968], "fy":[2.0968,5.69482,-2.0968,-5.69482]}, + {"t":1.92856, "x":6.79093, "y":5.65209, "heading":1.19354, "vx":0.46914, "vy":0.88281, "omega":1.46086, "ax":0.0, "ay":0.0, "alpha":-1.87746, "fx":[6.32614,-2.73554,-6.32614,2.73554], "fy":[2.73554,6.32614,-2.73554,-6.32614]}, + {"t":1.96495, "x":6.808, "y":5.68421, "heading":1.24669, "vx":0.46914, "vy":0.88281, "omega":1.39254, "ax":0.0, "ay":0.0, "alpha":-2.12994, "fx":[7.00186,-3.48036,-7.00186,3.48036], "fy":[3.48036,7.00186,-3.48036,-7.00186]}, + {"t":2.00134, "x":6.82507, "y":5.71633, "heading":1.29737, "vx":0.46914, "vy":0.88281, "omega":1.31504, "ax":0.0, "ay":0.0, "alpha":-2.41371, "fx":[7.72476,-4.34088,-7.72476,4.34088], "fy":[4.34088,7.72476,-4.34088,-7.72476]}, + {"t":2.03773, "x":6.84215, "y":5.74846, "heading":1.34522, "vx":0.46914, "vy":0.88281, "omega":1.22721, "ax":0.0, "ay":0.0, "alpha":-2.73215, "fx":[8.49885,-5.3262,-8.49885,5.3262], "fy":[5.3262,8.49885,-5.3262,-8.49885]}, + {"t":2.07412, "x":6.85922, "y":5.78058, "heading":1.38987, "vx":0.46914, "vy":0.88281, "omega":1.12779, "ax":0.0, "ay":0.0, "alpha":-3.0888, "fx":[9.32988,-6.44438,-9.32988,6.44438], "fy":[6.44438,9.32988,-6.44438,-9.32988]}, + {"t":2.1105, "x":6.87629, "y":5.81271, "heading":1.43091, "vx":0.46914, "vy":0.88281, "omega":1.01539, "ax":0.0, "ay":0.0, "alpha":-3.48729, "fx":[10.22615,-7.7018,-10.22615,7.7018], "fy":[7.7018,10.22615,-7.7018,-10.22615]}, + {"t":2.14689, "x":6.89336, "y":5.84483, "heading":1.46786, "vx":0.46914, "vy":0.88281, "omega":0.8885, "ax":0.0, "ay":0.0, "alpha":-3.93124, "fx":[11.19941,-9.10219,-11.19941,9.10219], "fy":[9.10219,11.19941,-9.10219,-11.19941]}, + {"t":2.18328, "x":6.91043, "y":5.87695, "heading":1.50019, "vx":0.46914, "vy":0.88281, "omega":0.74545, "ax":0.0, "ay":0.0, "alpha":-4.42416, "fx":[12.26595,-10.64554,-12.26595,10.64554], "fy":[10.64554,12.26595,-10.64554,-12.26595]}, + {"t":2.21967, "x":6.9275, "y":5.90908, "heading":1.52731, "vx":0.46914, "vy":0.88281, "omega":0.58446, "ax":0.0, "ay":0.0, "alpha":-4.96926, "fx":[13.44787,-12.32647,-13.44789,12.32644], "fy":[12.32647,13.44789,-12.32645,-13.44787]}, + {"t":2.25606, "x":6.94457, "y":5.9412, "heading":1.54858, "vx":0.46914, "vy":0.88281, "omega":0.40364, "ax":-0.00003, "ay":-0.00001, "alpha":-5.56929, "fx":[14.77396,-14.13275,-14.775,14.1317], "fy":[14.1321,14.77432,-14.13235,-14.77464]}, + {"t":2.29244, "x":6.96164, "y":5.97333, "heading":1.56327, "vx":0.46914, "vy":0.88281, "omega":0.20099, "ax":-0.79883, "ay":-1.50329, "alpha":-5.44418, "fx":[1.30648,-29.73243,-26.8513,0.9261], "fy":[-12.57935,-11.4431,-37.98319,-40.27654]}, + {"t":2.32883, "x":6.97818, "y":6.00445, "heading":1.57058, "vx":0.44007, "vy":0.82811, "omega":0.00288, "ax":-3.01662, "ay":-5.67661, "alpha":-0.0345, "fx":[-51.24607,-51.52468,-51.37751,-51.09926], "fy":[-96.59127,-96.44384,-96.52399,-96.6707]}, + {"t":2.36522, "x":6.9922, "y":6.03083, "heading":1.57069, "vx":0.3303, "vy":0.62155, "omega":0.00163, "ax":-3.02374, "ay":-5.69, "alpha":-0.01917, "fx":[-51.39644,-51.55145,-51.46932,-51.31443], "fy":[-96.80437,-96.72216,-96.76636,-96.84834]}, + {"t":2.40161, "x":7.00222, "y":6.04968, "heading":1.57075, "vx":0.22027, "vy":0.4145, "omega":0.00093, "ax":-3.02611, "ay":-5.69447, "alpha":-0.01406, "fx":[-51.44659,-51.56032,-51.49999,-51.38632], "fy":[-96.8754,-96.81504,-96.84737,-96.90761]}, + {"t":2.438, "x":7.00823, "y":6.06099, "heading":1.57078, "vx":0.11016, "vy":0.20729, "omega":0.00042, "ax":-3.0273, "ay":-5.69671, "alpha":-0.0115, "fx":[-51.47166,-51.56474,-51.51534,-51.4223], "fy":[-96.91091,-96.86149,-96.88791,-96.93725]}, + {"t":2.47438, "x":7.01023, "y":6.06476, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9ade0782..9b9a8ea3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,7 +5,7 @@ public final class Constants { public static final boolean TUNING_MODE = false; public static final double LOOP_PERIOD_SECONDS = 0.02; - public static final RobotType ROBOT = RobotType.V2_REDUNDANCY; + public static final RobotType ROBOT = RobotType.V3_POOT; public static Mode getMode() { switch (ROBOT) { @@ -14,6 +14,7 @@ public static Mode getMode() { case V0_GOMPEIVISION_TEST: case V1_STACKUP: case V2_REDUNDANCY: + case V3_POOT: return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case V0_FUNKY_SIM: @@ -21,6 +22,7 @@ public static Mode getMode() { case V0_GOMPEIVISION_TEST_SIM: case V1_STACKUP_SIM: case V2_REDUNDANCY_SIM: + case V3_POOT_SIM: return Mode.SIM; default: @@ -28,13 +30,13 @@ public static Mode getMode() { } } - public static enum Mode { + public enum Mode { REAL, SIM, REPLAY } - public static enum RobotType { + public enum RobotType { V0_FUNKY, V0_FUNKY_SIM, V0_WHIPLASH, @@ -45,11 +47,13 @@ public static enum RobotType { V1_STACKUP_SIM, V2_REDUNDANCY, V2_REDUNDANCY_SIM, + V3_POOT_SIM, + V3_POOT, } public static void main(String... args) { if (ROBOT == RobotType.V1_STACKUP_SIM) { - System.err.println("Cannot deploy, invalid mode selected: " + ROBOT.toString()); + System.err.println("Cannot deploy, invalid mode selected: " + ROBOT); System.exit(1); } } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index c85ad048..8078fe81 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -14,7 +14,7 @@ public class FieldConstants { public static final double fieldLength = Units.inchesToMeters(690.876); public static final double fieldWidth = Units.inchesToMeters(317); public static final double startingLineX = - Units.inchesToMeters(299.438); // Measured from the inside of starting line + Units.inchesToMeters(299.438); // Measured from the inside of starting line sigma public static class Processor { public static final Pose2d centerFace = @@ -48,18 +48,23 @@ public static class CoralStation { } public static class Reef { - public static enum ReefPose { + public static final double coralWidth = Units.inchesToMeters(2.0); // ffudge + + public enum ReefPose { RIGHT, LEFT, ALGAE, CENTER } - public static enum ReefState { + public enum ReefState { STOW, + POST_PROCESSOR, + HIGH_STOW, CORAL_INTAKE, ALGAE_FLOOR_INTAKE, ALGAE_MID, + HANDOFF, ASS_TOP, ASS_BOT, ALGAE_INTAKE_TOP, @@ -69,10 +74,12 @@ public static enum ReefState { L3, L4, L4_PLUS, - ALGAE_SCORE + ALGAE_SCORE, + STOW_UP, + STOW_DOWN } - public static record FaceSetpoints(Pose2d right, Pose2d left, Pose2d algae, Pose2d center) { + public record FaceSetpoints(Pose2d right, Pose2d left, Pose2d algae, Pose2d center) { public Pose2d getPostSetpoint(ReefPose post) { return post == ReefPose.LEFT ? left : post == ReefPose.RIGHT ? right : center; } @@ -128,8 +135,13 @@ public Pose2d getAlgaeSetpoint() { double adjustYBranch = Units.inchesToMeters(6.469); // Offset Y setpoint by center of tag to reef branch double adjustXBranch = - DriveConstants.DRIVE_CONFIG.bumperWidth() - / 2.0; // Offset X setpoint by center of robot to bumper + Constants.ROBOT.equals(Constants.RobotType.V3_POOT) + || Constants.ROBOT.equals(Constants.RobotType.V3_POOT_SIM) + ? (DriveConstants.DRIVE_CONFIG.bumperWidth() / 2.0) + + FieldConstants.Reef + .coralWidth // Offset X setpoint by center of robot to bumper + coral width + : DriveConstants.DRIVE_CONFIG.bumperWidth() + / 2.0; // Offset X setpoint by center of robot to bumper // double adjustYAlgae = // Units.inchesToMeters(3.5); // Offset Y setpoint by center of tag to algae setpoint @@ -137,6 +149,11 @@ public Pose2d getAlgaeSetpoint() { DriveConstants.DRIVE_CONFIG.bumperWidth() / 2.0 + Units.inchesToMeters(2); // Offset X setpoint by center of robot to bumper + // if (Constants.ROBOT.equals(Constants.RobotType.V3_POOT) + // || Constants.ROBOT.equals(Constants.RobotType.V3_POOT_SIM)) { + // adjustXAlgae += Units.inchesToMeters(5); + // } + reefMap.put( 18, new FaceSetpoints( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aa8761af..3b4fd577 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,14 +1,11 @@ package frc.robot; +import choreo.Choreo; import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.MathShared; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.IterativeRobotBase; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.Watchdog; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; @@ -20,16 +17,20 @@ import frc.robot.subsystems.v0_Whiplash.V0_WhiplashRobotContainer; import frc.robot.subsystems.v1_StackUp.V1_StackUpRobotContainer; import frc.robot.subsystems.v2_Redundancy.V2_RedundancyRobotContainer; +import frc.robot.subsystems.v3_Poot.V3_PootRobotContainer; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; import frc.robot.util.CanivoreReader; import frc.robot.util.InternalLoggedTracer; import frc.robot.util.PhoenixUtil; import frc.robot.util.VirtualSubsystem; +import java.io.File; import java.lang.reflect.Field; +import java.lang.reflect.InvocationTargetException; import java.util.HashMap; import java.util.Map; import java.util.function.BiConsumer; +import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -122,7 +123,10 @@ public void robotInit() { case SIM: // Running a physics simulator, log to NT + // setUseTiming(false); Logger.addDataReceiver(new NT4Publisher()); + // setting up maple sim field + SimulatedArena.getInstance(); break; case REPLAY: @@ -155,6 +159,7 @@ public void robotInit() { V0_GOMPEIVISION_TEST_SIM -> new v0_GompeivisionTestRobotContainer(); case V1_STACKUP, V1_STACKUP_SIM -> new V1_StackUpRobotContainer(); case V2_REDUNDANCY, V2_REDUNDANCY_SIM -> new V2_RedundancyRobotContainer(); + case V3_POOT, V3_POOT_SIM -> new V3_PootRobotContainer(); default -> new RobotContainer() {}; }; @@ -194,6 +199,17 @@ public double getTimestamp() { } }); + try { + var m = Choreo.class.getDeclaredMethod("setChoreoDir", File.class); + m.setAccessible(true); + m.invoke( + null, + new File( + Filesystem.getDeployDirectory(), + Constants.ROBOT.name().toLowerCase().replaceFirst("_sim", "") + "/choreo")); + } catch (NoSuchMethodException | InvocationTargetException | IllegalAccessException e) { + throw new RuntimeException(e); + } // Log active commands Map commandCounts = new HashMap<>(); BiConsumer logCommandFunction = @@ -255,44 +271,6 @@ public void robotPeriodic() { lowBatteryAlert.set(true); } InternalLoggedTracer.record("Check Battery Alert", "Robot"); - - // Check CAN status - // LoggedTracer.reset(); - // var canStatus = RobotController.getCANStatus(); - // if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { - // canErrorTimer.restart(); - // } - // canErrorAlert.set( - // !canErrorTimer.hasElapsed(canErrorTimeThreshold) - // && !canErrorTimerInitial.hasElapsed(canErrorTimeThreshold)); - - // // Log CANivore status - // if (Constants.getMode() == Constants.Mode.REAL) { - // var canivoreStatus = canivoreReader.getStatus(); - // if (canivoreStatus.isPresent()) { - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "Status", canivoreStatus.get().Status.getName()); - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "Utilization", canivoreStatus.get().BusUtilization); - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "OffCount", canivoreStatus.get().BusOffCount); - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "TxFullCount", canivoreStatus.get().TxFullCount); - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "ReceiveErrorCount", canivoreStatus.get().REC); - // Logger.recordOutput( - // NTPrefixes.CANIVORE_STATUS + "TransmitErrorCount", canivoreStatus.get().TEC); - // if (!canivoreStatus.get().Status.isOK() - // || canStatus.transmitErrorCount > 0 - // || canStatus.receiveErrorCount > 0) { - // canivoreErrorTimer.restart(); - // } - // } - // canivoreErrorAlert.set( - // !canivoreErrorTimer.hasElapsed(canivoreErrorTimeThreshold) - // && !canErrorTimerInitial.hasElapsed(canErrorTimeThreshold)); - // } - // LoggedTracer.record("Check CANivore Status", "Robot"); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index ad7ca746..7c211b2d 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -17,11 +18,7 @@ import frc.robot.FieldConstants.Reef.ReefState; import frc.robot.subsystems.shared.drive.DriveConstants; import frc.robot.subsystems.shared.vision.Camera; -import frc.robot.util.AllianceFlipUtil; -import frc.robot.util.ExternalLoggedTracer; -import frc.robot.util.GeometryUtil; -import frc.robot.util.InternalLoggedTracer; -import frc.robot.util.NTPrefixes; +import frc.robot.util.*; import java.util.Optional; import lombok.Getter; import lombok.Setter; @@ -42,11 +39,13 @@ public class RobotState { @Getter @Setter private static RobotMode mode; @Getter @Setter private static boolean hasAlgae; + @Getter private static ScoreSide scoreSide; @Getter @Setter private static boolean isIntakingCoral; @Getter @Setter private static boolean isIntakingAlgae; @Getter @Setter private static boolean isAutoAligning; @Getter @Setter private static boolean autoClapOverride; + @Getter @Setter private static boolean climberReady; private static final TimeInterpolatableBuffer poseBuffer; @@ -67,9 +66,13 @@ public class RobotState { case V2_REDUNDANCY: case V2_REDUNDANCY_SIM: break; + case V3_POOT: + case V3_POOT_SIM: + break; } OIData = new OperatorInputData(ReefPose.LEFT, ReefState.STOW); + scoreSide = ScoreSide.RIGHT; robotHeading = new Rotation2d(); headingOffset = new Rotation2d(); @@ -136,15 +139,87 @@ public static void periodic( InternalLoggedTracer.record("Get Minimum Distance To Reef Tag", "RobotState/Periodic"); // if (RobotMode.disabled()) { - // resetRobotPose(getRobotPoseField()); + // resetRobotPose(getRobotPoseField()); // } InternalLoggedTracer.reset(); - Pose2d autoAlignCoralSetpoint = + + // Code will set the robot to the correct position based on the reef tag it is + // closest to for + // auto + // alignment to the reef to score coral + + // --- 1. Get the BASE setpoint (the target location on the field) --- + + Pose2d baseCoralSetpoint = OIData.currentReefHeight().equals(ReefState.L1) ? Reef.reefMap.get(closestReefTag).getPostSetpoint(ReefPose.CENTER) : Reef.reefMap.get(closestReefTag).getPostSetpoint(OIData.currentReefPost()); + + Pose2d autoAlignCoralSetpoint = baseCoralSetpoint; + + if (OIData.currentReefHeight().equals(ReefState.L1) + || !(Constants.ROBOT.equals(Constants.RobotType.V3_POOT) + || Constants.ROBOT.equals(Constants.RobotType.V3_POOT_SIM))) { + scoreSide = ScoreSide.CENTER; + } else { + + // --- 2. Automatically determine the closest side of the ROBOT --- + if (Math.abs( + MathUtil.angleModulus( + baseCoralSetpoint + .getRotation() + .rotateBy(Rotation2d.fromDegrees(-90)) + .minus(RobotState.getRobotPoseField().getRotation()) + .getRadians())) + <= Math.abs( + MathUtil.angleModulus( + baseCoralSetpoint + .getRotation() + .rotateBy(Rotation2d.fromDegrees(90)) + .minus(RobotState.getRobotPoseField().getRotation()) + .getRadians()))) { + scoreSide = ScoreSide.RIGHT; + } else { + scoreSide = ScoreSide.LEFT; + } + + // --- 3. Now, create the FINAL setpoint using the newly-set scoreSide --- + if (scoreSide == ScoreSide.RIGHT) { + autoAlignCoralSetpoint = + new Pose2d( + baseCoralSetpoint.getX(), + baseCoralSetpoint.getY(), + baseCoralSetpoint.getRotation().rotateBy(new Rotation2d(-Math.PI / 2))); + } else { + autoAlignCoralSetpoint = + new Pose2d( + baseCoralSetpoint.getX(), + baseCoralSetpoint.getY(), + baseCoralSetpoint.getRotation().rotateBy(new Rotation2d(Math.PI / 2))); + } + } + + // @Author: Abhiraam Venigalla, Ananth Krishna Gomattam, Atharv Joshi, Chris Xu, + // Anshu Adiga, + // Adnan Dembele, Hartej Anand + // Code will rotate the robot if we need to score in a certain region + // Rotates the robot relative to the reef to the orientation desired + Pose2d autoAlignAlgaeSetpoint = Reef.reefMap.get(closestReefTag).getAlgaeSetpoint(); + if (scoreSide == ScoreSide.RIGHT) { + autoAlignAlgaeSetpoint = + new Pose2d( + autoAlignAlgaeSetpoint.getX(), + autoAlignAlgaeSetpoint.getY(), + autoAlignAlgaeSetpoint.getRotation().rotateBy(new Rotation2d(-Math.PI / 2))); + } else if (scoreSide == ScoreSide.LEFT) { + autoAlignAlgaeSetpoint = + new Pose2d( + autoAlignAlgaeSetpoint.getX(), + autoAlignAlgaeSetpoint.getY(), + autoAlignAlgaeSetpoint.getRotation().rotateBy(new Rotation2d(Math.PI / 2))); + } double distanceToCoralSetpoint = RobotState.getRobotPoseReef() @@ -157,33 +232,34 @@ public static void periodic( boolean atCoralSetpoint = Math.abs(distanceToCoralSetpoint) - <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS.positionThresholdMeters().get(); + <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS.positionThresholdMeters().get() + && Math.abs( + autoAlignCoralSetpoint + .getRotation() + .minus(getRobotPoseReef().getRotation()) + .getRadians()) + <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS + .omegaPIDConstants() + .tolerance() + .get(); boolean atAlgaeSetpoint = Math.abs(distanceToAlgaeSetpoint) - <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS.positionThresholdMeters().get(); - - ReefState algaeHeight; - switch (closestReefTag) { - case 10: - case 6: - case 8: - case 21: - case 17: - case 19: - algaeHeight = ReefState.ALGAE_INTAKE_BOTTOM; - break; - case 9: - case 11: - case 7: - case 22: - case 20: - case 18: - algaeHeight = ReefState.ALGAE_INTAKE_TOP; - break; - default: - algaeHeight = ReefState.ALGAE_INTAKE_BOTTOM; - break; - } + <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS.positionThresholdMeters().get() + && Math.abs( + autoAlignAlgaeSetpoint + .getRotation() + .minus(getRobotPoseReef().getRotation()) + .getRadians()) + <= DriveConstants.ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS + .omegaPIDConstants() + .tolerance() + .get(); + + ReefState algaeHeight = + switch (closestReefTag) { + case 9, 11, 7, 22, 20, 18 -> ReefState.ALGAE_INTAKE_TOP; + default -> ReefState.ALGAE_INTAKE_BOTTOM; + }; InternalLoggedTracer.record("Generate Setpoints", "RobotState/Periodic"); InternalLoggedTracer.reset(); @@ -225,6 +301,7 @@ public static void periodic( Logger.recordOutput(NTPrefixes.ALGAE_DATA + "Algae Setpoint Error", distanceToAlgaeSetpoint); Logger.recordOutput(NTPrefixes.ALGAE_DATA + "At Algae Setpoint", atAlgaeSetpoint); Logger.recordOutput(NTPrefixes.ALGAE_DATA + "Algae Height", algaeHeight); + Logger.recordOutput(NTPrefixes.REEF_DATA + "Score Side", scoreSide); ExternalLoggedTracer.record("Robot State Total", "RobotState/Periodic"); } @@ -270,47 +347,27 @@ private static int getMinDistanceReefTag() { } if (AllianceFlipUtil.shouldFlip()) { - switch (minDistanceTag) { - case 0: - minDistanceTag = 7; - break; - case 1: - minDistanceTag = 6; - break; - case 2: - minDistanceTag = 11; - break; - case 3: - minDistanceTag = 10; - break; - case 4: - minDistanceTag = 9; - break; - case 5: - minDistanceTag = 8; - break; - } + minDistanceTag = + switch (minDistanceTag) { + case 0 -> 7; + case 1 -> 6; + case 2 -> 11; + case 3 -> 10; + case 4 -> 9; + case 5 -> 8; + default -> minDistanceTag; + }; } else { - switch (minDistanceTag) { - case 0: - minDistanceTag = 18; - break; - case 1: - minDistanceTag = 19; - break; - case 2: - minDistanceTag = 20; - break; - case 3: - minDistanceTag = 21; - break; - case 4: - minDistanceTag = 22; - break; - case 5: - minDistanceTag = 17; - break; - } + minDistanceTag = + switch (minDistanceTag) { + case 0 -> 18; + case 1 -> 19; + case 2 -> 20; + case 3 -> 21; + case 4 -> 22; + case 5 -> 17; + default -> minDistanceTag; + }; } return minDistanceTag; } @@ -346,7 +403,7 @@ public static Optional getBufferedPose(double timestamp) { return poseBuffer.getSample(timestamp); } - public static final record ReefAlignData( + public record ReefAlignData( int closestReefTag, Pose2d coralSetpoint, Pose2d algaeSetpoint, @@ -357,14 +414,12 @@ public static final record ReefAlignData( ReefState algaeIntakeHeight, Camera... cameras) {} - public static final record OperatorInputData( - ReefPose currentReefPost, ReefState currentReefHeight) {} + public record OperatorInputData(ReefPose currentReefPost, ReefState currentReefHeight) {} - public static final record HeadingData( + public record HeadingData( Rotation2d robotHeading, long latestRobotHeadingTimestamp, double robotYawVelocity) {} - public static final record VisionObservation( - Pose2d pose, double timestamp, Matrix stddevs) {} + public record VisionObservation(Pose2d pose, double timestamp, Matrix stddevs) {} public enum RobotMode { DISABLED, @@ -403,4 +458,10 @@ public static boolean auto() { return auto(RobotState.getMode()); } } + + public enum ScoreSide { + LEFT, + RIGHT, + CENTER + } } diff --git a/src/main/java/frc/robot/commands/AutonomousCommands.java b/src/main/java/frc/robot/commands/AutonomousCommands.java index 53b17b96..a4f5cd34 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommands.java +++ b/src/main/java/frc/robot/commands/AutonomousCommands.java @@ -19,7 +19,12 @@ import frc.robot.subsystems.v2_Redundancy.superstructure.V2_RedundancySuperstructure; import frc.robot.subsystems.v2_Redundancy.superstructure.V2_RedundancySuperstructureStates; import frc.robot.subsystems.v2_Redundancy.superstructure.intake.V2_RedundancyIntake; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructure; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureStates; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.GeometryUtil; import frc.robot.util.LoggedChoreo.LoggedAutoRoutine; import frc.robot.util.LoggedChoreo.LoggedAutoTrajectory; import java.util.Optional; @@ -148,6 +153,14 @@ public static void loadAutoTrajectories(Drive drive) { drive.getAutoFactory().cache().loadTrajectory("C_RIGHT_PATH3"); drive.getAutoFactory().cache().loadTrajectory("D_CENTER_PATH"); + + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_1"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_2"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_3"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_4"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_5"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_6"); + drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_7"); } public static final Command autoALeft( @@ -1077,4 +1090,1081 @@ public static final LoggedAutoRoutine autoDCenter( V2_RedundancySuperstructureStates.SCORE_L4, 0.5))); return routine; } + + // V3 + + // public static final LoggedAutoRoutine autoALeft( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoALeft"); + + // LoggedAutoTrajectory path1 = routine.trajectory("A_LEFT_PATH1"); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("A_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("A_LEFT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path4 = + // routine + // .trajectory("A_LEFT_PATH4") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path4.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoALeftNashoba( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoALeftNashoba"); + + // LoggedAutoTrajectory path1 = routine.trajectory("A_LEFT_PATH1"); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("A_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("A_LEFT_PATH_ALT3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path4 = + // routine + // .trajectory("A_LEFT_PATH_ALT4") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path4.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoALeftDAVE( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoALeftD.A.V.E."); + + // LoggedAutoTrajectory path1 = routine.trajectory("A_LEFT_PATH1"); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("A_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("A_LEFT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path4 = + // routine + // .trajectory("A_LEFT_PATH4_ALT_ALT") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path4.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoARight( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoARight"); + + // LoggedAutoTrajectory path1 = routine.trajectory("A_RIGHT_PATH1"); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("A_RIGHT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("A_RIGHT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path4 = + // routine + // .trajectory("A_RIGHT_PATH4") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.25), + // Commands.deadline( + // path4.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoBLeft( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoBLeft"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("B_LEFT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("B_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // path1.cmd(), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE)), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // superstructure.runGoal(V3_PootSuperstructureStates.L4)), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // superstructure.runGoal(V3_PootSuperstructureStates.STOW_DOWN))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoCLeft( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoCLeft"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("C_LEFT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("C_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("C_LEFT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoCLeftPush( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoCLeft"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("C_LEFT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("C_LEFT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("C_LEFT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runEnd( + // () -> drive.runVelocity(new ChassisSpeeds(0.0, -1.0, 0.0)), + // () -> drive.stop()) + // .withTimeout(0.5), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoCRight( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoCRight"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("C_RIGHT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("C_RIGHT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("C_RIGHT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + // return routine; + // } + + // public static final LoggedAutoRoutine autoCRightPush( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoCRight"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("C_RIGHT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("C_RIGHT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path3 = + // routine + // .trajectory("C_RIGHT_PATH3") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // Commands.runEnd( + // () -> drive.runVelocity(new ChassisSpeeds(0.0, 1.0, 0.0)), + // () -> drive.stop()) + // .withTimeout(0.5), + // path1.cmd(), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path3.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.runOnce(() -> RobotState.setAutoClapOverride(false)), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // Commands.waitUntil(() -> superstructure.atGoal())), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + // return routine; + // } + + // public static final LoggedAutoRoutine autoBRight( + // Drive drive, + // V3_PootIntake intake, + // V3_PootSuperstructure superstructure, + // V3_PootManipulator manipulator, + // Camera... cameras) { + + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoBRight"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("B_RIGHT_PATH1") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + // LoggedAutoTrajectory path2 = + // routine + // .trajectory("B_RIGHT_PATH2") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + // path1.cmd(), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // superstructure.runGoal(V3_PootSuperstructureStates.L4)), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // Commands.deadline( + // path2.cmd(), + // CompositeCommands.V3_PootCompositeCommands.intakeCoralDriverSequence( + // superstructure, intake, manipulator), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE)), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5), + // superstructure.runGoal(V3_PootSuperstructureStates.STOW_DOWN))); + + // return routine; + // } + + // public static final LoggedAutoRoutine autoDCenter( + // Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + // LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoDCenter"); + // LoggedAutoTrajectory path1 = + // routine + // .trajectory("D_CENTER_PATH") + // .bindEvent("Funnel", Commands.runOnce(() -> + // RobotState.setAutoClapOverride(true))); + + // routine + // .active() + // .onTrue( + // Commands.sequence( + // path1.resetOdometry(), + // Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + // path1.cmd(), + // Commands.parallel( + // DriveCommands.autoAlignReefCoral(drive, cameras), + // superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE)), + // superstructure.runActionWithTimeout( + // V3_PootSuperstructureStates.STOW_DOWN, + // V3_PootSuperstructureStates.L4_SCORE, + // 0.5))); + // return routine; + // } + + public static final LoggedAutoRoutine autoERight( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoERight"); + LoggedAutoTrajectory path1 = routine.trajectory("E_RIGHT_PATH_1"); + LoggedAutoTrajectory path2 = routine.trajectory("E_RIGHT_PATH_2"); + LoggedAutoTrajectory path3 = routine.trajectory("E_RIGHT_PATH_3"); + LoggedAutoTrajectory path4 = routine.trajectory("E_RIGHT_PATH_4"); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + Commands.parallel( + path1.cmd(), superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP)), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path2.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.5))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L4))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + path3.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.5))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L4))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path4.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.5))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L4))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE))); + + return routine; + } + + public static final LoggedAutoRoutine autoELeft( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoELeft"); + LoggedAutoTrajectory path1 = + routine.trajectory(GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_1"))); + LoggedAutoTrajectory path2 = + routine.trajectory(GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_2"))); + LoggedAutoTrajectory path3 = + routine.trajectory(GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_3"))); + LoggedAutoTrajectory path4 = + routine.trajectory(GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_4"))); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path1.cmd(), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + path2.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((2.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path3.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.5))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + path4.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.5))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE))); + + return routine; + } + + public static final LoggedAutoRoutine autoERightBack( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoERightBack"); + LoggedAutoTrajectory path1 = routine.trajectory("E_RIGHT_PATH_1_BACK"); + LoggedAutoTrajectory path2 = routine.trajectory("E_RIGHT_PATH_2_BACK"); + LoggedAutoTrajectory path3 = routine.trajectory("E_RIGHT_PATH_3_BACK"); + LoggedAutoTrajectory path4 = routine.trajectory("E_RIGHT_PATH_4_BACK"); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + path1.cmd(), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + path2.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L2))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L2, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L2_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path3.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L2))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L2, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L2_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + path4.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L4))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE))); + + return routine; + } + + public static final LoggedAutoRoutine autoELeftBack( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoELeftBack"); + LoggedAutoTrajectory path1 = + routine.trajectory( + GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_1_BACK"))); + LoggedAutoTrajectory path2 = + routine.trajectory( + GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_2_BACK"))); + LoggedAutoTrajectory path3 = + routine.trajectory( + GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_3_BACK"))); + LoggedAutoTrajectory path4 = + routine.trajectory( + GeometryUtil.mirrorTrajectory(routine.trajectory("E_RIGHT_PATH_4_BACK"))); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT)), + path1.cmd(), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + path2.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L2))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L2, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L2_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + path3.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L2))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L2, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L2_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + path4.cmd(), + superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_CORAL) + .andThen(Commands.waitSeconds((1.0))) + .andThen(superstructure.runGoal(V3_PootSuperstructureStates.L4))), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE))); + + return routine; + } + + public static final LoggedAutoRoutine autoFLeft( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoFLeft"); + LoggedAutoTrajectory path1 = (routine.trajectory("F_PATH_1")); + LoggedAutoTrajectory path2 = (routine.trajectory("F_PATH_2")); + LoggedAutoTrajectory path3 = (routine.trajectory("F_PATH_3")); + LoggedAutoTrajectory path4 = (routine.trajectory("F_PATH_4")); + LoggedAutoTrajectory path5 = (routine.trajectory("F_PATH_5")); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.parallel( + path2.cmd(), + superstructure.runGoal(V3_PootSuperstructureStates.L3_ALGAE_INTAKE)), + Commands.parallel( + path3.cmd(), superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP)), + Commands.runOnce(() -> drive.stop()), + CompositeCommands.V3_PootCompositeCommands.optimalScoreBarge(superstructure), + Commands.parallel( + path4.cmd(), + superstructure.runGoal(V3_PootSuperstructureStates.L2_ALGAE_INTAKE)), + Commands.parallel( + path5.cmd(), superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP)), + Commands.runOnce(() -> drive.stop()), + CompositeCommands.V3_PootCompositeCommands.optimalScoreBarge(superstructure))); + + return routine; + } + + public static final LoggedAutoRoutine autoFLeftMinimal( + Drive drive, + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator, + Camera... cameras) { + + LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("autoFLeftMinimal"); + LoggedAutoTrajectory path1 = (routine.trajectory("F_PATH_1")); + + routine + .active() + .onTrue( + Commands.sequence( + path1.resetOdometry(), + Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT)), + CompositeCommands.V3_PootCompositeCommands.optimalAutoScoreCoralSequence( + drive, superstructure, ReefState.L4, cameras), + superstructure.runGoal(V3_PootSuperstructureStates.L4_SCORE), + superstructure.waitUntilAtGoal(), + Commands.runOnce(() -> drive.stop()))); + + return routine; + } } diff --git a/src/main/java/frc/robot/commands/CompositeCommands.java b/src/main/java/frc/robot/commands/CompositeCommands.java index 84677853..86b15524 100644 --- a/src/main/java/frc/robot/commands/CompositeCommands.java +++ b/src/main/java/frc/robot/commands/CompositeCommands.java @@ -22,7 +22,13 @@ import frc.robot.subsystems.v2_Redundancy.superstructure.V2_RedundancySuperstructureStates; import frc.robot.subsystems.v2_Redundancy.superstructure.intake.V2_RedundancyIntake; import frc.robot.subsystems.v2_Redundancy.superstructure.manipulator.V2_RedundancyManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructure; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureStates; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState; import frc.robot.util.AllianceFlipUtil; +import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -648,4 +654,297 @@ public static final Command homingSequences( intake.homingSequence(), Commands.runOnce(() -> elevator.setPosition())); } + + public static final class V3_PootCompositeCommands { + + public static final Command intakeCoralDriverSequence( + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + return Commands.sequence( + Commands.runOnce(() -> RobotState.setHasAlgae(false)), + superstructure.runGoalUntil(V3_PootSuperstructureStates.GROUND_INTAKE, () -> false), + superstructure.runGoalUntil(V3_PootSuperstructureStates.HANDOFF, () -> false)); + } + + public static final Command postIntakeCoralSequence( + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + return Commands.either( + Commands.either( + Commands.sequence( + superstructure.runGoalUntil( + V3_PootSuperstructureStates.HANDOFF_SPIN, () -> manipulator.hasCoral()), + superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP)), + Commands.none(), + () -> + !superstructure.getTargetState().equals(V3_PootSuperstructureStates.STOW_UP) + && !superstructure + .getCurrentState() + .equals(V3_PootSuperstructureStates.STOW_UP)), + superstructure.runGoal(V3_PootSuperstructureStates.L1), + () -> !RobotState.getOIData().currentReefHeight().equals(ReefState.L1)); + } + + /** + * Creates a command to automatically align the robot to the optimal side of the coral based on + * the current reef height and the robot's orientation. This command sets the score side in the + * RobotState and then runs the auto-alignment command. After the command ends, it resets the + * score side to center. + * + * @param drive The drive subsystem. + * @return A command to auto-align to the optimal side of the coral. + */ + public static final Command optimalAutoScoreCoralSequence( + Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + return Commands.either( + Commands.sequence( + superstructure.setPosition(), + DriveCommands.autoAlignReefCoral(drive, cameras) + .until(() -> RobotState.getReefAlignData().atCoralSetpoint()), + superstructure.runReefScoreGoal(() -> RobotState.getOIData().currentReefHeight())), + superstructure.runGoalUntil(V3_PootSuperstructureStates.L1_SCORE, () -> false), + () -> !RobotState.getOIData().currentReefHeight().equals(ReefState.L1)); + } + + public static final Command optimalAutoScoreCoralSequence( + Drive drive, V3_PootSuperstructure superstructure, ReefState height, Camera... cameras) { + return Commands.sequence( + superstructure.runReefGoal(() -> height), + DriveCommands.autoAlignReefCoral(drive, cameras), + Commands.waitUntil(() -> RobotState.getReefAlignData().atCoralSetpoint()), + superstructure + .runReefScoreGoal(() -> height) + .until( + () -> { + if (height.equals(ReefState.L4)) return superstructure.armBelowThreshold(); + else return false; + })); + } + + public static final Command optimalAutoAlignReefAlgae( + Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + return Commands.sequence( + superstructure.runGoal(V3_PootSuperstructureStates.STOW_DOWN), + DriveCommands.autoAlignReefAlgae(drive, cameras)); + } + + public static final Command optimalScoreBarge(V3_PootSuperstructure superstructure) { + return superstructure + .runGoal(V3_PootSuperstructureStates.BARGE_SCORE) + .andThen(superstructure.runActionWithTimeout(V3_PootSuperstructureStates.BARGE_SCORE, 1)); + } + + /** + * Creates a command to score coral. + * + * @param superstructure The superstructure subsystem. + * @param goal This is the goal. + * @return A command to score coral. + */ + public static final Command scoreCoral( + V3_PootSuperstructure superstructure, Supplier goal) { + return superstructure.runReefScoreGoal(goal); + } + + /** + * public static final Command intakeAlgaeReef(V3_PootSuperstructure superstructure, + * V3_PootSuperstructureStates goal, V3_PootSuperstructureAction action, V3_PootIntake intake, + * V3_PootSuperstructure hasalgae) { return Commands.sequence( superstructure.runGoal(), + * Commands.run(() -> action.runIntake(intake)), superstructure.isHasAlgae() == + * (edge.getGamePieceEdge() != (GamePieceEdge.NO_ALGAE) ); ); } + */ + + /** + * drive to reef go to algae level (L2 or L3) turn intake on go until it has algae then stow up + */ + public static final Command dropAlgae( + Drive drive, + ElevatorFSM elevator, + V3_PootManipulator manipulator, + V3_PootIntake intake, + V3_PootSuperstructure superstructure, + Supplier level, + Camera... cameras) { + return Commands.sequence( + Commands.parallel( + superstructure.runGoalUntil( + () -> { + switch (level.get()) { + case ALGAE_INTAKE_TOP: + return V3_PootSuperstructureStates.L3_ALGAE_INTAKE; + case ALGAE_INTAKE_BOTTOM: + return V3_PootSuperstructureStates.L2_ALGAE_INTAKE; + default: + return V3_PootSuperstructureStates.STOW_DOWN; + } + }, + () -> RobotState.isHasAlgae()), + DriveCommands.autoAlignReefAlgae(drive, cameras)), + Commands.runEnd( + () -> drive.runVelocity(new ChassisSpeeds(0.0, 2.0, 0.0)), () -> drive.stop()) + .withTimeout(0.5), + superstructure.runActionWithTimeout( + () -> { + switch (level.get()) { + case ALGAE_INTAKE_TOP: + return V3_PootSuperstructureStates.L3_ALGAE_DROP; + case ALGAE_INTAKE_BOTTOM: + return V3_PootSuperstructureStates.L2_ALGAE_DROP; + default: + return V3_PootSuperstructureStates.L2_ALGAE_DROP; + } + }, + () -> { + switch (level.get()) { + case ALGAE_INTAKE_TOP: + return V3_PootSuperstructureStates.L3_ALGAE_DROP; + case ALGAE_INTAKE_BOTTOM: + return V3_PootSuperstructureStates.L2_ALGAE_DROP; + default: + return V3_PootSuperstructureStates.L2_ALGAE_DROP; + } + }, + () -> 2)) + .finallyDo( + () -> { + RobotState.setHasAlgae(false); + }); + } + + public static final Command intakeAlgaeFromReef( + Drive drive, + V3_PootSuperstructure superstructure, + Supplier level, + Camera... cameras) { + + return Commands.sequence( + Commands.sequence( + superstructure.runGoalUntil( + () -> { + switch (level.get()) { + case ALGAE_INTAKE_TOP: + return V3_PootSuperstructureStates.L3_ALGAE_INTAKE; + case ALGAE_INTAKE_BOTTOM: + return V3_PootSuperstructureStates.L2_ALGAE_INTAKE; + default: + return V3_PootSuperstructureStates.STOW_DOWN; + } + }, + () -> RobotState.isHasAlgae()), + DriveCommands.autoAlignReefAlgae(drive, cameras)), + Commands.runEnd( + () -> drive.runVelocity(new ChassisSpeeds(0.0, 2.0, 0.0)), () -> drive.stop()) + .withTimeout(0.5), + postIntakeAlgaeFromReef(drive, superstructure, cameras)); + } + + public static final Command intakeAlgaeFromReefAuto( + Drive drive, + V3_PootSuperstructure superstructure, + Supplier level, + Camera... cameras) { + + return Commands.sequence( + Commands.parallel( + superstructure.runGoalUntil( + () -> { + switch (level.get()) { + case ALGAE_INTAKE_TOP: + return V3_PootSuperstructureStates.L3_ALGAE_INTAKE; + case ALGAE_INTAKE_BOTTOM: + return V3_PootSuperstructureStates.L2_ALGAE_INTAKE; + default: + return V3_PootSuperstructureStates.STOW_DOWN; + } + }, + () -> RobotState.isHasAlgae()), + DriveCommands.autoAlignReefAlgae(drive, cameras)), + postIntakeAlgaeFromReef(drive, superstructure, cameras)); + } + + public static final Command postIntakeAlgaeFromReef( + Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + return superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP); + } + + public static final Command intakeAlgaeFromReef( + Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + return intakeAlgaeFromReef( + drive, superstructure, () -> RobotState.getReefAlignData().algaeIntakeHeight(), cameras); + } + + public static final Command intakeAlgaeFromReefAuto( + Drive drive, V3_PootSuperstructure superstructure, Camera... cameras) { + return intakeAlgaeFromReefAuto( + drive, superstructure, () -> RobotState.getReefAlignData().algaeIntakeHeight(), cameras); + } + + public static final Command emergencyEject( + V3_PootManipulator manipulator, V3_PootSuperstructure superstructure) { + return Commands.sequence( + superstructure.override( + () -> { + manipulator.setArmGoal( + frc.robot + .subsystems + .v3_Poot + .superstructure + .manipulator + .V3_PootManipulatorConstants + .ManipulatorArmState + .EMERGENCY_EJECT_ANGLE); + manipulator.setRollerGoal( + frc.robot + .subsystems + .v3_Poot + .superstructure + .manipulator + .V3_PootManipulatorConstants + .ManipulatorRollerState + .L4_SCORE); + })); + } + + public static final Command intakeAlgaeFloor( + V3_PootSuperstructure superstructure, V3_PootManipulator manipulator) { + return superstructure + .runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_ALGAE) + .until(() -> manipulator.hasAlgae()); // add intake for algae after + } + + public static final Command handoffCoral( + V3_PootSuperstructure superstructure, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + return Commands.sequence( + superstructure.runGoal(V3_PootSuperstructureStates.HANDOFF), + Commands.waitUntil(() -> manipulator.hasCoral()), + superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP)); + } + + public static final Command manipulatorGroundIntake( + V3_PootManipulator manipulator, V3_PootSuperstructure superstructure) { + return Commands.sequence( + Commands.runOnce( + () -> superstructure.runGoal(V3_PootSuperstructureStates.GROUND_INTAKE_ALGAE)), + Commands.runOnce(() -> manipulator.setRollerGoal(ManipulatorRollerState.CORAL_INTAKE))); + } + + public static final Command setDynamicReefHeight( + ReefState height, V3_PootSuperstructure superstructure) { + return Commands.either( + Commands.sequence( + Commands.runOnce(() -> RobotState.setReefHeight(height)), + superstructure.setPosition()), + Commands.none(), + () -> + Set.of( + V3_PootSuperstructureStates.L2, + V3_PootSuperstructureStates.L3, + V3_PootSuperstructureStates.L4) + .contains(superstructure.getCurrentState())); + } + } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 36320981..eee42a9f 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -430,6 +430,35 @@ public static Command autoAlignReefCoral(Drive drive, Camera... cameras) { .coralSetpoint() .getRotation() .getRadians()); + + ChassisSpeeds measuredSpeeds = drive.getMeasuredChassisSpeeds(); + double vx_prime = + measuredSpeeds.vxMetersPerSecond + * Math.cos( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()) + + measuredSpeeds.vyMetersPerSecond + * Math.sin( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()); + + double vy_prime = + -measuredSpeeds.vxMetersPerSecond + * Math.sin( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()) + + measuredSpeeds.vyMetersPerSecond + * Math.cos( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()); InternalLoggedTracer.record( "Create Rotated Errors", "Command Scheduler/Drive Commands/Auto Align Coral"); @@ -441,7 +470,7 @@ public static Command autoAlignReefCoral(Drive drive, Camera... cameras) { "Create XSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } else { InternalLoggedTracer.reset(); - alignXController.reset(ex_prime); + alignXController.reset(ex_prime, vx_prime); InternalLoggedTracer.record( "Reset XSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } @@ -453,7 +482,7 @@ public static Command autoAlignReefCoral(Drive drive, Camera... cameras) { } else { InternalLoggedTracer.reset(); - alignYController.reset(ey_prime); + alignYController.reset(ey_prime, vy_prime); InternalLoggedTracer.record( "Reset YSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } @@ -603,6 +632,36 @@ public static Command autoAlignReefAlgae(Drive drive, Camera... cameras) { .algaeSetpoint() .getRotation() .getRadians()); + + ChassisSpeeds measuredSpeeds = drive.getMeasuredChassisSpeeds(); + double vx_prime = + measuredSpeeds.vxMetersPerSecond + * Math.cos( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()) + + measuredSpeeds.vyMetersPerSecond + * Math.sin( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()); + + double vy_prime = + -measuredSpeeds.vxMetersPerSecond + * Math.sin( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()) + + measuredSpeeds.vyMetersPerSecond + * Math.cos( + RobotState.getReefAlignData() + .algaeSetpoint() + .getRotation() + .getRadians()); + InternalLoggedTracer.record( "Create Rotated Errors", "Command Scheduler/Drive Commands/Auto Align Algae"); @@ -614,7 +673,7 @@ public static Command autoAlignReefAlgae(Drive drive, Camera... cameras) { "Create XSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } else { InternalLoggedTracer.reset(); - alignXController.reset(ex_prime); + alignXController.reset(ex_prime, vx_prime); InternalLoggedTracer.record( "Reset XSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } @@ -626,7 +685,7 @@ public static Command autoAlignReefAlgae(Drive drive, Camera... cameras) { } else { InternalLoggedTracer.reset(); - alignYController.reset(ey_prime); + alignYController.reset(ey_prime, vy_prime); InternalLoggedTracer.record( "Reset YSpeed", "Command Scheduler/Drive Commands/Auto Align Algae"); } diff --git a/src/main/java/frc/robot/subsystems/shared/climber/Climber.java b/src/main/java/frc/robot/subsystems/shared/climber/Climber.java index aa93b70c..b777915a 100644 --- a/src/main/java/frc/robot/subsystems/shared/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/shared/climber/Climber.java @@ -49,6 +49,12 @@ public Climber(ClimberIO io) { override = false; } + /** + * Periodic function for the climber subsystem. + * + *

Updates the inputs for the climber, processes the inputs, logs the state of the climber, and + * records the total time taken by the periodic function. + */ @Override public void periodic() { ExternalLoggedTracer.reset(); diff --git a/src/main/java/frc/robot/subsystems/shared/drive/Drive.java b/src/main/java/frc/robot/subsystems/shared/drive/Drive.java index a97d344b..6f336e79 100644 --- a/src/main/java/frc/robot/subsystems/shared/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/shared/drive/Drive.java @@ -56,6 +56,7 @@ public class Drive extends SubsystemBase { private final SwerveDriveKinematics kinematics; @Getter private Rotation2d rawGyroRotation; private SwerveModulePosition[] lastModulePositions; + @Getter private ChassisSpeeds measuredChassisSpeeds; @Getter private final LoggedAutoFactory autoFactory; @@ -178,6 +179,7 @@ public void periodic() { } ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates()); + measuredChassisSpeeds = chassisSpeeds; Translation2d rawFieldRelativeVelocity = new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) .rotateBy(getRawGyroRotation()); @@ -354,7 +356,7 @@ public void setFFGains(double kS, double kV) { /** Runs a choreo path from swerve samples */ public void choreoDrive(SwerveSample sample) { - Pose2d pose = RobotState.getRobotPoseField(); + Pose2d pose = RobotState.getRobotPoseReef(); double xFF = sample.vx; double yFF = sample.vy; double rotationFF = sample.omega; diff --git a/src/main/java/frc/robot/subsystems/shared/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/shared/drive/DriveConstants.java index eb5b898d..213f4374 100644 --- a/src/main/java/frc/robot/subsystems/shared/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/shared/drive/DriveConstants.java @@ -234,6 +234,52 @@ public class DriveConstants { DRIVER_DEADBAND = 0.025; OPERATOR_DEADBAND = 0.25; break; + case V3_POOT: + case V3_POOT_SIM: + FRONT_LEFT = TunerConstantsV3_Poot.FrontLeft; + FRONT_RIGHT = TunerConstantsV3_Poot.FrontRight; + BACK_LEFT = TunerConstantsV3_Poot.BackLeft; + BACK_RIGHT = TunerConstantsV3_Poot.BackRight; + + DRIVE_CONFIG = + new DriveConfig( + TunerConstantsV3_Poot.DrivetrainConstants.CANBusName, + TunerConstantsV3_Poot.DrivetrainConstants.Pigeon2Id, + TunerConstantsV3_Poot.kSpeedAt12Volts.in(MetersPerSecond), + TunerConstantsV3_Poot.kWheelRadius.in(Meters), + DCMotor.getKrakenX60Foc(1), + DCMotor.getKrakenX60Foc(1), + FRONT_LEFT, + FRONT_RIGHT, + BACK_LEFT, + BACK_RIGHT, + Units.inchesToMeters(33.5), + Units.inchesToMeters(33.5)); + + GAINS = + new Gains( + new LoggedTunableNumber("Drive/Drive KS", TunerConstantsV3_Poot.driveGains.kS), + new LoggedTunableNumber("Drive/Drive KV", TunerConstantsV3_Poot.driveGains.kV), + new LoggedTunableNumber("Drive/Drive KP", TunerConstantsV3_Poot.driveGains.kP), + new LoggedTunableNumber("Drive/Drive KD", TunerConstantsV3_Poot.driveGains.kD), + new LoggedTunableNumber("Drive/Turn KP", TunerConstantsV3_Poot.steerGains.kP), + new LoggedTunableNumber("Drive/Turn KD", TunerConstantsV3_Poot.steerGains.kD)); + AUTO_ALIGN_GAINS = + new AutoAlignGains( + new LoggedTunableNumber("Drive/Translation KP", 4.0), + new LoggedTunableNumber("Drive/Translation KD", 0.0), + new LoggedTunableNumber("Drive/Rotation KP", 5.0), + new LoggedTunableNumber("Drive/Rotation KD", 0.05)); + AUTO_GAINS = + new AutoAlignGains( + new LoggedTunableNumber("Drive/Auto Gains/Translation KP", 10.0), + new LoggedTunableNumber("Drive/Auto Gains/Translation KD", 0.0), + new LoggedTunableNumber("Drive/Auto Gains/Rotation KP", 5.0), + new LoggedTunableNumber("Drive/Auto Gains/Rotation KD", 0.00)); + ODOMETRY_FREQUENCY = 250.0; + DRIVER_DEADBAND = 0.025; + OPERATOR_DEADBAND = 0.25; + break; } ALIGN_ROBOT_TO_APRIL_TAG_CONSTANTS = new AlignRobotToAprilTagConstants( @@ -257,11 +303,11 @@ public class DriveConstants { new LoggedTunableNumber("Drive/Align Robot To April Tag/Omega Constants/kD", 0.05), new LoggedTunableNumber( "Drive/Align Robot To April Tag/Omega Constants/tolerance", - Units.degreesToRadians(0.25)), + Units.degreesToRadians(1)), new LoggedTunableNumber( "Drive/Align Robot To April Tag/Omega Constants/maxVelocity", Math.PI)), new LoggedTunableNumber( - "Drive/Align Robot To April Tag/positionThresholdDegrees", 0.03)); + "Drive/Align Robot To April Tag/positionThresholdDegrees", 0.02)); } public record DriveConfig( diff --git a/src/main/java/frc/robot/subsystems/shared/drive/TunerConstantsV3_Poot.java b/src/main/java/frc/robot/subsystems/shared/drive/TunerConstantsV3_Poot.java new file mode 100644 index 00000000..84fdca1d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shared/drive/TunerConstantsV3_Poot.java @@ -0,0 +1,229 @@ +package frc.robot.subsystems.shared.drive; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import edu.wpi.first.units.measure.*; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class TunerConstantsV3_Poot { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(3700) + .withKI(0) + .withKD(41) + .withKS(0.1) + .withKV(2.33) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs driveGains = + new Slot0Configs().withKP(47).withKI(0).withKD(0.001).withKS(7.266685).withKV(0); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + public static final ClosedLoopOutputType kSteerClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + public static final ClosedLoopOutputType kDriveClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; + + // The type of motor used for the drive motor + public static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + public static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + public static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + public static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + public static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + public static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + public static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Drive"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.48); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + public static final double kCoupleRatio = 3.125; + + public static final double kDriveGearRatio = 7.125; + public static final double kSteerGearRatio = 18.75; + public static final Distance kWheelRadius = Inches.of(1.955); + + public static final boolean kInvertLeftSide = false; + public static final boolean kInvertRightSide = true; + + public static final int kPigeonId = 15; + + // These are only used for simulation + public static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + public static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + public static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + public static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + public static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + public static final int kFrontLeftDriveMotorId = 1; + public static final int kFrontLeftSteerMotorId = 2; + public static final int kFrontLeftEncoderId = 11; + public static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.4423828125); + public static final boolean kFrontLeftSteerMotorInverted = true; + public static final boolean kFrontLeftEncoderInverted = false; + + public static final Distance kFrontLeftXPos = Inches.of(11.5); + public static final Distance kFrontLeftYPos = Inches.of(11.5); + + // Front Right + public static final int kFrontRightDriveMotorId = 3; + public static final int kFrontRightSteerMotorId = 4; + public static final int kFrontRightEncoderId = 12; + public static final Angle kFrontRightEncoderOffset = Rotations.of(0.37109375); + public static final boolean kFrontRightSteerMotorInverted = true; + public static final boolean kFrontRightEncoderInverted = false; + + public static final Distance kFrontRightXPos = Inches.of(11.5); + public static final Distance kFrontRightYPos = Inches.of(-11.5); + + // Back Left + public static final int kBackLeftDriveMotorId = 5; + public static final int kBackLeftSteerMotorId = 6; + public static final int kBackLeftEncoderId = 13; + public static final Angle kBackLeftEncoderOffset = Rotations.of(-0.240966796875); + public static final boolean kBackLeftSteerMotorInverted = true; + public static final boolean kBackLeftEncoderInverted = false; + + public static final Distance kBackLeftXPos = Inches.of(-11.5); + public static final Distance kBackLeftYPos = Inches.of(11.5); + + // Back Right + public static final int kBackRightDriveMotorId = 7; + public static final int kBackRightSteerMotorId = 8; + public static final int kBackRightEncoderId = 14; + public static final Angle kBackRightEncoderOffset = Rotations.of(-0.322998046875); + public static final boolean kBackRightSteerMotorInverted = true; + public static final boolean kBackRightEncoderInverted = false; + + public static final Distance kBackRightXPos = Inches.of(-11.5); + public static final Distance kBackRightYPos = Inches.of(-11.5); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); +} diff --git a/src/main/java/frc/robot/subsystems/shared/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/shared/elevator/Elevator.java index baf774fb..371752c6 100644 --- a/src/main/java/frc/robot/subsystems/shared/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/shared/elevator/Elevator.java @@ -1,8 +1,7 @@ package frc.robot.subsystems.shared.elevator; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.shared.elevator.ElevatorConstants.ElevatorPositions.L4; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -11,12 +10,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.Constants; +import frc.robot.Constants.Mode; import frc.robot.FieldConstants.Reef.ReefState; import frc.robot.RobotState; import frc.robot.subsystems.shared.elevator.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.shared.elevator.ElevatorIO.ElevatorIOInputs; import frc.robot.subsystems.v2_Redundancy.superstructure.V2_RedundancySuperstructure; import frc.robot.subsystems.v2_Redundancy.superstructure.V2_RedundancySuperstructureStates; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructure; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureStates; import frc.robot.util.ExternalLoggedTracer; import frc.robot.util.InternalLoggedTracer; import java.util.function.BooleanSupplier; @@ -28,14 +31,18 @@ public class Elevator { private final ElevatorIO io; private final ElevatorIOInputsAutoLogged inputs; - private ElevatorPositions position; + // --- MODIFIED: Store the goal primarily as a double for flexibility --- + private double positionGoalMeters; + private ElevatorPositions lastKnownPositionEnum; // Keep track of the last named position private boolean isClosedLoop; public Elevator(ElevatorIO io) { this.io = io; this.inputs = new ElevatorIOInputsAutoLogged(); - position = ElevatorPositions.STOW; + // Initialize to STOW position + this.lastKnownPositionEnum = ElevatorPositions.STOW; + this.positionGoalMeters = lastKnownPositionEnum.getPosition(); isClosedLoop = true; } @@ -48,11 +55,14 @@ private void periodic() { Logger.processInputs("Elevator", inputs); InternalLoggedTracer.record("Process Inputs", "Elevator/Periodic"); - Logger.recordOutput("Elevator/Position", position.name()); + Logger.recordOutput( + "Elevator/Position", + lastKnownPositionEnum != null ? lastKnownPositionEnum.name() : "CUSTOM"); InternalLoggedTracer.reset(); if (isClosedLoop) { - io.setPositionGoal(position.getPosition()); + // Always use the double goal for the IO layer + io.setPositionGoal(positionGoalMeters); } InternalLoggedTracer.record("Set Position Goal", "Elevator/Periodic"); } @@ -130,7 +140,18 @@ private boolean atGoal(double position) { */ @AutoLogOutput(key = "Elevator/At Goal") private boolean atGoal() { - return Math.abs(inputs.positionGoalMeters - inputs.positionMeters) + return Math.abs(positionGoalMeters - inputs.positionMeters) + <= ElevatorConstants.CONSTRAINTS.goalToleranceMeters().get(); + } + + /** + * Checks if the elevator is at the goal position. + * + * @return True if the elevator is at the goal position, false otherwise. + */ + @AutoLogOutput(key = "Elevator/At Goal") + private boolean atGoal(ElevatorPositions position) { + return Math.abs(position.getPosition() - inputs.positionMeters) <= ElevatorConstants.CONSTRAINTS.goalToleranceMeters().get(); } @@ -150,7 +171,7 @@ private Command waitUntilAtGoal() { * tolerance, false otherwise. */ private BooleanSupplier inFastScoringTolerance() { - return () -> Math.abs(inputs.positionMeters - inputs.positionGoalMeters) <= 0.03; + return () -> Math.abs(inputs.positionMeters - positionGoalMeters) <= 0.03; } /** @@ -165,18 +186,19 @@ private Command sysIdRoutine(Subsystem subsystem) { new SysIdRoutine( new SysIdRoutine.Config( Volts.of(1).per(Second), - Volts.of(6), - Seconds.of(10), + Volts.of(3), + Seconds.of(3), (state) -> Logger.recordOutput("Elevator/SysID State", state.toString())), new SysIdRoutine.Mechanism((volts) -> io.setVoltage(volts.in(Volts)), null, subsystem)); return Commands.sequence( + Commands.runOnce( + () -> { + isClosedLoop = false; + }), characterizationRoutine .quasistatic(Direction.kForward) - .until( - () -> - Elevator.this.atGoal( - ElevatorPositions.L4.getPosition() - Units.inchesToMeters(12.0))), + .until(() -> Elevator.this.atGoal(L4.getPosition() - Units.inchesToMeters(12.0))), characterizationRoutine .quasistatic(Direction.kReverse) .until( @@ -185,38 +207,36 @@ private Command sysIdRoutine(Subsystem subsystem) { ElevatorPositions.STOW.getPosition() + Units.inchesToMeters(12.0))), characterizationRoutine .dynamic(Direction.kForward) - .until( - () -> - Elevator.this.atGoal( - ElevatorPositions.L4.getPosition() - Units.inchesToMeters(12.0))), + .until(() -> Elevator.this.atGoal(L4.getPosition() - Units.inchesToMeters(12.0))), characterizationRoutine .dynamic(Direction.kReverse) .until( () -> Elevator.this.atGoal( ElevatorPositions.STOW.getPosition() + Units.inchesToMeters(12.0))), - subsystem.runOnce(() -> setPosition(() -> ReefState.STOW))); + subsystem.runOnce(() -> setPositionFromReef(() -> ReefState.STOW))); } - /** - * Sets the position of the elevator. - * - * @return A command that sets the elevator position. - */ - private void setPosition() { - setPosition(() -> RobotState.getOIData().currentReefHeight()); + /** Private method to set position based on ReefState. */ + private void setPositionFromReef(Supplier newPosition) { + isClosedLoop = true; + this.lastKnownPositionEnum = getPosition(newPosition.get()); + this.positionGoalMeters = this.lastKnownPositionEnum.getPosition(); } - /** - * Sets the position of the elevator. - * - * @param positionRadians The desired elevator position. - * @return A command that sets the elevator position. - */ - private void setPosition(Supplier newPosition) { + /** Private method to set position based on ElevatorPositions enum. */ + private void setPositionFromEnum(ElevatorPositions newPosition) { + isClosedLoop = true; + this.lastKnownPositionEnum = newPosition; + this.positionGoalMeters = this.lastKnownPositionEnum.getPosition(); + } + + /** Private method to set position based on a double value. */ + private void setPositionFromDouble(double newPositionMeters) { isClosedLoop = true; - Elevator.this.position = Elevator.this.getPosition(newPosition.get()); - io.setPositionGoal(Elevator.this.position.getPosition()); + this.positionGoalMeters = newPositionMeters; + // When a raw double is commanded, there's no corresponding enum state. + this.lastKnownPositionEnum = null; } public class ElevatorCSB extends SubsystemBase { @@ -228,16 +248,38 @@ public void periodic() { ExternalLoggedTracer.record("Elevator Total", "Elevator/Periodic"); } - public Command setPosition() { - return this.runOnce(() -> Elevator.this.setPosition()); + public ElevatorPositions getPosition() { + return Elevator.this.lastKnownPositionEnum; } - public ElevatorPositions getPosition() { - return Elevator.this.position; + /** + * Creates a command to set the elevator to a pre-defined position. + * + * @param newPosition The target ElevatorPositions enum. + * @return A command that sets the elevator position. + */ + public Command setPosition(ElevatorPositions newPosition) { + return this.runOnce(() -> Elevator.this.setPositionFromEnum(newPosition)); + } + + /** + * Creates a command to set the elevator to a specific height in meters. + * + * @param newPositionMeters The target height in meters. + * @return A command that sets the elevator position. + */ + public Command setPosition(double newPositionMeters) { + return this.runOnce(() -> Elevator.this.setPositionFromDouble(newPositionMeters)); } public Command setPosition(Supplier newPosition) { - return this.runOnce(() -> Elevator.this.setPosition(newPosition)); + return this.runOnce(() -> Elevator.this.setPositionFromReef(newPosition)); + } + + public Command setPosition() { + return Commands.runOnce( + () -> + Elevator.this.setPositionFromReef(() -> RobotState.getOIData().currentReefHeight())); } public Command setVoltage(double volts) { @@ -250,7 +292,7 @@ public Command setVoltage(double volts) { } public Command resetPosition() { - return runOnce(() -> Elevator.this.position = ElevatorPositions.STOW) + return runOnce(() -> Elevator.this.lastKnownPositionEnum = ElevatorPositions.STOW) .andThen( runOnce( () -> io.setPosition(ElevatorConstants.ELEVATOR_PARAMETERS.MIN_HEIGHT_METERS()))); @@ -289,7 +331,15 @@ public BooleanSupplier inFastScoringTolerance() { } } + // FSM for the Elevator public class ElevatorFSM { + @AutoLogOutput(key = "Elevator/Past Barge Threshold") + public boolean pastBargeThresholdgetPositionMeters() { + if (Constants.getMode() == Mode.REAL) return inputs.positionMeters >= L4.getPosition(); + else { + return inputs.positionMeters > .95; + } + } public void periodic() { ExternalLoggedTracer.reset(); @@ -298,19 +348,37 @@ public void periodic() { } public ElevatorPositions getPosition() { - return Elevator.this.position; + return Elevator.this.lastKnownPositionEnum; } - public void setPosition() { - Elevator.this.setPosition(); + /** + * Sets the elevator to a pre-defined position. + * + * @param newPosition The target ElevatorPositions enum. + */ + public void setPosition(ElevatorPositions newPosition) { + Elevator.this.setPositionFromEnum(newPosition); + } + + /** + * Sets the elevator to a specific height in meters. + * + * @param newPositionMeters The target height in meters. + */ + public void setPosition(double newPositionMeters) { + Elevator.this.setPositionFromDouble(newPositionMeters); } public void setPosition(Supplier newPosition) { - Elevator.this.setPosition(newPosition); + Elevator.this.setPositionFromReef(newPosition); + } + + public void setPosition() { + Elevator.this.setPositionFromReef(() -> RobotState.getOIData().currentReefHeight()); } public Command resetPosition() { - return Commands.runOnce(() -> Elevator.this.position = ElevatorPositions.STOW) + return Commands.runOnce(() -> Elevator.this.lastKnownPositionEnum = ElevatorPositions.STOW) .andThen( Commands.runOnce( () -> io.setPosition(ElevatorConstants.ELEVATOR_PARAMETERS.MIN_HEIGHT_METERS()))); @@ -323,6 +391,13 @@ public Command sysIdRoutine(V2_RedundancySuperstructure superstructure) { Elevator.this.sysIdRoutine(superstructure)); } + public Command sysIdRoutine(V3_PootSuperstructure superstructure) { + + return Commands.sequence( + superstructure.runGoal(V3_PootSuperstructureStates.OVERRIDE), + Elevator.this.sysIdRoutine(superstructure)); + } + public double getPositionMeters() { return Elevator.this.getPositionMeters(inputs); } @@ -343,6 +418,14 @@ public boolean atGoal() { return Elevator.this.atGoal(); } + public boolean atGoal(ReefState position) { + return Elevator.this.atGoal(Elevator.this.getPosition(position)); + } + + public boolean inTolerance(double toleranceMeters) { + return Math.abs(positionGoalMeters - inputs.positionMeters) <= toleranceMeters; + } + public Command waitUntilAtGoal() { return Elevator.this.waitUntilAtGoal(); } @@ -350,6 +433,10 @@ public Command waitUntilAtGoal() { public BooleanSupplier inFastScoringTolerance() { return Elevator.this.inFastScoringTolerance(); } + + public double getVelocityMetersPerSecond() { + return inputs.velocityMetersPerSecond; + } } public ElevatorFSM getFSM() { diff --git a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorConstants.java index d968385b..8e612432 100644 --- a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorConstants.java @@ -28,9 +28,12 @@ public class ElevatorConstants { REEF_STATE_ELEVATOR_POSITION_MAP = Map.ofEntries( Map.entry(ReefState.STOW, ElevatorPositions.STOW), + Map.entry(ReefState.POST_PROCESSOR, ElevatorPositions.POST_PROCESSOR), + Map.entry(ReefState.HIGH_STOW, ElevatorPositions.HIGH_STOW), Map.entry(ReefState.CORAL_INTAKE, ElevatorPositions.CORAL_INTAKE), Map.entry(ReefState.ALGAE_FLOOR_INTAKE, ElevatorPositions.ALGAE_INTAKE), Map.entry(ReefState.ALGAE_MID, ElevatorPositions.ALGAE_MID), + Map.entry(ReefState.HANDOFF, ElevatorPositions.HANDOFF), Map.entry(ReefState.ALGAE_INTAKE_TOP, ElevatorPositions.ALGAE_INTAKE_TOP), Map.entry(ReefState.ALGAE_INTAKE_BOTTOM, ElevatorPositions.ALGAE_INTAKE_BOT), Map.entry(ReefState.L1, ElevatorPositions.L1), @@ -60,7 +63,7 @@ public class ElevatorConstants { default: GAINS = new Gains( - new LoggedTunableNumber("Elevator/Gains/kP", 2.0), + new LoggedTunableNumber("Elevator/Gains/kP", 67.000000), new LoggedTunableNumber("Elevator/Gains/kD", 0.1), new LoggedTunableNumber("Elevator/Gains/kS", 0.225), new LoggedTunableNumber("Elevator/Gains/kG", 0.075), @@ -68,12 +71,12 @@ public class ElevatorConstants { new LoggedTunableNumber("Elevator/Gains/kA", 0.0)); CONSTRAINTS = new Constraints( - new LoggedTunableNumber("Elevator/Max Acceleration", 16.0), - new LoggedTunableNumber("Elevator/Cruising Velocity", 16.0), + new LoggedTunableNumber("Elevator/Max Acceleration", 190.0), + new LoggedTunableNumber("Elevator/Cruising Velocity", 41.0), new LoggedTunableNumber("Elevator/Goal Tolerance", 0.02)); STOW_GAINS = new Gains( - new LoggedTunableNumber("Elevator/Gains/kP", 2.0), + new LoggedTunableNumber("Elevator/Gains/kP", 67.000000), new LoggedTunableNumber("Elevator/Gains/kD", 0.1), new LoggedTunableNumber("Elevator/Gains/kS", 0.225), new LoggedTunableNumber("Elevator/Gains/kG", 0.075), @@ -115,6 +118,82 @@ public class ElevatorConstants { break; } break; + + case V3_POOT: + case V3_POOT_SIM: + ELEVATOR_CAN_ID = 20; + ELEVATOR_GEAR_RATIO = 4.0; + DRUM_RADIUS = Units.inchesToMeters(2.211 / 2.0); + + ELEVATOR_SUPPLY_CURRENT_LIMIT = 40; + ELEVATOR_STATOR_CURRENT_LIMIT = 80; + + ELEVATOR_PARAMETERS = + new ElevatorParameters( + DCMotor.getKrakenX60Foc(2), 6.803886, 0.0, Units.inchesToMeters(53), 2); + + switch (Constants.getMode()) { + case REAL: + case REPLAY: + default: + GAINS = + new Gains( + new LoggedTunableNumber("Elevator/Gains/kP", 5), + new LoggedTunableNumber("Elevator/Gains/kD", 0), + new LoggedTunableNumber("Elevator/Gains/kS", 0.070776), + new LoggedTunableNumber("Elevator/Gains/kG", 0.35521), + new LoggedTunableNumber("Elevator/Gains/kV", 0), + new LoggedTunableNumber("Elevator/Gains/kA", 0.0)); + CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Elevator/Max Acceleration", 11), + new LoggedTunableNumber("Elevator/Cruising Velocity", 5), + new LoggedTunableNumber("Elevator/Goal Tolerance", 0.02)); + STOW_GAINS = + new Gains( + new LoggedTunableNumber("Elevator/Stow Gains/kP", 5), + new LoggedTunableNumber("Elevator/Stow Gains/kD", 0), + new LoggedTunableNumber("Elevator/Stow Gains/kS", 0), + new LoggedTunableNumber("Elevator/Stow Gains/kG", 0), + new LoggedTunableNumber("Elevator/Stow Gains/kV", 0.0), + new LoggedTunableNumber("Elevator/Stow Gains/kA", 0.0)); + STOW_CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Elevator/Stow Max Acceleration", 11), + new LoggedTunableNumber("Elevator/Stow Cruising Velocity", 5), + new LoggedTunableNumber("Elevator/Stow Goal Tolerance", 0.02)); + break; + case SIM: + GAINS = + new Gains( + new LoggedTunableNumber("Elevator/Gains/kP", 50.0), + new LoggedTunableNumber("Elevator/Gains/kD", 0.0), + new LoggedTunableNumber("Elevator/Gains/kS", 0.0), + new LoggedTunableNumber("Elevator/Gains/kG", 0.0), + new LoggedTunableNumber("Elevator/Gains/kV", 0.0), + new LoggedTunableNumber("Elevator/Gains/kA", 0.0)); + CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Elevator/Max Acceleration", 201.078594), + new LoggedTunableNumber("Elevator/Cruising Velocity", 21.329982), + new LoggedTunableNumber("Elevator/Goal Tolerance", 0.02)); + STOW_GAINS = + new Gains( + new LoggedTunableNumber("Elevator/Stow Gains/kP", 20.0), + new LoggedTunableNumber("Elevator/Stow Gains/kD", 0.0), + new LoggedTunableNumber("Elevator/Stow Gains/kS", 0.0), + new LoggedTunableNumber("Elevator/Stow Gains/kG", 0.0), + new LoggedTunableNumber("Elevator/Stow Gains/kV", 0.0), + new LoggedTunableNumber("Elevator/Stow Gains/kA", 0.0)); + STOW_CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Elevator/Stow Max Acceleration", 101.078594), + new LoggedTunableNumber("Elevator/Stow Cruising Velocity", 11.329982), + new LoggedTunableNumber("Elevator/Stow Goal Tolerance", 0.02)); + break; + } + break; + case V2_REDUNDANCY: case V2_REDUNDANCY_SIM: default: @@ -135,16 +214,16 @@ public class ElevatorConstants { default: GAINS = new Gains( - new LoggedTunableNumber("Elevator/Gains/kP", 2.0), - new LoggedTunableNumber("Elevator/Gains/kD", 0.1), + new LoggedTunableNumber("Elevator/Gains/kP", 1), + new LoggedTunableNumber("Elevator/Gains/kD", 0.0), new LoggedTunableNumber("Elevator/Gains/kS", 0.225), new LoggedTunableNumber("Elevator/Gains/kG", 0.075), new LoggedTunableNumber("Elevator/Gains/kV", 0.0), new LoggedTunableNumber("Elevator/Gains/kA", 0.0)); CONSTRAINTS = new Constraints( - new LoggedTunableNumber("Elevator/Max Acceleration", 16.0), - new LoggedTunableNumber("Elevator/Cruising Velocity", 16.0), + new LoggedTunableNumber("Elevator/Max Acceleration", 1), + new LoggedTunableNumber("Elevator/Cruising Velocity", 1), new LoggedTunableNumber("Elevator/Goal Tolerance", 0.02)); STOW_GAINS = new Gains( @@ -213,27 +292,84 @@ public static record ElevatorParameters( double MAX_HEIGHT_METERS, int NUM_MOTORS) {} + public static record PositionConstants(double V1, double V2, double V3) {} + @RequiredArgsConstructor public static enum ElevatorPositions { - STOW(0.0), - CORAL_INTAKE(0.0), - ALGAE_INTAKE(0.2161583093038944 + Units.inchesToMeters(1)), - ALGAE_MID(0.7073684509805078), - ALGAE_INTAKE_TOP(1.17 - Units.inchesToMeters(8)), - ALGAE_INTAKE_BOT(0.79 - Units.inchesToMeters(8)), - ASS_TOP(1.2), - ASS_BOT(0.82), - L1(0.11295250319916351), - L2(0.37296301250898894), - L3(0.7606347556550676 + Units.inchesToMeters(1.0)), - L4(1.3864590139769697 + Units.inchesToMeters(0.5)), - L4_PLUS(1.3864590139769697 + Units.inchesToMeters(2.0)), - ALGAE_SCORE(1.3864590139769697 + Units.inchesToMeters(0.5)); - - private final double position; + STOW(new PositionConstants(0.0, 0.0, 0.0)), + POST_PROCESSOR(new PositionConstants(0.5, 0.5, 0.5)), + CORAL_INTAKE(new PositionConstants(0.0, 0.0, Units.inchesToMeters(34.85))), + ALGAE_INTAKE( + new PositionConstants( + 0.2161583093038944 + Units.inchesToMeters(1), + 0.2161583093038944 + Units.inchesToMeters(1), + 0.1819)), + ALGAE_MID( + new PositionConstants( + 0.7073684509805078, 0.7073684509805078, 1.2)), // USED AS PRE-HANDOFF FOR V3 + ALGAE_INTAKE_TOP( + new PositionConstants( + 1.17 - Units.inchesToMeters(8), + 1.17 - Units.inchesToMeters(8), + Units.inchesToMeters(40))), + ALGAE_INTAKE_BOT( + new PositionConstants( + 0.79 - Units.inchesToMeters(8), + 0.79 - Units.inchesToMeters(8), + Units.inchesToMeters(24))), + ASS_TOP(new PositionConstants(1.2, 0.0, 0.0)), + ASS_BOT(new PositionConstants(0.82, 0.0, 0.0)), + L1( + new PositionConstants( + 0.11295250319916351, 0.11295250319916351, Units.inchesToMeters(34.75))), + L2(new PositionConstants(0.37296301250898894, 0.37296301250898894, Units.inchesToMeters(12.5))), + L3( + new PositionConstants( + 0.7606347556550676 + Units.inchesToMeters(1.0), + 0.7606347556550676 + Units.inchesToMeters(1.0), + Units.inchesToMeters(12.5 + 15.87))), + L4( + new PositionConstants( + 1.3864590139769697 + Units.inchesToMeters(0.5), + 1.3864590139769697 + Units.inchesToMeters(0.5), + Units.inchesToMeters(50))), + L4_PLUS( + new PositionConstants( + 0.0, + 1.3864590139769697 + Units.inchesToMeters(2.0), + 0.0)), // DOES NOT EXIST FOR V3 AND V1 + ALGAE_SCORE( + new PositionConstants( + 1.3864590139769697 + Units.inchesToMeters(0.5), + 1.3864590139769697 + Units.inchesToMeters(0.5), + Units.inchesToMeters(53))), + + HIGH_STOW(new PositionConstants(0, 0, 0.5)), + HANDOFF(new PositionConstants(0, 0, Units.inchesToMeters(33.25))), + ; + + private final PositionConstants position; public double getPosition() { - return position; + switch (Constants.ROBOT) { + case V1_STACKUP, V1_STACKUP_SIM: + return position.V1(); + case V2_REDUNDANCY, V2_REDUNDANCY_SIM: + return position.V2(); + case V3_POOT, V3_POOT_SIM: + return position.V3(); + default: + return position.V3(); + } + } + + public static ElevatorPositions getPosition(ReefState state) { + for (ElevatorPositions pos : values()) { + if (REEF_STATE_ELEVATOR_POSITION_MAP.get(state) == pos) { + return pos; + } + } + return null; } } } diff --git a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIO.java index 83c7c9ff..b338dc0d 100644 --- a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIO.java @@ -7,6 +7,7 @@ public interface ElevatorIO { public static class ElevatorIOInputs { public double positionMeters = 0.0; public double velocityMetersPerSecond = 0.0; + public double accelerationMetersPerSecondSquared = 0.0; public double[] appliedVolts = {0.0, 0.0, 0.0, 0.0}; public double[] supplyCurrentAmps = {0.0, 0.0, 0.0, 0.0}; diff --git a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOSim.java index 60099bf5..ef999615 100644 --- a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOSim.java @@ -30,7 +30,6 @@ public ElevatorIOSim() { ElevatorConstants.ELEVATOR_PARAMETERS.MAX_HEIGHT_METERS(), true, ElevatorConstants.ELEVATOR_PARAMETERS.MIN_HEIGHT_METERS()); - feedback = new ProfiledPIDController( ElevatorConstants.GAINS.kP().get(), @@ -64,6 +63,7 @@ public void updateInputs(ElevatorIOInputs inputs) { // Position and velocity inputs.positionMeters = sim.getPositionMeters(); inputs.velocityMetersPerSecond = sim.getVelocityMetersPerSecond(); + inputs.accelerationMetersPerSecondSquared = 0; for (int i = 0; i < ElevatorConstants.ELEVATOR_PARAMETERS.NUM_MOTORS(); i++) { inputs.appliedVolts[i] = appliedVolts; @@ -104,4 +104,8 @@ public void updateGains(double kP, double kD, double kS, double kV, double kA, d public void updateConstraints(double maxAcceleration, double cruisingVelocity) { feedback.setConstraints(new Constraints(cruisingVelocity, maxAcceleration)); } + + public double getAcceleration(double previousVelocity, double currentVelocity) { + return (currentVelocity - previousVelocity) / 0.02; + } } diff --git a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOTalonFX.java index f2f12de4..d7399ba3 100644 --- a/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shared/elevator/ElevatorIOTalonFX.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; @@ -30,6 +31,7 @@ public class ElevatorIOTalonFX implements ElevatorIO { // Sensor inputs private StatusSignal positionRotations; private StatusSignal velocityRotationsPerSecond; + private StatusSignal accelerationRotationsPerSecSq; private ArrayList> appliedVolts; private ArrayList> supplyCurrentAmps; private ArrayList> torqueCurrentAmps; @@ -109,6 +111,7 @@ public ElevatorIOTalonFX() { positionRotations = talonFX.getPosition(); velocityRotationsPerSecond = talonFX.getVelocity(); + accelerationRotationsPerSecSq = talonFX.getAcceleration(); appliedVolts.add(talonFX.getMotorVoltage()); supplyCurrentAmps.add(talonFX.getSupplyCurrent()); torqueCurrentAmps.add(talonFX.getTorqueCurrent()); @@ -169,6 +172,11 @@ public void updateInputs(ElevatorIOInputs inputs) { * Math.PI * ElevatorConstants.DRUM_RADIUS * 2; + inputs.accelerationMetersPerSecondSquared = + (accelerationRotationsPerSecSq.getValueAsDouble() / ElevatorConstants.ELEVATOR_GEAR_RATIO) + * Math.PI + * ElevatorConstants.DRUM_RADIUS + * 2; for (int i = 0; i < ElevatorConstants.ELEVATOR_PARAMETERS.NUM_MOTORS(); i++) { inputs.appliedVolts[i] = appliedVolts.get(i).getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shared/vision/Camera.java b/src/main/java/frc/robot/subsystems/shared/vision/Camera.java index b4df6fbe..976303a1 100644 --- a/src/main/java/frc/robot/subsystems/shared/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/shared/vision/Camera.java @@ -4,11 +4,8 @@ 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.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import frc.robot.FieldConstants; import frc.robot.RobotState; import frc.robot.RobotState.VisionObservation; @@ -27,7 +24,7 @@ public class Camera { private final CameraIOInputsAutoLogged inputs; private final CameraIO io; - private final String name; + @Getter private final String name; @Getter int[] validIds; private final Map tagPoses2d = new HashMap<>(); @@ -89,73 +86,74 @@ public void periodic() { VecBuilder.fill(xyStdDev, xyStdDev, thetaStdev))); // ONLY DO THIS IF WE ARE RUNNING GOMPEIVISION - if (io instanceof CameraIOGompeiVision) { - for (int i = 0; i < frame.preciseTagIds().length; i++) { - final int tagIdIndex = i; - // Check if tag is valid - if (java.util.Arrays.stream(validIds) - .noneMatch(id -> id == frame.preciseTagIds()[tagIdIndex])) { - continue; - } - // Get rotation at timestamp - var sample = RobotState.getBufferedPose(frame.timestamp()); - if (sample.isEmpty()) { - // exit if not there - return; - } - Rotation2d robotRotation = - RobotState.getRobotPoseField() - .transformBy(new Transform2d(RobotState.getRobotPoseOdometry(), sample.get())) - .getRotation(); - - // Average tx's and ty's - double tx = 0.0; - double ty = 0.0; - for (int j = 0; j < 4; j++) { - tx += frame.preciseTx()[i][j]; - ty += frame.preciseTy()[i][j]; - } - tx /= 4.0; - ty /= 4.0; - - Pose3d cameraPose = - new Pose3d( - io.getGompeiVisionConfig().robotToCameraTransform().getTranslation(), - io.getGompeiVisionConfig().robotToCameraTransform().getRotation()); - - // Use 3D distance and tag angles to find robot pose - Translation2d camToTagTranslation = - new Pose3d(Translation3d.kZero, new Rotation3d(0, ty, -tx)) - .transformBy( - new Transform3d( - new Translation3d(frame.preciseDistance()[i], 0, 0), Rotation3d.kZero)) - .getTranslation() - .rotateBy(new Rotation3d(0, cameraPose.getRotation().getY(), 0)) - .toTranslation2d(); - Rotation2d camToTagRotation = - robotRotation.plus( - cameraPose.toPose2d().getRotation().plus(camToTagTranslation.getAngle())); - var tagPose2d = tagPoses2d.get(frame.preciseTagIds()[i]); - if (tagPose2d == null) return; - Translation2d fieldToCameraTranslation = - new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) - .transformBy(GeometryUtil.toTransform2d(camToTagTranslation.getNorm(), 0.0)) - .getTranslation(); - robotPose = - new Pose2d( - fieldToCameraTranslation, - robotRotation.plus(cameraPose.toPose2d().getRotation())) - .transformBy(new Transform2d(cameraPose.toPose2d(), Pose2d.kZero)); - // Use gyro angle at time for robot rotation - robotPose = new Pose2d(robotPose.getTranslation(), robotRotation); - - // Add transform to current odometry based pose for latency correction - txTyObservations.add( - new VisionObservation( - robotPose, - frame.timestamp(), - VecBuilder.fill(xyStdDev, xyStdDev, Double.POSITIVE_INFINITY))); + // ONLY do this if using GompeiVision + // Loop over each processed frame + // Only do this if we are running GompeiVision + if (!(io instanceof CameraIOGompeiVision)) continue; + + for (int i = 0; i < frame.preciseTagIds().length; i++) { + int tagId = frame.preciseTagIds()[i]; + + // Skip invalid tags + if (java.util.Arrays.stream(validIds).noneMatch(id -> id == tagId)) continue; + + // Get odometry-based pose at the timestamp + var sample = RobotState.getBufferedPose(frame.timestamp()); + if (sample.isEmpty()) continue; + + // Average tx and ty over four corners + double tx = 0.0; + double ty = 0.0; + for (int j = 0; j < 4; j++) { + tx += frame.preciseTx()[i][j]; + ty += frame.preciseTy()[i][j]; } + tx /= 4.0; + ty /= 4.0; + + Pose3d cameraPose = io.getGompeiVisionConfig().robotRelativePose(); + + // Project 3D distance onto horizontal plane + double distance2d = + frame.preciseDistance()[i] + * Math.cos(-cameraPose.getRotation().getY() - ty); // pitch + tag vertical angle + + // Compute rotation from camera to tag + Rotation2d camToTagRotation = + sample + .get() + .getRotation() + .plus(cameraPose.toPose2d().getRotation().plus(Rotation2d.fromRadians(-tx))); + + Pose2d tagPose2d = tagPoses2d.get(tagId); + if (tagPose2d == null) continue; + + // Compute camera position in field frame + Translation2d fieldToCameraTranslation = + new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) + .transformBy(GeometryUtil.toTransform2d(distance2d, 0.0)) + .getTranslation(); + + // Compute robot pose + Pose2d robotPose = + new Pose2d( + fieldToCameraTranslation, + sample.get().getRotation().plus(cameraPose.toPose2d().getRotation())) + .transformBy(new Transform2d(cameraPose.toPose2d(), Pose2d.kZero)); + + // Use odometry rotation only + robotPose = new Pose2d(robotPose.getTranslation(), sample.get().getRotation()); + + // Store observation for fusion + xyStdDev = + io.getPrimaryXYStandardDeviationCoefficient() + * Math.pow(frame.averageDistance(), 1.2) + / Math.pow(frame.totalTargets(), 2.0); + txTyObservations.add( + new VisionObservation( + robotPose, + frame.timestamp(), + VecBuilder.fill(xyStdDev, xyStdDev, Double.POSITIVE_INFINITY))); } } @@ -163,16 +161,35 @@ public void periodic() { poseObservations.stream() .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) .forEach(RobotState::addFieldLocalizerVisionMeasurement); - poseObservations.stream() - .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) - .forEach(RobotState::addReefLocalizerVisionMeasurement); - // txTyObservations.stream() + // poseObservations.stream() // .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) // .forEach(RobotState::addReefLocalizerVisionMeasurement); + txTyObservations.stream() + .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) + .forEach(RobotState::addReefLocalizerVisionMeasurement); + + List imprecisePoses = new ArrayList<>(); + for (VisionObservation obs : poseObservations) { + imprecisePoses.add(obs.pose()); + } + + List precisePoses = new ArrayList<>(); + for (VisionObservation obs : txTyObservations) { + precisePoses.add(obs.pose()); + } + + Logger.recordOutput( + "Vision/Camera/" + name + "/Imprecise Poses", imprecisePoses.toArray(Pose2d[]::new)); + Logger.recordOutput( + "Vision/Camera/" + name + "/Precise Poses", precisePoses.toArray(Pose2d[]::new)); } public void setValidTags(int... validIds) { this.validIds = validIds; io.setValidTags(validIds); } + + public Pose3d getTransform() { + return io.getGompeiVisionConfig().robotRelativePose(); + } } diff --git a/src/main/java/frc/robot/subsystems/shared/vision/CameraIOGompeiVision.java b/src/main/java/frc/robot/subsystems/shared/vision/CameraIOGompeiVision.java index 69300f9a..adbd502e 100644 --- a/src/main/java/frc/robot/subsystems/shared/vision/CameraIOGompeiVision.java +++ b/src/main/java/frc/robot/subsystems/shared/vision/CameraIOGompeiVision.java @@ -6,17 +6,13 @@ import edu.wpi.first.math.geometry.Quaternion; 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.networktables.BooleanSubscriber; -import edu.wpi.first.networktables.ConnectionInfo; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.networktables.*; import edu.wpi.first.wpilibj.Timer; import frc.robot.FieldConstants; import frc.robot.RobotState; import frc.robot.subsystems.shared.vision.VisionConstants.GompeiVisionConfig; +import frc.robot.util.GeometryUtil; import java.util.ArrayList; import java.util.HashMap; import java.util.List; @@ -24,8 +20,10 @@ import java.util.Optional; import java.util.function.Supplier; import lombok.Getter; +import lombok.experimental.ExtensionMethod; import org.littletonrobotics.junction.Logger; +@ExtensionMethod({GeometryUtil.class}) public class CameraIOGompeiVision implements CameraIO { private final GompeiVisionConfig config; private final Supplier aprilTagLayoutSupplier; @@ -40,6 +38,7 @@ public class CameraIOGompeiVision implements CameraIO { private final DoubleArraySubscriber observationSubscriber; // private final IntegerSubscriber fpsAprilTagSubscriber; private final BooleanSubscriber cameraConnectedSubscriber; + private final DoubleSubscriber cameraUSBSpeedSubscriber; private final Map lastTagDetectionTimes; @@ -95,6 +94,7 @@ public CameraIOGompeiVision( PubSubOption.periodic(0.01)); // this.fpsAprilTagSubscriber = outputTable.getIntegerTopic("fps_apriltags").subscribe(0); this.cameraConnectedSubscriber = outputTable.getBooleanTopic("connected").subscribe(false); + this.cameraUSBSpeedSubscriber = outputTable.getDoubleTopic("usb_speed").subscribe(0.0); lastTagDetectionTimes = new HashMap<>(); @@ -144,7 +144,10 @@ public void updateInputs(CameraIOInputs inputs) { values[3], values[4], new Rotation3d(new Quaternion(values[5], values[6], values[7], values[8]))); - robotPose = cameraPose.transformBy(config.robotToCameraTransform().inverse()).toPose2d(); + robotPose = + cameraPose + .toPose2d() + .transformBy(config.robotRelativePose().toPose2d().toTransform2d().inverse()); break; case 2: @@ -164,14 +167,14 @@ public void updateInputs(CameraIOInputs inputs) { values[11], values[12], new Rotation3d(new Quaternion(values[13], values[14], values[15], values[16]))); - Transform3d cameraToRobot = config.robotToCameraTransform().inverse(); - - Pose2d robotPose0 = cameraPose0.transformBy(cameraToRobot).toPose2d(); - Pose2d robotPose1 = cameraPose1.transformBy(cameraToRobot).toPose2d(); + Transform2d cameraToRobot = + config.robotRelativePose().toPose2d().toTransform2d().inverse(); + Pose2d robotPose0 = cameraPose0.toPose2d().transformBy(cameraToRobot); + Pose2d robotPose1 = cameraPose1.toPose2d().transformBy(cameraToRobot); if (error0 < error1 * VisionConstants.AMBIGUITY_THRESHOLD || error1 < error0 * VisionConstants.AMBIGUITY_THRESHOLD) { - Rotation2d currentRotation = RobotState.getHeadingData().robotHeading(); + Rotation2d currentRotation = RobotState.getRobotPoseField().getRotation(); Rotation2d visionRotation0 = robotPose0.getRotation(); Rotation2d visionRotation1 = robotPose1.getRotation(); if (Math.abs(currentRotation.minus(visionRotation0).getRadians()) @@ -207,17 +210,19 @@ public void updateInputs(CameraIOInputs inputs) { for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i += 10) { int tagId = (int) values[i]; lastTagDetectionTimes.put(tagId, Timer.getTimestamp()); - Optional tagPose = aprilTagLayoutSupplier.get().getTagPose((int) values[i]); + Optional tagPose = aprilTagLayoutSupplier.get().getTagPose(tagId); tagPose.ifPresent(tagPoses::add); } - int[] tagIds = new int[tagPoses.size()]; - double[][] txs = new double[tagPoses.size()][]; - double[][] tys = new double[tagPoses.size()][]; - double[] distances = new double[tagPoses.size()]; + // Prepare arrays + int totalTags = tagPoses.size(); + int[] tagIds = new int[totalTags]; + double[][] txs = new double[totalTags][]; + double[][] tys = new double[totalTags][]; + double[] distances = new double[totalTags]; if (!tagPoses.isEmpty()) { - // Calculate average distance to tag + // Calculate average distance double totalDistance = 0.0; for (Pose3d tagPose : tagPoses) { totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); @@ -225,30 +230,49 @@ public void updateInputs(CameraIOInputs inputs) { averageDistance = totalDistance / tagPoses.size(); totalTargets = tagPoses.size(); - // Add TxTy observations + // --- Parse tag angle + distance data --- int tagEstimationDataEndIndex = switch ((int) values[0]) { - default -> 0; case 1 -> 8; case 2 -> 16; + default -> 0; }; int indexCounter = 0; - for (int index = tagEstimationDataEndIndex + 1; index < values.length; index += 10) { + + // Step through each 10-value chunk safely + for (int index = tagEstimationDataEndIndex + 1; index + 9 < values.length; index += 10) { + int tagId = (int) values[index]; double[] tx = new double[4]; double[] ty = new double[4]; + + // Read 4 corner pairs for (int i = 0; i < 4; i++) { tx[i] = values[index + 1 + (2 * i)]; ty[i] = values[index + 1 + (2 * i) + 1]; } - int tagId = (int) values[index]; + double distance = values[index + 9]; - tagIds[indexCounter] = tagId; - txs[indexCounter] = tx; - tys[indexCounter] = ty; - distances[indexCounter] = distance; - indexCounter++; + // Store data + if (indexCounter < totalTags) { + tagIds[indexCounter] = tagId; + txs[indexCounter] = tx; + tys[indexCounter] = ty; + distances[indexCounter] = distance; + indexCounter++; + } else { + System.out.println("[WARN] More tag data than expected: indexCounter=" + indexCounter); + } + } + + // Optional debug check + if ((values.length - (tagEstimationDataEndIndex + 1)) % 10 != 0) { + System.out.println( + "[WARN] Observation array not multiple of 10! Length=" + + values.length + + " start=" + + (tagEstimationDataEndIndex + 1)); } } @@ -317,11 +341,13 @@ public void updateInputs(CameraIOInputs inputs) { Logger.recordOutput( "Vision/Cameras/" + config.key() + "/Camera Connected", cameraConnectedSubscriber.get()); + Logger.recordOutput( + "Vision/Cameras/" + config.key() + "/USB Speed", cameraUSBSpeedSubscriber.get()); } @Override public boolean getIsConnected(CameraIOInputs inputs) { - return true; + return inputs.isConnected; } @Override diff --git a/src/main/java/frc/robot/subsystems/shared/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/shared/vision/VisionConstants.java index a8aa7f46..d3e57687 100644 --- a/src/main/java/frc/robot/subsystems/shared/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/shared/vision/VisionConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; +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.numbers.N1; @@ -183,10 +184,10 @@ public static class RobotCameras { new Rotation3d(0, 0, Units.degreesToRadians(-225)))) .build(); - private static final GompeiVisionConfig BACK_TOP_LEFT = + private static final GompeiVisionConfig BACK_BOTTOM_LEFT = // Back Right robot relative GompeiVisionConfig.builder() - .key("camera_backtopleft") - .hardwareID("camera_backtopleft") + .key("camera_backbottomleft") + .hardwareID("camera_backbottomleft") .cameraType(CameraType.THRIFTYCAM) .exposure(50.0) .gain(0.0) @@ -198,59 +199,39 @@ public static class RobotCameras { MatBuilder.fill( N3.instance, N3.instance, - 1624.2147990980468, - 0.0, - 678.5977326688123, - 0.0, - 1623.8010651757406, - 581.2937963816543, - 0.0, - 0.0, - 1.0))) + 1381.859898393546, + 0, + 741.0665606797552, + 0, + 1388.430990713648, + 626.5683262179745, + 0, + 0, + 1))) .distortionCoefficients( new Matrix( MatBuilder.fill( N5.instance, N1.instance, - 0.0962739678996323, - -0.17425866758812222, - -0.0010256591695401809, - 0.00203018525003567, - 0.1499388749754604))) + -0.026870821933410, + 0.034738098307543, + -0.0005183171, + 0.0005800466, + -0.050574364954119))) .verticalFOV(ThriftyCamConstants.VERTICAL_FOV) .singletagXYStdev(ThriftyCamConstants.SINGLETAG_XY_STANDARD_DEVIATION_COEFFICIENT) .thetaStdev(ThriftyCamConstants.THETA_STANDARD_DEVIATION_COEFFICIENT) .multitagXYStdev(ThriftyCamConstants.MULTITAG_XY_STANDARD_DEVIATION_COEFFICIENT) .cameraDuties(List.of(CameraDuty.FIELD_LOCALIZATION, CameraDuty.REEF_LOCALIZATION)) - .robotToCameraTransform(new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0))) + .robotRelativePose( + new Pose3d( + Units.inchesToMeters(-11.0), + Units.inchesToMeters(-11.75), + Units.inchesToMeters(9.182678), + new Rotation3d(0, Units.degreesToRadians(-20), Units.degreesToRadians(-65)))) .build(); - private static final GompeiVisionConfig BACK_BOTTOM_LEFT = - GompeiVisionConfig.builder() - .key("camera_backbottomleft") - .hardwareID("camera_backbottomleft") - .cameraType(CameraType.THRIFTYCAM) - .exposure(50.0) - .gain(0.0) - .horizontalFOV(ThriftyCamConstants.HORIZONTAL_FOV) - .width(ThriftyCamConstants.WIDTH) - .height(ThriftyCamConstants.HEIGHT) - .cameraMatrix( - new Matrix( - MatBuilder.fill( - N3.instance, N3.instance, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0))) - .distortionCoefficients( - new Matrix( - MatBuilder.fill(N5.instance, N1.instance, 0.0, 0.0, 0.0, 0.0, 0.0))) - .verticalFOV(ThriftyCamConstants.VERTICAL_FOV) - .singletagXYStdev(ThriftyCamConstants.SINGLETAG_XY_STANDARD_DEVIATION_COEFFICIENT) - .thetaStdev(ThriftyCamConstants.THETA_STANDARD_DEVIATION_COEFFICIENT) - .multitagXYStdev(ThriftyCamConstants.MULTITAG_XY_STANDARD_DEVIATION_COEFFICIENT) - .cameraDuties(List.of(CameraDuty.FIELD_LOCALIZATION, CameraDuty.REEF_LOCALIZATION)) - .robotToCameraTransform(new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0))) - .build(); - - private static final GompeiVisionConfig BACK_RIGHT = + private static final GompeiVisionConfig BACK_RIGHT = // Front Left robot relative GompeiVisionConfig.builder() .key("camera_backright") .hardwareID("camera_backright") @@ -263,19 +244,42 @@ public static class RobotCameras { .cameraMatrix( new Matrix( MatBuilder.fill( - N3.instance, N3.instance, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0))) + N3.instance, + N3.instance, + 1364.233774114576, + 0, + 833.9370492297464, + 0, + 1359.524805084978, + 697.4271592073976, + 0, + 0, + 1))) .distortionCoefficients( new Matrix( - MatBuilder.fill(N5.instance, N1.instance, 0.0, 0.0, 0.0, 0.0, 0.0))) + MatBuilder.fill( + N5.instance, + N1.instance, + -0.032643825214208, + 0.006677356066269, + 0.0009003298, + 0.0000022707354437, + 0.004509255043043))) .verticalFOV(ThriftyCamConstants.VERTICAL_FOV) .singletagXYStdev(ThriftyCamConstants.SINGLETAG_XY_STANDARD_DEVIATION_COEFFICIENT) .thetaStdev(ThriftyCamConstants.THETA_STANDARD_DEVIATION_COEFFICIENT) .multitagXYStdev(ThriftyCamConstants.MULTITAG_XY_STANDARD_DEVIATION_COEFFICIENT) .cameraDuties(List.of(CameraDuty.FIELD_LOCALIZATION, CameraDuty.REEF_LOCALIZATION)) - .robotToCameraTransform(new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0))) + .robotRelativePose( + new Pose3d( + Units.inchesToMeters(11.0), + Units.inchesToMeters(11.75), + Units.inchesToMeters(9.182678), + new Rotation3d( + 0, Units.degreesToRadians(-20.0), Units.degreesToRadians(115.0)))) .build(); - private static final GompeiVisionConfig FRONT_RIGHT = + private static final GompeiVisionConfig FRONT_RIGHT = // Back Left robot relative GompeiVisionConfig.builder() .key("camera_frontright") .hardwareID("camera_frontright") @@ -290,34 +294,39 @@ public static class RobotCameras { MatBuilder.fill( N3.instance, N3.instance, - 1624.2147990980468, - 0.0, - 678.5977326688123, - 0.0, - 1623.8010651757406, - 581.2937963816543, - 0.0, - 0.0, - 1.0))) + 1369.692474966544, + 0, + 787.7364691969475, + 0, + 1369.438576249971, + 623.5492216309891, + 0, + 0, + 1))) .distortionCoefficients( new Matrix( MatBuilder.fill( N5.instance, N1.instance, - 0.0962739678996323, - -0.17425866758812222, - -0.0010256591695401809, - 0.00203018525003567, - 0.1499388749754604))) + -0.012505958090617, + -0.055114601985320, + 0.000799901335506, + 0.003926554248417, + 0.079675565790226))) .verticalFOV(ThriftyCamConstants.VERTICAL_FOV) .singletagXYStdev(ThriftyCamConstants.SINGLETAG_XY_STANDARD_DEVIATION_COEFFICIENT) .thetaStdev(ThriftyCamConstants.THETA_STANDARD_DEVIATION_COEFFICIENT) .multitagXYStdev(ThriftyCamConstants.MULTITAG_XY_STANDARD_DEVIATION_COEFFICIENT) .cameraDuties(List.of(CameraDuty.FIELD_LOCALIZATION, CameraDuty.REEF_LOCALIZATION)) - .robotToCameraTransform(new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0))) + .robotRelativePose( + new Pose3d( + Units.inchesToMeters(-11.0), + Units.inchesToMeters(11.75), + Units.inchesToMeters(9.182678), + new Rotation3d(0, Units.degreesToRadians(-20.0), Units.degreesToRadians(65.0)))) .build(); - private static final GompeiVisionConfig FRONT_LEFT = + private static final GompeiVisionConfig FRONT_LEFT = // Front Right robot relative GompeiVisionConfig.builder() .key("camera_frontleft") .hardwareID("camera_frontleft") @@ -330,16 +339,39 @@ public static class RobotCameras { .cameraMatrix( new Matrix( MatBuilder.fill( - N3.instance, N3.instance, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0))) + N3.instance, + N3.instance, + 1110.6402873755883, + 0, + 613.8829964701134, + 0, + 1105.8384147122115, + 492.2432667528521, + 0, + 0, + 1))) .distortionCoefficients( new Matrix( - MatBuilder.fill(N5.instance, N1.instance, 0.0, 0.0, 0.0, 0.0, 0.0))) + MatBuilder.fill( + N5.instance, + N1.instance, + -0.028655371446734232, + -0.011652596818001043, + -0.0013733428778411353, + 0.003188600213446814, + 0.03699152783241122))) .verticalFOV(ThriftyCamConstants.VERTICAL_FOV) .singletagXYStdev(ThriftyCamConstants.SINGLETAG_XY_STANDARD_DEVIATION_COEFFICIENT) .thetaStdev(ThriftyCamConstants.THETA_STANDARD_DEVIATION_COEFFICIENT) .multitagXYStdev(ThriftyCamConstants.MULTITAG_XY_STANDARD_DEVIATION_COEFFICIENT) .cameraDuties(List.of(CameraDuty.FIELD_LOCALIZATION, CameraDuty.REEF_LOCALIZATION)) - .robotToCameraTransform(new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0))) + .robotRelativePose( + new Pose3d( + Units.inchesToMeters(11.0), + Units.inchesToMeters(-11.75), + Units.inchesToMeters(9.182678), + new Rotation3d( + 0, Units.degreesToRadians(-20.0), Units.degreesToRadians(-115.0)))) .build(); public static final Camera[] V0_FUNKY_CAMS = { @@ -357,11 +389,7 @@ public static class RobotCameras { new Camera(new CameraIOLimelight(V2_REDUNDANCY_LEFT)), new Camera(new CameraIOLimelight(V2_REDUNDANCY_RIGHT)) }; - public static final Camera[] V3_EPSILON_CAMS = { - new Camera( - new CameraIOGompeiVision( - BACK_TOP_LEFT, - () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark))), + public static final Camera[] V3_POOT_CAMS = { new Camera( new CameraIOGompeiVision( BACK_BOTTOM_LEFT, @@ -410,5 +438,5 @@ public record GompeiVisionConfig( double thetaStdev, double multitagXYStdev, List cameraDuties, - Transform3d robotToCameraTransform) {} + Pose3d robotRelativePose) {} } diff --git a/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java b/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java index 86bced52..d3ff93bb 100644 --- a/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java +++ b/src/main/java/frc/robot/subsystems/v0_Funky/V0_FunkyRobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.subsystems.v0_Funky.kitbot_roller.V0_FunkyRollerIO; import frc.robot.subsystems.v0_Funky.kitbot_roller.V0_FunkyRollerIOTalonFX; import frc.robot.util.LTNUpdater; +import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class V0_FunkyRobotContainer implements RobotContainer { @@ -75,6 +76,8 @@ public V0_FunkyRobotContainer() { break; } } + + LTNUpdater.registerDrive(drive); } public void configureButtonBindings() { @@ -112,7 +115,7 @@ public void robotPeriodic() { drive.getModulePositions(), vision.getCameras()); - LTNUpdater.updateDrive(drive); + LoggedTunableNumber.updateAll(); } @Override diff --git a/src/main/java/frc/robot/subsystems/v0_GompeivisionTest/v0_GompeivisionTestRobotContainer.java b/src/main/java/frc/robot/subsystems/v0_GompeivisionTest/v0_GompeivisionTestRobotContainer.java index 7a176299..7586fc1d 100644 --- a/src/main/java/frc/robot/subsystems/v0_GompeivisionTest/v0_GompeivisionTestRobotContainer.java +++ b/src/main/java/frc/robot/subsystems/v0_GompeivisionTest/v0_GompeivisionTestRobotContainer.java @@ -27,7 +27,7 @@ public v0_GompeivisionTestRobotContainer() { vision = new Vision( () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded), - RobotCameras.V3_EPSILON_CAMS); + RobotCameras.V3_POOT_CAMS); break; default: break; @@ -38,7 +38,7 @@ public v0_GompeivisionTestRobotContainer() { vision = new Vision( () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded), - RobotCameras.V3_EPSILON_CAMS); + RobotCameras.V3_POOT_CAMS); } configureButtonBindings(); diff --git a/src/main/java/frc/robot/subsystems/v0_Whiplash/V0_WhiplashRobotContainer.java b/src/main/java/frc/robot/subsystems/v0_Whiplash/V0_WhiplashRobotContainer.java index 257275d0..6067944a 100644 --- a/src/main/java/frc/robot/subsystems/v0_Whiplash/V0_WhiplashRobotContainer.java +++ b/src/main/java/frc/robot/subsystems/v0_Whiplash/V0_WhiplashRobotContainer.java @@ -19,7 +19,7 @@ import frc.robot.subsystems.shared.drive.ModuleIOSim; import frc.robot.subsystems.shared.drive.ModuleIOTalonFX; import frc.robot.subsystems.shared.vision.Vision; -import frc.robot.util.LTNUpdater; +import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class V0_WhiplashRobotContainer implements RobotContainer { @@ -108,7 +108,7 @@ public void robotPeriodic() { drive.getYawVelocity(), drive.getModulePositions(), vision.getCameras()); - LTNUpdater.updateDrive(drive); + LoggedTunableNumber.updateAll(); } @Override diff --git a/src/main/java/frc/robot/subsystems/v1_StackUp/V1_StackUpRobotContainer.java b/src/main/java/frc/robot/subsystems/v1_StackUp/V1_StackUpRobotContainer.java index 7749ad61..5e995bbb 100644 --- a/src/main/java/frc/robot/subsystems/v1_StackUp/V1_StackUpRobotContainer.java +++ b/src/main/java/frc/robot/subsystems/v1_StackUp/V1_StackUpRobotContainer.java @@ -50,6 +50,7 @@ import frc.robot.subsystems.v1_StackUp.manipulator.V1_StackUpManipulatorIOSim; import frc.robot.subsystems.v1_StackUp.manipulator.V1_StackUpManipulatorIOTalonFX; import frc.robot.util.LTNUpdater; +import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class V1_StackUpRobotContainer implements RobotContainer { @@ -137,6 +138,7 @@ public V1_StackUpRobotContainer() { if (vision == null) { vision = new Vision(() -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded)); } + LTNUpdater.registerAll(drive, elevator, funnel); configureButtonBindings(); configureAutos(); @@ -208,10 +210,13 @@ private void configureButtonBindings() { driver.rightBumper().onTrue(Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))); // Driver algae - /*driver.back().onTrue(manipulator.toggleAlgaeArm()); - driver - .start() - .onTrue(AlgaeCommands.twerk(drive, elevator, manipulator, RobotCameras.V1_STACKUP_CAMS));*/ + /* + * driver.back().onTrue(manipulator.toggleAlgaeArm()); + * driver + * .start() + * .onTrue(AlgaeCommands.twerk(drive, elevator, manipulator, + * RobotCameras.V1_STACKUP_CAMS)); + */ // Driver POV driver.povUp().onTrue(elevator.setPosition()); @@ -327,9 +332,7 @@ public void robotPeriodic() { drive.getModulePositions(), vision.getCameras()); - LTNUpdater.updateDrive(drive); - LTNUpdater.updateElevator(elevator); - LTNUpdater.updateFunnel(funnel); + LoggedTunableNumber.updateAll(); if (Constants.getMode().equals(Mode.SIM)) { Logger.recordOutput( diff --git a/src/main/java/frc/robot/subsystems/v2_Redundancy/V2_RedundancyRobotContainer.java b/src/main/java/frc/robot/subsystems/v2_Redundancy/V2_RedundancyRobotContainer.java index 8039fd68..6593e392 100644 --- a/src/main/java/frc/robot/subsystems/v2_Redundancy/V2_RedundancyRobotContainer.java +++ b/src/main/java/frc/robot/subsystems/v2_Redundancy/V2_RedundancyRobotContainer.java @@ -57,6 +57,7 @@ import frc.robot.subsystems.v2_Redundancy.superstructure.manipulator.V2_RedundancyManipulatorIOTalonFX; import frc.robot.util.LTNUpdater; import frc.robot.util.LoggedChoreo.ChoreoChooser; +import frc.robot.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class V2_RedundancyRobotContainer implements RobotContainer { @@ -155,6 +156,8 @@ public V2_RedundancyRobotContainer() { } superstructure = new V2_RedundancySuperstructure(elevator, funnel, intake, manipulator); + LTNUpdater.registerAll(drive, elevator, funnel, intake, manipulator); + configureButtonBindings(); configureAutos(); } @@ -428,11 +431,7 @@ public void robotPeriodic() { drive.getModulePositions(), vision.getCameras()); - LTNUpdater.updateDrive(drive); - LTNUpdater.updateElevator(elevator); - LTNUpdater.updateFunnel(funnel); - LTNUpdater.updateAlgaeArm(manipulator); - LTNUpdater.updateIntake(intake); + LoggedTunableNumber.updateAll(); Logger.recordOutput( "Component Poses", diff --git a/src/main/java/frc/robot/subsystems/v2_Redundancy/superstructure/manipulator/V2_RedundancyManipulator.java b/src/main/java/frc/robot/subsystems/v2_Redundancy/superstructure/manipulator/V2_RedundancyManipulator.java index 22cd2649..b71bc242 100644 --- a/src/main/java/frc/robot/subsystems/v2_Redundancy/superstructure/manipulator/V2_RedundancyManipulator.java +++ b/src/main/java/frc/robot/subsystems/v2_Redundancy/superstructure/manipulator/V2_RedundancyManipulator.java @@ -77,8 +77,7 @@ public Rotation2d getArmAngle() { return inputs.armPosition; } - public void setRollerGoal(ManipulatorRollerState goal) { - rollerGoal = goal; + public void setRollerGoal(ManipulatorRollerState rollerGoal) { if (RobotState.isHasAlgae() && Set.of( ManipulatorRollerState.STOP, diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootMechanism3d.java b/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootMechanism3d.java new file mode 100644 index 00000000..b52f73d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootMechanism3d.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems.v3_Poot; + +import edu.wpi.first.math.MathUtil; +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 org.littletonrobotics.junction.Logger; + +public class V3_PootMechanism3d { + private static final double ELEVATOR_STAGE_1_MIN_HEIGHT = 0.095250; // Meters off the ground + private static final double ELEVATOR_STAGE_1_MAX_HEIGHT = 0.7809; + private static final double ELEVATOR_CARRIAGE_MANIPULATOR_MIN_HEIGHT = 0.120650; + private static final double ELEVATOR_CARRIAGE_MANIPULATOR_MAX_HEIGHT = 0.877522325; + + private static final double MIN_EXTENSION_METERS = ELEVATOR_CARRIAGE_MANIPULATOR_MIN_HEIGHT; + private static final double MAX_EXTENSION_METERS = + ELEVATOR_STAGE_1_MAX_HEIGHT + ELEVATOR_CARRIAGE_MANIPULATOR_MAX_HEIGHT; + + private static final Pose3d ELEVATOR_STAGE_1 = + new Pose3d(-0.177800, 0, 0.095250, new Rotation3d()); + private static final Pose3d ELEVATOR_CARRIAGE_MANIPULATOR = + new Pose3d(-0.15381615, 0, 0.120650, new Rotation3d()); + + public static final Pose3d[] getPoses( + double elevatorExtensionMeters, Rotation2d intakeAngle, Rotation2d armAngle) { + double extensionMeters = + MathUtil.clamp(elevatorExtensionMeters, MIN_EXTENSION_METERS, MAX_EXTENSION_METERS); + + double stage1Height = ELEVATOR_STAGE_1_MIN_HEIGHT; + double carriageHeight = ELEVATOR_CARRIAGE_MANIPULATOR_MIN_HEIGHT; + + // If extension is within the first stage's range, only move carriage + if (extensionMeters <= ELEVATOR_STAGE_1_MAX_HEIGHT) { + carriageHeight = extensionMeters; + } else { + // Carriage is fully extended, start moving stage 1 + double remainingExtension = extensionMeters - ELEVATOR_CARRIAGE_MANIPULATOR_MAX_HEIGHT; + stage1Height = ELEVATOR_STAGE_1_MIN_HEIGHT + remainingExtension; + carriageHeight = ELEVATOR_CARRIAGE_MANIPULATOR_MAX_HEIGHT + remainingExtension; + } + + // Create transformed poses + Pose3d ELEVATOR_STAGE_1_POSE = + ELEVATOR_STAGE_1.transformBy( + new Transform3d(0, 0, stage1Height - ELEVATOR_STAGE_1_MIN_HEIGHT, new Rotation3d())); + Pose3d ELEVATOR_CARRIAGE_POSE = + ELEVATOR_CARRIAGE_MANIPULATOR.transformBy( + new Transform3d(0, 0, carriageHeight, new Rotation3d())); + + Logger.recordOutput( + "Zero Poses", + new Pose3d[] { + new Pose3d(), new Pose3d(), new Pose3d(), new Pose3d(), new Pose3d(), new Pose3d() + }); + + return new Pose3d[] { + new Pose3d(0.1875, 0, 0.1363, new Rotation3d(0.0, intakeAngle.getRadians(), 0.0)), + ELEVATOR_STAGE_1_POSE, + ELEVATOR_CARRIAGE_POSE, + new Pose3d(-0.15381615, 0, 0.270855, new Rotation3d()) + .transformBy( + new Transform3d( + 0, 0, carriageHeight, new Rotation3d(armAngle.getRadians(), 0.0, 0.0))), + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootRobotContainer.java b/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootRobotContainer.java new file mode 100644 index 00000000..890407aa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/V3_PootRobotContainer.java @@ -0,0 +1,373 @@ +package frc.robot.subsystems.v3_Poot; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.FieldConstants.Reef.ReefPose; +import frc.robot.FieldConstants.Reef.ReefState; +import frc.robot.RobotContainer; +import frc.robot.RobotState; +import frc.robot.commands.AutonomousCommands; +import frc.robot.commands.CompositeCommands.SharedCommands; +import frc.robot.commands.CompositeCommands.V3_PootCompositeCommands; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.shared.drive.*; +import frc.robot.subsystems.shared.elevator.Elevator; +import frc.robot.subsystems.shared.elevator.Elevator.ElevatorFSM; +import frc.robot.subsystems.shared.elevator.ElevatorConstants.ElevatorPositions; +import frc.robot.subsystems.shared.elevator.ElevatorIO; +import frc.robot.subsystems.shared.elevator.ElevatorIOSim; +import frc.robot.subsystems.shared.elevator.ElevatorIOTalonFX; +import frc.robot.subsystems.shared.vision.Vision; +import frc.robot.subsystems.shared.vision.VisionConstants.RobotCameras; +import frc.robot.subsystems.v3_Poot.climber.V3_PootClimber; +import frc.robot.subsystems.v3_Poot.climber.V3_PootClimberIO; +import frc.robot.subsystems.v3_Poot.climber.V3_PootClimberIOSim; +import frc.robot.subsystems.v3_Poot.climber.V3_PootClimberIOTalonFX; +import frc.robot.subsystems.v3_Poot.leds.V3_PootLEDs; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructure; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureStates; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIO; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIOSim; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIOTalonFX; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorIO; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorIOSim; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorIOTalonFX; +import frc.robot.util.LTNUpdater; +import frc.robot.util.LoggedChoreo.ChoreoChooser; +import frc.robot.util.LoggedTunableNumber; +import org.ironmaple.simulation.SimulatedArena; +import org.littletonrobotics.junction.Logger; + +public class V3_PootRobotContainer implements RobotContainer { + // Subsystems + private Drive drive; + private ElevatorFSM elevator; + private V3_PootIntake intake; + private V3_PootManipulator manipulator; + private V3_PootSuperstructure superstructure; + private V3_PootClimber climber; + private V3_PootLEDs leds; + private Vision vision; + + // Controller + private static final CommandXboxController driver = new CommandXboxController(0); + private static final CommandXboxController operator = new CommandXboxController(1); + + // Auto chooser + private final ChoreoChooser autoChooser = new ChoreoChooser(); + + public V3_PootRobotContainer() { + + if (Constants.getMode() != Mode.REPLAY) { + switch (Constants.ROBOT) { + case V3_POOT: + drive = + new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(0, DriveConstants.FRONT_LEFT), + new ModuleIOTalonFX(1, DriveConstants.FRONT_RIGHT), + new ModuleIOTalonFX(2, DriveConstants.BACK_LEFT), + new ModuleIOTalonFX(3, DriveConstants.BACK_RIGHT)); + elevator = new Elevator(new ElevatorIOTalonFX()).getFSM(); + intake = new V3_PootIntake(new V3_PootIntakeIOTalonFX()); + manipulator = new V3_PootManipulator(new V3_PootManipulatorIOTalonFX()); + climber = new V3_PootClimber(new V3_PootClimberIOTalonFX()); + superstructure = new V3_PootSuperstructure(elevator, intake, manipulator); + leds = new V3_PootLEDs(); + vision = + new Vision( + () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded), + RobotCameras.V3_POOT_CAMS); + break; + case V3_POOT_SIM: + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(DriveConstants.FRONT_LEFT), + new ModuleIOSim(DriveConstants.FRONT_RIGHT), + new ModuleIOSim(DriveConstants.BACK_LEFT), + new ModuleIOSim(DriveConstants.BACK_RIGHT)); + elevator = new Elevator(new ElevatorIOSim()).getFSM(); + intake = new V3_PootIntake(new V3_PootIntakeIOSim()); + manipulator = new V3_PootManipulator(new V3_PootManipulatorIOSim()); + climber = new V3_PootClimber(new V3_PootClimberIOSim()); + superstructure = new V3_PootSuperstructure(elevator, intake, manipulator); + leds = new V3_PootLEDs(); + vision = + new Vision( + () -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded), + RobotCameras.V3_POOT_CAMS); + break; + default: + break; + } + } + + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + } + if (elevator == null) { + elevator = new Elevator(new ElevatorIO() {}).getFSM(); + } + if (intake == null) { + intake = new V3_PootIntake(new V3_PootIntakeIO() {}); + } + if (manipulator == null) { + manipulator = new V3_PootManipulator(new V3_PootManipulatorIO() {}); + } + if (climber == null) { + climber = new V3_PootClimber(new V3_PootClimberIO() {}); + } + if (leds == null) { + leds = new V3_PootLEDs(); + } + if (superstructure == null) { + superstructure = new V3_PootSuperstructure(elevator, intake, manipulator); + } + + LTNUpdater.registerAll(drive, elevator, intake, manipulator); + + configureButtonBindings(); + configureAutos(); + } + + /** + * Configure the button bindings for the robot. This method is called in the constructor and is + * responsible for setting up the default commands for each button on the controllers. + */ + private void configureButtonBindings() { + Trigger elevatorStow = + new Trigger( + () -> + elevator.getPosition().equals(ElevatorPositions.CORAL_INTAKE) + || elevator.getPosition().equals(ElevatorPositions.STOW)); + Trigger elevatorNotStow = + new Trigger( + () -> + !elevator.getPosition().equals(ElevatorPositions.CORAL_INTAKE) + && !elevator.getPosition().equals(ElevatorPositions.STOW)); + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -driver.getLeftY(), + () -> -driver.getLeftX(), + () -> -driver.getRightX(), + () -> false, + () -> false, + driver.povRight())); + + driver.povDown().onTrue(SharedCommands.resetHeading(drive)); + + driver.y().and(elevatorStow).onTrue(SharedCommands.setStaticReefHeight(ReefState.L4)); + driver.x().and(elevatorStow).onTrue(SharedCommands.setStaticReefHeight(ReefState.L3)); + driver.b().and(elevatorStow).onTrue(SharedCommands.setStaticReefHeight(ReefState.L2)); + driver.a().and(elevatorStow).onTrue(SharedCommands.setStaticReefHeight(ReefState.L1)); + + driver + .y() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L4, superstructure)); + driver + .x() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L3, superstructure)); + driver + .b() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L2, superstructure)); + driver + .a() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L1, superstructure)); + + driver + .rightTrigger(0.5) + .whileTrue(V3_PootCompositeCommands.optimalAutoScoreCoralSequence(drive, superstructure)); + driver + .leftTrigger(0.5) + .whileTrue( + V3_PootCompositeCommands.intakeCoralDriverSequence(superstructure, intake, manipulator)) + .onFalse( + V3_PootCompositeCommands.postIntakeCoralSequence(superstructure, intake, manipulator)); + + driver + .leftBumper() + .whileTrue(V3_PootCompositeCommands.intakeAlgaeFloor(superstructure, manipulator)) + .onFalse( + Commands.either( + superstructure.runGoal(V3_PootSuperstructureStates.STOW_UP), + superstructure.runGoal(V3_PootSuperstructureStates.STOW_DOWN), + () -> RobotState.isHasAlgae())); + driver.rightBumper().onTrue(Commands.runOnce(() -> RobotState.toggleReefPost())); + + driver.povUp().onTrue(superstructure.setPosition()); + driver.povDown().onTrue(SharedCommands.resetHeading(drive)); + // driver.povLeft().onTrue(DriveCommands.inchMovement(drive, -0.5, .07)); + + driver + .leftStick() + .onTrue(V3_PootCompositeCommands.optimalAutoScoreCoralSequence(drive, superstructure)); + + driver + .back() + .whileTrue(V3_PootCompositeCommands.intakeAlgaeFromReef(drive, superstructure)) + .whileFalse(V3_PootCompositeCommands.postIntakeAlgaeFromReef(drive, superstructure)); + + driver + .start() + .whileTrue( + V3_PootCompositeCommands.dropAlgae( + drive, + elevator, + manipulator, + intake, + superstructure, + () -> RobotState.getReefAlignData().algaeIntakeHeight())); + + operator.y().onTrue(SharedCommands.setStaticReefHeight(ReefState.L4)); + operator.x().onTrue(SharedCommands.setStaticReefHeight(ReefState.L3)); + operator.b().onTrue(SharedCommands.setStaticReefHeight(ReefState.L2)); + operator.a().onTrue(SharedCommands.setStaticReefHeight(ReefState.L1)); + + operator + .y() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L4, superstructure)); + operator + .x() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L3, superstructure)); + operator + .b() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L2, superstructure)); + operator + .a() + .and(elevatorNotStow) + .onTrue(V3_PootCompositeCommands.setDynamicReefHeight(ReefState.L1, superstructure)); + + operator + .rightTrigger(0.5) + .whileTrue( + superstructure.override( + () -> manipulator.setRollerGoal(ManipulatorRollerState.SCORE_CORAL), 0.4)); + + operator.leftBumper().onTrue(Commands.runOnce(() -> RobotState.setReefPost(ReefPose.LEFT))); + operator.rightBumper().onTrue(Commands.runOnce(() -> RobotState.setReefPost(ReefPose.RIGHT))); + + operator + .povUp() + .onTrue( + Commands.sequence( + Commands.runOnce(() -> manipulator.setArmGoal(Rotation2d.fromDegrees(90))), + Commands.runOnce(() -> elevator.setPosition(() -> ReefState.HANDOFF)), + Commands.runOnce(() -> intake.setPivotGoal(IntakePivotState.L1)), + climber.releaseClimber())); + operator + .povDown() + .whileTrue( + Commands.sequence( + Commands.runOnce(() -> RobotState.setClimberReady(false)), + Commands.runOnce(() -> elevator.setPosition(() -> ReefState.STOW)), + climber.winchClimber())); + operator + .povLeft() + .whileTrue(superstructure.runGoal(V3_PootSuperstructureStates.PROCESSOR)) + .onFalse( + superstructure + .runActionWithTimeout(V3_PootSuperstructureStates.PROCESSOR_SCORE, 1) + .finallyDo(() -> RobotState.setHasAlgae(false))); + + operator.start().whileTrue(superstructure.runGoal(V3_PootSuperstructureStates.BARGE)); + + operator.back().onTrue(V3_PootCompositeCommands.optimalScoreBarge(superstructure)); + + Trigger trigger = driver.povLeft(); + Trigger trigger2 = driver.povRight(); + + trigger.onTrue(superstructure.everythingsFucked(trigger2)); + } + + private void configureAutos() { + autoChooser.addCmd( + "Drive FF Characterization", () -> DriveCommands.feedforwardCharacterization(drive)); + autoChooser.addCmd( + "Wheel Radius Characterization", () -> DriveCommands.wheelRadiusCharacterization(drive)); + autoChooser.addRoutine( + "4 Piece Right Early Madtown", + () -> AutonomousCommands.autoERight(drive, superstructure, intake, manipulator)); + autoChooser.addRoutine( + "4 Piece Right Late Madtown", + () -> AutonomousCommands.autoERightBack(drive, superstructure, intake, manipulator)); + autoChooser.addRoutine( + "4 Piece Left Early Madtown", + () -> AutonomousCommands.autoELeft(drive, superstructure, intake, manipulator)); + autoChooser.addRoutine( + "4 Piece Left Late Madtown", + () -> AutonomousCommands.autoELeftBack(drive, superstructure, intake, manipulator)); + autoChooser.addRoutine( + "Algae", () -> AutonomousCommands.autoFLeft(drive, superstructure, intake, manipulator)); + autoChooser.addRoutine( + "1 piece do nothing", + () -> AutonomousCommands.autoFLeftMinimal(drive, superstructure, intake, manipulator)); + SmartDashboard.putData("Autonomous Modes", autoChooser); + } + + /** + * Periodic function for the robot. This function is called every 20ms, and is responsible for + * updating the robot's state and logging relevant data. + */ + @Override + public void robotPeriodic() { + RobotState.periodic( + drive.getRawGyroRotation(), + NetworkTablesJNI.now(), + drive.getYawVelocity(), + drive.getModulePositions(), + vision.getCameras()); + + LoggedTunableNumber.updateAll(); + + Logger.recordOutput( + "Component Poses", + V3_PootMechanism3d.getPoses( + elevator.getPositionMeters(), intake.getPivotAngle(), manipulator.getArmAngle())); + + if (!Constants.getMode().equals(Constants.Mode.REAL)) { + Logger.recordOutput( + "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); + Logger.recordOutput( + "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); + } + } + + /** + * Returns the autonomous command for the robot. This command will be scheduled for the entire + * autonomous period. + * + * @return the autonomous command for the robot + */ + @Override + public Command getAutonomousCommand() { + return autoChooser.selectedCommand(); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimber.java b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimber.java new file mode 100644 index 00000000..29ac5d4f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimber.java @@ -0,0 +1,122 @@ +package frc.robot.subsystems.v3_Poot.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; +import frc.robot.util.ExternalLoggedTracer; +import frc.robot.util.InternalLoggedTracer; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class V3_PootClimber extends SubsystemBase { + private final V3_PootClimberIO io; + private final V3_PootClimberIOInputsAutoLogged inputs; + + @AutoLogOutput(key = "Climber/override") + private boolean override; + + @AutoLogOutput(key = "Climber/isClimbed") + private boolean isClimbed; + + public V3_PootClimber(V3_PootClimberIO io) { + this.io = io; + inputs = new V3_PootClimberIOInputsAutoLogged(); + + isClimbed = false; + + override = false; + } + + @Override + public void periodic() { + ExternalLoggedTracer.reset(); + InternalLoggedTracer.reset(); + io.updateInputs(inputs); + InternalLoggedTracer.record("Climber Input Update", "Climber/Periodic"); + + InternalLoggedTracer.reset(); + Logger.processInputs("Climber", inputs); + InternalLoggedTracer.record("Climber Input Processing", "Climber/Periodic"); + + isClimbed = io.isClimbed(); + + if (RobotState.isClimberReady()) { + io.setRollerVoltage(-12); + } + + ExternalLoggedTracer.record("Climber Total", "Climber/Periodic"); + } + + /** + * Creates a command to set the voltage of the climber deployment motor. + * + * @param volts The voltage to set. + * @return A command to set the voltage. + */ + public Command setDeploymentVoltage(double volts) { + return Commands.run(() -> io.setDeploymentVoltage(volts)); + } + + /** + * Creates a command to set the voltage of the climber roller motor. + * + * @param volts The voltage to set. + * @return A command to set the voltage. + */ + public Command setRollerVoltage(double volts) { + return Commands.run(() -> io.setRollerVoltage(volts)); + } + + /** + * Creates a command to release the climber. The climber is released by applying voltage until the + * position is greater than or equal to 20 radians. + * + * @return A command to release the climber. + */ + public Command releaseClimber() { + return Commands.sequence( + Commands.runOnce(() -> io.setRollerVoltage(12)), + this.runEnd(() -> io.setDeploymentVoltage(-1), () -> io.setDeploymentVoltage(0)) + .until( + () -> + inputs.deploymentPosition.getRadians() + <= V3_PootClimberConstants.CLIMBER_CLIMBED_DEPLOYED_RADIANS + || override) + .finallyDo(() -> RobotState.setClimberReady(true))); + } + + /** + * Creates a command to winch the climber. The climber is winched by applying voltage until the + * climb is complete. + * + * @return A command to winch the climber. + */ + public Command winchClimber() { + return Commands.parallel( + Commands.runOnce(() -> io.setRollerVoltage(0)), + Commands.runEnd(() -> io.setDeploymentVoltage(-12), () -> io.setDeploymentVoltage(0)) + .until(() -> isClimbed)); + } + + /** + * Creates a command to manually winch the climber with a lower voltage. + * + * @return A command to manually winch the climber. + */ + public Command winchClimberManual() { + return Commands.parallel( + Commands.runOnce(() -> io.setRollerVoltage(0)), + this.runEnd(() -> io.setDeploymentVoltage(-12.0), () -> io.setDeploymentVoltage(0))); + } + + /** + * Creates a command to override the climber deployment readiness check. + * + * @param override True to override, false otherwise. + * @return A command to set the override. + */ + public Command manualDeployOverride(boolean override) { // set using debug board button + return Commands.runOnce(() -> this.override = override); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberConstants.java b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberConstants.java new file mode 100644 index 00000000..e2d3f50a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberConstants.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.v3_Poot.climber; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +public class V3_PootClimberConstants { + public static final int DEPLOYMENT_CAN_ID; + + public static final int ROLLER_CAN_ID; + + public static final MotorParameters MOTOR_PARAMETERS; + + public static final double CLIMBER_CLIMBED_RADIANS; + + static { + DEPLOYMENT_CAN_ID = 50; + ROLLER_CAN_ID = 51; + + MOTOR_PARAMETERS = + new MotorParameters(DCMotor.getKrakenX60Foc(1), 16.0, 0.81, Units.inchesToMeters(1.78)); + + CLIMBER_CLIMBED_RADIANS = -400; + } + + public static record MotorParameters( + DCMotor MOTOR_CONFIG, double GEAR_RATIO, double GEARBOX_EFFICIENCY, double SPOOL_DIAMETER) {} + + public static final double CLIMBER_CLIMBED_ZERO_RADIANS = 41; + public static final double CLIMBER_CLIMBED_DEPLOYED_RADIANS = -10; + public static final ClimberTimingConfig CLIMBER_TIMING_CONFIG = + new ClimberTimingConfig(1.1, 0.25, 0.5); + public static final CurrentLimits CURRENT_LIMITS = new CurrentLimits(40.0, 80.0); + + public static record CurrentLimits(double SUPPLY_CURRENT_LIMIT, double STATOR_CURRENT_LIMIT) {} + + public static record ClimberTimingConfig( + double WAIT_AFTER_RELEASE_SECONDS, + double REDUNDANCY_DELAY_SECONDS, + double REDUNDANCY_TRUSTING_TIMEOUT_SECONDS) {} +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIO.java b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIO.java new file mode 100644 index 00000000..ab6ede0c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIO.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.v3_Poot.climber; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface V3_PootClimberIO { + @AutoLog + public static class V3_PootClimberIOInputs { + public Rotation2d deploymentPosition = new Rotation2d(); + public double deploymentVelocityRadiansPerSecond = 0.0; + public double deploymentAppliedVolts = 0.0; + public double deploymentSupplyCurrentAmps = 0.0; + public double deploymentTorqueCurrentAmps = 0.0; + public double deploymentTemperatureCelsius = 0.0; + + public Rotation2d rollerPosition = new Rotation2d(); + public double rollerVelocityRadiansPerSecond = 0.0; + public double rollerAppliedVolts = 0.0; + public double rollerSupplyCurrentAmps = 0.0; + public double rollerTorqueCurrentAmps = 0.0; + public double rollerTemperatureCelsius = 0.0; + } + /** + * Updates the inputs for the manipulator subsystem. + * + * @param inputs The inputs to update. + */ + public default void updateInputs(V3_PootClimberIOInputs inputs) {} + + /** + * Sets the voltage for the intake. + * + * @param volts The voltage to set. + */ + public default void setDeploymentVoltage(double volts) {} + + /** + * Sets the voltage for the inner manipulator roller. + * + * @param volts The voltage to set. + */ + public default void setRollerVoltage(double volts) {} + + /** + * Gets the state of the climber based on the current draw. + * + * @return The state of the climber (climbed or not). + */ + public default boolean isClimbed() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOSim.java b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOSim.java new file mode 100644 index 00000000..9770a41f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOSim.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.v3_Poot.climber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class V3_PootClimberIOSim implements V3_PootClimberIO { + private final DCMotorSim sim; + + private double deploymentAppliedVolts; + private double rollerAppliedVolts; + + public V3_PootClimberIOSim() { + sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + V3_PootClimberConstants.MOTOR_PARAMETERS.MOTOR_CONFIG(), + 0.004, + V3_PootClimberConstants.MOTOR_PARAMETERS.GEAR_RATIO()), + V3_PootClimberConstants.MOTOR_PARAMETERS.MOTOR_CONFIG()); + + deploymentAppliedVolts = 0.0; + rollerAppliedVolts = 0.0; + } + + @Override + public void updateInputs(V3_PootClimberIOInputs inputs) { + deploymentAppliedVolts = MathUtil.clamp(deploymentAppliedVolts, -12.0, 12.0); + sim.setInputVoltage(deploymentAppliedVolts); + rollerAppliedVolts = MathUtil.clamp(rollerAppliedVolts, -12.0, 12.0); + sim.setInputVoltage(rollerAppliedVolts); + sim.update(Constants.LOOP_PERIOD_SECONDS); + + // Update in standardized order + inputs.deploymentPosition = Rotation2d.fromRadians(sim.getAngularPositionRad()); + inputs.deploymentVelocityRadiansPerSecond = sim.getAngularVelocityRadPerSec(); + inputs.deploymentAppliedVolts = deploymentAppliedVolts; + inputs.deploymentSupplyCurrentAmps = + sim.getCurrentDrawAmps() / V3_PootClimberConstants.MOTOR_PARAMETERS.GEARBOX_EFFICIENCY(); + inputs.deploymentTorqueCurrentAmps = sim.getCurrentDrawAmps(); + + inputs.rollerPosition = Rotation2d.fromRadians(sim.getAngularPositionRad()); + inputs.rollerVelocityRadiansPerSecond = sim.getAngularVelocityRadPerSec(); + inputs.rollerAppliedVolts = rollerAppliedVolts; + inputs.rollerSupplyCurrentAmps = + sim.getCurrentDrawAmps() / V3_PootClimberConstants.MOTOR_PARAMETERS.GEARBOX_EFFICIENCY(); + inputs.rollerTorqueCurrentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setDeploymentVoltage(double volts) { + deploymentAppliedVolts = volts; + } + + @Override + public void setRollerVoltage(double volts) { + rollerAppliedVolts = volts; + } + + @Override + public boolean isClimbed() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOTalonFX.java new file mode 100644 index 00000000..715ba716 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/climber/V3_PootClimberIOTalonFX.java @@ -0,0 +1,119 @@ +package frc.robot.subsystems.v3_Poot.climber; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.util.PhoenixUtil; + +public class V3_PootClimberIOTalonFX implements V3_PootClimberIO { + + private final TalonFX rollerTalonFX; + private final TalonFX deploymentTalonFX; + + private final TalonFXConfiguration rollerConfig; + private final TalonFXConfiguration deploymentConfig; + + private final VoltageOut rollerVoltageRequest; + + private final VoltageOut deploymentVoltageRequest; + + private final StatusSignal rollerPositionRotations; + private final StatusSignal rollerVelocityRotationsPerSecond; + private final StatusSignal rollerAppliedVoltage; + private final StatusSignal rollerSupplyCurrentAmps; + private final StatusSignal rollerTorqueCurrentAmps; + private final StatusSignal rollerTemperatureCelsius; + + private final StatusSignal deploymentPositionRotations; + private final StatusSignal deploymentVelocityRotationsPerSecond; + private final StatusSignal deploymentAppliedVoltage; + private final StatusSignal deploymentSupplyCurrentAmps; + private final StatusSignal deploymentTorqueCurrentAmps; + private final StatusSignal deploymentTemperatureCelsius; + + public V3_PootClimberIOTalonFX() { + rollerTalonFX = new TalonFX(V3_PootClimberConstants.ROLLER_CAN_ID); + deploymentTalonFX = new TalonFX(V3_PootClimberConstants.DEPLOYMENT_CAN_ID); + + rollerConfig = new TalonFXConfiguration(); + rollerPositionRotations = rollerTalonFX.getPosition(); + rollerVelocityRotationsPerSecond = rollerTalonFX.getVelocity(); + rollerAppliedVoltage = rollerTalonFX.getMotorVoltage(); + rollerSupplyCurrentAmps = rollerTalonFX.getSupplyCurrent(); + rollerTorqueCurrentAmps = rollerTalonFX.getTorqueCurrent(); + rollerTemperatureCelsius = rollerTalonFX.getDeviceTemp(); + tryUntilOk(5, () -> rollerTalonFX.getConfigurator().apply(rollerConfig, 0.25)); + + deploymentConfig = new TalonFXConfiguration(); + deploymentPositionRotations = deploymentTalonFX.getPosition(); + deploymentVelocityRotationsPerSecond = deploymentTalonFX.getVelocity(); + deploymentAppliedVoltage = deploymentTalonFX.getMotorVoltage(); + deploymentSupplyCurrentAmps = deploymentTalonFX.getSupplyCurrent(); + deploymentTorqueCurrentAmps = deploymentTalonFX.getTorqueCurrent(); + deploymentTemperatureCelsius = deploymentTalonFX.getDeviceTemp(); + tryUntilOk(5, () -> deploymentTalonFX.getConfigurator().apply(deploymentConfig, 0.25)); + + deploymentVoltageRequest = new VoltageOut(0); + rollerVoltageRequest = new VoltageOut(0); + + PhoenixUtil.registerSignals( + false, + deploymentPositionRotations, + deploymentVelocityRotationsPerSecond, + deploymentAppliedVoltage, + deploymentSupplyCurrentAmps, + deploymentTorqueCurrentAmps, + deploymentTemperatureCelsius, + rollerPositionRotations, + rollerVelocityRotationsPerSecond, + rollerAppliedVoltage, + rollerSupplyCurrentAmps, + rollerTorqueCurrentAmps, + rollerTemperatureCelsius); + } + + @Override + public void updateInputs(V3_PootClimberIOInputs inputs) { + inputs.deploymentPosition = new Rotation2d(deploymentPositionRotations.getValue()); + inputs.deploymentVelocityRadiansPerSecond = + deploymentVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.deploymentAppliedVolts = deploymentAppliedVoltage.getValueAsDouble(); + inputs.deploymentSupplyCurrentAmps = deploymentSupplyCurrentAmps.getValueAsDouble(); + inputs.deploymentTorqueCurrentAmps = deploymentTorqueCurrentAmps.getValueAsDouble(); + inputs.deploymentTemperatureCelsius = deploymentTemperatureCelsius.getValueAsDouble(); + + inputs.rollerPosition = new Rotation2d(rollerPositionRotations.getValue()); + inputs.rollerVelocityRadiansPerSecond = + rollerVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.rollerAppliedVolts = rollerAppliedVoltage.getValueAsDouble(); + inputs.rollerSupplyCurrentAmps = rollerSupplyCurrentAmps.getValueAsDouble(); + inputs.rollerTorqueCurrentAmps = rollerTorqueCurrentAmps.getValueAsDouble(); + inputs.rollerTemperatureCelsius = rollerTemperatureCelsius.getValueAsDouble(); + } + + @Override + public void setDeploymentVoltage(double volts) { + deploymentTalonFX.setControl(deploymentVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + public void setRollerVoltage(double volts) { + rollerTalonFX.setControl(rollerVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + @Override + public boolean isClimbed() { + return Units.rotationsToRadians(deploymentPositionRotations.getValueAsDouble()) + <= V3_PootClimberConstants.CLIMBER_CLIMBED_RADIANS; + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/leds/V3_PootLEDs.java b/src/main/java/frc/robot/subsystems/v3_Poot/leds/V3_PootLEDs.java new file mode 100644 index 00000000..328ba24e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/leds/V3_PootLEDs.java @@ -0,0 +1,122 @@ +package frc.robot.subsystems.v3_Poot.leds; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.FieldConstants.Reef.ReefPose; +import frc.robot.Robot; +import frc.robot.RobotState; +import frc.robot.RobotState.RobotMode; +import frc.robot.subsystems.shared.leds.Leds; + +public class V3_PootLEDs extends Leds { + private static Leds instance; + + // Robot state tracking + public int loopCycleCount = 0; + public boolean autoFinished = false; + public boolean lowBatteryAlert = false; + + private boolean lastEnabledAuto = false; + private double lastEnabledTime = 0.0; + private boolean estopped = false; + + private static final int LENGTH = 203; + private static final int PORT = 0; + private static final int LEFT_LENGTH_START = 0; + private static final int LEFT_LENGTH_END = 33; + private static final int RIGHT_LENGTH_START = 34; + private static final int RIGHT_LENGTH_END = 68; + + public V3_PootLEDs() { + super(LENGTH, PORT); + } + + public static Leds getInstance() { + if (instance == null) { + instance = new V3_PootLEDs(); + } + return instance; + } + + public synchronized void periodic() { + lowBatteryAlert = RobotController.getBatteryVoltage() <= 11.5; + // Update auto state + if (RobotMode.disabled()) { + autoFinished = false; + } else { + lastEnabledAuto = RobotMode.auto(); + lastEnabledTime = Timer.getFPGATimestamp(); + } + + // Update estop state + if (DriverStation.isEStopped()) { + estopped = true; + } + + // Exit during initial cycles + loopCycleCount += 1; + if (loopCycleCount < Leds.MIN_LOOP_CYCLE_COUNT) { + return; + } + + // Stop loading notifier if running + loadingNotifier.stop(); + + setPattern(); + + // Update LEDs + leds.setData(buffer); + } + + private void setPattern() { + // Select LED mode + solid(Color.kBlack); // Default to off% + if (estopped) { + solid(Color.kRed); + } else if (RobotMode.disabled()) { + if (lastEnabledAuto && Timer.getFPGATimestamp() - lastEnabledTime < AUTO_FADE_MAX_TIME) { + // Auto fade + solid(1.0 - ((Timer.getFPGATimestamp() - lastEnabledTime) / AUTO_FADE_TIME), Color.kGreen); + } else if (lowBatteryAlert) { + // Low battery + strobe(Color.kOrangeRed, Color.kBlack, STROBE_FAST_DURATION); + } else if (Robot.isJitting()) { + rainbow(LENGTH, 5); + } else { + rainbow(LENGTH, 5); + } + } else if (RobotMode.enabled()) { + if (RobotState.isAutoAligning()) { + rainbow(LENGTH, 0.25); + } else if (RobotState.isHasAlgae()) { + wave(Color.kBlack, Color.kDarkGreen, RAINBOW_CYCLE_LENGTH, 1.0); + } else { + if (RobotState.getOIData().currentReefPost().equals(ReefPose.RIGHT)) { + if (RobotState.isIntakingCoral()) { + solid(Color.kAqua, LEFT_LENGTH_START, LEFT_LENGTH_END); + } else { + breath( + Color.kBlack, + Color.kDarkViolet, + Timer.getFPGATimestamp(), + LEFT_LENGTH_START, + LEFT_LENGTH_END); + } + } else if (RobotState.getOIData().currentReefPost().equals(ReefPose.LEFT)) { + if (RobotState.isIntakingCoral()) { + solid(Color.kAqua, RIGHT_LENGTH_START, RIGHT_LENGTH_END); + } else { + breath( + Color.kBlack, + Color.kDarkViolet, + Timer.getFPGATimestamp(), + RIGHT_LENGTH_START, + RIGHT_LENGTH_END); + } + } + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructure.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructure.java new file mode 100644 index 00000000..9c0fdec8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructure.java @@ -0,0 +1,739 @@ +package frc.robot.subsystems.v3_Poot.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.FieldConstants.Reef.ReefState; +import frc.robot.RobotState; +import frc.robot.RobotState.RobotMode; +import frc.robot.RobotState.ScoreSide; +import frc.robot.subsystems.shared.elevator.Elevator.ElevatorFSM; +import frc.robot.subsystems.shared.elevator.ElevatorConstants.ElevatorPositions; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureEdges.EdgeCommand; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureEdges.GamePieceEdge; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorArmState; +import frc.robot.util.NTPrefixes; +import java.util.*; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import java.util.stream.Collectors; +import lombok.Getter; +import org.jgrapht.Graph; +import org.jgrapht.graph.DefaultDirectedGraph; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +/** + * The V3_PootSuperstructure class manages the coordinated movement and state transitions of the + * robot's major subsystems including elevator, manipulator, and intake. + */ +public class V3_PootSuperstructure extends SubsystemBase { + private final Graph graph; + private final ElevatorFSM elevator; + private final V3_PootIntake intake; + private final V3_PootManipulator manipulator; + + /** + * The previous, current, and next states of the superstructure. These are used to track the state + * transitions and manage the command scheduling. + */ + @Getter private V3_PootSuperstructureStates previousState; + + /** + * The current state of the superstructure, which is updated periodically based on the command + * scheduling and state transitions. + */ + @Getter private V3_PootSuperstructureStates currentState; + + /** + * The next state that the superstructure is transitioning to. This is determined by the command + * scheduling and the current target state. + */ + @Getter private V3_PootSuperstructureStates nextState; + + /** + * The target state that the superstructure is trying to achieve. This is set by the robot and + * determines the next action to be taken. + */ + @Getter private V3_PootSuperstructureStates targetState; + + /** The command that is currently being executed to transition between states. */ + private EdgeCommand edgeCommand; + + /** + * Constructs a V2_RedundancySuperstructure. + * + * @param elevator The elevator subsystem. + * @param intake The intake subsystem. + * @param manipulator The manipulator subsystem. + */ + public V3_PootSuperstructure( + ElevatorFSM elevator, V3_PootIntake intake, V3_PootManipulator manipulator) { + this.elevator = elevator; + this.intake = intake; + this.manipulator = manipulator; + + previousState = null; + currentState = V3_PootSuperstructureStates.START; + nextState = null; + + targetState = V3_PootSuperstructureStates.START; + + // Initialize the graph + graph = new DefaultDirectedGraph<>(EdgeCommand.class); + + for (V3_PootSuperstructureStates vertex : V3_PootSuperstructureStates.values()) { + graph.addVertex(vertex); + } + + // Add edges between states + V3_PootSuperstructureEdges.addEdges(graph, elevator, intake, manipulator); + } + + /** + * Periodic method that handles state transitions and subsystem updates. Updates robot state + * variables and manages command scheduling based on current state. + */ + @Override + public void periodic() { + if (currentState != null && !currentState.equals(V3_PootSuperstructureStates.OVERRIDE)) { + + if (nextState != null) { + // If we are in a transition, run the actions for the destination state + if (atIntermediateGoal()) { + nextState.getAction().get(intake, manipulator); + } + } else { + if (atIntermediateGoal()) { + currentState.getAction().get(intake, manipulator); + } + } + } + if (RobotMode.disabled()) { + nextState = null; + } else if (edgeCommand == null || !edgeCommand.getCommand().isScheduled()) { + // Update edge to new state + if (nextState != null) { + previousState = currentState; + currentState = nextState; + nextState = null; + } + + // Schedule next command in sequence + if (currentState != targetState) { + bfs(currentState, targetState) + .ifPresent( + next -> { + this.nextState = next; + edgeCommand = graph.getEdge(currentState, next); + edgeCommand.getCommand().schedule(); + }); + } + } + + // Log the current state of the superstructure and edge command + Logger.recordOutput( + NTPrefixes.SUPERSTRUCTURE + "Goal", targetState == null ? "NULL" : targetState.toString()); + Logger.recordOutput( + NTPrefixes.SUPERSTRUCTURE + "Previous State", + previousState == null ? "NULL" : previousState.toString()); + Logger.recordOutput(NTPrefixes.SUPERSTRUCTURE + "Current State", currentState.toString()); + Logger.recordOutput( + NTPrefixes.SUPERSTRUCTURE + "Next State", + nextState == null ? "NULL" : nextState.toString()); + if (edgeCommand != null) { + Logger.recordOutput( + NTPrefixes.SUPERSTRUCTURE + "EdgeCommand", + graph.getEdgeSource(edgeCommand) + " --> " + graph.getEdgeTarget(edgeCommand)); + } else { + Logger.recordOutput(NTPrefixes.SUPERSTRUCTURE + "EdgeCommand", "NO EDGES SCHEDULED"); + } + + elevator.periodic(); + intake.periodic(); + manipulator.periodic(); + + double armHeight = + -manipulator.getArmAngle().rotateBy(new Rotation2d(-Math.PI / 2)).getSin() + * V3_PootManipulatorConstants.ARM_PARAMETERS.LENGTH_METERS() + + elevator.getPositionMeters(); + Logger.recordOutput(NTPrefixes.SUPERSTRUCTURE + "Arm Height", armHeight); + Logger.recordOutput(NTPrefixes.SUPERSTRUCTURE + "Clears Base", 0.075 < armHeight); + Logger.recordOutput( + NTPrefixes.SUPERSTRUCTURE + "Clears Intake", + intake.getPivotAngle().getDegrees() > 15 + || armHeight > 0.37 + || Math.abs( + manipulator.getArmAngle().rotateBy(new Rotation2d(-Math.PI / 2)).getCos() + * V3_PootManipulatorConstants.ARM_PARAMETERS.LENGTH_METERS()) + > 0.35); + } + + /** + * Updates the target state and handles command rescheduling for optimal path. + * + * @param goal New target state to achieve + */ + private void setGoal(V3_PootSuperstructureStates goal) { + // Don't do anything if goal is the same + if (this.targetState == goal) return; + else { + this.targetState = goal; + } + + if (nextState == null) return; + + var edgeToCurrentState = graph.getEdge(nextState, currentState); + // Figure out if we should schedule a different command to get to goal faster + if (edgeCommand.getCommand().isScheduled() + && edgeToCurrentState != null + && isEdgeAllowed(edgeToCurrentState, goal)) { + // Figure out where we would have gone from the previous state + bfs(currentState, goal) + .ifPresent( + newNext -> { + if (newNext == nextState) { + // We are already on track + return; + } + + if (newNext != currentState && graph.getEdge(nextState, newNext) != null) { + // We can skip directly to the newNext edge + edgeCommand.getCommand().cancel(); + edgeCommand = graph.getEdge(currentState, newNext); + edgeCommand.getCommand().schedule(); + nextState = newNext; + } else { + // Follow the reverse edge from next back to the current edge + edgeCommand.getCommand().cancel(); + edgeCommand = graph.getEdge(nextState, currentState); + edgeCommand.getCommand().schedule(); + var temp = currentState; + previousState = currentState; + currentState = nextState; + nextState = temp; + } + }); + } + } + + /** + * Performs breadth-first search to find the next state in the path to the goal. + * + * @param start Starting state + * @param goal Target state + * @return Optional containing the next state in the path, empty if no path exists + */ + private Optional bfs( + V3_PootSuperstructureStates start, V3_PootSuperstructureStates goal) { + Map parents = new HashMap<>(); + Queue queue = new LinkedList<>(); + queue.add(start); + parents.put(start, null); + while (!queue.isEmpty()) { + V3_PootSuperstructureStates current = queue.poll(); + if (current == goal) break; + for (EdgeCommand edge : + graph.outgoingEdgesOf(current).stream() + .filter(edge -> isEdgeAllowed(edge, goal)) + .toList()) { + V3_PootSuperstructureStates neighbor = graph.getEdgeTarget(edge); + if (!parents.containsKey(neighbor)) { + parents.put(neighbor, current); + queue.add(neighbor); + } + } + } + + if (!parents.containsKey(goal)) return Optional.empty(); + + V3_PootSuperstructureStates nextState = goal; + while (!nextState.equals(start)) { + V3_PootSuperstructureStates parent = parents.get(nextState); + if (parent == null) return Optional.empty(); + else if (parent.equals(start)) return Optional.of(nextState); + nextState = parent; + } + return Optional.of(nextState); + } + + /** + * Checks if a state transition is allowed based on algae presence. + * + * @param edge The transition edge to check + * @param goal The target state + * @return true if the transition is allowed + */ + private boolean isEdgeAllowed(EdgeCommand edge, V3_PootSuperstructureStates goal) { + // --- 1. Algae Check (Original Logic) --- + boolean gamePieceAllowed = + edge.getGamePieceEdge() == GamePieceEdge.UNCONSTRAINED + || RobotState.isHasAlgae() == (edge.getGamePieceEdge() != GamePieceEdge.NO_ALGAE); + + if (!gamePieceAllowed) { + return false; // Fail fast if game piece logic disallows it + } + + // Condition is TRUE, so we must apply the path forcing logic. + if (!RobotState.isHasAlgae()) { + V3_PootSuperstructureStates from = graph.getEdgeSource(edge); + V3_PootSuperstructureStates to = graph.getEdgeTarget(edge); + + boolean needsFlip = RobotState.getScoreSide().equals(ScoreSide.RIGHT); + + if (goal == V3_PootSuperstructureStates.STOW_UP && needsFlip) { + if (to == V3_PootSuperstructureStates.STOW_UP + && from != V3_PootSuperstructureStates.INVERSE_FLIP_UP) { + return false; + } + } + + if (goal == V3_PootSuperstructureStates.STOW_UP && !needsFlip) { + if (to == V3_PootSuperstructureStates.STOW_UP + && from != V3_PootSuperstructureStates.INVERSE_FLIP_UP) { + return false; + } + } + } + + // --- 3. If all checks pass, the edge is allowed --- + return true; + } + + /** Resets the superstructure to initial auto state. */ + public void setAutoStart() { + currentState = V3_PootSuperstructureStates.START; + nextState = null; + targetState = V3_PootSuperstructureStates.STOW_DOWN; + if (edgeCommand != null) { + edgeCommand.getCommand().cancel(); + } + } + + /** + * Maps current OI reef height to corresponding elevator position state. + * + * @return Appropriate superstructure state for current reef height + */ + private V3_PootSuperstructureStates getElevatorPosition() { + switch (RobotState.getOIData().currentReefHeight()) { + case STOW, CORAL_INTAKE -> { + return V3_PootSuperstructureStates.STOW_DOWN; + } + case L1 -> { + return V3_PootSuperstructureStates.L1; + } + case L2 -> { + return V3_PootSuperstructureStates.L2; + } + case L3 -> { + return V3_PootSuperstructureStates.L3; + } + case L4 -> { + return V3_PootSuperstructureStates.L4; + } + case ALGAE_INTAKE_BOTTOM -> { + return V3_PootSuperstructureStates.L2_ALGAE; + } + case ALGAE_INTAKE_TOP -> { + return V3_PootSuperstructureStates.L3_ALGAE; + } + case ALGAE_SCORE -> { + return V3_PootSuperstructureStates.BARGE; + } + default -> { + return V3_PootSuperstructureStates.START; + } + } + } + + // --- Control Commands --- + + /** + * Returns a command that sets the superstructure to the given goal state. + * + * @param goal The desired superstructure state + * @return Command to run the goal + */ + public Command runGoal(V3_PootSuperstructureStates goal) { + return runOnce(() -> setGoal(goal)); + } + + /** + * Returns a command that sets the superstructure to the goal provided by a supplier. + * + * @param goal Supplier providing the desired superstructure state + * @return Command to run the goal + */ + public Command runGoal(Supplier goal) { + return runOnce(() -> setGoal(goal.get())); + } + + /** + * Checks whether the superstructure has reached its target state. + * + * @return true if current state matches the target state + */ + @AutoLogOutput(key = NTPrefixes.SUPERSTRUCTURE + "At Goal") + public boolean atGoal() { + return currentState == targetState + && elevator.atGoal() + && intake.pivotAtGoal() + && manipulator.armAtGoal(); + } + + public boolean atIntermediateGoal() { + var tolerance = + Optional.of( + Rotation2d.fromRadians( + V3_PootManipulatorConstants.CONSTRAINTS.goalToleranceRadians().get())); + if ((currentState.getPose().getFlyByArmTolerance()).isPresent()) { + tolerance = currentState.getPose().getFlyByArmTolerance(); + } + return elevator.atGoal() && intake.pivotAtGoal() && manipulator.armInTolerance(tolerance.get()); + } + + public Command waitUntilAtGoal() { + return Commands.sequence(Commands.waitSeconds(.02), Commands.waitUntil(() -> atGoal())); + } + + /** + * Runs a temporary override action, returning to a previous goal after. + * + * @param action The override action to perform + * @param oldGoal The goal to return to after override + * @return Command that runs the override and resumes the old goal + */ + public Command override(Runnable action) { + return Commands.sequence(runGoal(V3_PootSuperstructureStates.OVERRIDE), Commands.run(action)) + .finallyDo(() -> setGoal(currentState)); + } + + /** + * Runs a temporary override action, returning to a previous goal after. + * + * @param action The override action to perform + * @param oldGoal The goal to return to after override + * @return Command that runs the override and resumes the old goal + */ + public Command override(Runnable action, double timeSeconds) { + return override(action).withTimeout(timeSeconds); + } + + /** + * Runs the goal state and waits until a given condition becomes true. + * + * @param goal The desired superstructure state + * @param condition The condition to wait for after running the goal + * @return Combined command for running and waiting + */ + public Command runGoalUntil(V3_PootSuperstructureStates goal, BooleanSupplier condition) { + return Commands.sequence(runGoal(goal), Commands.waitUntil(condition)); + } + + public Command runGoalUntil( + Supplier goal, BooleanSupplier condition) { + return Commands.sequence(runGoal(goal), Commands.waitUntil(condition)); + } + + /** + * Returns a short command to run the previous state, useful for temporary state restoration. + * + * @return Command to go back to the previous state + */ + public Command runPreviousState() { + return runGoal(() -> previousState); + } + + /** + * Converts a ReefState (field-level enum) into a corresponding elevator goal and runs it. + * + * @param goal Supplier of ReefState + * @return Command to move to the elevator position for the reef level + */ + public Command runReefGoal(Supplier goal) { + return runGoal( + () -> { + // Translate ReefState to superstructure state + switch (goal.get()) { + case L1: + return V3_PootSuperstructureStates.L1; + case L2: + return V3_PootSuperstructureStates.L2; + case L3: + return V3_PootSuperstructureStates.L3; + case L4: + return V3_PootSuperstructureStates.L4; + default: + return V3_PootSuperstructureStates.STOW_DOWN; + } + }); + } + + /** + * Moves to a scoring position, executes the score action for a fixed time, then returns to pose. + * + * @param goal Supplier of the ReefState (target height/level) + * @return Command to run the score cycle (pose → action → timeout → pose) + */ + public Command runReefScoreGoal(Supplier goal) { + // Run appropriate action sequence depending on reef level + return this.runGoal( + () -> { + switch (goal.get()) { + case L1: + return V3_PootSuperstructureStates.L1_SCORE; + case L2: + return V3_PootSuperstructureStates.L2_SCORE; + case L3: + return V3_PootSuperstructureStates.L3_SCORE; + case L4: + return V3_PootSuperstructureStates.L4_SCORE; + default: + return V3_PootSuperstructureStates.STOW_DOWN; + } + }); + + // return runActionWithTimeout( + // () -> + // switch (goal.get()) { + // case L1 -> V3_PootSuperstructureStates.L1; + // case L2 -> V3_PootSuperstructureStates.L2; + // case L3 -> V3_PootSuperstructureStates.L3; + // case L4 -> V3_PootSuperstructureStates.L4; + // default -> V3_PootSuperstructureStates.STOW_DOWN; + // }, + // () -> + // switch (goal.get()) { + // case L1 -> 0.8; + // case L2 -> 0.15; + // case L3 -> 0.15; + // case L4 -> 0.1; + // default -> 0; + // }); + } + + /** + * Runs an action by going to a pose, performing the action, waiting, and returning. + * + * @param pose The pose to return to after action + * @param action The action to perform + * @param timeout How long to wait during the action phase (in seconds) + * @return Full command sequence + */ + public Command runActionWithTimeout( + Supplier pose, + Supplier action, + DoubleSupplier timeout) { + return Commands.sequence( + runGoal(action), // Run the action + waitUntilAtGoal(), + Commands.defer(() -> Commands.waitSeconds(timeout.getAsDouble()), Set.of())) + .finallyDo(() -> setGoal(pose.get())); // Return to original pose + } + + /** + * Runs an action by going to a pose, performing the action, waiting, and returning. + * + * @param pose The pose to return to after action + * @param action The action to perform + * @param timeout How long to wait during the action phase (in seconds) + * @return Full command sequence + */ + public Command runActionWithTimeout( + V3_PootSuperstructureStates pose, V3_PootSuperstructureStates action, double timeout) { + return Commands.sequence( + runGoal(action), // Run the action + waitUntilAtGoal(), + Commands.waitSeconds(timeout), + runGoal(pose)) + .finallyDo(() -> setGoal(pose)); // Return to original pose + } + + /** + * Smart overload that runs an action using a cached pose–action mapping, determining the pose + * from the action. + * + * @param action The scoring or action state + * @param timeout Timeout for the action duration + * @return Command sequence to perform and recover from the action + */ + public Command runActionWithTimeout( + Supplier action, DoubleSupplier timeout) { + // Maps each action state to its corresponding pose state + Map actionPoseMap = + new HashMap<>() { + { + put(V3_PootSuperstructureStates.L1_SCORE, V3_PootSuperstructureStates.L1); + put(V3_PootSuperstructureStates.L2_SCORE, V3_PootSuperstructureStates.L2); + put(V3_PootSuperstructureStates.L3_SCORE, V3_PootSuperstructureStates.L3); + put(V3_PootSuperstructureStates.L4_SCORE, V3_PootSuperstructureStates.L4); + put(V3_PootSuperstructureStates.BARGE_SCORE, V3_PootSuperstructureStates.BARGE); + put(V3_PootSuperstructureStates.PROCESSOR_SCORE, V3_PootSuperstructureStates.PROCESSOR); + } + }; + + // Reverse the map so we can look up the pose from action if needed + Map poseActionMap = + actionPoseMap.entrySet().stream() + .collect(Collectors.toMap(Map.Entry::getValue, Map.Entry::getKey)); + + // Try to look up pose from action (either direction works) + if (actionPoseMap.containsKey(action.get())) { + return runActionWithTimeout(() -> actionPoseMap.get(action.get()), action, timeout); + } else if (actionPoseMap.containsValue(action.get())) { + return runActionWithTimeout(action, () -> poseActionMap.get(action.get()), timeout); + } else return Commands.none(); // If action is not recognized, do nothing + } + + /** + * Smart overload that runs an action using a cached pose–action mapping, determining the pose + * from the action. + * + * @param action The scoring or action state + * @param timeout Timeout for the action duration + * @return Command sequence to perform and recover from the action + */ + public Command runActionWithTimeout(V3_PootSuperstructureStates action, double timeout) { + // Maps each action state to its corresponding pose state + Map actionPoseMap = + new HashMap<>() { + { + put(V3_PootSuperstructureStates.L1_SCORE, V3_PootSuperstructureStates.L1); + put(V3_PootSuperstructureStates.L2_SCORE, V3_PootSuperstructureStates.L2); + put(V3_PootSuperstructureStates.L3_SCORE, V3_PootSuperstructureStates.L3); + put(V3_PootSuperstructureStates.L4_SCORE, V3_PootSuperstructureStates.L4); + put(V3_PootSuperstructureStates.BARGE_SCORE, V3_PootSuperstructureStates.BARGE); + put(V3_PootSuperstructureStates.PROCESSOR_SCORE, V3_PootSuperstructureStates.PROCESSOR); + } + }; + + // Reverse the map so we can look up the pose from action if needed + Map poseActionMap = + actionPoseMap.entrySet().stream() + .collect(Collectors.toMap(Map.Entry::getValue, Map.Entry::getKey)); + + // Try to look up pose from action (either direction works) + if (actionPoseMap.containsKey(action)) { + return runActionWithTimeout(actionPoseMap.get(action), action, timeout); + } else if (actionPoseMap.containsValue(action)) { + return runActionWithTimeout(action, poseActionMap.get(action), timeout); + } else return Commands.none(); // If action is not recognized, do nothing + } + + /** + * Updates the superstructure to match the current OI-defined reef height. + * + * @return Command to move to elevator position state + */ + public Command setPosition() { + return runGoal(() -> getElevatorPosition()).withTimeout(0.02); + } + + /** + * Checks if the elevator is at its goal position. + * + * @return True if the elevator is at its goal, false otherwise. + */ + public boolean elevatorAtGoal() { + return elevator.atGoal(); + } + + @AutoLogOutput(key = NTPrefixes.SUPERSTRUCTURE + "IsTransitioning") + public boolean isTransitioning() { + return edgeCommand != null && edgeCommand.getCommand().isScheduled(); + } + + public Command allTransition() { + Command all = runGoal(V3_PootSuperstructureStates.STOW_DOWN); + for (var source : V3_PootSuperstructureStates.values()) { + for (var sink : V3_PootSuperstructureStates.values()) { + if (source == sink) continue; + if (sink == V3_PootSuperstructureStates.FLIP_DOWN) continue; + if (sink == V3_PootSuperstructureStates.FLIP_UP) continue; + if (sink == V3_PootSuperstructureStates.STOW_DOWN) continue; + if (sink == V3_PootSuperstructureStates.STOW_UP) continue; + if (sink == V3_PootSuperstructureStates.GROUND_ACQUISITION) continue; + if (sink == V3_PootSuperstructureStates.GROUND_INTAKE_ALGAE) continue; + if (sink == V3_PootSuperstructureStates.GROUND_AQUISITION_ALGAE) continue; + if (sink == V3_PootSuperstructureStates.GROUND_INTAKE) continue; + if (sink == V3_PootSuperstructureStates.L1) continue; + if (sink == V3_PootSuperstructureStates.L1_SCORE) continue; + if (sink == V3_PootSuperstructureStates.L2) continue; + if (sink == V3_PootSuperstructureStates.L2_SCORE) continue; + if (sink == V3_PootSuperstructureStates.L3) continue; + if (sink == V3_PootSuperstructureStates.L3_SCORE) continue; + + if (source != V3_PootSuperstructureStates.START + && sink != V3_PootSuperstructureStates.START + && source != V3_PootSuperstructureStates.OVERRIDE + && sink != V3_PootSuperstructureStates.OVERRIDE) { + all = + all.andThen( + runGoal(sink), + runOnce( + () -> + Logger.recordOutput( + "Superstructure/Current Objective", + source.toString() + " -> " + sink.toString())), + Commands.waitUntil(() -> atGoal())); + all = + all.andThen( + runGoal(source), + runOnce( + () -> + Logger.recordOutput( + "Superstructure/Current Objective", + sink.toString() + " -> " + source.toString())), + Commands.waitUntil(() -> atGoal())); + } + } + } + return all; + } + + public Command stateTransitions(V3_PootSuperstructureStates source) { + Command all = Commands.none(); + + for (var sink : V3_PootSuperstructureStates.values()) { + if (sink == source + || sink == V3_PootSuperstructureStates.START + || sink == V3_PootSuperstructureStates.OVERRIDE) continue; + + all = + all.andThen( + Commands.sequence( + runOnce(() -> System.out.println("→ " + source + " to " + sink)), + runGoal(sink), + Commands.waitUntil(this::atGoal), + Commands.waitSeconds(1.0), + runOnce(() -> System.out.println("← " + sink + " back to " + source)), + runGoal(source), + Commands.waitUntil(this::atGoal), + Commands.waitSeconds(1.0))); + } + + return all; + } + + public boolean armBelowThreshold() { + return manipulator.getArmAngle().getDegrees() >= 90; + } + + public Command everythingsFucked(BooleanSupplier condition) { + return Commands.parallel( + override( + () -> { + elevator.setPosition(ElevatorPositions.L4); + manipulator.setArmGoal(ManipulatorArmState.HANDOFF); + intake.setPivotGoal(IntakePivotState.INTAKE_CORAL); + })) + .until(condition); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureAction.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureAction.java new file mode 100644 index 00000000..a34a6bde --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureAction.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.v3_Poot.superstructure; + +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakeRollerState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState; +import lombok.Getter; + +/** + * Coordinates and executes roller actions across multiple subsystems. This class manages the states + * of manipulator, funnel, and intake rollers and provides methods to control them in a coordinated + * manner. + */ +public class V3_PootSuperstructureAction { + @Getter private final IntakeRollerState intakeRollerState; + @Getter private final ManipulatorRollerState manipulatorRollerState; + + /** + * Creates a new SuperstructureAction with specified roller states. + * + * @param key Identifier for the action set + * @param rollerStates Combined states for all subsystem rollers + */ + public V3_PootSuperstructureAction(String key, SubsystemActions rollerStates) { + this.intakeRollerState = rollerStates.intakeRollerState(); + this.manipulatorRollerState = rollerStates.manipulatorRollerState(); + } + + /** + * Sets the manipulator roller to its configured state. + * + * @param manipulator The manipulator subsystem to control + */ + public void runManipulator(V3_PootManipulator manipulator) { + manipulator.setRollerGoal(manipulatorRollerState); + } + + /** + * Sets the intake roller to its configured state. + * + * @param intake The intake subsystem to control + */ + public void runIntake(V3_PootIntake intake) { + intake.setRollerGoal(intakeRollerState); + } + + /** + * Executes all roller actions simultaneously across all subsystems. + * + * @param funnel The funnel subsystem + * @param intake The intake subsystem + * @param manipulator The manipulator subsystem + */ + public void get(V3_PootIntake intake, V3_PootManipulator manipulator) { + runIntake(intake); + runManipulator(manipulator); + } + + /** + * Record that holds the states for all roller subsystems. Provides a way to group and manage + * multiple roller states as a single unit. + */ + public record SubsystemActions( + ManipulatorRollerState manipulatorRollerState, IntakeRollerState intakeRollerState) { + /** + * Creates a SubsystemActions instance with all rollers in STOP state. + * + * @return A new SubsystemActions with all states set to STOP + */ + public static SubsystemActions empty() { + return new SubsystemActions(ManipulatorRollerState.STOP, IntakeRollerState.STOP); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureEdges.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureEdges.java new file mode 100644 index 00000000..350371b7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureEdges.java @@ -0,0 +1,160 @@ +package frc.robot.subsystems.v3_Poot.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.shared.elevator.Elevator.ElevatorFSM; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import java.io.FileReader; +import java.io.Reader; +import java.util.ArrayList; +import lombok.Builder; +import lombok.Getter; +import lombok.Setter; +import org.jgrapht.Graph; +import org.jgrapht.graph.DefaultEdge; +import org.jgrapht.nio.dot.DOTImporter; + +public class V3_PootSuperstructureEdges { + public static final ArrayList UNCONSTRAINED = new ArrayList<>(); + public static final ArrayList NO_ALGAE_EDGES = new ArrayList<>(); + + public record Edge(V3_PootSuperstructureStates from, V3_PootSuperstructureStates to) { + public Edge(V3_PootSuperstructureStates from, V3_PootSuperstructureStates to) { + this.from = from; + this.to = to; + } + + @Override + public String toString() { + return from + " -> " + to; + } + } + + public enum GamePieceEdge { + UNCONSTRAINED, + NO_ALGAE + } + + public static void loadEdgesFromDot( + String path, + Graph graph, + ElevatorFSM elevator, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + + DOTImporter importer = new DOTImporter<>(); + + // --- Fix 1: safer vertex factory --- + importer.setVertexFactory( + id -> { + try { + return V3_PootSuperstructureStates.valueOf(id); + } catch (IllegalArgumentException e) { + System.err.println("[DOT Import] Unknown vertex in DOT: " + id); + return null; + } + }); + + // --- Fix 2: ensure edge objects are valid --- + importer.setEdgeWithAttributesFactory( + attributes -> { + GamePieceEdge type = GamePieceEdge.UNCONSTRAINED; + + if (attributes.containsKey("type")) { + try { + type = GamePieceEdge.valueOf(attributes.get("type").getValue().toUpperCase()); + } catch (Exception e) { + System.err.println("[DOT Import] Invalid edge type, defaulting to UNCONSTRAINED"); + } + } + + // Create a proper EdgeCommand with no command yet + return EdgeCommand.builder().gamePieceEdge(type).command(null).build(); + }); + + // --- Fix 3: actually import the file and verify success --- + try (Reader r = new FileReader(path)) { + importer.importGraph(graph, r); + } catch (Exception ex) { + throw new RuntimeException("Failed to parse DOT file: " + path, ex); + } + + // --- Fix 4: assign the edge commands after load --- + for (EdgeCommand e : graph.edgeSet()) { + V3_PootSuperstructureStates from = graph.getEdgeSource(e); + V3_PootSuperstructureStates to = graph.getEdgeTarget(e); + e.setCommand(getEdgeCommand(from, to, elevator, intake, manipulator)); + } + + System.out.printf( + "[DOT Import] Loaded %d vertices, %d edges%n", + graph.vertexSet().size(), graph.edgeSet().size()); + } + + @Builder(toBuilder = true) + @Getter + public static class EdgeCommand extends DefaultEdge { + @Setter private Command command; + @Builder.Default private GamePieceEdge gamePieceEdge = GamePieceEdge.UNCONSTRAINED; + } + + /** + * Gets the command to execute for a given edge in the superstructure state graph. This command + * typically involves coordinating the elevator, funnel, intake, and manipulator subsystems to + * move from one state to another. + * + * @param from The starting state of the superstructure. + * @param to The target state of the superstructure. + * @param elevator The elevator subsystem. + * @param intake The intake subsystem. + * @param manipulator The manipulator subsystem. + * @return A {@link Command} that, when executed, transitions the superstructure from the 'from' + * state to the 'to' state. + */ + private static Command getEdgeCommand( + V3_PootSuperstructureStates from, + V3_PootSuperstructureStates to, + ElevatorFSM elevator, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + + V3_PootSuperstructurePose pose = to.getPose(); + + if (to.equals(V3_PootSuperstructureStates.BARGE_SCORE)) { + return Commands.sequence( + pose.asConfigurationSpaceCommand(elevator, intake, manipulator), + Commands.waitUntil(() -> manipulator.armInTolerance(Rotation2d.fromDegrees(6)))); + } + + return Commands.sequence( + pose.asConfigurationSpaceCommand(elevator, intake, manipulator), + pose.wait(elevator, intake, manipulator)); + } + + /** + * Adds all predefined edges to the superstructure state graph, categorized by algae condition. + * + * @param graph The graph to which edges are added. + * @param elevator The elevator subsystem. + * @param manipulator The manipulator subsystem. + * @param intake The intake subsystem. + */ + public static void addEdges( + Graph graph, + ElevatorFSM elevator, + V3_PootIntake intake, + V3_PootManipulator manipulator) { + loadEdgesFromDot( + Filesystem.getDeployDirectory() + .toPath() + .resolve("v3_poot/" + "Superstructure.dot") + .toString(), + graph, + elevator, + intake, + manipulator); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructurePose.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructurePose.java new file mode 100644 index 00000000..a01b096b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructurePose.java @@ -0,0 +1,149 @@ +package frc.robot.subsystems.v3_Poot.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.FieldConstants.Reef.ReefState; +import frc.robot.subsystems.shared.elevator.Elevator.ElevatorFSM; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorArmState; +import java.util.Optional; +import lombok.Getter; + +/** + * Represents a specific pose (configuration) of the superstructure, defining the states of the + * elevator, manipulator arm, intake, and funnel. This class allows for coordinated control of these + * subsystems to achieve a desired configuration. + */ +public class V3_PootSuperstructurePose { + private final String key; + + @Getter private final ReefState elevatorHeight; + @Getter private final ManipulatorArmState armState; + @Getter private final IntakePivotState intakeState; + + @Getter private final Optional flyByElevatorTolerance; + @Getter private final Optional flyByArmTolerance; + + public V3_PootSuperstructurePose(String key, SubsystemPoses poses) { + this.key = key; + + this.elevatorHeight = poses.elevatorHeight(); + this.armState = poses.manipulatorArmState(); + this.intakeState = poses.intakePivotState(); + + this.flyByElevatorTolerance = Optional.empty(); + this.flyByArmTolerance = Optional.empty(); + } + + public V3_PootSuperstructurePose(String key, SubsystemPoses poses, double elevatorTolerance) { + this.key = key; + + this.elevatorHeight = poses.elevatorHeight(); + this.armState = poses.manipulatorArmState(); + this.intakeState = poses.intakePivotState(); + + this.flyByElevatorTolerance = Optional.of(elevatorTolerance); + this.flyByArmTolerance = Optional.empty(); + } + + public V3_PootSuperstructurePose(String key, SubsystemPoses poses, Rotation2d flyByArmTolerance) { + this.key = key; + + this.elevatorHeight = poses.elevatorHeight(); + this.armState = poses.manipulatorArmState(); + this.intakeState = poses.intakePivotState(); + + this.flyByElevatorTolerance = Optional.empty(); + this.flyByArmTolerance = Optional.of(flyByArmTolerance); + } + + /** + * Creates a command to set the elevator to the specified height for this pose. + * + * @param elevator The elevator subsystem to control. + * @return A Command that sets the elevator height and waits until it reaches the goal. + */ + public Command setElevatorHeight(ElevatorFSM elevator) { + return Commands.parallel(Commands.runOnce(() -> elevator.setPosition(() -> elevatorHeight))); + } + + /** + * Creates a command to set the intake to the specified extension state for this pose. + * + * @param intake The intake subsystem to control. + * @return A Command that sets the intake extension state and waits until it reaches the goal. + */ + public Command setIntakeState(V3_PootIntake intake) { + return Commands.parallel(Commands.runOnce(() -> intake.setPivotGoal(intakeState))); + } + + /** + * Creates a command to set the manipulator arm to the specified state for this pose. + * + * @param manipulator The manipulator subsystem to control. + * @return A Command that sets the arm state and waits until it reaches the goal. + */ + public Command setManipulatorState(V3_PootManipulator manipulator) { + return Commands.parallel(Commands.runOnce(() -> manipulator.setArmGoal(armState))); + } + + /** + * Creates a command that sets all subsystems (elevator, manipulator, intake, and funnel) to the + * states defined by this pose. + * + * @param elevator The elevator subsystem. + * @param intake The intake subsystem. + * @param manipulator The manipulator subsystem. + * @return A Command that sets all subsystems to their respective states in parallel. + */ + public Command asConfigurationSpaceCommand( + ElevatorFSM elevator, V3_PootIntake intake, V3_PootManipulator manipulator) { + return Commands.parallel( + Commands.runOnce(() -> elevator.setPosition(() -> elevatorHeight)), + Commands.runOnce(() -> manipulator.setArmGoal(armState)), + Commands.runOnce(() -> intake.setPivotGoal(intakeState))); + } + + public Command wait(ElevatorFSM elevator, V3_PootIntake intake, V3_PootManipulator manipulator) { + + if (flyByElevatorTolerance.isPresent()) { + return Commands.waitUntil(() -> elevator.inTolerance(flyByElevatorTolerance.get())); + } else if (flyByArmTolerance.isPresent()) { + return Commands.waitUntil(() -> manipulator.armInTolerance(flyByArmTolerance.get())); + } else { + return Commands.parallel( + elevator.waitUntilAtGoal(), + manipulator.waitUntilArmAtGoal(), + intake.waitUntilPivotAtGoal()); + } + } + + /** + * Returns a string representation of this pose (the key). + * + * @return The key of this pose. + */ + public String toString() { + return key; + } + + /** + * A record that groups the states of the elevator, manipulator arm, intake, and funnel + * subsystems. This is used to define a complete pose for the superstructure. + */ + public record SubsystemPoses( + ReefState elevatorHeight, + ManipulatorArmState manipulatorArmState, + IntakePivotState intakePivotState) { + + /** + * Creates a SubsystemPoses instance with default states (STOW for elevator, arm, and intake). + */ + public SubsystemPoses() { + this(ReefState.STOW, ManipulatorArmState.VERTICAL_UP, IntakePivotState.STOW); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureStates.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureStates.java new file mode 100644 index 00000000..4b0e0247 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/V3_PootSuperstructureStates.java @@ -0,0 +1,200 @@ +package frc.robot.subsystems.v3_Poot.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.FieldConstants.Reef.ReefState; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureAction.SubsystemActions; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructurePose.SubsystemPoses; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakeRollerState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorArmState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState; +import lombok.Getter; + +public enum V3_PootSuperstructureStates { + START("START", new SubsystemPoses(), SubsystemActions.empty()), + STOW_DOWN( + "STOW_DOWN", + new SubsystemPoses(ReefState.STOW, ManipulatorArmState.VERTICAL_UP, IntakePivotState.STOW), + SubsystemActions.empty()), + STOW_UP( + "STOW_UP", + new SubsystemPoses( + ReefState.HIGH_STOW, ManipulatorArmState.VERTICAL_UP, IntakePivotState.STOW), + SubsystemActions.empty()), + OVERRIDE("OVERRIDE", new SubsystemPoses(), SubsystemActions.empty()), + GROUND_ACQUISITION( + "GROUND_ACQUISITION", + new SubsystemPoses( + ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.INTAKE_CORAL), + SubsystemActions.empty()), + GROUND_INTAKE_ALGAE( + "GROUND_INTAKE_ALGAE", + new SubsystemPoses( + ReefState.ALGAE_FLOOR_INTAKE, + ManipulatorArmState.ALGAE_INTAKE_FLOOR, + IntakePivotState.INTAKE_ALGAE), + new SubsystemActions(ManipulatorRollerState.ALGAE_INTAKE, IntakeRollerState.STOP)), + GROUND_AQUISITION_ALGAE( + "GROUND_AQUISITION_ALGAE", + new SubsystemPoses( + ReefState.ALGAE_FLOOR_INTAKE, + ManipulatorArmState.ALGAE_INTAKE_FLOOR, + IntakePivotState.INTAKE_ALGAE), + SubsystemActions.empty()), + + GROUND_INTAKE_CORAL( + "GROUND_INTAKE_CORAL", + new SubsystemPoses( + ReefState.STOW, ManipulatorArmState.CORAL_INTAKE_FLOOR, IntakePivotState.INTAKE_CORAL), + new SubsystemActions(ManipulatorRollerState.CORAL_INTAKE, IntakeRollerState.STOP)), + + GROUND_INTAKE( + "GROUND_INTAKE", + new SubsystemPoses( + ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.INTAKE_CORAL), + new SubsystemActions(ManipulatorRollerState.CORAL_INTAKE, IntakeRollerState.CORAL_INTAKE)), + + L1( + "L1", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.L1), + new SubsystemActions(ManipulatorRollerState.STOP, IntakeRollerState.STOP)), + L1_SCORE( + "L1_SCORE", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.L1), + new SubsystemActions(ManipulatorRollerState.STOP, IntakeRollerState.SCORE_CORAL)), + + L2( + "L2", + new SubsystemPoses(ReefState.L2, ManipulatorArmState.TRANSITION, IntakePivotState.HANDOFF), + SubsystemActions.empty()), + L2_SCORE( + "L2_SCORE", + new SubsystemPoses(ReefState.L2, ManipulatorArmState.FLIPPED_SCORE, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.SCORE_CORAL, IntakeRollerState.STOP)), + + L3( + "L3", + new SubsystemPoses(ReefState.L3, ManipulatorArmState.TRANSITION, IntakePivotState.HANDOFF), + SubsystemActions.empty()), + L3_SCORE( + "L3_SCORE", + new SubsystemPoses(ReefState.L3, ManipulatorArmState.SCORE, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.SCORE_CORAL, IntakeRollerState.STOP)), + + L4( + "L4", + new SubsystemPoses(ReefState.L4, ManipulatorArmState.TRANSITION, IntakePivotState.HANDOFF), + SubsystemActions.empty()), + L4_SCORE( + "L4_SCORE", + new SubsystemPoses(ReefState.L4, ManipulatorArmState.SCORE_L4, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.L4_SCORE, IntakeRollerState.STOP)), + + HANDOFF( + "HANDOFF", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.STOP, IntakeRollerState.CENTERING)), + + HANDOFF_SPIN( + "HANDOFF", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.HANDOFF, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.CORAL_INTAKE, IntakeRollerState.OUTTAKE)), + + L2_ALGAE( + "L2_ALGAE", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_BOTTOM, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + SubsystemActions.empty()), + L2_ALGAE_DROP( + "L2_ALGAE_DROP", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_BOTTOM, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + new SubsystemActions(ManipulatorRollerState.SCORE_ALGAE, IntakeRollerState.STOP)), + L2_ALGAE_INTAKE( + "L2_ALGAE_INTAKE", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_BOTTOM, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + new SubsystemActions(ManipulatorRollerState.ALGAE_INTAKE, IntakeRollerState.STOP)), + + L3_ALGAE( + "L3_ALGAE", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_TOP, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + SubsystemActions.empty()), + L3_ALGAE_DROP( + "L3_ALGAE_DROP", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_TOP, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + new SubsystemActions(ManipulatorRollerState.SCORE_ALGAE, IntakeRollerState.STOP)), + L3_ALGAE_INTAKE( + "L3_ALGAE_INTAKE", + new SubsystemPoses( + ReefState.ALGAE_INTAKE_TOP, ManipulatorArmState.REEF_INTAKE, IntakePivotState.STOW), + new SubsystemActions(ManipulatorRollerState.ALGAE_INTAKE, IntakeRollerState.STOP)), + + PROCESSOR( + "PROCESSOR", + new SubsystemPoses(ReefState.STOW, ManipulatorArmState.PROCESSOR, IntakePivotState.STOW), + SubsystemActions.empty()), + POST_PROCESSOR( + "POST_PROCESSOR", + new SubsystemPoses( + ReefState.POST_PROCESSOR, ManipulatorArmState.PROCESSOR, IntakePivotState.STOW), + SubsystemActions.empty()), + PROCESSOR_SCORE( + "PROCESSOR_SCORE", + new SubsystemPoses(ReefState.STOW, ManipulatorArmState.PROCESSOR, IntakePivotState.STOW), + new SubsystemActions(ManipulatorRollerState.SCORE_ALGAE, IntakeRollerState.STOP)), + + BARGE( + "BARGE", + new SubsystemPoses( + ReefState.ALGAE_SCORE, ManipulatorArmState.PRE_SCORE, IntakePivotState.HANDOFF), + SubsystemActions.empty()), + BARGE_SCORE( + "BARGE_SCORE", + new SubsystemPoses( + ReefState.ALGAE_SCORE, ManipulatorArmState.ALGAE_SCORE, IntakePivotState.HANDOFF), + new SubsystemActions(ManipulatorRollerState.SCORE_ALGAE, IntakeRollerState.STOP), + Rotation2d.fromDegrees(15)), + FLIP_DOWN( + "FLIP_DOWN", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.FLIP_ANGLE, IntakePivotState.STOW), + SubsystemActions.empty()), + FLIP_UP( + "FLIP_UP", + new SubsystemPoses(ReefState.HANDOFF, ManipulatorArmState.FLIP_ANGLE, IntakePivotState.STOW), + SubsystemActions.empty()), + INVERSE_FLIP_UP( + "INVERSE_FLIP_UP", + new SubsystemPoses( + ReefState.HANDOFF, ManipulatorArmState.INVERSE_FLIP_ANGLE, IntakePivotState.STOW), + SubsystemActions.empty()); + + @Getter private final V3_PootSuperstructurePose pose; + @Getter private final V3_PootSuperstructureAction action; + + /** + * Constructor for V3_SuperstructureStates. + * + * @param name The name of the state. + * @param pose The subsystem poses for this state. + * @param action The subsystem actions for this state. + */ + V3_PootSuperstructureStates(String name, SubsystemPoses pose, SubsystemActions action) { + this.pose = new V3_PootSuperstructurePose(name, pose); + this.action = new V3_PootSuperstructureAction(name, action); + } + + V3_PootSuperstructureStates( + String name, SubsystemPoses pose, SubsystemActions action, Double elevatorTolerance) { + this.pose = new V3_PootSuperstructurePose(name, pose, elevatorTolerance); + this.action = new V3_PootSuperstructureAction(name, action); + } + + V3_PootSuperstructureStates( + String name, SubsystemPoses pose, SubsystemActions action, Rotation2d armTolerance) { + this.pose = new V3_PootSuperstructurePose(name, pose, armTolerance); + this.action = new V3_PootSuperstructureAction(name, action); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntake.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntake.java new file mode 100644 index 00000000..f9b7555b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntake.java @@ -0,0 +1,219 @@ +package frc.robot.subsystems.v3_Poot.superstructure.intake; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakeRollerState; +import java.util.Set; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class V3_PootIntake { + private final V3_PootIntakeIO io; + private final V3_PootIntakeIOInputsAutoLogged inputs; + // private final double INTAKE_CORAL_THRESHOLD = 1.0; + + @Getter + @AutoLogOutput(key = "Intake/Pivot Goal") + private IntakePivotState pivotGoal; + + @Getter + @AutoLogOutput(key = "Intake/Roller Goal") + private IntakeRollerState rollerGoal; + + private boolean isClosedLoop; + + public V3_PootIntake(V3_PootIntakeIO io) { + this.io = io; + inputs = new V3_PootIntakeIOInputsAutoLogged(); + + pivotGoal = IntakePivotState.STOW; + rollerGoal = IntakeRollerState.STOP; + + isClosedLoop = true; + } + + /** + * Periodic function for intake subsystem. Updates inputs from the intake, and then sets the goals + * for the pivot and roller motors. If the intake is in closed-loop mode, it sets the pivot motor + * goal to the desired angle. Otherwise, it does not set the pivot motor goal. It always sets the + * inner and outer roller motor goals to the desired voltage. + */ + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + if (isClosedLoop) { + io.setPivotGoal(pivotGoal.getAngle()); + } + + if (rollerGoal.equals(IntakeRollerState.SCORE_CORAL)) { + io.setInnerRollerVoltage(IntakeRollerState.SCORE_CORAL.getInnerVoltage()); + io.setOuterRollerVoltage(IntakeRollerState.SCORE_CORAL.getOuterVoltage()); + } else if (hasCoralLocked() && (!rollerGoal.equals(IntakeRollerState.OUTTAKE))) { + io.setInnerRollerVoltage(IntakeRollerState.STOP.getInnerVoltage()); + io.setOuterRollerVoltage(IntakeRollerState.STOP.getOuterVoltage()); + } else { + io.setInnerRollerVoltage(rollerGoal.getInnerVoltage()); + io.setOuterRollerVoltage(rollerGoal.getOuterVoltage()); + } + } + + // Double check if this is right + + /** + * Checks if the intake is currently detecting coral. This is done by checking if both the left + * and right CAN range sensors are detecting a distance greater than the coral detection + * threshold. + * + * @return True if the intake is detecting coral, false otherwise. + */ + @AutoLogOutput(key = "Intake/Has Coral") + public boolean hasCoralLocked() { + return inputs.leftCANRange && inputs.rightCANRange; + } + + public boolean hasCoralLoose() { + return inputs.leftCANRange || inputs.rightCANRange; + } + + /** + * Checks if the pivot motor is at the goal angle. This is done by checking if the absolute + * difference between the current pivot motor angle and the goal angle is less than the goal + * tolerance. + * + * @return True if the pivot motor is at the goal angle, false otherwise. + */ + @AutoLogOutput(key = "Intake/At Goal") + public boolean pivotAtGoal() { + return Math.abs(inputs.pivotPosition.getRadians() - pivotGoal.getAngle().getRadians()) + < V3_PootIntakeConstants.PIVOT_CONSTRAINTS.GOAL_TOLERANCE().getRadians(); + } + + /** + * Checks if the pivot motor is at the goal angle. This is done by checking if the absolute + * difference between the current pivot motor angle and the goal angle is less than the goal + * tolerance. + * + * @param goal The goal angle to check against. + * @return True if the pivot motor is at the goal angle, false otherwise. + */ + public boolean pivotAtGoal(IntakePivotState goal) { + return Math.abs(inputs.pivotPosition.getRadians() - goal.getAngle().getRadians()) + < V3_PootIntakeConstants.PIVOT_CONSTRAINTS.GOAL_TOLERANCE().getRadians(); + } + + /** + * Sets the goal state of the pivot motor. If the intake is currently in open-loop mode, this + * method will set the isClosedLoop flag to true. It then sets the pivotGoal field to the + * specified goal state. The pivot motor will then be controlled in closed-loop mode to reach the + * specified goal state. + * + * @param goal The desired IntakePivotState for the pivot motor. + */ + public void setPivotGoal(IntakePivotState goal) { + isClosedLoop = true; + this.pivotGoal = goal; + } + + /** + * Sets the voltage for the intake pivot motor. + * + *

This method is used to set the voltage for the intake pivot motor in open-loop mode. It sets + * the isClosedLoop flag to false, and then calls the setPivotVoltage method of the IO interface. + * + * @param volts The voltage to set for the intake pivot motor. + */ + public void setPivotVoltage(double volts) { + isClosedLoop = false; + io.setPivotVoltage(volts); + } + + /** + * Waits until the pivot motor is at the goal angle. This command is created by sequencing a wait + * command with a waitUntil command. The wait command waits for 0.02 seconds, and the waitUntil + * command checks if the pivot motor is at the goal angle. The command will block until the pivot + * motor is at the goal angle. + * + * @return A Command that waits until the pivot motor is at the goal angle. + */ + public Command waitUntilPivotAtGoal() { + return Commands.sequence(Commands.waitSeconds(0.02), Commands.waitUntil(this::pivotAtGoal)); + } + + public void setRollerGoal(IntakeRollerState rollerGoal) { + this.rollerGoal = rollerGoal; + if (hasCoralLocked() + || hasCoralLoose() + && Set.of( + V3_PootIntakeConstants.IntakeRollerState.ALGAE_INTAKE, + V3_PootIntakeConstants.IntakeRollerState.CORAL_INTAKE, + V3_PootIntakeConstants.IntakeRollerState.STOP) + .contains(rollerGoal)) { + + io.setInnerRollerVoltage(0); + io.setOuterRollerVoltage(0); + } else { + io.setInnerRollerVoltage(rollerGoal.getInnerVoltage()); + io.setOuterRollerVoltage(rollerGoal.getOuterVoltage()); + } + } + + /** + * Runs the SysId routine for the intake subsystem + * + * @param subsystem The subsystem to run the SysId routine on. + * @return A command that runs the SysId routine. + */ + public Command sysIdRoutine(Subsystem subsystem) { + + SysIdRoutine characterizationRoutine = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.1).per(Second), + Volts.of(3), + Seconds.of(10), + (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> io.setPivotVoltage(volts.in(Volts)), null, subsystem)); + + return Commands.sequence( + Commands.runOnce(() -> isClosedLoop = false), + characterizationRoutine + .quasistatic(Direction.kForward) + .until(() -> pivotAtGoal(IntakePivotState.INTAKE_CORAL)), + characterizationRoutine + .quasistatic(Direction.kReverse) + .until(() -> pivotAtGoal(IntakePivotState.HANDOFF)), + characterizationRoutine + .dynamic(Direction.kForward) + .until(() -> pivotAtGoal(IntakePivotState.INTAKE_CORAL)), + characterizationRoutine + .dynamic(Direction.kReverse) + .until(() -> pivotAtGoal(IntakePivotState.HANDOFF))); + } + + /** + * Gets the current angle of the intake pivot motor. + * + * @return The current angle of the intake pivot motor, in radians. + */ + public Rotation2d getPivotAngle() { + return inputs.pivotPosition; + } + + public void updateIntakeGains(double kP, double kD, double kS, double kV, double kA, double kG) { + io.updateIntakeGains(kP, kD, kS, kV, kA, kG); + } + + public void updateIntakeConstraints(double maxAcceleration, double cruisingVelocity) { + io.updateIntakeConstraints(maxAcceleration, cruisingVelocity); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeConstants.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeConstants.java new file mode 100644 index 00000000..09fe1799 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeConstants.java @@ -0,0 +1,168 @@ +package frc.robot.subsystems.v3_Poot.superstructure.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import frc.robot.Constants; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +public class V3_PootIntakeConstants { + + public static final int PIVOT_CAN_ID; + public static final int ROLLER_CAN_ID_OUTER; + public static final int ROLLER_CAN_ID_INNER; + public static final int LEFT_SENSOR_CAN_ID; + public static final int RIGHT_SENSOR_CAN_ID; + + public static final double INTAKE_CAN_CORAL_DETECTED_THRESHOLD_METERS; + + public static final IntakeCurrentLimits CURRENT_LIMITS; + + public static final Gains PIVOT_GAINS; + public static final Constraints PIVOT_CONSTRAINTS; + + public static final IntakeParems PIVOT_PARAMS; + public static final IntakeParems ROLLER_PARAMS; + + static { + PIVOT_CAN_ID = 40; + ROLLER_CAN_ID_OUTER = 41; + ROLLER_CAN_ID_INNER = 42; + LEFT_SENSOR_CAN_ID = 44; + RIGHT_SENSOR_CAN_ID = 43; + + INTAKE_CAN_CORAL_DETECTED_THRESHOLD_METERS = 0.05; + + PIVOT_PARAMS = + new IntakeParems( + (60.0 / 12.0) * (52.0 / 28.0) * (64.0 / 18.0) * (20.0 / 9.0), + DCMotor.getKrakenX60Foc(1), + 0.0042, + Rotation2d.fromDegrees(40.0), + Rotation2d.fromDegrees(124.6 + 48)); + ROLLER_PARAMS = + new IntakeParems( + 1, DCMotor.getKrakenX60Foc(1), 0, new Rotation2d(), Rotation2d.fromDegrees(0)); + + switch (Constants.ROBOT) { + case V3_POOT: + PIVOT_CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Intake/Max Acceleration", 160), + new LoggedTunableNumber("Intake/Cruising Velocity", 80), + Rotation2d.fromDegrees(1.5)); + PIVOT_GAINS = + new Gains( + new LoggedTunableNumber("Intake/kP", 175.0), + new LoggedTunableNumber("Intake/kD", 5.0), + new LoggedTunableNumber("Intake/kS", 0.38466), + new LoggedTunableNumber("Intake/kV", 0.0), + new LoggedTunableNumber("Intake/kA", 0), + new LoggedTunableNumber("Intake/kG", 0.083386)); + CURRENT_LIMITS = new IntakeCurrentLimits(40.0, 40.0, 40.0, 80.0, 40.0, 80.0); + + break; + + case V3_POOT_SIM: + PIVOT_CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Intake/Max Acceleration", 100.0), + new LoggedTunableNumber("Intake/Cruising Velocity", 75.0), + Rotation2d.fromDegrees(1.5)); + PIVOT_GAINS = + new Gains( + new LoggedTunableNumber("Intake/kP", 100), + new LoggedTunableNumber("Intake/kD", 0.1), + new LoggedTunableNumber("Intake/kS", 0.0), + new LoggedTunableNumber("Intake/kV", 0.0), + new LoggedTunableNumber("Intake/kA", 0.011537), + new LoggedTunableNumber("Intake/kG", 0.15326)); + CURRENT_LIMITS = new IntakeCurrentLimits(40.0, 40.0, 40.0, 40.0, 40.0, 40.0); + + break; + + default: + PIVOT_CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Intake/Max Acceleration", 0.0), + new LoggedTunableNumber("Intake/Cruising Velocity", 0.0), + Rotation2d.fromDegrees(1.5)); + PIVOT_GAINS = + new Gains( + new LoggedTunableNumber("Intake/kP", 1.85), + new LoggedTunableNumber("Intake/kD", 0.1), + new LoggedTunableNumber("Intake/kS", 0.0), + new LoggedTunableNumber("Intake/kV", 0.0), + new LoggedTunableNumber("Intake/kA", 0.0), + new LoggedTunableNumber("Intake/kG", 0.0)); + CURRENT_LIMITS = new IntakeCurrentLimits(40.0, 40.0, 40.0, 40.0, 40.0, 40.0); + + break; + } + } + + @RequiredArgsConstructor + public enum IntakePivotState { + STOW(Rotation2d.fromDegrees(-82 + 123.6 + 48)), + INTAKE_CORAL(Rotation2d.fromDegrees(123.6 + 48)), + HANDOFF(Rotation2d.fromDegrees(48)), + L1(Rotation2d.fromDegrees(-82 + 123.6 + 48)), + INTAKE_ALGAE(Rotation2d.fromDegrees(25.0 + 48)), + ARM_CLEAR(Rotation2d.fromDegrees(35 + 48)); + + @Getter private final Rotation2d angle; + } + + public static record IntakeCurrentLimits( + double PIVOT_SUPPLY_CURRENT_LIMIT, + double PIVOT_STATOR_CURRENT_LIMIT, + double INNER_ROLLER_SUPPLY_CURRENT_LIMIT, + double INNER_ROLLER_STATOR_CURRENT_LIMIT, + double OUTER_ROLLER_SUPPLY_CURRENT_LIMIT, + double OUTER_ROLLER_STATOR_CURRENT_LIMIT) {} + + public static record Gains( + LoggedTunableNumber kP, + LoggedTunableNumber kD, + LoggedTunableNumber kS, + LoggedTunableNumber kV, + LoggedTunableNumber kA, + LoggedTunableNumber kG) {} + + public static record Constraints( + LoggedTunableNumber MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED, + LoggedTunableNumber CRUISING_VELOCITY_RADIANS_PER_SECOND, + Rotation2d GOAL_TOLERANCE) { + public edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints getTrapezoidConstraints() { + return new edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints( + CRUISING_VELOCITY_RADIANS_PER_SECOND.get(), + MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED.get()); + } + } + + public static record IntakeParems( + double PIVOT_GEAR_RATIO, + DCMotor MOTOR, + double MASS_KG, + Rotation2d MIN_ANGLE, + Rotation2d MAX_ANGLE) {} + + // Will add more states later + public static enum IntakeRollerState { + STOP(0.0, 0.0), + CENTERING(-12.0, -12.0), + CORAL_INTAKE(-12.0, -12.0), + ALGAE_INTAKE(12.0, 12.0), + SCORE_CORAL(0.0, 6.0), + OUTTAKE(0.0, 12.0); + + @Getter private final double innerVoltage; + @Getter private final double outerVoltage; + + IntakeRollerState(double innerVoltage, double outerVoltage) { + this.innerVoltage = innerVoltage; + this.outerVoltage = outerVoltage; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIO.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIO.java new file mode 100644 index 00000000..c13e2fad --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIO.java @@ -0,0 +1,94 @@ +package frc.robot.subsystems.v3_Poot.superstructure.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface V3_PootIntakeIO { + @AutoLog + public class V3_PootIntakeIOInputs { + public Rotation2d pivotPosition = new Rotation2d(); + public double pivotVelocityRadiansPerSecond = 0.0; + public double pivotAppliedVolts = 0.0; + public double pivotSupplyCurrentAmps = 0.0; + public double pivotTorqueCurrentAmps = 0.0; + public double pivotTemperatureCelsius = 0.0; + + public Rotation2d pivotPositionGoal = new Rotation2d(); + public Rotation2d pivotPositionSetpoint = new Rotation2d(); + public Rotation2d pivotPositionError = new Rotation2d(); + + public Rotation2d rollerOuterPosition = new Rotation2d(); + public double rollerOuterVelocityRadiansPerSecond = 0.0; + public double rollerOuterAppliedVolts = 0.0; + public double rollerOuterSupplyCurrentAmps = 0.0; + public double rollerOuterTorqueCurrentAmps = 0.0; + public double rollerOuterTemperatureCelsius = 0.0; + + public Rotation2d rollerInnerPosition = new Rotation2d(); + public double rollerInnerVelocityRadiansPerSecond = 0.0; + public double rollerInnerAppliedVolts = 0.0; + public double rollerInnerSupplyCurrentAmps = 0.0; + public double rollerInnerTorqueCurrentAmps = 0.0; + public double rollerInnerTemperatureCelsius = 0.0; + + public boolean leftCANRange; // Left and Right based on the robot's perspective with intake + // at the front + public boolean rightCANRange; + } + + /** + * Updates the inputs for the manipulator subsystem. + * + * @param inputs The inputs to update. + */ + public default void updateInputs(V3_PootIntakeIOInputs inputs) {} + + /** + * Sets the voltage for the intake. + * + * @param volts The voltage to set. + */ + public default void setPivotVoltage(double volts) {} + + /** + * Sets the voltage for the inner manipulator roller. + * + * @param volts The voltage to set. + */ + public default void setInnerRollerVoltage(double volts) {} + + /** + * Sets the voltage for the outer manipulator roller. + * + * @param volts The voltage to set. + */ + public default void setOuterRollerVoltage(double volts) {} + + /** + * Sets the position goal for the intake. + * + * @param meters The position goal to set in meters. + */ + public default void setPivotGoal(Rotation2d rotation) {} + + /** + * Sets the gains for the intake + * + * @param kP The proportional gain. + * @param kD The derivative gain. + * @param kS The static gain. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param kG The gravity gain. + */ + public default void updateIntakeGains( + double kP, double kD, double kS, double kV, double kA, double kG) {} + + /** + * Sets the constraints for the intake. + * + * @param maxAcceleration The maximum acceleration. + * @param cruisingVelocity The cruising velocity. + */ + public default void updateIntakeConstraints(double maxAcceleration, double cruisingVelocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOSim.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOSim.java new file mode 100644 index 00000000..e103c26a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOSim.java @@ -0,0 +1,157 @@ +package frc.robot.subsystems.v3_Poot.superstructure.intake; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants; + +public class V3_PootIntakeIOSim implements V3_PootIntakeIO { + private SingleJointedArmSim pivotMotorSim; + private DCMotorSim rollerInnerMotorSim; + private DCMotorSim rollerOuterMotorSim; + + private final ProfiledPIDController armFeedbackController; + private final ArmFeedforward armFeedforwardController; + + private double pivotAppliedVoltage; + private double rollerInnerAppliedVoltage; + private double rollerOuterAppliedVoltage; + + private boolean isClosedLoop; + + public V3_PootIntakeIOSim() { + pivotMotorSim = + new SingleJointedArmSim( + LinearSystemId.createSingleJointedArmSystem( + V3_PootIntakeConstants.PIVOT_PARAMS.MOTOR(), + V3_PootIntakeConstants.PIVOT_PARAMS.MASS_KG(), + V3_PootIntakeConstants.PIVOT_PARAMS.PIVOT_GEAR_RATIO()), + V3_PootIntakeConstants.PIVOT_PARAMS.MOTOR(), + V3_PootIntakeConstants.PIVOT_PARAMS.PIVOT_GEAR_RATIO(), + 1, + V3_PootIntakeConstants.PIVOT_PARAMS.MIN_ANGLE().getRadians(), + V3_PootIntakeConstants.PIVOT_PARAMS.MAX_ANGLE().getRadians(), + true, + V3_PootIntakeConstants.PIVOT_PARAMS.MIN_ANGLE().getRadians()); + + rollerInnerMotorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + V3_PootIntakeConstants.ROLLER_PARAMS.MOTOR(), + 0.04, + V3_PootIntakeConstants.ROLLER_PARAMS.PIVOT_GEAR_RATIO()), + V3_PootIntakeConstants.ROLLER_PARAMS.MOTOR()); + + rollerOuterMotorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + V3_PootIntakeConstants.ROLLER_PARAMS.MOTOR(), + 0.04, + V3_PootIntakeConstants.ROLLER_PARAMS.PIVOT_GEAR_RATIO()), + V3_PootIntakeConstants.ROLLER_PARAMS.MOTOR()); + + armFeedbackController = + new ProfiledPIDController( + V3_PootIntakeConstants.PIVOT_GAINS.kP().get(), + 0.0, + V3_PootIntakeConstants.PIVOT_GAINS.kD().get(), + V3_PootIntakeConstants.PIVOT_CONSTRAINTS.getTrapezoidConstraints()); + armFeedbackController.disableContinuousInput(); + + armFeedforwardController = + new ArmFeedforward( + V3_PootIntakeConstants.PIVOT_GAINS.kS().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kA().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kV().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kA().get()); + + pivotAppliedVoltage = 0.0; + rollerInnerAppliedVoltage = 0.0; + rollerOuterAppliedVoltage = 0.0; + isClosedLoop = false; + } + + @Override + public void updateInputs(V3_PootIntakeIOInputs inputs) { + if (isClosedLoop) { + pivotAppliedVoltage = + armFeedbackController.calculate(pivotMotorSim.getAngleRads()) + + armFeedforwardController.calculate( + pivotMotorSim.getAngleRads(), pivotMotorSim.getVelocityRadPerSec()); + } + + pivotAppliedVoltage = MathUtil.clamp(pivotAppliedVoltage, -12.0, 12.0); + rollerInnerAppliedVoltage = MathUtil.clamp(rollerInnerAppliedVoltage, -12.0, 12.0); + rollerOuterAppliedVoltage = MathUtil.clamp(rollerOuterAppliedVoltage, -12.0, 12.0); + + pivotMotorSim.setInputVoltage(pivotAppliedVoltage); + pivotMotorSim.update(Constants.LOOP_PERIOD_SECONDS); + + rollerInnerMotorSim.setInputVoltage(rollerInnerAppliedVoltage); + rollerInnerMotorSim.update(Constants.LOOP_PERIOD_SECONDS); + + rollerOuterMotorSim.setInputVoltage(rollerOuterAppliedVoltage); + rollerOuterMotorSim.update(Constants.LOOP_PERIOD_SECONDS); + + inputs.pivotPosition = new Rotation2d(pivotMotorSim.getAngleRads()); + inputs.pivotVelocityRadiansPerSecond = pivotMotorSim.getVelocityRadPerSec(); + inputs.pivotAppliedVolts = pivotAppliedVoltage; + inputs.pivotSupplyCurrentAmps = pivotMotorSim.getCurrentDrawAmps(); + + inputs.pivotPositionGoal = new Rotation2d(armFeedbackController.getGoal().position); + inputs.pivotPositionSetpoint = new Rotation2d(armFeedbackController.getSetpoint().position); + inputs.pivotPositionError = new Rotation2d(armFeedbackController.getPositionError()); + + inputs.rollerInnerPosition = new Rotation2d(rollerInnerMotorSim.getAngularPosition()); + inputs.rollerInnerVelocityRadiansPerSecond = rollerInnerMotorSim.getAngularVelocityRadPerSec(); + inputs.rollerInnerAppliedVolts = rollerInnerAppliedVoltage; + inputs.rollerInnerSupplyCurrentAmps = rollerInnerMotorSim.getCurrentDrawAmps(); + + inputs.rollerOuterPosition = new Rotation2d(rollerOuterMotorSim.getAngularPosition()); + inputs.rollerOuterVelocityRadiansPerSecond = rollerOuterMotorSim.getAngularVelocityRadPerSec(); + inputs.rollerOuterAppliedVolts = rollerOuterAppliedVoltage; + inputs.rollerOuterSupplyCurrentAmps = rollerOuterMotorSim.getCurrentDrawAmps(); + } + + @Override + public void setPivotVoltage(double voltage) { + isClosedLoop = false; + pivotAppliedVoltage = voltage; + } + + @Override + public void setInnerRollerVoltage(double voltage) { + rollerInnerAppliedVoltage = voltage; + } + + public void setOuterRollerVoltage(double voltage) { + rollerOuterAppliedVoltage = voltage; + } + + @Override + public void setPivotGoal(Rotation2d rotation) { + isClosedLoop = true; + armFeedbackController.setGoal(rotation.getRadians()); + } + + @Override + public void updateIntakeGains(double kP, double kD, double kS, double kV, double kA, double kG) { + armFeedbackController.setPID(kP, 0.0, kD); + armFeedforwardController.setKa(kA); + armFeedforwardController.setKs(kS); + armFeedforwardController.setKv(kV); + armFeedforwardController.setKg(kG); + } + + @Override + public void updateIntakeConstraints( + double maxVelocityRadiansPerSecond, double maxAccelerationRadiansPerSecondSquared) { + armFeedbackController.setConstraints( + new Constraints(maxVelocityRadiansPerSecond, maxAccelerationRadiansPerSecondSquared)); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOTalonFX.java new file mode 100644 index 00000000..3cd0e034 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/intake/V3_PootIntakeIOTalonFX.java @@ -0,0 +1,356 @@ +package frc.robot.subsystems.v3_Poot.superstructure.intake; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.util.PhoenixUtil; +import java.util.ArrayList; + +public class V3_PootIntakeIOTalonFX implements V3_PootIntakeIO { + private final TalonFX pivotTalonFX; + + private final StatusSignal pivotPositionRotations; + private final StatusSignal pivotVelocityRotationsPerSecond; + private final StatusSignal pivotAppliedVoltage; + private final StatusSignal pivotSupplyCurrentAmps; + private final StatusSignal pivotTorqueCurrentAmps; + private final StatusSignal pivotTemperatureCelsius; + + private final StatusSignal pivotPositionSetpoint; + private final StatusSignal pivotPositionError; + + private final VoltageOut pivotVoltageRequest; + private final MotionMagicVoltage pivotMotionMagicRequest; + + private final TalonFXConfiguration pivotConfig; + + private final TalonFX rollerTalonFXOuter; + private final TalonFX rollerTalonFXInner; + + private final StatusSignal rollerInnerPositionRotations; + private final StatusSignal rollerInnerVelocityRotationsPerSecond; + private final StatusSignal rollerInnerAppliedVoltage; + private final StatusSignal rollerInnerSupplyCurrentAmps; + private final StatusSignal rollerInnerTorqueCurrentAmps; + private final StatusSignal rollerInnerTemperatureCelsius; + + private final StatusSignal rollerOuterPositionRotations; + private final StatusSignal rollerOuterVelocityRotationsPerSecond; + private final StatusSignal rollerOuterAppliedVoltage; + private final StatusSignal rollerOuterSupplyCurrentAmps; + private final StatusSignal rollerOuterTorqueCurrentAmps; + private final StatusSignal rollerOuterTemperatureCelsius; + + private final VoltageOut rollerInnerVoltageRequest; + private final VoltageOut rollerOuterVoltageRequest; + + private final TalonFXConfiguration rollerInnerConfig; + private final TalonFXConfiguration rollerOuterConfig; + + private final CANrange leftCANrange; + private final CANrange rightCANrange; + + private final StatusSignal leftCANrangeStatusSignal; + private final StatusSignal rightCANrangeStatusSignal; + + private StatusSignal[] statusSignals; + + public V3_PootIntakeIOTalonFX() { + pivotTalonFX = new TalonFX(V3_PootIntakeConstants.PIVOT_CAN_ID); + rollerTalonFXOuter = new TalonFX(V3_PootIntakeConstants.ROLLER_CAN_ID_OUTER); + rollerTalonFXInner = new TalonFX(V3_PootIntakeConstants.ROLLER_CAN_ID_INNER); + + leftCANrange = new CANrange(V3_PootIntakeConstants.LEFT_SENSOR_CAN_ID); + rightCANrange = new CANrange(V3_PootIntakeConstants.RIGHT_SENSOR_CAN_ID); + + pivotConfig = new TalonFXConfiguration(); + pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + pivotConfig.CurrentLimits.SupplyCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.PIVOT_SUPPLY_CURRENT_LIMIT(); + pivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + pivotConfig.CurrentLimits.StatorCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.PIVOT_STATOR_CURRENT_LIMIT(); + pivotConfig.Feedback.SensorToMechanismRatio = + V3_PootIntakeConstants.PIVOT_PARAMS.PIVOT_GEAR_RATIO(); + pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + V3_PootIntakeConstants.PIVOT_PARAMS.MAX_ANGLE().getRotations(); + pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + V3_PootIntakeConstants.PIVOT_PARAMS.MIN_ANGLE().getRotations(); + pivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + pivotConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + pivotConfig.Slot0 = + new Slot0Configs() + .withGravityType(GravityTypeValue.Arm_Cosine) + .withKP(V3_PootIntakeConstants.PIVOT_GAINS.kP().get()) + .withKD(V3_PootIntakeConstants.PIVOT_GAINS.kD().get()) + .withKA(V3_PootIntakeConstants.PIVOT_GAINS.kA().get()) + .withKV(V3_PootIntakeConstants.PIVOT_GAINS.kV().get()) + .withKS(V3_PootIntakeConstants.PIVOT_GAINS.kS().get()) + .withKG(V3_PootIntakeConstants.PIVOT_GAINS.kG().get()); + pivotConfig.MotionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity( + AngularVelocity.ofRelativeUnits( + V3_PootIntakeConstants.PIVOT_CONSTRAINTS + .CRUISING_VELOCITY_RADIANS_PER_SECOND() + .get(), + RadiansPerSecond)) + .withMotionMagicAcceleration( + AngularAcceleration.ofRelativeUnits( + V3_PootIntakeConstants.PIVOT_CONSTRAINTS + .MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED() + .get(), + RadiansPerSecondPerSecond)); + + tryUntilOk(5, () -> pivotTalonFX.getConfigurator().apply(pivotConfig, 0.25)); + + pivotPositionRotations = pivotTalonFX.getPosition(); + pivotVelocityRotationsPerSecond = pivotTalonFX.getVelocity(); + pivotAppliedVoltage = pivotTalonFX.getMotorVoltage(); + pivotSupplyCurrentAmps = pivotTalonFX.getSupplyCurrent(); + pivotTorqueCurrentAmps = pivotTalonFX.getTorqueCurrent(); + pivotTemperatureCelsius = pivotTalonFX.getDeviceTemp(); + + pivotPositionSetpoint = pivotTalonFX.getClosedLoopReference(); + pivotPositionError = pivotTalonFX.getClosedLoopError(); + + leftCANrangeStatusSignal = leftCANrange.getIsDetected(); + rightCANrangeStatusSignal = rightCANrange.getIsDetected(); + + rollerInnerConfig = new TalonFXConfiguration(); + rollerInnerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + rollerInnerConfig.CurrentLimits.SupplyCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.INNER_ROLLER_SUPPLY_CURRENT_LIMIT(); + rollerInnerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + rollerInnerConfig.CurrentLimits.StatorCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.INNER_ROLLER_STATOR_CURRENT_LIMIT(); + rollerInnerConfig.Feedback.SensorToMechanismRatio = + V3_PootIntakeConstants.ROLLER_PARAMS.PIVOT_GEAR_RATIO(); + rollerInnerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rollerInnerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + rollerOuterConfig = new TalonFXConfiguration(); + rollerOuterConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + rollerOuterConfig.CurrentLimits.SupplyCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.OUTER_ROLLER_SUPPLY_CURRENT_LIMIT(); + rollerOuterConfig.CurrentLimits.StatorCurrentLimitEnable = true; + rollerOuterConfig.CurrentLimits.StatorCurrentLimit = + V3_PootIntakeConstants.CURRENT_LIMITS.OUTER_ROLLER_STATOR_CURRENT_LIMIT(); + rollerOuterConfig.Feedback.SensorToMechanismRatio = + V3_PootIntakeConstants.ROLLER_PARAMS.PIVOT_GEAR_RATIO(); + rollerOuterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rollerOuterConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + tryUntilOk(5, () -> rollerTalonFXOuter.getConfigurator().apply(rollerOuterConfig, 0.25)); + tryUntilOk(5, () -> rollerTalonFXInner.getConfigurator().apply(rollerInnerConfig, 0.25)); + + rollerInnerPositionRotations = rollerTalonFXInner.getPosition(); + rollerInnerVelocityRotationsPerSecond = rollerTalonFXInner.getVelocity(); + rollerInnerAppliedVoltage = rollerTalonFXInner.getMotorVoltage(); + rollerInnerSupplyCurrentAmps = rollerTalonFXInner.getSupplyCurrent(); + rollerInnerTorqueCurrentAmps = rollerTalonFXInner.getTorqueCurrent(); + rollerInnerTemperatureCelsius = rollerTalonFXInner.getDeviceTemp(); + + rollerOuterPositionRotations = rollerTalonFXOuter.getPosition(); + rollerOuterVelocityRotationsPerSecond = rollerTalonFXOuter.getVelocity(); + rollerOuterAppliedVoltage = rollerTalonFXOuter.getMotorVoltage(); + rollerOuterSupplyCurrentAmps = rollerTalonFXOuter.getSupplyCurrent(); + rollerOuterTorqueCurrentAmps = rollerTalonFXOuter.getTorqueCurrent(); + rollerOuterTemperatureCelsius = rollerTalonFXOuter.getDeviceTemp(); + + pivotVoltageRequest = new VoltageOut(0); + pivotMotionMagicRequest = + new MotionMagicVoltage(V3_PootIntakeConstants.PIVOT_PARAMS.MIN_ANGLE().getMeasure()); + + rollerInnerVoltageRequest = new VoltageOut(0); + rollerOuterVoltageRequest = new VoltageOut(0); + + var signalsList = new ArrayList>(); + + signalsList.add(pivotPositionRotations); + signalsList.add(pivotVelocityRotationsPerSecond); + signalsList.add(pivotAppliedVoltage); + signalsList.add(pivotSupplyCurrentAmps); + signalsList.add(pivotTorqueCurrentAmps); + signalsList.add(pivotTemperatureCelsius); + signalsList.add(rollerInnerPositionRotations); + signalsList.add(rollerOuterPositionRotations); + signalsList.add(rollerInnerVelocityRotationsPerSecond); + signalsList.add(rollerOuterVelocityRotationsPerSecond); + signalsList.add(rollerInnerAppliedVoltage); + signalsList.add(rollerOuterAppliedVoltage); + signalsList.add(rollerInnerSupplyCurrentAmps); + signalsList.add(rollerOuterSupplyCurrentAmps); + signalsList.add(rollerInnerTorqueCurrentAmps); + signalsList.add(rollerOuterTorqueCurrentAmps); + signalsList.add(rollerInnerTemperatureCelsius); + signalsList.add(rollerOuterTemperatureCelsius); + signalsList.add(leftCANrangeStatusSignal); + signalsList.add(rightCANrangeStatusSignal); + + statusSignals = new StatusSignal[signalsList.size()]; + + for (int i = 0; i < signalsList.size(); i++) { + statusSignals[i] = signalsList.get(i); + } + + BaseStatusSignal.setUpdateFrequencyForAll(50, statusSignals); + + pivotTalonFX.optimizeBusUtilization(); + rollerTalonFXInner.optimizeBusUtilization(); + rollerTalonFXOuter.optimizeBusUtilization(); + leftCANrange.optimizeBusUtilization(); + rightCANrange.optimizeBusUtilization(); + + PhoenixUtil.registerSignals(false, statusSignals); + + pivotTalonFX.setPosition(Units.degreesToRotations(48)); + } + + /** + * Updates the inputs of the subsystem. + * + *

This function is called by the robot periodic loop and should update all of the inputs of + * the subsystem. The inputs should be updated from the CAN bus and other sensors. + * + * @param inputs The inputs of the subsystem. + */ + @Override + public void updateInputs(V3_PootIntakeIOInputs inputs) { + inputs.pivotPosition = new Rotation2d(pivotPositionRotations.getValue()); + inputs.pivotVelocityRadiansPerSecond = + pivotVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.pivotAppliedVolts = pivotAppliedVoltage.getValueAsDouble(); + inputs.pivotSupplyCurrentAmps = pivotSupplyCurrentAmps.getValueAsDouble(); + inputs.pivotTorqueCurrentAmps = pivotTorqueCurrentAmps.getValueAsDouble(); + inputs.pivotTemperatureCelsius = pivotTemperatureCelsius.getValueAsDouble(); + + inputs.pivotPositionGoal = new Rotation2d(pivotMotionMagicRequest.getPositionMeasure()); + inputs.pivotPositionSetpoint = + Rotation2d.fromRotations(pivotPositionSetpoint.getValueAsDouble()); + inputs.pivotPositionError = Rotation2d.fromRotations(pivotPositionError.getValueAsDouble()); + + inputs.rollerInnerPosition = new Rotation2d(rollerInnerPositionRotations.getValue()); + inputs.rollerInnerVelocityRadiansPerSecond = + rollerInnerVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.rollerInnerAppliedVolts = rollerInnerAppliedVoltage.getValueAsDouble(); + inputs.rollerInnerSupplyCurrentAmps = rollerInnerSupplyCurrentAmps.getValueAsDouble(); + inputs.rollerInnerTorqueCurrentAmps = rollerInnerTorqueCurrentAmps.getValueAsDouble(); + inputs.rollerInnerTemperatureCelsius = rollerInnerTemperatureCelsius.getValueAsDouble(); + + inputs.rollerOuterPosition = new Rotation2d(rollerOuterPositionRotations.getValue()); + inputs.rollerOuterVelocityRadiansPerSecond = + rollerOuterVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.rollerOuterAppliedVolts = rollerOuterAppliedVoltage.getValueAsDouble(); + inputs.rollerOuterSupplyCurrentAmps = rollerOuterSupplyCurrentAmps.getValueAsDouble(); + inputs.rollerOuterTorqueCurrentAmps = rollerOuterTorqueCurrentAmps.getValueAsDouble(); + inputs.rollerOuterTemperatureCelsius = rollerOuterTemperatureCelsius.getValueAsDouble(); + + inputs.leftCANRange = leftCANrangeStatusSignal.getValue(); + // result = condition ? true : false; + inputs.rightCANRange = rightCANrangeStatusSignal.getValue(); + } + + /** + * Sets the voltage for the intake pivot motor. + * + *

This method is used to set the voltage for the intake pivot motor in open-loop mode. It sets + * the isClosedLoop flag to false, and then calls the setPivotVoltage method of the IO interface. + * + * @param volts The voltage to set for the intake pivot motor. + */ + public void setPivotVoltage(double volts) { + pivotTalonFX.setControl(pivotVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + /** + * Sets the voltage for the inner manipulator roller. + * + *

This method is used to set the voltage for the inner manipulator roller in open-loop mode. + * It sets the isClosedLoop flag to false, and then calls the setControl method of the TalonFX + * object with the voltage request. + * + * @param volts The voltage to set for the inner manipulator roller. + */ + public void setInnerRollerVoltage(double volts) { + rollerTalonFXInner.setControl(rollerInnerVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + /** + * Sets the voltage for the outer manipulator roller. + * + *

This method is used to set the voltage for the outer manipulator roller in open-loop mode. + * It sets the isClosedLoop flag to false, and then calls the setControl method of the TalonFX + * object with the voltage request. + * + * @param volts The voltage to set for the outer manipulator roller. + */ + public void setOuterRollerVoltage(double volts) { + rollerTalonFXOuter.setControl(rollerOuterVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + /** + * Sets the position of the intake pivot motor using Motion Magic. + * + *

This method is used to set the position of the intake pivot motor using Motion Magic. It + * sets the isClosedLoop flag to true, and then calls the setControl method of the TalonFX object + * with the position request. + * + * @param position The desired position of the intake pivot motor. + */ + @Override + public void setPivotGoal(Rotation2d position) { + + pivotTalonFX.setControl( + pivotMotionMagicRequest.withPosition(position.getMeasure()).withEnableFOC(true)); + } + + public void updateIntakeGains(double kP, double kD, double kS, double kG, double kV, double kA) { + pivotConfig.Slot0.kP = kP; + pivotConfig.Slot0.kD = kD; + pivotConfig.Slot0.kS = kS; + pivotConfig.Slot0.kG = kG; + pivotConfig.Slot0.kV = kV; + pivotConfig.Slot0.kA = kA; + + tryUntilOk(5, () -> pivotTalonFX.getConfigurator().apply(pivotConfig, 0.25)); + } + + public void updateIntakeConstraints( + double maxVelocityRadiansPerSecond, double maxAccelerationRadiansPerSecondSquared) { + pivotConfig.MotionMagic.MotionMagicCruiseVelocity = + AngularVelocity.ofRelativeUnits(maxVelocityRadiansPerSecond, RadiansPerSecond) + .in(RotationsPerSecond); + pivotConfig.MotionMagic.MotionMagicAcceleration = + AngularAcceleration.ofRelativeUnits( + maxAccelerationRadiansPerSecondSquared, RadiansPerSecondPerSecond) + .in(RotationsPerSecondPerSecond); + + tryUntilOk(5, () -> pivotTalonFX.getConfigurator().apply(pivotConfig, 0.25)); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulator.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulator.java new file mode 100644 index 00000000..c7d4c339 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulator.java @@ -0,0 +1,337 @@ +package frc.robot.subsystems.v3_Poot.superstructure.manipulator; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.FieldConstants.Reef.ReefState; +import frc.robot.RobotState; +import frc.robot.RobotState.ScoreSide; +import frc.robot.subsystems.shared.elevator.Elevator.ElevatorFSM; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructure; +import frc.robot.subsystems.v3_Poot.superstructure.V3_PootSuperstructureStates; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorArmState; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState; +import java.util.Set; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class V3_PootManipulator { + private final V3_PootManipulatorIO io; + private final ManipulatorIOInputsAutoLogged inputs; + + @AutoLogOutput(key = "Manipulator/Arm Goal") + @Getter + private Rotation2d armGoal; + + @AutoLogOutput(key = "Manipulator/Roller Goal") + @Getter + private ManipulatorRollerState rollerGoal; + + private boolean isClosedLoop; + + Set algaeStates; + + public V3_PootManipulator(V3_PootManipulatorIO io) { + this.io = io; + inputs = new ManipulatorIOInputsAutoLogged(); + + isClosedLoop = true; + armGoal = ManipulatorArmState.VERTICAL_UP.getAngle(RobotState.getScoreSide()); + rollerGoal = ManipulatorRollerState.STOP; + + algaeStates = + Set.of( + ManipulatorArmState.ALGAE_INTAKE_FLOOR, + ManipulatorArmState.REEF_INTAKE, + ManipulatorArmState.ALGAE_SCORE, + ManipulatorArmState.PRE_SCORE); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Manipulator", inputs); + + if (isClosedLoop) { + io.setArmGoal(armGoal); + } + + if (rollerGoal.equals(ManipulatorRollerState.SCORE_ALGAE)) { + io.setRollerVoltage(ManipulatorRollerState.SCORE_ALGAE.getVoltage()); + } else if (hasAlgae() + && Set.of( + ManipulatorRollerState.CORAL_INTAKE, + ManipulatorRollerState.STOP, + ManipulatorRollerState.ALGAE_INTAKE) + .contains(rollerGoal) + && !rollerGoal.equals(ManipulatorRollerState.SCORE_ALGAE)) { + RobotState.setHasAlgae(true); + io.setRollerVoltage(holdVoltage()); + } else if (hasCoral()) { + io.setRollerVoltage(holdVoltage()); + } else { + io.setRollerVoltage(rollerGoal.getVoltage()); + } + + Logger.recordOutput("Manipulator/Arm In Tolerance", armInTolerance(Rotation2d.fromDegrees(15))); + } + + /** + * Checks if the manipulator is currently detecting coral. This is done by checking if the CAN + * range sensor is detecting a distance less than the coral detection threshold and greater than + * 0. + * + * @return True if the manipulator is detecting coral, false otherwise. + */ + @AutoLogOutput(key = "Manipulator/Has Coral") + public boolean hasCoral() { + return inputs.canRangeGot; + } + + /** + * Checks if the manipulator is currently detecting algae. This is done by checking if the CAN + * range sensor is detecting a distance less than the algae detection threshold and greater than + * 0. + * + * @return True if the manipulator is detecting algae, false otherwise. + */ + @AutoLogOutput(key = "Manipulator/Has Algae") + public boolean hasAlgae() { + boolean hasAlgae = rollerGoal.equals(ManipulatorRollerState.ALGAE_INTAKE) && inputs.canRangeGot; + return hasAlgae; + } + + /** + * Creates a command to run the manipulator arm at a specified voltage. + * + * @param volts The voltage to set the arm to. + * @return A command to run the arm. + */ + public Command runArm(double volts) { + return Commands.runEnd( + () -> { + isClosedLoop = false; + io.setArmVoltage(volts); + }, + () -> io.setArmVoltage(0)); + } + + /** + * Sets the goal for the manipulator arm to reach. + * + * @param goal The goal state to set the arm to. + */ + public void setArmGoal(ManipulatorArmState goal) { + isClosedLoop = true; + if (!algaeStates.contains(goal)) { + armGoal = goal.getAngle(RobotState.getScoreSide()); + } else { + armGoal = goal.getAngle(ScoreSide.CENTER); + } + } + + public void setArmGoal(Rotation2d goal) { + isClosedLoop = true; + armGoal = goal; + } + + /** + * Updates the gains for the manipulator arm. This function sets the PID gains for the three slots + * of the arm. The gains are used to control the arm's movement. + * + * @param kP0 The proportional gain for slot 0. + * @param kD0 The derivative gain for slot 0. + * @param kS0 The static gain for slot 0. + * @param kV0 The velocity gain for slot 0. + * @param kA0 The acceleration gain for slot 0. + * @param kG0 The gravity gain for slot 0. + * @param kP1 The proportional gain for slot 1. + * @param kD1 The derivative gain for slot 1. + * @param kS1 The static gain for slot 1. + * @param kV1 The velocity gain for slot 1. + * @param kA1 The acceleration gain for slot 1. + * @param kG1 The gravity gain for slot 1. + * @param kP2 The proportional gain for slot 2. + * @param kD2 The derivative gain for slot 2. + * @param kS2 The static gain for slot 2. + * @param kV2 The velocity gain for slot 2. + * @param kA2 The acceleration gain for slot 2. + * @param kG2 The gravity gain for slot 2. + */ + public void updateArmGains( + double kP0, + double kD0, + double kS0, + double kV0, + double kA0, + double kG0, + double kP1, + double kD1, + double kS1, + double kV1, + double kA1, + double kG1, + double kP2, + double kD2, + double kS2, + double kV2, + double kA2, + double kG2) { + io.updateSlot0ArmGains(kP0, kD0, kS0, kV0, kA0, kG0); + io.updateSlot1ArmGains(kP1, kD1, kS1, kV1, kA1, kG1); + io.updateSlot2ArmGains(kP2, kD2, kS2, kV2, kA2, kG2); + } + + /** + * Updates the constraints for the arm. + * + * @param maxAcceleration The maximum acceleration. + * @param maxVelocity The maximum velocity. + */ + public void updateArmConstraints(double maxAcceleration, double maxVelocity) { + io.updateArmConstraints(maxAcceleration, maxVelocity); + } + + /** + * Checks if the arm is at the goal position. + * + *

This function checks if the arm is within the goal tolerance of the currently set arm goal + * position. If the arm is within the tolerance, it returns true. Otherwise, it returns false. + * + * @return If the arm is at the goal position. + */ + @AutoLogOutput(key = "Manipulator/Arm At Goal") + public boolean armAtGoal() { + return armAtGoal(armGoal); + } + + public boolean armAtGoal(Rotation2d state) { + return Math.abs(inputs.armPosition.minus(state).getRadians()) + <= V3_PootManipulatorConstants.CONSTRAINTS.goalToleranceRadians().get(); + } + + public boolean armInTolerance(Rotation2d tolerance) { + return Math.abs(inputs.armPosition.minus(armGoal).getRadians()) <= tolerance.getRadians(); + } + + /** + * Waits until the arm is at the goal position. + * + *

This command waits for 0.02 seconds and then checks if the arm is at the goal position. If + * the arm is not at the goal position, it waits for 0.02 seconds and checks again. This process + * repeats until the arm is at the goal position. + * + * @return A command that waits until the arm is at the goal position. + */ + public Command waitUntilArmAtGoal() { + return Commands.sequence(Commands.waitSeconds(0.02), Commands.waitUntil(this::armAtGoal)); + } + + /** + * Returns a voltage to hold the roller at the current position. The voltage is calculated based + * on the current torque current of the roller. The calculation is done using a piecewise + * polynomial function. The function first checks if the current torque current is less than or + * equal to 20. If it is, it uses one set of coefficients to calculate the voltage. Otherwise, it + * uses another set of coefficients. + */ + private double holdVoltage() { + return RobotState.isHasAlgae() ? -12.0 : -1; + } + + /** + * Sets the current slot of the manipulator arm based on the current state of the subsystem. If + * the subsystem has algae, it sets the slot to 2. If the subsystem has coral, it sets the slot to + * 1. Otherwise, it sets the slot to 0. + */ + public void setSlot() { + if (RobotState.isHasAlgae()) { + io.setSlot(2); + } else if (hasCoral()) { + io.setSlot(1); + } else { + io.setSlot(0); + } + } + + /** + * Creates a command to run the SysId routine for the manipulator arm, generating the constants + * and gains for a PID. + * + * @param superstructure The V3 Epsiolon Superstructure. + * @return A command to run the SysId routine for the manipulator arm. + */ + public Command sysIdRoutine(V3_PootSuperstructure superstructure, ElevatorFSM elevator) { + SysIdRoutine algaeCharacterizationRoutine = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.5).per(Second), + Volts.of(8), + Seconds.of(10), + (state) -> Logger.recordOutput("Manipulator/SysID State", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> io.setArmVoltage(volts.in(Volts)), null, superstructure)); + return Commands.sequence( + superstructure.runGoal(V3_PootSuperstructureStates.OVERRIDE), + Commands.runOnce(() -> elevator.setPosition(() -> ReefState.L4)), + Commands.runOnce(() -> isClosedLoop = false), + algaeCharacterizationRoutine.quasistatic(Direction.kForward), + Commands.waitSeconds(.25), + algaeCharacterizationRoutine.quasistatic(Direction.kReverse), + Commands.waitSeconds(.25), + algaeCharacterizationRoutine.dynamic(Direction.kForward), + Commands.waitSeconds(.25), + algaeCharacterizationRoutine.dynamic(Direction.kReverse)); + } + + /** + * Sets the manipulator arm to the specified state. + * + * @param state The state to set the arm to. + */ + public void setManipulatorState(V3_PootManipulatorConstants.ManipulatorArmState state) { + io.setManipulatorState(state); + } + + /** + * Sets the roller goal state of the manipulator. If the subsystem has algae or coral and the goal + * is one of the intake states, it sets the roller voltage to the hold voltage. Otherwise, it sets + * the roller voltage to the goal voltage. + * + * @param rollerGoal The desired state of the roller. + */ + public void setRollerGoal(V3_PootManipulatorConstants.ManipulatorRollerState rollerGoal) { + this.rollerGoal = rollerGoal; + if ((hasAlgae() || hasCoral()) + && Set.of( + V3_PootManipulatorConstants.ManipulatorRollerState.ALGAE_INTAKE, + V3_PootManipulatorConstants.ManipulatorRollerState.CORAL_INTAKE, + V3_PootManipulatorConstants.ManipulatorRollerState.STOP) + .contains(rollerGoal)) { + + io.setRollerVoltage(holdVoltage()); + } else { + io.setRollerVoltage(rollerGoal.getVoltage()); + } + } + + /** + * Gets the current angle of the manipulator arm. + * + * @return The current angle of the manipulator arm, in radians. + */ + public Rotation2d getArmAngle() { + return inputs.armPosition; + } + + public double getArmVelocity() { + return inputs.armVelocityRadiansPerSecond; + } + + public double getRollerVelocity() { + return inputs.rollerVelocityRadiansPerSecond; + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorConstants.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorConstants.java new file mode 100644 index 00000000..db0f938a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorConstants.java @@ -0,0 +1,277 @@ +package frc.robot.subsystems.v3_Poot.superstructure.manipulator; + +import com.ctre.phoenix6.configs.SlotConfigs; +import com.ctre.phoenix6.signals.GravityTypeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.RobotState; +import frc.robot.RobotState.ScoreSide; +import frc.robot.util.LoggedTunableNumber; +import lombok.RequiredArgsConstructor; + +public final class V3_PootManipulatorConstants { + public static final ArmParameters ARM_PARAMETERS; + + public static final Gains EMPTY_GAINS; + public static final Gains CORAL_GAINS; + public static final Gains ALGAE_GAINS; + public static final Constraints CONSTRAINTS; + + public static final int ROLLER_CAN_ID; + public static final double ROLLER_CURRENT_THRESHOLD; + public static final Rotation2d ROLLER_TOGGLE_ARM_ROTATION; + public static final Voltages ROLLER_VOLTAGES; + + public static final ManipulatorCurrentLimits CURRENT_LIMITS; + + public static final double ALGAE_CAN_RANGE_THRESHOLD_METERS; + public static final double CORAL_CAN_RANGE_THRESHOLD_METERS; + public static final int ARM_CAN_ID; + + public static final int CAN_RANGE_ID; + + static { + ARM_CAN_ID = 30; + CAN_RANGE_ID = 32; + ROLLER_CAN_ID = 31; + + ARM_PARAMETERS = + new ArmParameters( + DCMotor.getKrakenX60Foc(1), 1, (60.0 / 12.0) * (40.0 / 18.0) * (80.0 / 16.0), .695); + + ALGAE_CAN_RANGE_THRESHOLD_METERS = 0.5; + CORAL_CAN_RANGE_THRESHOLD_METERS = 0.5; + + ROLLER_CURRENT_THRESHOLD = 60.0; + ROLLER_TOGGLE_ARM_ROTATION = Rotation2d.fromRadians(10); + ROLLER_VOLTAGES = + new Voltages( + new LoggedTunableNumber("Manipulator/Coral Intake Volts", 6.0), + new LoggedTunableNumber("Manipulator/Algae Intake Volts", 12.0), + new LoggedTunableNumber("Manipulator/L4 Volts", 4.6 * 1.56), + new LoggedTunableNumber("Manipulator/Score Coral Volts", 4.8 * 1.56), + new LoggedTunableNumber("Manipulator/Score Algae Volts", -6), + new LoggedTunableNumber("Manipulator/Remove Algae Volts", 12), + new LoggedTunableNumber("Manipulator/HalfScore Volts", 1.0 * 1.56), + new LoggedTunableNumber("Manipulator/L1 Volts", 3.5 * 1.56)); + + switch (Constants.ROBOT) { + case V3_POOT: + EMPTY_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/Arm/Empty/kP", 100), + new LoggedTunableNumber("Manipulator/Arm/Empty/kD", 0), + new LoggedTunableNumber("Manipulator/Arm/Empty/kS", 0.12926), + new LoggedTunableNumber("Manipulator/Arm/Empty/kG", 0.024193), + new LoggedTunableNumber("Manipulator/Arm/Empty/kV", 0), + new LoggedTunableNumber("Manipulator/Arm/Empty/kA", 0)); + CORAL_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kP", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kS", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kG", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kA", 0.0)); + ALGAE_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kP", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kS", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kG", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kA", 0.0)); + CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Manipulator/Arm/MaxAcceleration", 8), + new LoggedTunableNumber("Manipulator/Arm/CruisingVelocity", 5), + new LoggedTunableNumber( + "Manipulator/Arm/GoalTolerance", Units.degreesToRadians(3))); + CURRENT_LIMITS = new ManipulatorCurrentLimits(40, 20, 40, 20); + break; + case V3_POOT_SIM: + EMPTY_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/Arm/Empty/kP", 100), + new LoggedTunableNumber("Manipulator/Arm/Empty/kD", 0), + new LoggedTunableNumber("Manipulator/Arm/Empty/kS", 0.24274), + new LoggedTunableNumber("Manipulator/Arm/Empty/kG", 0.66177), + new LoggedTunableNumber("Manipulator/Arm/Empty/kV", 0), + new LoggedTunableNumber("Manipulator/Arm/Empty/kA", 0)); + CORAL_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kP", 125), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kS", 0.24274), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kG", 0.66177), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kA", 0.0)); + ALGAE_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kP", 125), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kS", 0.65347), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kG", 2.0762), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kA", 0.0)); + CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Manipulator/Arm/MaxAcceleration", 100.0), + new LoggedTunableNumber("Manipulator/Arm/CruisingVelocity", 50.0), + new LoggedTunableNumber( + "Manipulator/Arm/GoalTolerance", Units.degreesToRadians(3))); + CURRENT_LIMITS = new ManipulatorCurrentLimits(40, 40, 40, 40); + break; + default: + EMPTY_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/Arm/Empty/kP", 1), + new LoggedTunableNumber("Manipulator/Arm/Empty/kD", 0), + new LoggedTunableNumber("Manipulator/Arm/Empty/kS", 0.00014503), + new LoggedTunableNumber("Manipulator/Arm/Empty/kG", 0.66177), + new LoggedTunableNumber("Manipulator/Arm/Empty/kV", 1.7708), + new LoggedTunableNumber("Manipulator/Arm/Empty/kA", 0.00032712)); + CORAL_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kP", 125), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kS", 0.24274), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kG", 0.66177), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithoutAlgae/kA", 0.0)); + ALGAE_GAINS = + new Gains( + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kP", 125), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kD", 0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kS", 0.65347), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kG", 2.0762), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kV", 0.0), + new LoggedTunableNumber("Manipulator/ArmWithAlgae/kA", 0.0)); + CONSTRAINTS = + new Constraints( + new LoggedTunableNumber("Manipulator/Arm/MaxAcceleration", 20.0), + new LoggedTunableNumber("Manipulator/Arm/CruisingVelocity", 50.0), + new LoggedTunableNumber( + "Manipulator/Arm/GoalTolerance", Units.degreesToRadians(3))); + CURRENT_LIMITS = new ManipulatorCurrentLimits(40, 40, 40, 40); + break; + } + } + + public static record Gains( + LoggedTunableNumber kP, + LoggedTunableNumber kD, + LoggedTunableNumber kS, + LoggedTunableNumber kG, + LoggedTunableNumber kV, + LoggedTunableNumber kA) { + public SlotConfigs toTalonFXSlotConfigs() { + return new SlotConfigs() + .withKP(kP.get()) + .withKD(kD.get()) + .withKS(kS.get()) + .withKG(kG.get()) + .withKV(kV.get()) + .withKA(kA.get()) + .withGravityType(GravityTypeValue.Arm_Cosine); + } + } + + public static final record ManipulatorCurrentLimits( + double MANIPULATOR_SUPPLY_CURRENT_LIMIT, + double ROLLER_SUPPLY_CURRENT_LIMIT, + double MANIPULATOR_STATOR_CURRENT_LIMIT, + double ROLLER_STATOR_CURRENT_LIMIT) {} + + public static record Constraints( + LoggedTunableNumber maxAccelerationRotationsPerSecondSquared, + LoggedTunableNumber cruisingVelocityRotationsPerSecond, + LoggedTunableNumber goalToleranceRadians) {} + + public static final record Voltages( + LoggedTunableNumber CORAL_INTAKE_VOLTS, + LoggedTunableNumber ALGAE_INTAKE_VOLTS, + LoggedTunableNumber L4_VOLTS, + LoggedTunableNumber SCORE_CORAL_VOLTS, + LoggedTunableNumber SCORE_ALGAE_VOLTS, + LoggedTunableNumber REMOVE_ALGAE, + LoggedTunableNumber HALF_VOLTS, + LoggedTunableNumber L1_VOLTS) {} + + public static record ArmParameters( + DCMotor MOTOR_CONFIG, int NUM_MOTORS, double GEAR_RATIO, double LENGTH_METERS) {} + + @RequiredArgsConstructor + public static enum ManipulatorArmState { + PRE_SCORE(Rotation2d.fromDegrees(25.0).unaryMinus()), + SCORE(Rotation2d.fromDegrees(90.0)), // Placeholder value. Make sure to test + SCORE_L4(Rotation2d.kPi), + PROCESSOR(Rotation2d.fromDegrees(90)), + ALGAE_INTAKE_FLOOR(Rotation2d.fromDegrees(113.378906)), + CORAL_INTAKE_FLOOR(Rotation2d.fromDegrees(-99)), + REEF_INTAKE(Rotation2d.fromDegrees(90)), + INTAKE_OUT_LINE(Rotation2d.fromDegrees(61)), + FLOOR_INTAKE(Rotation2d.fromDegrees(73.5)), + STOW_LINE(Rotation2d.fromDegrees(75)), // What is STOW_LINE? + STOW_DOWN(Rotation2d.fromDegrees(88)), + TRANSITION(Rotation2d.fromDegrees(25.0)), // Placeholder value. Make sure to test + VERTICAL_UP(Rotation2d.fromDegrees(0)), + HANDOFF(Rotation2d.kPi), + FLIPPED_SCORE(Rotation2d.fromDegrees(-270)), + WINDMILL_ANGLE(Rotation2d.fromDegrees(-90)), + SAFE_ANGLE(Rotation2d.fromDegrees(150)), + FLIP_ANGLE(Rotation2d.fromDegrees(135)), + ALGAE_SCORE(Rotation2d.fromDegrees(25)), + INVERSE_FLIP_ANGLE(Rotation2d.fromDegrees(135).unaryMinus()), + EMERGENCY_EJECT_ANGLE( + Rotation2d.fromDegrees(90)); // Idk if tested. Looks fine but double check. + + private final Rotation2d angle; + + public Rotation2d setRotationDirection(boolean direction) { + if (direction == true) { + return angle; + } else { + return angle.minus(Rotation2d.kPi).minus(Rotation2d.kPi); + } + } + + public Rotation2d getAngle(RobotState.ScoreSide scoreSide) { + if (scoreSide.equals(ScoreSide.RIGHT)) { + return angle; + } else if (scoreSide.equals(ScoreSide.LEFT)) { + return angle.unaryMinus(); + } else { + return angle; + } + } + } + + // Will add more states later + @RequiredArgsConstructor + public static enum ManipulatorRollerState { + STOP(0.0), + CORAL_INTAKE(-12.0), + ALGAE_INTAKE(-12.0), + L4_SCORE(4.6 * 1.56), + SCORE_CORAL(4.8 * 1.56), + SCORE_ALGAE(12), + REMOVE_ALGAE(-12), + L1_SCORE(3.5 * 1.56), + ALGAE_HOLD(-12); + + private final double voltage; + + /* + * ManipulatorRollerState(double voltage) { + * this.voltage = voltage; + * } + */ + + public double getVoltage() { + return voltage; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIO.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIO.java new file mode 100644 index 00000000..6fdf83eb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIO.java @@ -0,0 +1,81 @@ +package frc.robot.subsystems.v3_Poot.superstructure.manipulator; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface V3_PootManipulatorIO { + + @AutoLog + public static class ManipulatorIOInputs { + public Rotation2d armPosition = new Rotation2d(); + public double armVelocityRadiansPerSecond = 0.0; + public double armAppliedVolts = 0.0; + public double armSupplyCurrentAmps = 0.0; + public double armTorqueCurrentAmps = 0.0; + public double armTemperatureCelsius = 0.0; + + public Rotation2d armPositionGoal = new Rotation2d(); + public Rotation2d armPositionSetpoint = new Rotation2d(); + public Rotation2d armPositionError = new Rotation2d(); + + public Rotation2d rollerPosition = new Rotation2d(); + public double rollerVelocityRadiansPerSecond = 0.0; + public double rollerAppliedVolts = 0.0; + public double rollerSupplyCurrentAmps = 0.0; + public double rollerTorqueCurrentAmps = 0.0; + public double rollerTemperatureCelsius = 0.0; + + public boolean canRangeGot = false; + } + + /** + * Updates the inputs for the manipulator subsystem . + * + * @param inputs The inputs to update. + */ + public default void updateInputs(ManipulatorIOInputs inputs) {} + + public default void setPosition() {} + + /** + * Sets the voltage for the manipulator. + * + * @param volts The voltage to set. + */ + public default void setArmVoltage(double volts) {} + + public default void setRollerVoltage(double volts) {} + + public default void setArmGoal(Rotation2d rotation) {} + + public default void setManipulatorState(V3_PootManipulatorConstants.ManipulatorArmState state) {} + + /** + * Sets the gains for the arm. + * + * @param kP The proportional gain. + * @param kD The derivative gain. + * @param kS The static gain. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param kG The gravity gain. + */ + public default void updateSlot0ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) {} + + public default void updateSlot1ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) {} + + public default void updateSlot2ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) {} + + /** + * Sets the constraints for the arm. + * + * @param maxAcceleration The maximum acceleration. + * @param cruisingVelocity The cruising velocity. + */ + public default void updateArmConstraints(double maxAcceleration, double cruisingVelocity) {} + + public default void setSlot(int slot) {} +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOSim.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOSim.java new file mode 100644 index 00000000..fca303bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOSim.java @@ -0,0 +1,204 @@ +package frc.robot.subsystems.v3_Poot.superstructure.manipulator; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +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.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants; + +public class V3_PootManipulatorIOSim implements V3_PootManipulatorIO { + private final SingleJointedArmSim armMotorSim; + private final DCMotorSim rollerMotorSim; + + private final ProfiledPIDController armFeedbackController; + private final ArmFeedforward armFeedforwardController; + + private double[] slot0; + private double[] slot1; + private double[] slot2; + + private double armAppliedVolts; + private double rollerAppliedVolts; + + private boolean isClosedLoop; + + public V3_PootManipulatorIOSim() { + armMotorSim = + new SingleJointedArmSim( + LinearSystemId.createSingleJointedArmSystem( + V3_PootManipulatorConstants.ARM_PARAMETERS.MOTOR_CONFIG(), + 0.4389150229, + V3_PootManipulatorConstants.ARM_PARAMETERS.GEAR_RATIO()), + V3_PootManipulatorConstants.ARM_PARAMETERS.MOTOR_CONFIG(), + V3_PootManipulatorConstants.ARM_PARAMETERS.GEAR_RATIO(), + V3_PootManipulatorConstants.ARM_PARAMETERS.LENGTH_METERS(), + Double.NEGATIVE_INFINITY, + Double.POSITIVE_INFINITY, + true, + 0.0); + rollerMotorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(1), 0.4389150229, 3.0), + DCMotor.getKrakenX60Foc(1)); + + slot0 = + new double[] { + V3_PootManipulatorConstants.EMPTY_GAINS.kP().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kD().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kS().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kV().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kA().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kG().get() + }; + slot1 = + new double[] { + V3_PootManipulatorConstants.CORAL_GAINS.kP().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kD().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kS().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kV().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kA().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kG().get() + }; + slot2 = + new double[] { + V3_PootManipulatorConstants.ALGAE_GAINS.kP().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kD().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kS().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kV().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kA().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kG().get() + }; + armFeedbackController = + new ProfiledPIDController( + V3_PootManipulatorConstants.EMPTY_GAINS.kP().get(), + 0.0, + V3_PootManipulatorConstants.EMPTY_GAINS.kD().get(), + new Constraints( + V3_PootManipulatorConstants.CONSTRAINTS.cruisingVelocityRotationsPerSecond().get(), + V3_PootManipulatorConstants.CONSTRAINTS + .maxAccelerationRotationsPerSecondSquared() + .get())); + armFeedforwardController = + new ArmFeedforward( + V3_PootManipulatorConstants.EMPTY_GAINS.kS().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kG().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kV().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kA().get()); + armFeedbackController.enableContinuousInput( + -Math.PI, Math.PI); // Wrap around at -180 and 180 degrees + + armAppliedVolts = 0.0; + rollerAppliedVolts = 0.0; + isClosedLoop = true; + } + + @Override + public void updateInputs(ManipulatorIOInputs inputs) { + if (isClosedLoop) { + armAppliedVolts = + armFeedbackController.calculate(armMotorSim.getAngleRads()) + + armFeedforwardController.calculate( + armMotorSim.getAngleRads(), armMotorSim.getVelocityRadPerSec()); + } + + armAppliedVolts = MathUtil.clamp(armAppliedVolts, -12.0, 12.0); + rollerAppliedVolts = MathUtil.clamp(rollerAppliedVolts, -12.0, 12.0); + + armMotorSim.setInputVoltage(armAppliedVolts); + armMotorSim.update(Constants.LOOP_PERIOD_SECONDS); + + rollerMotorSim.setInputVoltage(rollerAppliedVolts); + rollerMotorSim.update(Constants.LOOP_PERIOD_SECONDS); + + inputs.armPosition = Rotation2d.fromRadians(armMotorSim.getAngleRads()); + inputs.armVelocityRadiansPerSecond = armMotorSim.getVelocityRadPerSec(); + inputs.armAppliedVolts = armAppliedVolts; + inputs.armSupplyCurrentAmps = armMotorSim.getCurrentDrawAmps(); + + inputs.armPositionGoal = Rotation2d.fromRadians(armFeedbackController.getGoal().position); + inputs.armPositionSetpoint = + Rotation2d.fromRadians(armFeedbackController.getSetpoint().position); + inputs.armPositionError = Rotation2d.fromRadians(armFeedbackController.getPositionError()); + + inputs.rollerPosition = new Rotation2d(rollerMotorSim.getAngularPositionRad()); + inputs.rollerVelocityRadiansPerSecond = rollerMotorSim.getAngularVelocityRadPerSec(); + inputs.rollerAppliedVolts = rollerAppliedVolts; + inputs.rollerSupplyCurrentAmps = rollerMotorSim.getCurrentDrawAmps(); + } + + public void setArmVoltage(double volts) { + isClosedLoop = false; + armAppliedVolts = volts; + } + + public void setRollerVoltage(double volts) { + rollerAppliedVolts = volts; + } + + public void setArmGoal(Rotation2d rotation) { + isClosedLoop = true; + armFeedbackController.setGoal(rotation.getRadians()); + } + + public void updateSlot0ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + slot0[0] = kP; + slot0[1] = kD; + slot0[2] = kS; + slot0[3] = kV; + slot0[4] = kA; + slot0[5] = kG; + } + + public void updateSlot1ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + slot1[0] = kP; + slot1[1] = kD; + slot1[2] = kS; + slot1[3] = kV; + slot1[4] = kA; + slot1[5] = kG; + } + + public void updateSlot2ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + slot2[0] = kP; + slot2[1] = kD; + slot2[2] = kS; + slot2[3] = kV; + slot2[4] = kA; + slot2[5] = kG; + } + + public void setSlot(int slot) { + if (slot >= 0 && slot <= 2) { + double[] gains; + if (slot == 0) { + gains = slot0; + } else if (slot == 1) { + gains = slot1; + } else { + gains = slot2; + } + armFeedbackController.setPID(gains[0], 0.0, gains[1]); + armFeedforwardController.setKa(gains[4]); + armFeedforwardController.setKs(gains[2]); + armFeedforwardController.setKv(gains[3]); + armFeedforwardController.setKg(gains[5]); + } else { + throw new IllegalArgumentException("Slot must be between 0 and 2"); + } + } + + public void updateArmConstraints( + double cruisingVelocityRotationsPerSecond, double maxAccelerationRotationsPerSecondSquared) { + armFeedbackController.setConstraints( + new Constraints( + cruisingVelocityRotationsPerSecond, maxAccelerationRotationsPerSecondSquared)); + } +} diff --git a/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOTalonFX.java new file mode 100644 index 00000000..7d803533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/v3_Poot/superstructure/manipulator/V3_PootManipulatorIOTalonFX.java @@ -0,0 +1,312 @@ +package frc.robot.subsystems.v3_Poot.superstructure.manipulator; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.Slot2Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANrange; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.util.PhoenixUtil; +import java.util.ArrayList; + +public class V3_PootManipulatorIOTalonFX implements V3_PootManipulatorIO { + + private final TalonFX armTalonFX; + private final StatusSignal armPositionRotations; + private final StatusSignal armVelocityRotationsPerSecond; + private final StatusSignal armAppliedVoltage; + private final StatusSignal armSupplyCurrentAmps; + private final StatusSignal armTorqueCurrentAmps; + private final StatusSignal armTemperatureCelsius; + private final StatusSignal armPositionSetpointRotations; + private final StatusSignal armPositionErrorRotations; + + private final VoltageOut armVoltageRequest; + private final MotionMagicVoltage armMotionMagicRequest; + + private final TalonFXConfiguration armConfig; + + private final TalonFX rollerTalonFX; + private final StatusSignal rollerPositionRotations; + private final StatusSignal rollerVelocityRotationsPerSecond; + private final StatusSignal rollerAppliedVoltage; + private final StatusSignal rollerSupplyCurrentAmps; + private final StatusSignal rollerTorqueCurrentAmps; + private final StatusSignal rollerTemperatureCelsius; + + private StatusSignal[] statusSignals; + + private final VoltageOut rollerVoltageRequest; + private final TalonFXConfiguration rollerConfig; + + private final CANrange canRange; + private final StatusSignal isDetected; + + public V3_PootManipulatorIOTalonFX() { + armTalonFX = new TalonFX(V3_PootManipulatorConstants.ARM_CAN_ID); + rollerTalonFX = new TalonFX(V3_PootManipulatorConstants.ROLLER_CAN_ID); + canRange = new CANrange(V3_PootManipulatorConstants.CAN_RANGE_ID); + + armConfig = new TalonFXConfiguration(); + + armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + armConfig.CurrentLimits.SupplyCurrentLimit = + V3_PootManipulatorConstants.CURRENT_LIMITS.MANIPULATOR_SUPPLY_CURRENT_LIMIT(); + armConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + armConfig.CurrentLimits.StatorCurrentLimit = + V3_PootManipulatorConstants.CURRENT_LIMITS.MANIPULATOR_STATOR_CURRENT_LIMIT(); + armConfig.CurrentLimits.StatorCurrentLimitEnable = true; + armConfig.Feedback.SensorToMechanismRatio = + V3_PootManipulatorConstants.ARM_PARAMETERS.GEAR_RATIO(); + armConfig.Slot0 = + Slot0Configs.from(V3_PootManipulatorConstants.EMPTY_GAINS.toTalonFXSlotConfigs()); + armConfig.Slot1 = + Slot1Configs.from(V3_PootManipulatorConstants.CORAL_GAINS.toTalonFXSlotConfigs()); + armConfig.Slot2 = + Slot2Configs.from(V3_PootManipulatorConstants.ALGAE_GAINS.toTalonFXSlotConfigs()); + armConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + armConfig.ClosedLoopGeneral.ContinuousWrap = true; + armConfig.MotionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration( + AngularAcceleration.ofRelativeUnits( + V3_PootManipulatorConstants.CONSTRAINTS + .maxAccelerationRotationsPerSecondSquared() + .get(), + RotationsPerSecondPerSecond)) + .withMotionMagicCruiseVelocity( + AngularVelocity.ofRelativeUnits( + V3_PootManipulatorConstants.CONSTRAINTS + .cruisingVelocityRotationsPerSecond() + .get(), + RotationsPerSecond)); + + tryUntilOk(5, () -> armTalonFX.getConfigurator().apply(armConfig, 0.25)); + + armPositionRotations = armTalonFX.getPosition(); + armVelocityRotationsPerSecond = armTalonFX.getVelocity(); + armAppliedVoltage = armTalonFX.getMotorVoltage(); + armSupplyCurrentAmps = armTalonFX.getSupplyCurrent(); + armTorqueCurrentAmps = armTalonFX.getTorqueCurrent(); + armTemperatureCelsius = armTalonFX.getDeviceTemp(); + + armPositionSetpointRotations = armTalonFX.getClosedLoopReference(); + armPositionErrorRotations = armTalonFX.getClosedLoopError(); + + rollerConfig = new TalonFXConfiguration(); + rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rollerConfig.CurrentLimits.SupplyCurrentLimit = + V3_PootManipulatorConstants.CURRENT_LIMITS.ROLLER_SUPPLY_CURRENT_LIMIT(); + rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + tryUntilOk(5, () -> rollerTalonFX.getConfigurator().apply(rollerConfig, 0.25)); + + rollerPositionRotations = rollerTalonFX.getPosition(); + rollerVelocityRotationsPerSecond = rollerTalonFX.getVelocity(); + rollerAppliedVoltage = rollerTalonFX.getMotorVoltage(); + rollerSupplyCurrentAmps = rollerTalonFX.getSupplyCurrent(); + rollerTorqueCurrentAmps = rollerTalonFX.getTorqueCurrent(); + rollerTemperatureCelsius = rollerTalonFX.getDeviceTemp(); + + rollerVoltageRequest = new VoltageOut(0); + armVoltageRequest = new VoltageOut(0); + armMotionMagicRequest = new MotionMagicVoltage(0); + + isDetected = canRange.getIsDetected(); + + var signalsList = new ArrayList>(); + + signalsList.add(armPositionRotations); + signalsList.add(armVelocityRotationsPerSecond); + signalsList.add(armAppliedVoltage); + signalsList.add(armSupplyCurrentAmps); + signalsList.add(armTorqueCurrentAmps); + signalsList.add(armTemperatureCelsius); + signalsList.add(armPositionSetpointRotations); + signalsList.add(armPositionErrorRotations); + signalsList.add(rollerPositionRotations); + signalsList.add(rollerVelocityRotationsPerSecond); + signalsList.add(rollerAppliedVoltage); + signalsList.add(rollerSupplyCurrentAmps); + signalsList.add(rollerTorqueCurrentAmps); + signalsList.add(rollerTemperatureCelsius); + signalsList.add(isDetected); + + statusSignals = new StatusSignal[signalsList.size()]; + + for (int i = 0; i < signalsList.size(); i++) { + statusSignals[i] = signalsList.get(i); + } + + BaseStatusSignal.setUpdateFrequencyForAll(50, statusSignals); + + armTalonFX.optimizeBusUtilization(); + rollerTalonFX.optimizeBusUtilization(); + canRange.optimizeBusUtilization(); + + PhoenixUtil.registerSignals(false, statusSignals); + + armTalonFX.setPosition(0); + } + + /** + * Updates the inputs of the manipulator with the current state of the TalonFXs. + * + * @param inputs the inputs to update + */ + @Override + public void updateInputs(ManipulatorIOInputs inputs) { + + inputs.armPosition = new Rotation2d(armPositionRotations.getValue()); + inputs.armVelocityRadiansPerSecond = + armVelocityRotationsPerSecond.getValue().in(RadiansPerSecond); + inputs.armAppliedVolts = armAppliedVoltage.getValueAsDouble(); + inputs.armSupplyCurrentAmps = armSupplyCurrentAmps.getValueAsDouble(); + inputs.armTorqueCurrentAmps = armTorqueCurrentAmps.getValueAsDouble(); + inputs.armTemperatureCelsius = armTemperatureCelsius.getValueAsDouble(); + + inputs.rollerPosition = new Rotation2d(rollerPositionRotations.getValue()); + inputs.rollerVelocityRadiansPerSecond = + Units.rotationsToRadians(rollerVelocityRotationsPerSecond.getValueAsDouble()); + inputs.rollerAppliedVolts = rollerAppliedVoltage.getValueAsDouble(); + inputs.rollerSupplyCurrentAmps = rollerSupplyCurrentAmps.getValueAsDouble(); + inputs.rollerTorqueCurrentAmps = rollerTorqueCurrentAmps.getValueAsDouble(); + inputs.rollerTemperatureCelsius = rollerTemperatureCelsius.getValueAsDouble(); + + inputs.armPositionGoal = new Rotation2d(armMotionMagicRequest.getPositionMeasure()); + inputs.armPositionSetpoint = + Rotation2d.fromRotations(armPositionSetpointRotations.getValueAsDouble()); + inputs.armPositionError = + Rotation2d.fromRotations(armPositionErrorRotations.getValueAsDouble()); + + inputs.canRangeGot = isDetected.getValue(); + } + + /** + * Sets the voltage of the arm TalonFX to the specified value. The voltage is set in terms of + * volts, with positive values corresponding to clockwise rotation and negative values + * corresponding to counterclockwise rotation. This method is used to control the velocity of the + * arm, which is useful for tasks such as picking up objects or depositing objects. + * + * @param volts the voltage to set, in volts + */ + @Override + public void setArmVoltage(double volts) { + armTalonFX.setControl(armVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + /** + * Sets the voltage of the roller TalonFX to the specified value. The voltage is set in terms of + * volts, with positive values corresponding to clockwise rotation and negative values + * corresponding to counterclockwise rotation. This method is used to control the velocity of the + * roller, which is useful for tasks such as picking up objects or depositing objects. + */ + @Override + public void setRollerVoltage(double volts) { + rollerTalonFX.setControl(rollerVoltageRequest.withOutput(volts).withEnableFOC(true)); + } + + /** + * The position is set in terms of rotations of the TalonFX's motor shaft. This method is used to + * set the manipulator arm to a specific position, which is useful for tasks such as picking up + * objects or depositing objects. + * + * @param rotation The desired position of the manipulator arm, in terms of rotations of the + * TalonFX's motor shaft. + */ + @Override + public void setArmGoal(Rotation2d rotation) { + armTalonFX.setControl( + armMotionMagicRequest.withPosition(rotation.getRotations()).withEnableFOC(true)); + } + + /** + * Sets the current slot of the manipulator arm based on the current state of the subsystem. If + * the subsystem has algae, it sets the slot to 2. If the subsystem has coral, it sets the slot to + * 1. Otherwise, it sets the slot to 0. + * + * @param slot The slot to set the arm to. + * @throws IllegalArgumentException If the slot is not between 0 and 2, inclusive. + */ + @Override + public void setSlot(int slot) { + if (slot >= 0 && slot <= 2) { + armTalonFX.setControl(armMotionMagicRequest.withSlot(slot)); + } else { + throw new IllegalArgumentException("Invalid slot: " + slot); + } + } + + public void updateSlot0ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + armConfig.Slot0 = + new Slot0Configs() + .withKP(kP) + .withKD(kD) + .withKS(kS) + .withKV(kV) + .withKA(kA) + .withKG(kG) + .withGravityType(GravityTypeValue.Arm_Cosine); + tryUntilOk(5, () -> armTalonFX.getConfigurator().apply(armConfig, 0.25)); + } + + public void updateSlot1ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + armConfig.Slot1 = + new Slot1Configs() + .withKP(kP) + .withKD(kD) + .withKS(kS) + .withKV(kV) + .withKA(kA) + .withKG(kG) + .withGravityType(GravityTypeValue.Arm_Cosine); + tryUntilOk(5, () -> armTalonFX.getConfigurator().apply(armConfig, 0.25)); + } + + public void updateSlot2ArmGains( + double kP, double kD, double kS, double kV, double kA, double kG) { + armConfig.Slot2 = + new Slot2Configs() + .withKP(kP) + .withKD(kD) + .withKS(kS) + .withKV(kV) + .withKA(kA) + .withKG(kG) + .withGravityType(GravityTypeValue.Arm_Cosine); + tryUntilOk(5, () -> armTalonFX.getConfigurator().apply(armConfig, 0.25)); + } + + public void updateArmConstraints(double maxAcceleration, double cruisingVelocity) { + armConfig.MotionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration( + AngularAcceleration.ofRelativeUnits(maxAcceleration, RotationsPerSecondPerSecond)) + .withMotionMagicCruiseVelocity( + AngularVelocity.ofRelativeUnits(cruisingVelocity, RotationsPerSecond)); + tryUntilOk(5, () -> armTalonFX.getConfigurator().apply(armConfig, 0.25)); + } +} diff --git a/src/main/java/frc/robot/util/GeometryUtil.java b/src/main/java/frc/robot/util/GeometryUtil.java index cf8249f5..559d9765 100644 --- a/src/main/java/frc/robot/util/GeometryUtil.java +++ b/src/main/java/frc/robot/util/GeometryUtil.java @@ -1,9 +1,15 @@ package frc.robot.util; +import choreo.trajectory.SwerveSample; +import choreo.trajectory.Trajectory; 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 frc.robot.FieldConstants; +import frc.robot.util.LoggedChoreo.LoggedAutoTrajectory; +import java.util.ArrayList; +import java.util.List; public class GeometryUtil { /** @@ -67,4 +73,48 @@ public static final boolean isZero(Translation2d translation) { public static final boolean isZero(Rotation2d rotation) { return rotation.getDegrees() == 0.0; } + + public static final Trajectory mirrorTrajectory( + LoggedAutoTrajectory inputAutoPath) { + List mirroredSamples = new ArrayList<>(); + Trajectory trajectory = inputAutoPath.getRawTrajectory(); + trajectory + .samples() + .forEach( + sample -> { + mirroredSamples.add( + new SwerveSample( + sample.t, + sample.x, + FieldConstants.fieldWidth - (sample.y), + -sample.heading, + sample.vx, + -sample.vy, + -sample.omega, + sample.ax, + -sample.ay, + -sample.alpha, + // TODO: VERIFY THIS + // FL, FR, BL, BR + // Mirrored + // FR, FL, BR, BL + new double[] { + sample.moduleForcesX()[1], + sample.moduleForcesX()[0], + sample.moduleForcesX()[3], + sample.moduleForcesX()[2] + }, + // FL, FR, BL, BR + // Mirrored + // -FR, -FL, -BR, -BL + new double[] { + -sample.moduleForcesY()[1], + -sample.moduleForcesY()[0], + -sample.moduleForcesY()[3], + -sample.moduleForcesY()[2] + })); + }); + return new Trajectory( + trajectory.name(), mirroredSamples, trajectory.splits(), trajectory.events()); + } } diff --git a/src/main/java/frc/robot/util/LTNUpdater.java b/src/main/java/frc/robot/util/LTNUpdater.java index 3a1c2db9..5aaaabc1 100644 --- a/src/main/java/frc/robot/util/LTNUpdater.java +++ b/src/main/java/frc/robot/util/LTNUpdater.java @@ -13,11 +13,15 @@ import frc.robot.subsystems.v2_Redundancy.superstructure.intake.V2_RedundancyIntakeConstants; import frc.robot.subsystems.v2_Redundancy.superstructure.manipulator.V2_RedundancyManipulator; import frc.robot.subsystems.v2_Redundancy.superstructure.manipulator.V2_RedundancyManipulatorConstants; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntake; +import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator; +import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants; public class LTNUpdater { - public static final void updateDrive(Drive drive) { - LoggedTunableNumber.ifChanged( - drive.hashCode(), + + public static void registerDrive(Drive drive) { + LoggedTunableNumber.createGroup( () -> { drive.setPIDGains( DriveConstants.GAINS.drive_Kp().get(), @@ -34,8 +38,7 @@ public static final void updateDrive(Drive drive) { DriveConstants.GAINS.turn_Kp(), DriveConstants.GAINS.turn_Kd()); - LoggedTunableNumber.ifChanged( - drive.hashCode(), + LoggedTunableNumber.createGroup( () -> { DriveCommands.setRotationPID( DriveConstants.AUTO_ALIGN_GAINS.rotation_Kp().get(), @@ -50,9 +53,8 @@ public static final void updateDrive(Drive drive) { DriveConstants.AUTO_ALIGN_GAINS.translation_Kd()); } - public static final void updateElevator(ElevatorCSB elevator) { - LoggedTunableNumber.ifChanged( - elevator.hashCode(), + public static void registerElevator(ElevatorCSB elevator) { + LoggedTunableNumber.createGroup( () -> { elevator.setGains( ElevatorConstants.GAINS.kP().get(), @@ -75,9 +77,8 @@ public static final void updateElevator(ElevatorCSB elevator) { ElevatorConstants.CONSTRAINTS.cruisingVelocityMetersPerSecond()); } - public static final void updateElevator(ElevatorFSM elevator) { - LoggedTunableNumber.ifChanged( - elevator.hashCode(), + public static void registerElevator(ElevatorFSM elevator) { + LoggedTunableNumber.createGroup( () -> { elevator.setGains( ElevatorConstants.GAINS.kP().get(), @@ -100,9 +101,8 @@ public static final void updateElevator(ElevatorFSM elevator) { ElevatorConstants.CONSTRAINTS.cruisingVelocityMetersPerSecond()); } - public static final void updateFunnel(FunnelCSB funnel) { - LoggedTunableNumber.ifChanged( - funnel.hashCode(), + public static void registerFunnel(FunnelCSB funnel) { + LoggedTunableNumber.createGroup( () -> { funnel.updateGains( FunnelConstants.CLAP_DADDY_MOTOR_GAINS.kP().get(), @@ -123,9 +123,8 @@ public static final void updateFunnel(FunnelCSB funnel) { FunnelConstants.CLAP_DADDY_MOTOR_CONSTRAINTS.MAX_VELOCITY()); } - public static final void updateFunnel(FunnelFSM funnel) { - LoggedTunableNumber.ifChanged( - funnel.hashCode(), + public static void registerFunnel(FunnelFSM funnel) { + LoggedTunableNumber.createGroup( () -> { funnel.updateGains( FunnelConstants.CLAP_DADDY_MOTOR_GAINS.kP().get(), @@ -146,9 +145,8 @@ public static final void updateFunnel(FunnelFSM funnel) { FunnelConstants.CLAP_DADDY_MOTOR_CONSTRAINTS.MAX_VELOCITY()); } - public static final void updateAlgaeArm(V2_RedundancyManipulator manipulator) { - LoggedTunableNumber.ifChanged( - manipulator.hashCode(), + public static void registerAlgaeArm(V2_RedundancyManipulator manipulator) { + LoggedTunableNumber.createGroup( () -> { manipulator.updateArmGains( V2_RedundancyManipulatorConstants.WITHOUT_ALGAE_GAINS.kP().get(), @@ -189,9 +187,8 @@ public static final void updateAlgaeArm(V2_RedundancyManipulator manipulator) { .MAX_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED()); } - public static final void updateIntake(V2_RedundancyIntake intake) { - LoggedTunableNumber.ifChanged( - intake.hashCode(), + public static void registerIntake(V2_RedundancyIntake intake) { + LoggedTunableNumber.createGroup( () -> { intake.updateGains( V2_RedundancyIntakeConstants.EXTENSION_MOTOR_GAINS.kP().get(), @@ -211,4 +208,114 @@ public static final void updateIntake(V2_RedundancyIntake intake) { V2_RedundancyIntakeConstants.EXTENSION_MOTOR_CONSTRAINTS.MAX_ACCELERATION(), V2_RedundancyIntakeConstants.EXTENSION_MOTOR_CONSTRAINTS.MAX_VELOCITY()); } + + public static void registerIntake(V3_PootIntake intake) { + LoggedTunableNumber.createGroup( + () -> { + intake.updateIntakeGains( + V3_PootIntakeConstants.PIVOT_GAINS.kP().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kD().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kS().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kV().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kA().get(), + V3_PootIntakeConstants.PIVOT_GAINS.kG().get()); + intake.updateIntakeConstraints( + V3_PootIntakeConstants.PIVOT_CONSTRAINTS + .MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED() + .get(), + V3_PootIntakeConstants.PIVOT_CONSTRAINTS + .CRUISING_VELOCITY_RADIANS_PER_SECOND() + .get()); + }, + V3_PootIntakeConstants.PIVOT_GAINS.kP(), + V3_PootIntakeConstants.PIVOT_GAINS.kD(), + V3_PootIntakeConstants.PIVOT_GAINS.kS(), + V3_PootIntakeConstants.PIVOT_GAINS.kV(), + V3_PootIntakeConstants.PIVOT_GAINS.kA(), + V3_PootIntakeConstants.PIVOT_GAINS.kG(), + V3_PootIntakeConstants.PIVOT_CONSTRAINTS.MAX_ACCELERATION_RADIANS_PER_SECOND_SQUARED(), + V3_PootIntakeConstants.PIVOT_CONSTRAINTS.CRUISING_VELOCITY_RADIANS_PER_SECOND()); + } + + public static void registerManipulatorArm(V3_PootManipulator manipulator) { + LoggedTunableNumber.createGroup( + () -> { + manipulator.updateArmGains( + V3_PootManipulatorConstants.EMPTY_GAINS.kP().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kD().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kS().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kV().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kA().get(), + V3_PootManipulatorConstants.EMPTY_GAINS.kG().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kP().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kD().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kS().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kV().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kA().get(), + V3_PootManipulatorConstants.CORAL_GAINS.kG().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kP().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kD().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kS().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kV().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kA().get(), + V3_PootManipulatorConstants.ALGAE_GAINS.kG().get()); + manipulator.updateArmConstraints( + V3_PootManipulatorConstants.CONSTRAINTS + .maxAccelerationRotationsPerSecondSquared() + .get(), + V3_PootManipulatorConstants.CONSTRAINTS.cruisingVelocityRotationsPerSecond().get()); + }, + V3_PootManipulatorConstants.ALGAE_GAINS.kP(), + V3_PootManipulatorConstants.ALGAE_GAINS.kD(), + V3_PootManipulatorConstants.ALGAE_GAINS.kS(), + V3_PootManipulatorConstants.ALGAE_GAINS.kV(), + V3_PootManipulatorConstants.ALGAE_GAINS.kA(), + V3_PootManipulatorConstants.ALGAE_GAINS.kG(), + V3_PootManipulatorConstants.EMPTY_GAINS.kP(), + V3_PootManipulatorConstants.EMPTY_GAINS.kD(), + V3_PootManipulatorConstants.EMPTY_GAINS.kS(), + V3_PootManipulatorConstants.EMPTY_GAINS.kV(), + V3_PootManipulatorConstants.EMPTY_GAINS.kA(), + V3_PootManipulatorConstants.EMPTY_GAINS.kG(), + V3_PootManipulatorConstants.CORAL_GAINS.kP(), + V3_PootManipulatorConstants.CORAL_GAINS.kD(), + V3_PootManipulatorConstants.CORAL_GAINS.kS(), + V3_PootManipulatorConstants.CORAL_GAINS.kV(), + V3_PootManipulatorConstants.CORAL_GAINS.kA(), + V3_PootManipulatorConstants.CORAL_GAINS.kG(), + V3_PootManipulatorConstants.CONSTRAINTS.maxAccelerationRotationsPerSecondSquared(), + V3_PootManipulatorConstants.CONSTRAINTS.cruisingVelocityRotationsPerSecond(), + V3_PootManipulatorConstants.CONSTRAINTS.goalToleranceRadians()); + } + + /** + * Register all subsystem auto-updates. Call this once during robot initialization. Then call + * LoggedTunableNumber.updateAll() periodically in robotPeriodic(). + */ + public static void registerAll(Drive drive, ElevatorCSB elevator, FunnelCSB funnel) { + registerDrive(drive); + registerElevator(elevator); + registerFunnel(funnel); + } + + public static void registerAll( + Drive drive, + ElevatorFSM elevator, + FunnelFSM funnel, + V2_RedundancyIntake intake, + V2_RedundancyManipulator manipulator) { + registerDrive(drive); + registerElevator(elevator); + registerFunnel(funnel); + registerIntake(intake); + registerAlgaeArm(manipulator); + } + + public static void registerAll( + Drive drive, ElevatorFSM elevator, V3_PootIntake intake, V3_PootManipulator manipulator) { + registerDrive(drive); + registerElevator(elevator); + registerIntake(intake); + registerManipulatorArm(manipulator); + } } diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index a9956984..2a0b848c 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -8,8 +8,10 @@ package frc.robot.util; import frc.robot.Constants; +import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; +import java.util.List; import java.util.Map; import java.util.function.Consumer; import java.util.function.DoubleSupplier; @@ -17,16 +19,20 @@ /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or - * value not in dashboard. + * value not in dashboard. Automatically executes callbacks when values change. */ public class LoggedTunableNumber implements DoubleSupplier { private static final String tableKey = "TunableNumbers"; + private static final ArrayList strayTunableNumbers = new ArrayList<>(); + private static final List autoUpdateGroups = new ArrayList<>(); private final String key; private boolean hasDefault = false; private double defaultValue; private LoggedNetworkNumber dashboardNumber; private Map lastHasChangedValues = new HashMap<>(); + private double lastKnownValue = Double.NaN; + private List callbacks = new ArrayList<>(); /** * Create a new LoggedTunableNumber @@ -35,6 +41,7 @@ public class LoggedTunableNumber implements DoubleSupplier { */ public LoggedTunableNumber(String dashboardKey) { this.key = tableKey + "/" + dashboardKey; + strayTunableNumbers.add(this); } /** @@ -120,4 +127,103 @@ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tun public double getAsDouble() { return get(); } + + /** + * Register a callback to be executed when this tunable number changes + * + * @param callback The callback to execute when value changes + */ + public void onChange(Runnable callback) { + callbacks.add(callback); + } + + /** Check if value has changed and execute callbacks if so */ + private void checkAndUpdate() { + double currentValue = get(); + if (Double.isNaN(lastKnownValue) || currentValue != lastKnownValue) { + lastKnownValue = currentValue; + callbacks.forEach(Runnable::run); + } + } + + /** + * Create a group of tunable numbers that will execute a callback when any of them change + * + * @param callback The callback to execute when any tunable number in the group changes + * @param tunableNumbers The tunable numbers to monitor + * @return AutoUpdateGroup for further configuration if needed + */ + public static AutoUpdateGroup createGroup( + Runnable callback, LoggedTunableNumber... tunableNumbers) { + AutoUpdateGroup group = new AutoUpdateGroup(callback, tunableNumbers); + autoUpdateGroups.add(group); + strayTunableNumbers.removeAll(Arrays.asList(tunableNumbers)); + return group; + } + + /** + * Create a group of tunable numbers that will execute a callback with values when any of them + * change + * + * @param callback The callback to execute with current values when any tunable number changes + * @param tunableNumbers The tunable numbers to monitor + * @return AutoUpdateGroup for further configuration if needed + */ + public static AutoUpdateGroup createGroup( + Consumer callback, LoggedTunableNumber... tunableNumbers) { + AutoUpdateGroup group = + new AutoUpdateGroup( + () -> + callback.accept( + Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()), + tunableNumbers); + autoUpdateGroups.add(group); + return group; + } + + /** + * Call this method periodically (e.g., in robotPeriodic) to check for changes and execute + * callbacks + */ + public static void updateAll() { + if (Constants.TUNING_MODE) { + // Update un grouped tunable numbers + strayTunableNumbers.forEach(LoggedTunableNumber::checkAndUpdate); + + // Update groups + autoUpdateGroups.forEach(AutoUpdateGroup::checkAndUpdate); + } + } + + /** Helper class for managing groups of tunable numbers */ + public static class AutoUpdateGroup { + private final Runnable callback; + private final LoggedTunableNumber[] tunableNumbers; + private final Map lastValues = new HashMap<>(); + + private AutoUpdateGroup(Runnable callback, LoggedTunableNumber... tunableNumbers) { + this.callback = callback; + this.tunableNumbers = tunableNumbers; + // Initialize last values + for (LoggedTunableNumber number : tunableNumbers) { + lastValues.put(number, number.get()); + } + } + + private void checkAndUpdate() { + boolean hasChanged = false; + for (LoggedTunableNumber number : tunableNumbers) { + double currentValue = number.get(); + Double lastValue = lastValues.get(number); + if (lastValue == null || currentValue != lastValue) { + lastValues.put(number, currentValue); + hasChanged = true; + } + } + + if (hasChanged) { + callback.run(); + } + } + } } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 00000000..65f44133 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.3.14", + "frcYear": "2025", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.3.14" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +}