diff --git a/.gitignore b/.gitignore index 34cbaac..1e690d1 100644 --- a/.gitignore +++ b/.gitignore @@ -185,3 +185,6 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath + +# Generated BuildConstants +src/main/java/frc/robot/BuildConstants.java \ No newline at end of file diff --git a/advantagescope/Robot_Shrieking_Eel/config.json b/advantagescope/Robot_Shrieking_Eel/config.json new file mode 100644 index 0000000..7541330 --- /dev/null +++ b/advantagescope/Robot_Shrieking_Eel/config.json @@ -0,0 +1,124 @@ +{ + "name": "Shrieking Eel", + "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.026, + 0, + -0.055 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + -0.026, + 0, + -0.081 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + -0.071, + 0, + -0.22 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + -0.19, + 0, + -0.74 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + } + ] +} \ No newline at end of file diff --git a/advantagescope/Robot_Shrieking_Eel/model.glb b/advantagescope/Robot_Shrieking_Eel/model.glb new file mode 100644 index 0000000..7a1012a Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_0.glb b/advantagescope/Robot_Shrieking_Eel/model_0.glb new file mode 100644 index 0000000..a0ddc1d Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_0.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_1.glb b/advantagescope/Robot_Shrieking_Eel/model_1.glb new file mode 100644 index 0000000..a13ab09 Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_1.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_2.glb b/advantagescope/Robot_Shrieking_Eel/model_2.glb new file mode 100644 index 0000000..2ca1bfc Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_2.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_3.glb b/advantagescope/Robot_Shrieking_Eel/model_3.glb new file mode 100644 index 0000000..01928da Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_3.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_4.glb b/advantagescope/Robot_Shrieking_Eel/model_4.glb new file mode 100644 index 0000000..a6b4ca2 Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_4.glb differ diff --git a/advantagescope/Robot_Shrieking_Eel/model_5.glb b/advantagescope/Robot_Shrieking_Eel/model_5.glb new file mode 100644 index 0000000..f8a77e4 Binary files /dev/null and b/advantagescope/Robot_Shrieking_Eel/model_5.glb differ diff --git a/advantagescope/layout.json b/advantagescope/layout.json new file mode 100644 index 0000000..38b7074 --- /dev/null +++ b/advantagescope/layout.json @@ -0,0 +1,1201 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1552, + "height": 936, + "state": { + "sidebar": { + "width": 327, + "expanded": [ + "/DS/joystick0/axes", + "/NT/Robot/m_robotContainer", + "/NT/PathPlanner/targetPose/translation", + "/NT/PathPlanner/currentPose/translation", + "/NT/PathPlanner/currentPose/rotation", + "/NT/PathPlanner/targetPose/rotation", + "/NT/Swerve States/0", + "/NT/Swerve Target States/0", + "/NT/Swerve Target States/1", + "/NT/Swerve Target States/1/angle", + "/NT/Swerve States/1", + "/NT/Swerve Target States/0/angle", + "/NT/Swerve States/0/angle", + "/CoralVision/targets/0", + "/CoralVision/targets/1", + "/CoralVision/selectedTargetPose/translation", + "/SmartDashboard/Coral/Roll", + "/DS/joystick0/buttons", + "/SmartDashboard/Scheduler/Names", + "/SmartDashboard/Scheduler/Ids", + "/SmartDashboard/Coral/Pitch", + "/Log1/Phoenix6", + "/Running Commands", + "/NT/PathPlanner/vel", + "/Robot/m_robotContainer/swerveDriveSubsystem/lastChassisSpeeds", + "/Log1/Phoenix6/TalonFX-34", + "/Log2", + "/Log2/Phoenix6", + "/Log2/Phoenix6/TalonFX-34", + "/SmartDashboard/CoralVision", + "/SmartDashboard/CoralVision/raw", + "/SmartDashboard/CoralVision/raw/distances", + "/SmartDashboard/CoralVision/raw/angles", + "/CoralVision/raw/angles", + "/CoralVision/raw/distances", + "/CoralVision/raw", + "/SmartDashboard/Coral", + "/SmartDashboard/Elevator", + "/SmartDashboard/Coral/Pivot", + "/SmartDashboard/Algae", + "/SmartDashboard/Algae/Pivot", + "/NT/CoralVision/targets", + "/NT/limelight-fr/rawtargets", + "/NT/limelight-bf", + "/Version", + "/NT/SmartDashboard/Coral/Pitch", + "/DS/joystick2/axes", + "/SmartDashboard", + "/DS/joystick2/buttons", + "/Phoenix6/CANcoder-28", + "/Log1/NT", + "/Log1/NT/URCL", + "/Log1/NT/URCL/Spark-14", + "/Log1/NT/URCL/Spark-14/Fault", + "/Phoenix6/TalonFX-33", + "/Log1", + "/Log1/Robot", + "/Robot/m_robotContainer", + "/Robot/m_robotContainer/swerveDriveSubsystem", + "/NT/URCL/Raw", + "/NT/SmartDashboard/Auto Chooser", + "/Robot/m_robotContainer/swerveDriveSubsystem/getModulePositions/0", + "/Robot/m_robotContainer/swerveDriveSubsystem/getModulePositions/0/angle", + "/Robot/m_robotContainer/swerveDriveSubsystem/getModulePositions/1", + "/Robot/m_robotContainer/swerveDriveSubsystem/getModulePositions/2", + "/Robot/m_robotContainer/swerveDriveSubsystem/getModulePositions/3", + "/Log1/NT/PathPlanner", + "/Log1/NT/PathPlanner/currentPose", + "/Log1/NT/PathPlanner/targetPose", + "/Log2/NT", + "/Log2/NT/PathPlanner", + "/Log2/NT/PathPlanner/currentPose", + "/Log2/NT/PathPlanner/targetPose", + "/Log2/NT/PathPlanner/targetPose/rotation", + "/NT/URCL", + "/NT/URCL/Spark-4", + "/NT/PathPlanner" + ] + }, + "tabs": { + "selected": 9, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "58qtu5cfzkfp9yjpqos7ayy0r3xjpv1u", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Auto Tracking", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/PathPlanner/targetPose/translation/x", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/targetPose/translation/y", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/currentPose/translation/x", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/currentPose/translation/y", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "Robot/commandSchedulerTime", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "Robot/loopTime", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "DS:enabled", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#858584" + } + }, + { + "type": "stripes", + "logKey": "DS:autonomous", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#3b875a" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Auto Chooser/active", + "logType": "String", + "visible": true, + "options": { + "color": "#d993aa" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "qxbhmb7s4se4vhuwvdjo5q50py8hkuvl", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Swerve Rotation", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/PathPlanner/targetPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/currentPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Log1/NT:/PathPlanner/targetPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Log1/NT:/PathPlanner/currentPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Log2/NT:/PathPlanner/currentPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Log2/NT:/PathPlanner/targetPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/PathPlanner/vel/0", + "logType": "Number", + "visible": false, + "options": { + "color": "#858584", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/vel/1", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "Robot/loopTime", + "logType": "Number", + "visible": false, + "options": { + "color": "#d993aa", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "51edbyu44ebbfcixehuzwgmj1wy033pk", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 9, + "title": "Swerve States", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:Swerve States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:Swerve Target States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "Robot/m_robotContainer/swerveDriveSubsystem/getChassisSpeeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "Robot/m_robotContainer/swerveDriveSubsystem/lastChassisSpeeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#0000ff" + } + }, + { + "type": "rotation", + "logKey": "NT:Odometry Pose/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 4.7, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 0 + }, + "controllerUUID": "p5i7rd873d6ad2d82ngn25t8cede5r1n", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Swerve Module Rotations", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:Swerve States/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:Swerve Target States/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:csTime", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ue3t9x7938p6ig1gluthl0c5oj3j8za8", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/URCL/Spark-4/AppliedOutputVoltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/URCL/Spark-4/PrimaryEncoderVelocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "8rkocjeo2xoeous8ywl6w10mvyyxo2ra", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Swerve Velocity", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:Swerve States/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:Swerve Target States/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/URCL/Spark-1/Voltage", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Log1/Phoenix6/TalonFX-34/MotorVoltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "qgq951ijzzm7ft7i64hqdl7gr3g4dzfp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 2, + "title": "Odometry", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:Odometry Pose", + "logType": "Pose2d", + "visible": true, + "options": {} + }, + { + "type": "swerveStates", + "logKey": "NT:Swerve States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/scoringPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ffff", + "size": "bold" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/selectedTargetPose", + "logType": "Pose3d", + "visible": true, + "options": { + "color": "#ff00ff", + "size": "bold" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/targetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "swerveStates", + "logKey": "NT:Swerve Target States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "trajectory", + "logKey": "NT:/PathPlanner/activePath", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/currentPose", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/targetPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-bl", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-br", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-fr", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#00ff00" + } + } + ], + "game": "2025 Field (Welded)", + "bumpers": "auto", + "origin": "blue", + "orientation": 0, + "size": 30 + }, + "controllerUUID": "qmx56d8q2yb01eyzidcu8tl091od50ei", + "renderer": null, + "controlsHeight": 178 + }, + { + "type": 5, + "title": "Console", + "controller": "console", + "controllerUUID": "q7ut6zgmjs8u4m1wtgp1qmf967jibe1j", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:Odometry Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Shrieking Eel" + } + }, + { + "type": "component", + "logKey": "NT:0_ElevatorS1", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:1_ElevatorS2", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:2_Arm", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:3_Wrist1", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:4_Wrist2", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:5_AlgaeArm", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "vision", + "logKey": "NT:CoralVision/targetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/selectedTargetPose", + "logType": "Pose3d", + "visible": true, + "options": { + "color": "#ff0000", + "size": "bold" + } + }, + { + "type": "gamePiece", + "logKey": "NT:Coral_Pose", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Coral" + } + }, + { + "type": "cone", + "logKey": "NT:CoralVision/scoringPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00", + "position": "front" + } + }, + { + "type": "axes", + "logKey": "NT:CoralVision/cameraPoseFieldSpace", + "logType": "Pose3d", + "visible": false, + "options": {} + }, + { + "type": "gamePiece", + "logKey": "NT:Algae_Pose", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Algae" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/targetPose", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Shrieking Eel", + "color": "#00ff00" + } + }, + { + "type": "trajectory", + "logKey": "NT:/PathPlanner/activePath", + "logType": "Pose2d[]", + "visible": false, + "options": { + "color": "#00ff00", + "size": "normal" + } + } + ], + "game": "2025 Field (Welded)", + "origin": "blue" + }, + "controllerUUID": "kjouwy32sie8360d06yldv6qk8xeb42t", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + -13.040848666951174, + 6.063738914406409, + 0.6721729488244716 + ], + "cameraTarget": [ + 5.092505688551431, + -2.228109485612998, + 2.4349855865591303 + ] + }, + "controlsHeight": 104 + }, + { + "type": 1, + "title": "All Axes", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Elevator/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pivot/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Algae/Pivot/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Coral Intake Preset", + "logType": "String", + "visible": true, + "options": { + "color": "#e48b32" + } + }, + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Going to", + "logType": "String", + "visible": true, + "options": { + "color": "#c0b487" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "sehy3t58nyc1z42l4y4zzr9ptkru1kt0", + "renderer": null, + "controlsHeight": 312 + }, + { + "type": 1, + "title": "Roll", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/target", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/motor_position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/out", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/sim_mechanism_position", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/target", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/position", + "logType": "Number", + "visible": false, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Going to", + "logType": "String", + "visible": true, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "Robot/m_robotContainer/coralSubsystem/currentPreset", + "logType": "String", + "visible": true, + "options": { + "color": "#e48b32" + } + }, + { + "type": "stripes", + "logKey": "Robot/m_robotContainer/coralSubsystem/arm/rollreset", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "rf6v3vfi95awd8jfa147d2z79vvczasw", + "renderer": null, + "controlsHeight": 350 + }, + { + "type": 1, + "title": "Pitch", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/target", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/out", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/ff_out", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/pid_out", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Holding Coral", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ats8i329gjp0ncnpjys9z1gq9get6nh9", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 7, + "title": "Video", + "controller": null, + "controllerUUID": "k9elyxmx14ptx7fvkmxbc4dx56n0wqs3", + "renderer": null, + "controlsHeight": 85 + }, + { + "type": 6, + "title": "Vision Stats", + "controller": { + "sources": [ + { + "type": "independent", + "logKey": "NT:/limelight-bl/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "independent", + "logKey": "NT:/limelight-br/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "independent", + "logKey": "NT:/limelight-fr/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "timeRange": "visible", + "rangeMin": 12, + "rangeMax": 22, + "stepSize": 1 + }, + "controllerUUID": "pt4ry3yz159fb4lybdxpfubjm8yf0xew", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.1.6" +} diff --git a/advantagescope/vision_pp_debugging.json b/advantagescope/vision_pp_debugging.json new file mode 100644 index 0000000..dffd8ef --- /dev/null +++ b/advantagescope/vision_pp_debugging.json @@ -0,0 +1,806 @@ +{ + "hubs": [ + { + "x": -8, + "y": -8, + "width": 1552, + "height": 936, + "state": { + "sidebar": { + "width": 327, + "expanded": [ + "/DS/joystick0/axes", + "/NT/Robot/m_robotContainer", + "/NT/PathPlanner/targetPose/translation", + "/NT/PathPlanner/currentPose/translation", + "/NT/PathPlanner/currentPose/rotation", + "/NT/PathPlanner/targetPose", + "/NT/PathPlanner/targetPose/rotation", + "/NT/Swerve States/0", + "/NT/Swerve Target States/0", + "/NT/Swerve Target States/1", + "/NT/Swerve Target States/1/angle", + "/NT/Swerve States/1", + "/NT/Swerve Target States/0/angle", + "/NT/Swerve States/0/angle", + "/CoralVision/targets/0", + "/CoralVision/targets/1", + "/CoralVision/selectedTargetPose/translation", + "/SmartDashboard/Coral/Roll", + "/DS/joystick0/buttons", + "/SmartDashboard/Scheduler/Names", + "/SmartDashboard/Scheduler/Ids", + "/SmartDashboard/Coral/Pitch", + "/Log1/Phoenix6", + "/Running Commands", + "/NT/PathPlanner/vel", + "/DS/joystick1/axes", + "/Robot/m_robotContainer/swerveDriveSubsystem/lastChassisSpeeds", + "/NT/Version", + "/Log1/Phoenix6/TalonFX-34", + "/Log2", + "/Log2/Phoenix6", + "/Log2/Phoenix6/TalonFX-34", + "/NT", + "/NT/SmartDashboard", + "/NT/Swerve States", + "/NT/Swerve Target States", + "/NT/URCL", + "/NT/URCL/Spark-1", + "/SmartDashboard/CoralVision", + "/SmartDashboard/CoralVision/raw", + "/SmartDashboard/CoralVision/raw/distances", + "/SmartDashboard/CoralVision/raw/angles", + "/SmartDashboard", + "/CoralVision", + "/CoralVision/raw/angles", + "/CoralVision/raw/distances", + "/Robot", + "/DS", + "/CoralVision/raw", + "/SmartDashboard/Coral", + "/SmartDashboard/Elevator", + "/SmartDashboard/Coral/Pivot", + "/SmartDashboard/Algae", + "/SmartDashboard/Algae/Pivot", + "/NT/CoralVision/targets", + "/NT/limelight-fr/rawtargets", + "/NT/limelight-bf", + "/Robot/m_robotContainer", + "/Robot/m_robotContainer/normalDrive" + ] + }, + "tabs": { + "selected": 7, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "58qtu5cfzkfp9yjpqos7ayy0r3xjpv1u", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/PathPlanner/targetPose/translation/x", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/targetPose/translation/y", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/currentPose/translation/x", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/PathPlanner/currentPose/translation/y", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:Swerve Target States/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "qxbhmb7s4se4vhuwvdjo5q50py8hkuvl", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 9, + "title": "Swerve States", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:Swerve States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:Swerve Target States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "p5i7rd873d6ad2d82ngn25t8cede5r1n", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Elevator/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pivot/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Algae/Pivot/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "sehy3t58nyc1z42l4y4zzr9ptkru1kt0", + "renderer": null, + "controlsHeight": 312 + }, + { + "type": 5, + "title": "Console", + "controller": "console", + "controllerUUID": "q7ut6zgmjs8u4m1wtgp1qmf967jibe1j", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, + { + "type": 2, + "title": "Odometry", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:Odometry Pose", + "logType": "Pose2d", + "visible": true, + "options": {} + }, + { + "type": "swerveStates", + "logKey": "NT:Swerve States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/scoringPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ffff", + "size": "bold" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/selectedTargetPose", + "logType": "Pose3d", + "visible": true, + "options": { + "color": "#ff00ff", + "size": "bold" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/targetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "swerveStates", + "logKey": "NT:Swerve Target States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "trajectory", + "logKey": "NT:/PathPlanner/activePath", + "logType": "Pose2d[]", + "visible": false, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/currentPose", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/targetPose", + "logType": "Pose2d", + "visible": false, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-bf", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-bl", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-br", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:limelight-fr", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + } + ], + "game": "2025 Field (Welded)", + "bumpers": "auto", + "origin": "blue", + "orientation": 0, + "size": 30 + }, + "controllerUUID": "qmx56d8q2yb01eyzidcu8tl091od50ei", + "renderer": null, + "controlsHeight": 276 + }, + { + "type": 1, + "title": "Line Graph", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:Swerve States/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:Swerve Target States/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/URCL/Spark-1/Voltage", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "qgq951ijzzm7ft7i64hqdl7gr3g4dzfp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:Odometry Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Shrieking Eel" + } + }, + { + "type": "component", + "logKey": "NT:0_ElevatorS1", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:1_ElevatorS2", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:2_Arm", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:3_Wrist1", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:4_Wrist2", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "component", + "logKey": "NT:5_AlgaeArm", + "logType": "Pose3d", + "visible": true, + "options": {} + }, + { + "type": "vision", + "logKey": "NT:CoralVision/targetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:CoralVision/selectedTargetPose", + "logType": "Pose3d", + "visible": true, + "options": { + "color": "#ff0000", + "size": "bold" + } + }, + { + "type": "gamePiece", + "logKey": "NT:Coral_Pose", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Coral" + } + }, + { + "type": "cone", + "logKey": "NT:CoralVision/scoringPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00", + "position": "front" + } + }, + { + "type": "axes", + "logKey": "NT:CoralVision/cameraPoseFieldSpace", + "logType": "Pose3d", + "visible": false, + "options": {} + }, + { + "type": "gamePiece", + "logKey": "NT:Algae_Pose", + "logType": "Pose3d", + "visible": true, + "options": { + "variant": "Algae" + } + }, + { + "type": "ghost", + "logKey": "NT:/PathPlanner/targetPose", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Shrieking Eel", + "color": "#00ff00" + } + }, + { + "type": "trajectory", + "logKey": "NT:/PathPlanner/activePath", + "logType": "Pose2d[]", + "visible": false, + "options": { + "color": "#00ff00", + "size": "normal" + } + } + ], + "game": "2025 Field (Welded)", + "origin": "blue" + }, + "controllerUUID": "kjouwy32sie8360d06yldv6qk8xeb42t", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 2.667186236863164, + 1.8554552081687488, + 4.131655271905313 + ], + "cameraTarget": [ + 1.19486194727207, + 0.2906220825046136, + 0.8752633556329479 + ] + }, + "controlsHeight": 0 + }, + { + "type": 4, + "title": "Table", + "controller": [ + "NT:Running Commands" + ], + "controllerUUID": "ktiliw8rqfuhxgiriskw949kyhhpuj57", + "renderer": null, + "controlsHeight": 0 + }, + { + "type": 1, + "title": "Debugging Roll", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/target", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/motor_position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/out", + "logType": "Number", + "visible": false, + "options": { + "color": "#3b875a", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Roll/sim_mechanism_position", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/target", + "logType": "Number", + "visible": false, + "options": { + "color": "#c0b487", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/SmartDashboard/Coral/Pitch/position", + "logType": "Number", + "visible": false, + "options": { + "color": "#858584", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/SmartDashboard/Going to", + "logType": "String", + "visible": true, + "options": { + "color": "#80588e" + } + }, + { + "type": "stripes", + "logKey": "Robot/m_robotContainer/coralSubsystem/currentPreset", + "logType": "String", + "visible": true, + "options": { + "color": "#e48b32" + } + }, + { + "type": "stripes", + "logKey": "Robot/m_robotContainer/coralSubsystem/arm/rollreset", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#d993aa" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "rf6v3vfi95awd8jfa147d2z79vvczasw", + "renderer": null, + "controlsHeight": 350 + }, + { + "type": 6, + "title": "Statistics", + "controller": { + "sources": [ + { + "type": "independent", + "logKey": "NT:/limelight-bf/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2" + } + }, + { + "type": "independent", + "logKey": "NT:/limelight-bl/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b" + } + }, + { + "type": "independent", + "logKey": "NT:/limelight-br/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437" + } + }, + { + "type": "independent", + "logKey": "NT:/limelight-fr/tid", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "timeRange": "visible", + "rangeMin": 12, + "rangeMax": 22, + "stepSize": 1 + }, + "controllerUUID": "pt4ry3yz159fb4lybdxpfubjm8yf0xew", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.1.1" +} diff --git a/build.gradle b/build.gradle index 9d8ae14..8e34a5a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "com.peterabeles.gversion" version "1.10" } java { @@ -27,6 +28,12 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.25.30.2") // Replace TE.AM with team number } // Static files artifact @@ -102,3 +109,13 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Chicago" + indent = " " +} \ No newline at end of file diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..32abacb --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,105 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, + { + "guid": "030000005e040000fd02000003090000", + "useGamepad": true + } + ] +} diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..6685aa8 --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,210 @@ +{ + "version": 1.0, + "grid_size": 64, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 1344.0, + "y": 448.0, + "width": 576.0, + "height": 256.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.945, + "robot_length": 0.945, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 + } + }, + { + "title": "Holding Coral", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Holding Coral", + "period": 0.06, + "data_type": "boolean", + "true_color": 4294967295, + "false_color": 4278190080, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Auto Chooser", + "x": 1344.0, + "y": 0.0, + "width": 576.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "FMSInfo", + "x": 0.0, + "y": 512.0, + "width": 512.0, + "height": 192.0, + "type": "FMSInfo", + "properties": { + "topic": "/FMSInfo", + "period": 0.06 + } + }, + { + "title": "Match Time", + "x": 1344.0, + "y": 128.0, + "width": 576.0, + "height": 320.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Holding Algae", + "x": 256.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Holding Algae", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215739, + "false_color": 4278190080, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "hasTarget", + "x": 0.0, + "y": 0.0, + "width": 512.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "CoralVision/hasTarget", + "period": 0.06, + "data_type": "boolean", + "true_color": 4289582874, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "USB Camera 0", + "x": 512.0, + "y": 0.0, + "width": 832.0, + "height": 704.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/USB Camera 0", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 1536.0, + "height": 704.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 + } + }, + { + "title": "Auto Chooser", + "x": 1536.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Holding Coral", + "x": 1536.0, + "y": 128.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Holding Coral", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "USB Camera 0", + "x": 1536.0, + "y": 384.0, + "width": 384.0, + "height": 320.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/USB Camera 0", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 coral Jimmy Comp.auto b/src/main/deploy/pathplanner/autos/1 coral Jimmy Comp.auto new file mode 100644 index 0000000..eeddc57 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 coral Jimmy Comp.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle start" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Around the World.auto b/src/main/deploy/pathplanner/autos/Around the World.auto new file mode 100644 index 0000000..cbf9a8c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Around the World.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Reef EF" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + }, + { + "type": "path", + "data": { + "pathName": "To CS" + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "To CD" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Testing", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bump 1 auto - bottom.auto b/src/main/deploy/pathplanner/autos/Bump 1 auto - bottom.auto new file mode 100644 index 0000000..27b8462 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bump 1 auto - bottom.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "bottom 0 to bump start" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump top" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump top to end" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Testing", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Bump 2 auto - bottom.auto b/src/main/deploy/pathplanner/autos/Bump 2 auto - bottom.auto new file mode 100644 index 0000000..871242f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Bump 2 auto - bottom.auto @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "bottom 0 to bump start" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump top" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump top to bottom start" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump bottom to end" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Chicken non-processor side 3 Comp.auto b/src/main/deploy/pathplanner/autos/Chicken non-processor side 3 Comp.auto new file mode 100644 index 0000000..4449822 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Chicken non-processor side 3 Comp.auto @@ -0,0 +1,147 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Chicken 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Chicken 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Chicken 4" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Chicken 5" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Chicken 6" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top Algae 1" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Drive Out.auto b/src/main/deploy/pathplanner/autos/Drive Out.auto new file mode 100644 index 0000000..432d447 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Drive Out.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Byron run out" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Extra Friendly 2 Jimmy Comp.auto b/src/main/deploy/pathplanner/autos/Extra Friendly 2 Jimmy Comp.auto new file mode 100644 index 0000000..244a10b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Extra Friendly 2 Jimmy Comp.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle start" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "path", + "data": { + "pathName": "middle algae 1 barge" + } + }, + { + "type": "path", + "data": { + "pathName": "Processor auto 1" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Processor" + } + }, + { + "type": "path", + "data": { + "pathName": "Processor auto 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Processor auto 3" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Processor" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Flying bottom 4.auto b/src/main/deploy/pathplanner/autos/Flying bottom 4.auto new file mode 100644 index 0000000..3522d3d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Flying bottom 4.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom fly 0" + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "bottom fly 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Flying Series", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Flying top 4.auto b/src/main/deploy/pathplanner/autos/Flying top 4.auto new file mode 100644 index 0000000..5c5ebd9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Flying top 4.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "fly top 0" + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "top 4" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Flying Series", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Friendly 1 Jimmy Comp.auto b/src/main/deploy/pathplanner/autos/Friendly 1 Jimmy Comp.auto new file mode 100644 index 0000000..adc40cd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Friendly 1 Jimmy Comp.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle start" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + }, + { + "type": "path", + "data": { + "pathName": "middle algae 1 barge" + } + }, + { + "type": "path", + "data": { + "pathName": "Processor auto 1" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Processor" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Jimmy barge Comp.auto b/src/main/deploy/pathplanner/autos/Jimmy barge Comp.auto new file mode 100644 index 0000000..7caa886 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Jimmy barge Comp.auto @@ -0,0 +1,83 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle start" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle algae 1 barge" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle 2 barge" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle barge 3" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "middle barge 4" + } + }, + { + "type": "path", + "data": { + "pathName": "middle barge 5" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Jockey Processor side 3 Comp.auto b/src/main/deploy/pathplanner/autos/Jockey Processor side 3 Comp.auto new file mode 100644 index 0000000..b62eb5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Jockey Processor side 3 Comp.auto @@ -0,0 +1,147 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 2" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 3" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 4" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom algae 1" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Killing FWERB.auto b/src/main/deploy/pathplanner/autos/Killing FWERB.auto new file mode 100644 index 0000000..69af29b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Killing FWERB.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "YO MOMMA" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Killing Series", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Processor side 3 Comp.auto b/src/main/deploy/pathplanner/autos/Processor side 3 Comp.auto new file mode 100644 index 0000000..ffd9c02 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Processor side 3 Comp.auto @@ -0,0 +1,147 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 2" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 3" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 4" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 bottom 5" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Processor side 4 Comp.auto b/src/main/deploy/pathplanner/autos/Processor side 4 Comp.auto new file mode 100644 index 0000000..65a7151 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Processor side 4 Comp.auto @@ -0,0 +1,160 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 2" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 3" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 4" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 5" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 6" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Super Rem Processor side 4 Comp.auto b/src/main/deploy/pathplanner/autos/Super Rem Processor side 4 Comp.auto new file mode 100644 index 0000000..e3a4775 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Super Rem Processor side 4 Comp.auto @@ -0,0 +1,172 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 2" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 3" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 4" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "bottom 5" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Super Rem bottom 6" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/middle 3.auto b/src/main/deploy/pathplanner/autos/middle 3.auto new file mode 100644 index 0000000..633d197 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/middle 3.auto @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle start" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle algae 1" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle algae 2" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle algae 3" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middle algae 4" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/non-processor side 3 Comp.auto b/src/main/deploy/pathplanner/autos/non-processor side 3 Comp.auto new file mode 100644 index 0000000..0124b5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/non-processor side 3 Comp.auto @@ -0,0 +1,147 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top 4" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top 5" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 top 6" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 NEW top 3" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/non-processor side 4 Comp.auto b/src/main/deploy/pathplanner/autos/non-processor side 4 Comp.auto new file mode 100644 index 0000000..c8e32c4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/non-processor side 4 Comp.auto @@ -0,0 +1,172 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 0" + } + }, + { + "type": "named", + "data": { + "name": "Score" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 1" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 4" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 5" + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 6" + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NEW top 3" + } + }, + { + "type": "named", + "data": { + "name": "Wait Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "top 2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Score Waitless" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 NEW top 3.path b/src/main/deploy/pathplanner/paths/3 NEW top 3.path new file mode 100644 index 0000000..b266741 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 NEW top 3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": null, + "nextControl": { + "x": 2.800824784466776, + "y": 5.752186284781292 + }, + "isLocked": false, + "linkedName": "top 6 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.8341188524590164, + "y": 6.830122950819672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 1.925543058334256e-15, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 0.24577987846049823, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 botom 6.path b/src/main/deploy/pathplanner/paths/3 botom 6.path new file mode 100644 index 0000000..7a8a371 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 botom 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 3.999897540983607, + "y": 1.4948749999999984 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 5.2, + "y": 2.895 + }, + "prevControl": { + "x": 5.957249999999999, + "y": 2.0067499999999994 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.8029776674937981, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 0.path b/src/main/deploy/pathplanner/paths/3 bottom 0.path new file mode 100644 index 0000000..d0fe463 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 0.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.971, + "y": 2.882 + }, + "prevControl": null, + "nextControl": { + "x": 6.721, + "y": 2.882 + }, + "isLocked": false, + "linkedName": "bottom start" + }, + { + "anchor": { + "x": 5.202184925138958, + "y": 2.909304533347066 + }, + "prevControl": { + "x": 5.452184925138958, + "y": 2.909304533347066 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.47344913151365087, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.46352357320099474, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 1.path b/src/main/deploy/pathplanner/paths/3 bottom 1.path new file mode 100644 index 0000000..fbb3a16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 1.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": null, + "nextControl": { + "x": 4.68, + "y": 2.3089999999999997 + }, + "isLocked": false, + "linkedName": "bottom 0 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.9495210211909202, + "y": 1.033027541514335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23434704830053607, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 0.7745974955277183, + "rotationDegrees": 145.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.3195666892349369, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 2.path b/src/main/deploy/pathplanner/paths/3 bottom 2.path new file mode 100644 index 0000000..786fac0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 2.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 1.6985063509461096, + "y": 0.9422499999999991 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 4.160319952595912, + "y": 2.692120690772514 + }, + "prevControl": { + "x": 3.7690699525959124, + "y": 2.184370690772513 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6004962779156325, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 3.path b/src/main/deploy/pathplanner/paths/3 bottom 3.path new file mode 100644 index 0000000..71a35a5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": null, + "nextControl": { + "x": 2.7881665398800317, + "y": 1.7304697255901826 + }, + "isLocked": false, + "linkedName": "bottom 2 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 2.25225, + "y": 1.3339999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.20311442112390035, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0.0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 4.path b/src/main/deploy/pathplanner/paths/3 bottom 4.path new file mode 100644 index 0000000..034bba4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 4.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 2.495146958304853, + "y": 1.6857997265892002 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": { + "x": 3.2198363428067798, + "y": 2.303466019506892 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 4 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.49330024813896345, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.49925558312655216, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom 5.path b/src/main/deploy/pathplanner/paths/3 bottom 5.path new file mode 100644 index 0000000..40b4bc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom 5.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": null, + "nextControl": { + "x": 3.0315658814769533, + "y": 2.169439598147495 + }, + "isLocked": false, + "linkedName": "bottom 4 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 2.11575, + "y": 1.3729999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 0.35921674544227394, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom algae 1.path b/src/main/deploy/pathplanner/paths/3 bottom algae 1.path new file mode 100644 index 0000000..19a9291 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom algae 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": null, + "nextControl": { + "x": 3.7342500000000003, + "y": 2.620999999999999 + }, + "isLocked": false, + "linkedName": "bottom 4 end" + }, + { + "anchor": { + "x": 4.017, + "y": 2.7184999999999993 + }, + "prevControl": { + "x": 3.8402233047033634, + "y": 2.5417233047033627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 bottom algae 1 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Grab Low", + "waypointRelativePos": 0.13257142857142984, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab Low" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 bottom algae 2.path b/src/main/deploy/pathplanner/paths/3 bottom algae 2.path new file mode 100644 index 0000000..94a3d84 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 bottom algae 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.017, + "y": 2.7184999999999993 + }, + "prevControl": null, + "nextControl": { + "x": 3.6952499999999997, + "y": 1.987249999999999 + }, + "isLocked": false, + "linkedName": "3 bottom algae 1 end" + }, + { + "anchor": { + "x": 5.976749999999999, + "y": 2.230999999999999 + }, + "prevControl": { + "x": 5.46, + "y": 2.0164999999999997 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Processor side 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 0.path b/src/main/deploy/pathplanner/paths/3 top 0.path new file mode 100644 index 0000000..3e88fde --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 0.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 6.155127049180329, + "y": 5.201088114754099 + }, + "isLocked": false, + "linkedName": "top start" + }, + { + "anchor": { + "x": 5.09, + "y": 5.17 + }, + "prevControl": { + "x": 5.6397499999999985, + "y": 5.125999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.29709655638083343, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 1.path b/src/main/deploy/pathplanner/paths/3 top 1.path new file mode 100644 index 0000000..1f4b665 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.09, + "y": 5.177 + }, + "prevControl": null, + "nextControl": { + "x": 4.047069672131147, + "y": 5.90825 + }, + "isLocked": false, + "linkedName": "top 0 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.7644303278688527, + "y": 7.064922131147541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.2289205702647632, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 2.path b/src/main/deploy/pathplanner/paths/3 top 2.path new file mode 100644 index 0000000..c9e25d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.327852459016393, + "y": 6.765229508196721 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": { + "x": 2.4907213114754096, + "y": 4.459362704918032 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.14419551934826969, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 3.path b/src/main/deploy/pathplanner/paths/3 top 3.path new file mode 100644 index 0000000..7c7ca5f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": null, + "nextControl": { + "x": 2.167053278688525, + "y": 4.659549180327869 + }, + "isLocked": false, + "linkedName": "top 2 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.9924754098360646, + "y": 6.9090819672131145 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.1702647657841135, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 4.path b/src/main/deploy/pathplanner/paths/3 top 4.path new file mode 100644 index 0000000..82adf70 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.4836926229508194, + "y": 6.261745901639343 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": { + "x": 3.0448770491803274, + "y": 5.715266393442623 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 5 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6509115462525283, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 5.path b/src/main/deploy/pathplanner/paths/3 top 5.path new file mode 100644 index 0000000..5dfe2d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": null, + "nextControl": { + "x": 2.91301229508587, + "y": 5.907069672136539 + }, + "isLocked": false, + "linkedName": "top 5 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 2.001946721315378, + "y": 6.6982581967267025 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.12790224032586558, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top 6.path b/src/main/deploy/pathplanner/paths/3 top 6.path new file mode 100644 index 0000000..3309f1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.623322068747944, + "y": 7.0499791889741505 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": { + "x": 3.2563575713520216, + "y": 5.2247272683878485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 6 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6671168129642098, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "non-processor 3 autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top Algae 1.path b/src/main/deploy/pathplanner/paths/3 top Algae 1.path new file mode 100644 index 0000000..5dfc17d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top Algae 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": null, + "nextControl": { + "x": 3.374327334174971, + "y": 5.325376600759876 + }, + "isLocked": false, + "linkedName": "top 6 end" + }, + { + "anchor": { + "x": 3.64, + "y": 5.05 + }, + "prevControl": { + "x": 3.4807500000000005, + "y": 5.3315 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top Algae 1 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Grab Low", + "waypointRelativePos": 0.15085714285714064, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab Low" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 top Algae 2.path b/src/main/deploy/pathplanner/paths/3 top Algae 2.path new file mode 100644 index 0000000..05ed7c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 top Algae 2.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.64, + "y": 5.05 + }, + "prevControl": null, + "nextControl": { + "x": 3.074500000000001, + "y": 5.7715000000000005 + }, + "isLocked": false, + "linkedName": "top Algae 1 end" + }, + { + "anchor": { + "x": 7.72, + "y": 6.2 + }, + "prevControl": { + "x": 7.472966564282866, + "y": 6.1616010203548495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.19470198675496184, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow Bumper", + "waypointRelativePos": 0.04051316677920362, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow Bumper" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump bottom to end.path b/src/main/deploy/pathplanner/paths/Bump bottom to end.path new file mode 100644 index 0000000..8ee3764 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump bottom to end.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.673, + "y": 1.5 + }, + "prevControl": null, + "nextControl": { + "x": 7.61475, + "y": 2.515249999999998 + }, + "isLocked": false, + "linkedName": "Bump bottom end" + }, + { + "anchor": { + "x": 6.279000000000001, + "y": 3.9177499999999994 + }, + "prevControl": { + "x": 7.1175, + "y": 3.946999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump bottom.path b/src/main/deploy/pathplanner/paths/Bump bottom.path new file mode 100644 index 0000000..e0626ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump bottom.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.927, + "y": 3.707 + }, + "prevControl": null, + "nextControl": { + "x": 7.94625, + "y": 3.1572499999999994 + }, + "isLocked": false, + "linkedName": "Bump bottom start" + }, + { + "anchor": { + "x": 7.673, + "y": 1.5 + }, + "prevControl": { + "x": 8.020793537902826, + "y": 1.5159147727924058 + }, + "nextControl": { + "x": 6.815250000000001, + "y": 1.4607499999999995 + }, + "isLocked": false, + "linkedName": "Bump bottom end" + }, + { + "anchor": { + "x": 7.673, + "y": 1.5 + }, + "prevControl": { + "x": 8.0535, + "y": 1.4509999999999992 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bump bottom end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump top to bottom start.path b/src/main/deploy/pathplanner/paths/Bump top to bottom start.path new file mode 100644 index 0000000..6bddf9c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump top to bottom start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.999999999999999 + }, + "prevControl": null, + "nextControl": { + "x": 7.67325, + "y": 4.414918755641577 + }, + "isLocked": false, + "linkedName": "Bump 2 end" + }, + { + "anchor": { + "x": 7.927, + "y": 3.707 + }, + "prevControl": { + "x": 7.927, + "y": 4.707000000000002 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bump bottom start" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump top to end.path b/src/main/deploy/pathplanner/paths/Bump top to end.path new file mode 100644 index 0000000..ccf571f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump top to end.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.999999999999999 + }, + "prevControl": null, + "nextControl": { + "x": 7.6245, + "y": 4.3759999999999994 + }, + "isLocked": false, + "linkedName": "Bump 2 end" + }, + { + "anchor": { + "x": 6.56175, + "y": 4.444249999999999 + }, + "prevControl": { + "x": 7.097125, + "y": 4.43725 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump top.path b/src/main/deploy/pathplanner/paths/Bump top.path new file mode 100644 index 0000000..8cb1f2c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump top.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.926749999999999, + "y": 4.2395 + }, + "prevControl": null, + "nextControl": { + "x": 7.969585293687813, + "y": 4.485802938704918 + }, + "isLocked": false, + "linkedName": "Bump 1 end" + }, + { + "anchor": { + "x": 7.67325, + "y": 4.999999999999999 + }, + "prevControl": { + "x": 7.901092649914669, + "y": 4.8971033193933735 + }, + "nextControl": { + "x": 7.371, + "y": 5.1365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.67325, + "y": 4.999999999999999 + }, + "prevControl": { + "x": 7.426309258667733, + "y": 5.038990643368252 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bump 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": -89.07595464722735 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Byron run out.path b/src/main/deploy/pathplanner/paths/Byron run out.path new file mode 100644 index 0000000..9618241 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Byron run out.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.204610655737704, + "y": 2.2867827868852464 + }, + "prevControl": null, + "nextControl": { + "x": 6.42454972061789, + "y": 2.2867827868852464 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.2595, + "y": 2.2867827868852464 + }, + "prevControl": { + "x": 6.826343232296197, + "y": 2.2867827868852464 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 0.path b/src/main/deploy/pathplanner/paths/Chicken 0.path new file mode 100644 index 0000000..4c3ed98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 0.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 6.155127049180329, + "y": 5.201088114754099 + }, + "isLocked": false, + "linkedName": "top start" + }, + { + "anchor": { + "x": 5.09, + "y": 5.17 + }, + "prevControl": { + "x": 5.6397499999999985, + "y": 5.125999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.29709655638083343, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 1.path b/src/main/deploy/pathplanner/paths/Chicken 1.path new file mode 100644 index 0000000..598bf69 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.09, + "y": 5.177 + }, + "prevControl": null, + "nextControl": { + "x": 4.047069672131147, + "y": 5.90825 + }, + "isLocked": false, + "linkedName": "top 0 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.7644303278688527, + "y": 7.064922131147541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.2289205702647632, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 2.path b/src/main/deploy/pathplanner/paths/Chicken 2.path new file mode 100644 index 0000000..7715f62 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.327852459016393, + "y": 6.765229508196721 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": { + "x": 2.4907213114754096, + "y": 4.459362704918032 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.14419551934826969, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 3.path b/src/main/deploy/pathplanner/paths/Chicken 3.path new file mode 100644 index 0000000..9402b47 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": null, + "nextControl": { + "x": 2.800824784466776, + "y": 5.752186284781292 + }, + "isLocked": false, + "linkedName": "top 6 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.8341188524590164, + "y": 6.830122950819672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 1.925543058334256e-15, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 0.24577987846049823, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 4.path b/src/main/deploy/pathplanner/paths/Chicken 4.path new file mode 100644 index 0000000..4561f48 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.6479672023607574, + "y": 7.076576238111393 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": { + "x": 3.6008021566360076, + "y": 5.336064150591324 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 5 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6509115462525283, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 5.path b/src/main/deploy/pathplanner/paths/Chicken 5.path new file mode 100644 index 0000000..ded0c31 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": null, + "nextControl": { + "x": 2.91301229508587, + "y": 5.907069672136539 + }, + "isLocked": false, + "linkedName": "top 5 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 2.001946721315378, + "y": 6.6982581967267025 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.12790224032586558, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Chicken 6.path b/src/main/deploy/pathplanner/paths/Chicken 6.path new file mode 100644 index 0000000..6a8a4d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Chicken 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.623322068747944, + "y": 7.0499791889741505 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": { + "x": 3.2563575713520216, + "y": 5.2247272683878485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 6 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.6671168129642098, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "Chicken", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of bottom 1.path b/src/main/deploy/pathplanner/paths/Copy of bottom 1.path new file mode 100644 index 0000000..a316fd9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of bottom 1.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": null, + "nextControl": { + "x": 5.462381147540985, + "y": 2.4419999999999997 + }, + "isLocked": false, + "linkedName": "bottom 0 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.949520491803279, + "y": 1.0330286885245892 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23434704830053607, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 0.7745974955277183, + "rotationDegrees": 144.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.6214014122759356, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Flying", + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example auto 1.path b/src/main/deploy/pathplanner/paths/Example auto 1.path new file mode 100644 index 0000000..b9e1781 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example auto 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.034, + "y": 3.021 + }, + "prevControl": null, + "nextControl": { + "x": 7.034057793897123, + "y": 3.0102489789269713 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.31375, + "y": 2.913499999999999 + }, + "prevControl": { + "x": 5.82337178071095, + "y": 3.2077302722783627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Example auto 1 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 31.7594800848128 + }, + "reversed": false, + "folder": "Example autos", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example auto 2.path b/src/main/deploy/pathplanner/paths/Example auto 2.path new file mode 100644 index 0000000..3701058 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example auto 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.31375, + "y": 2.913499999999999 + }, + "prevControl": null, + "nextControl": { + "x": 3.79275, + "y": 1.4315 + }, + "isLocked": false, + "linkedName": "Example auto 1 end" + }, + { + "anchor": { + "x": 1.6672500000000001, + "y": 0.66125 + }, + "prevControl": { + "x": 2.5642499999999995, + "y": 1.0512499999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.634113875967515 + }, + "reversed": false, + "folder": "Example autos", + "idealStartingState": { + "velocity": 0, + "rotation": 31.7594800848128 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 NEW top 3.path b/src/main/deploy/pathplanner/paths/L3 NEW top 3.path new file mode 100644 index 0000000..23a9d0b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 NEW top 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": null, + "nextControl": { + "x": 2.5041649484012027, + "y": 5.650290792978014 + }, + "isLocked": false, + "linkedName": "top 6 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.9924754098360646, + "y": 6.9090819672131145 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.30635524171646056, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 0.path b/src/main/deploy/pathplanner/paths/L3 bottom 0.path new file mode 100644 index 0000000..7249c4d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 0.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.192622950819672, + "y": 3.05 + }, + "prevControl": null, + "nextControl": { + "x": 6.541592584663156, + "y": 3.05 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": { + "x": 5.760086413666071, + "y": 2.91 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 1.path b/src/main/deploy/pathplanner/paths/L3 bottom 1.path new file mode 100644 index 0000000..fb3c7df --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 1.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": null, + "nextControl": { + "x": 5.462381147540985, + "y": 2.4419999999999997 + }, + "isLocked": false, + "linkedName": "bottom 0 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.949520491803279, + "y": 1.0330286885245892 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.23434704830053607, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 0.7745974955277183, + "rotationDegrees": 144.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.6214014122759356, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 2.path b/src/main/deploy/pathplanner/paths/L3 bottom 2.path new file mode 100644 index 0000000..da04e7a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 2.235147540983607, + "y": 1.3973749999999985 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": { + "x": 3.6999165398800327, + "y": 1.9844697255901829 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.42585551330799026, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 3.path b/src/main/deploy/pathplanner/paths/L3 bottom 3.path new file mode 100644 index 0000000..c53733d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": null, + "nextControl": { + "x": 3.733367359552163, + "y": 2.007566037065593 + }, + "isLocked": false, + "linkedName": "bottom 2 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 2.1533114754098355, + "y": 1.3087459016393437 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.3283095723014281, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0.0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 4.path b/src/main/deploy/pathplanner/paths/L3 bottom 4.path new file mode 100644 index 0000000..95fbca9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 2.2546475409836066, + "y": 1.7093749999999988 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": { + "x": 3.470365881476954, + "y": 2.3351895981474957 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 4 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.299837045084196, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 bottom 6.path b/src/main/deploy/pathplanner/paths/L3 bottom 6.path new file mode 100644 index 0000000..81c8edb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 bottom 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 3.999897540983607, + "y": 1.4948749999999984 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 5.2, + "y": 2.895 + }, + "prevControl": { + "x": 5.957000000000001, + "y": 1.5144999999999993 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.16512764801738072, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "bottom L3", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 0.path b/src/main/deploy/pathplanner/paths/L3 top 0.path new file mode 100644 index 0000000..d9bb296 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 0.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 6.155127049180329, + "y": 5.201088114754099 + }, + "isLocked": false, + "linkedName": "top start" + }, + { + "anchor": { + "x": 5.09, + "y": 5.17 + }, + "prevControl": { + "x": 5.6397499999999985, + "y": 5.125999999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 1.path b/src/main/deploy/pathplanner/paths/L3 top 1.path new file mode 100644 index 0000000..64e5f21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.09, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 4.047069672131147, + "y": 5.90125 + }, + "isLocked": false, + "linkedName": "top 0 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.7644303278688527, + "y": 7.064922131147541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.41716458446496674, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 2.path b/src/main/deploy/pathplanner/paths/L3 top 2.path new file mode 100644 index 0000000..6c72e42 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.327852459016393, + "y": 6.765229508196721 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": { + "x": 2.4907213114754096, + "y": 4.459362704918032 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.26476578411405316, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 4.path b/src/main/deploy/pathplanner/paths/L3 top 4.path new file mode 100644 index 0000000..bbc5f83 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.4836926229508194, + "y": 6.261745901639343 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.77, + "y": 5.14625 + }, + "prevControl": { + "x": 3.1586270491803274, + "y": 5.769610655737705 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 5 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.20285132382892213, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 5.path b/src/main/deploy/pathplanner/paths/L3 top 5.path new file mode 100644 index 0000000..3b70407 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": null, + "nextControl": { + "x": 2.924360655737705, + "y": 6.021352459016394 + }, + "isLocked": false, + "linkedName": "top 5 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.848344262295082, + "y": 6.657340163934426 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.4193373166757208, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3 top 6.path b/src/main/deploy/pathplanner/paths/L3 top 6.path new file mode 100644 index 0000000..b01776f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3 top 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 2.148036885245901, + "y": 6.369635245901639 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": { + "x": 2.9566649484012024, + "y": 5.734204727404242 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 6 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.13441955193483, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "top L3", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NEW top 3.path b/src/main/deploy/pathplanner/paths/NEW top 3.path new file mode 100644 index 0000000..1ccf8bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NEW top 3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": null, + "nextControl": { + "x": 2.800824784466776, + "y": 5.752186284781292 + }, + "isLocked": false, + "linkedName": "top 6 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.8341188524590164, + "y": 6.830122950819672 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 1.925543058334256e-15, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 0.24577987846049823, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.9, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..0257e27 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8585, + "y": 3.9274999999999993 + }, + "prevControl": null, + "nextControl": { + "x": 8.858500000000005, + "y": 3.9274999999999993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Processor auto 1.path b/src/main/deploy/pathplanner/paths/Processor auto 1.path new file mode 100644 index 0000000..634eea5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Processor auto 1.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": null, + "nextControl": { + "x": 6.614176229508196, + "y": 4.245254098360656 + }, + "isLocked": false, + "linkedName": "middle algae end" + }, + { + "anchor": { + "x": 5.762, + "y": 0.5929999999999995 + }, + "prevControl": { + "x": 5.762, + "y": 0.8659999999999991 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Processor" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.24768211920529676, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Go Processor", + "waypointRelativePos": 0.2636190476190485, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Go Processor" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Processor auto 2.path b/src/main/deploy/pathplanner/paths/Processor auto 2.path new file mode 100644 index 0000000..83128c2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Processor auto 2.path @@ -0,0 +1,82 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.762, + "y": 0.5929999999999995 + }, + "prevControl": null, + "nextControl": { + "x": 5.76608606557377, + "y": 1.5555327868852453 + }, + "isLocked": false, + "linkedName": "Processor" + }, + { + "anchor": { + "x": 5.343762234260163, + "y": 2.9923951245193643 + }, + "prevControl": { + "x": 5.751344201473278, + "y": 2.237169714683299 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EF Algae" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": -150.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Grab High", + "waypointRelativePos": 0.4915597569209984, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab High" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Processor auto 3.path b/src/main/deploy/pathplanner/paths/Processor auto 3.path new file mode 100644 index 0000000..1f99415 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Processor auto 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.343762234260163, + "y": 2.9923951245193643 + }, + "prevControl": null, + "nextControl": { + "x": 5.814036885245901, + "y": 1.9870901639344272 + }, + "isLocked": false, + "linkedName": "EF Algae" + }, + { + "anchor": { + "x": 5.762, + "y": 0.5929999999999995 + }, + "prevControl": { + "x": 5.71813524590164, + "y": 1.4236680327868863 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Processor" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Go Processor", + "waypointRelativePos": 0.4, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Go Processor" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef EF.path b/src/main/deploy/pathplanner/paths/Reef EF.path new file mode 100644 index 0000000..8370cae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef EF.path @@ -0,0 +1,82 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.04375, + "y": 4.001024590163935 + }, + "prevControl": null, + "nextControl": { + "x": 7.300512295081968, + "y": 3.233811475409836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.737090163934426, + "y": 2.874180327868852 + }, + "prevControl": { + "x": 7.612192622950821, + "y": 3.3896516393442626 + }, + "nextControl": { + "x": 5.87545952624249, + "y": 2.36664447279004 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.166700819672132, + "y": 2.874180327868852 + }, + "prevControl": { + "x": 5.5383196721311485, + "y": 2.358709016393443 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EF" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 1.0047265361242472, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 30.579226872489098 + }, + "reversed": false, + "folder": "Finn's testing autos", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Super Rem bottom 5.path b/src/main/deploy/pathplanner/paths/Super Rem bottom 5.path new file mode 100644 index 0000000..3477a17 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Super Rem bottom 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": null, + "nextControl": { + "x": 3.678971748662292, + "y": 2.701052740137967 + }, + "isLocked": false, + "linkedName": "bottom 4 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 2.11575, + "y": 1.3729999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Super Rem bottom 6.path b/src/main/deploy/pathplanner/paths/Super Rem bottom 6.path new file mode 100644 index 0000000..67a3e20 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Super Rem bottom 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 1.643485560366543, + "y": 1.008096571342278 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 3.83175, + "y": 2.855 + }, + "prevControl": { + "x": 3.6172500000000003, + "y": 2.6502499999999998 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L3", + "waypointRelativePos": 0.6796190476190479, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.7, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 146.10383343663608 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/To CD.path b/src/main/deploy/pathplanner/paths/To CD.path new file mode 100644 index 0000000..9ee2ef8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/To CD.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1148565573770493, + "y": 1.0400614754098354 + }, + "prevControl": null, + "nextControl": { + "x": 1.846106557377049, + "y": 2.0230532786885247 + }, + "isLocked": false, + "linkedName": "At CS" + }, + { + "anchor": { + "x": 3.848053278688525, + "y": 2.86219262295082 + }, + "prevControl": { + "x": 3.2606557377049183, + "y": 1.9631147540983616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.270087778528024, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "Finn's testing autos", + "idealStartingState": { + "velocity": 0, + "rotation": -35.21759296819251 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/To CS.path b/src/main/deploy/pathplanner/paths/To CS.path new file mode 100644 index 0000000..54e527a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/To CS.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.166700819672132, + "y": 2.874180327868852 + }, + "prevControl": null, + "nextControl": { + "x": 5.8619877049180324, + "y": 1.6993852459016396 + }, + "isLocked": false, + "linkedName": "EF" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 1.0400614754098354 + }, + "prevControl": { + "x": 1.953995901639343, + "y": 2.130942622950819 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "At CS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake", + "waypointRelativePos": 0.6995273463875721, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.21759296819251 + }, + "reversed": false, + "folder": "Finn's testing autos", + "idealStartingState": { + "velocity": 0, + "rotation": 30.579226872489098 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/YO MOMMA.path b/src/main/deploy/pathplanner/paths/YO MOMMA.path new file mode 100644 index 0000000..df58b31 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/YO MOMMA.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0377500000000004, + "y": 5.7215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.005999999999999, + "y": 1.0414999999999999 + }, + "prevControl": { + "x": 5.177249999999999, + "y": 1.1377500000000003 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.8834031330453 + }, + "reversed": false, + "folder": "Testing", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 0 to bump start.path b/src/main/deploy/pathplanner/paths/bottom 0 to bump start.path new file mode 100644 index 0000000..b4642b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 0 to bump start.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": null, + "nextControl": { + "x": 5.901131147540985, + "y": 3.2414999999999994 + }, + "isLocked": false, + "linkedName": "bottom 0 end" + }, + { + "anchor": { + "x": 7.926749999999999, + "y": 4.2395 + }, + "prevControl": { + "x": 6.93225, + "y": 3.7129999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Bump 1 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -89.07595464722735 + }, + "reversed": false, + "folder": "Bump auto", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 0.path b/src/main/deploy/pathplanner/paths/bottom 0.path new file mode 100644 index 0000000..2138a3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 0.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.971, + "y": 2.882 + }, + "prevControl": null, + "nextControl": { + "x": 6.668592782824219, + "y": 2.882 + }, + "isLocked": false, + "linkedName": "bottom start" + }, + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": { + "x": 5.5965, + "y": 2.8939999999999997 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.4972704714640236, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 1.path b/src/main/deploy/pathplanner/paths/bottom 1.path new file mode 100644 index 0000000..0c4e74d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 1.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.21, + "y": 2.91 + }, + "prevControl": null, + "nextControl": { + "x": 4.633631147540984, + "y": 2.5687500000000005 + }, + "isLocked": false, + "linkedName": "bottom 0 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.9495210211909202, + "y": 1.033027541514335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7745974955277183, + "rotationDegrees": 145.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.646095238095239, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.3, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.3195666892349369, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0.0, + "rotation": -150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 2.path b/src/main/deploy/pathplanner/paths/bottom 2.path new file mode 100644 index 0000000..060344b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 2.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 1.6868079801297595, + "y": 0.9606155860908319 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": { + "x": 3.938666539880033, + "y": 2.5007197255901827 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5250620347394567, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "L4", + "waypointRelativePos": 0.46352357320099075, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 3.path b/src/main/deploy/pathplanner/paths/bottom 3.path new file mode 100644 index 0000000..d0bb3fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 3.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": null, + "nextControl": { + "x": 3.938666539880033, + "y": 2.529969725590182 + }, + "isLocked": false, + "linkedName": "bottom 2 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.6819999999999997, + "y": 0.9672499999999994 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6826666666666688, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.443428571428572, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0.0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 4.path b/src/main/deploy/pathplanner/paths/bottom 4.path new file mode 100644 index 0000000..171b608 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 4.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 2.495146958304853, + "y": 1.6857997265892002 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": { + "x": 3.3199839607660855, + "y": 2.3620137608269203 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 4 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5250620347394612, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.10024813895781831, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "L4", + "waypointRelativePos": 0.4456575682382172, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 5.path b/src/main/deploy/pathplanner/paths/bottom 5.path new file mode 100644 index 0000000..f4a1ac2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 5.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8748158814769535, + "y": 2.8564395981474955 + }, + "prevControl": null, + "nextControl": { + "x": 3.0315658814769533, + "y": 2.169439598147495 + }, + "isLocked": false, + "linkedName": "bottom 4 end" + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 2.11575, + "y": 1.3729999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7344761904761916, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom 6.path b/src/main/deploy/pathplanner/paths/bottom 6.path new file mode 100644 index 0000000..120c860 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom 6.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 1.70625, + "y": 1.402249999999999 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 3.1492500000000003, + "y": 3.6545 + }, + "prevControl": { + "x": 2.40825, + "y": 3.6545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8335238095238101, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.10223325062034208, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "L2", + "waypointRelativePos": 0.6282878411910683, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L2" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.9, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "processor side autos", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom fly 0.path b/src/main/deploy/pathplanner/paths/bottom fly 0.path new file mode 100644 index 0000000..6bba90f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom fly 0.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.192622950819672, + "y": 3.05 + }, + "prevControl": null, + "nextControl": { + "x": 6.541592584663156, + "y": 3.05 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.495, + "y": 3.05 + }, + "prevControl": { + "x": 6.021685154004536, + "y": 3.2087381839735483 + }, + "nextControl": { + "x": 5.255635265242046, + "y": 2.9778576147174625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": { + "x": 1.708647540983607, + "y": 0.9586249999999985 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Low Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 1.2589473684210482, + "rotationDegrees": -150.0 + }, + { + "waypointRelativePos": 1.618245614035083, + "rotationDegrees": 150.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5751527494908352, + "maxWaypointRelativePos": 1.2464358452138493, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + }, + { + "name": "Score", + "waypointRelativePos": 1.0118126272912207, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Score" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 1.471283095723018, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Flying", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bottom fly 1.path b/src/main/deploy/pathplanner/paths/bottom fly 1.path new file mode 100644 index 0000000..6c4cfa9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bottom fly 1.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.482, + "y": 0.8172499999999991 + }, + "prevControl": null, + "nextControl": { + "x": 2.235147540983607, + "y": 1.3973749999999985 + }, + "isLocked": false, + "linkedName": "Low Source" + }, + { + "anchor": { + "x": 4.164166539880032, + "y": 2.696469725590183 + }, + "prevControl": { + "x": 3.743666539880033, + "y": 2.364219725590183 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bottom 2 end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 150.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.2, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Flying", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/fly top 0.path b/src/main/deploy/pathplanner/paths/fly top 0.path new file mode 100644 index 0000000..2ed24bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/fly top 0.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 6.613377049180329, + "y": 4.9768381147540985 + }, + "isLocked": false, + "linkedName": "top start" + }, + { + "anchor": { + "x": 5.343, + "y": 5.07 + }, + "prevControl": { + "x": 5.742750000000001, + "y": 4.82375 + }, + "nextControl": { + "x": 5.130144860041561, + "y": 5.201120896096974 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.75372131147541, + "y": 7.013375000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.877192982456141, + "rotationDegrees": -29.999999999999996 + }, + { + "waypointRelativePos": 1.1747368421052669, + "rotationDegrees": -29.999999999999996 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.46761710794297334, + "maxWaypointRelativePos": 1.207331975560059, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.2737642585551328, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + }, + { + "name": "Score", + "waypointRelativePos": 0.9368635437881803, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Score" + } + } + }, + { + "name": "Start Intake L", + "waypointRelativePos": 1.5560081466395137, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "Flying", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle 1.path b/src/main/deploy/pathplanner/paths/middle 1.path new file mode 100644 index 0000000..74aa11e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle 1.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": null, + "nextControl": { + "x": 8.935290983606556, + "y": 6.358280737704916 + }, + "isLocked": false, + "linkedName": "middle algae end" + }, + { + "anchor": { + "x": 1.67827868852459, + "y": 7.375563524590164 + }, + "prevControl": { + "x": 1.9275423099001139, + "y": 7.394737649309677 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7048300536672623, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 38.08877288097538 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle 2 barge.path b/src/main/deploy/pathplanner/paths/middle 2 barge.path new file mode 100644 index 0000000..8d536b2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle 2 barge.path @@ -0,0 +1,96 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": null, + "nextControl": { + "x": 7.033745901639343, + "y": 5.887569672131148 + }, + "isLocked": false, + "linkedName": "middle algae end" + }, + { + "anchor": { + "x": 7.720081967213115, + "y": 5.619364754098362 + }, + "prevControl": { + "x": 6.701127049180327, + "y": 5.607377049180328 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "barge shot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5827814569536415, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.49966239027684034, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.1, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Go Barge", + "waypointRelativePos": 0.5881904761904768, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Go Barge" + } + } + }, + { + "name": "Shoot Barge", + "waypointRelativePos": 0.9279022403258654, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Shoot Barge" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "middle Barge", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle algae 1 barge.path b/src/main/deploy/pathplanner/paths/middle algae 1 barge.path new file mode 100644 index 0000000..35f85ad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle algae 1 barge.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.07 + }, + "prevControl": null, + "nextControl": { + "x": 6.132750000000001, + "y": 4.112749999999999 + }, + "isLocked": false, + "linkedName": "middle start end" + }, + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": { + "x": 6.1034999999999995, + "y": 4.1907499999999995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Grab Low", + "waypointRelativePos": 0.09723160027009274, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab Low" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "middle Barge", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle algae 1.path b/src/main/deploy/pathplanner/paths/middle algae 1.path new file mode 100644 index 0000000..608d1a4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle algae 1.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.07 + }, + "prevControl": null, + "nextControl": { + "x": 7.089184426229508, + "y": 3.9590676229508195 + }, + "isLocked": false, + "linkedName": "middle start end" + }, + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": { + "x": 6.154143442622951, + "y": 4.347389344262295 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.05947046843177223, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Algae Low", + "waypointRelativePos": 0.3299389002036788, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Algae Low" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle algae 2.path b/src/main/deploy/pathplanner/paths/middle algae 2.path new file mode 100644 index 0000000..2431c1c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle algae 2.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8305, + "y": 4.2395 + }, + "prevControl": null, + "nextControl": { + "x": 6.909368852459017, + "y": 5.4142950819672135 + }, + "isLocked": false, + "linkedName": "middle algae end" + }, + { + "anchor": { + "x": 4.96275, + "y": 5.2925 + }, + "prevControl": { + "x": 5.9217663934426215, + "y": 5.20858606557377 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.17381857686040506, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Algae High", + "waypointRelativePos": 0.5258011950027189, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Algae High" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle algae 3.path b/src/main/deploy/pathplanner/paths/middle algae 3.path new file mode 100644 index 0000000..b5b8a8f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle algae 3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.96275, + "y": 5.2925 + }, + "prevControl": null, + "nextControl": { + "x": 5.870700819672131, + "y": 6.319108606557377 + }, + "isLocked": false, + "linkedName": "middle algae 2 end" + }, + { + "anchor": { + "x": 5.343658717270384, + "y": 2.9994161857734656 + }, + "prevControl": { + "x": 7.657285766450714, + "y": 4.054334218560353 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae 3 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.2737642585551328, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Algae High", + "waypointRelativePos": 0.7908745247148251, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Algae High" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle algae 4.path b/src/main/deploy/pathplanner/paths/middle algae 4.path new file mode 100644 index 0000000..3ac8964 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle algae 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.343658717270384, + "y": 2.9994161857734656 + }, + "prevControl": null, + "nextControl": { + "x": 6.343658717270384, + "y": 2.9994161857734656 + }, + "isLocked": false, + "linkedName": "middle algae 3 end" + }, + { + "anchor": { + "x": 5.976749999999999, + "y": 1.6557499999999994 + }, + "prevControl": { + "x": 4.976749999999999, + "y": 1.6557499999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0.3172189027702321, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.30972280213487 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -150.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle barge 3.path b/src/main/deploy/pathplanner/paths/middle barge 3.path new file mode 100644 index 0000000..6507fbe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle barge 3.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.720081967213115, + "y": 5.619364754098362 + }, + "prevControl": null, + "nextControl": { + "x": 5.909938524590164, + "y": 5.619364754098362 + }, + "isLocked": false, + "linkedName": "barge shot" + }, + { + "anchor": { + "x": 4.96275, + "y": 5.2925 + }, + "prevControl": { + "x": 5.13825, + "y": 5.67275 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae 2 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Grab High", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab High" + } + } + }, + { + "name": "Intake Algae", + "waypointRelativePos": 0.5995948683322097, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake Algae" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "middle Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle barge 4.path b/src/main/deploy/pathplanner/paths/middle barge 4.path new file mode 100644 index 0000000..fed7fe4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle barge 4.path @@ -0,0 +1,96 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.96275, + "y": 5.2925 + }, + "prevControl": null, + "nextControl": { + "x": 5.778073770491804, + "y": 5.535450819672132 + }, + "isLocked": false, + "linkedName": "middle algae 2 end" + }, + { + "anchor": { + "x": 7.720081967213115, + "y": 5.619364754098362 + }, + "prevControl": { + "x": 6.533299180327869, + "y": 5.643340163934426 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "barge shot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6294736842105241, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.586090479405808, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 1.1, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Go Barge", + "waypointRelativePos": 0.6158001350438894, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Go Barge" + } + } + }, + { + "name": "Shoot Barge", + "waypointRelativePos": 0.9507089804186348, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Shoot Barge" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "middle Barge", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle barge 5.path b/src/main/deploy/pathplanner/paths/middle barge 5.path new file mode 100644 index 0000000..3461db1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle barge 5.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.720081967213115, + "y": 5.619364754098362 + }, + "prevControl": null, + "nextControl": { + "x": 7.004093845707395, + "y": 4.3093563718741414 + }, + "isLocked": false, + "linkedName": "barge shot" + }, + { + "anchor": { + "x": 5.343658717270384, + "y": 2.9994161857734656 + }, + "prevControl": { + "x": 5.868705533594152, + "y": 2.0780158210760726 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle algae 3 end" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.4213369345037113, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.6, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stow", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stow" + } + } + }, + { + "name": "Grab High", + "waypointRelativePos": 0.4321404456448406, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Grab High" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "middle Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle start.path b/src/main/deploy/pathplanner/paths/middle start.path new file mode 100644 index 0000000..faa4019 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middle start.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.192622950819672, + "y": 4.07 + }, + "prevControl": null, + "nextControl": { + "x": 6.32950819672131, + "y": 4.081987704918033 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8305, + "y": 4.07 + }, + "prevControl": { + "x": 6.913286885245901, + "y": 4.090952868852459 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle start end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.4, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "middle autos", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/route 1.path b/src/main/deploy/pathplanner/paths/route 1.path new file mode 100644 index 0000000..bad34f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/route 1.path @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.658196721311475, + "y": 0.8782274590163933 + }, + "prevControl": null, + "nextControl": { + "x": 5.082786885245901, + "y": 1.7653176229508185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.04625, + "y": 2.699 + }, + "prevControl": { + "x": 4.460517251099734, + "y": 2.665591350717763 + }, + "nextControl": { + "x": 3.303012295081967, + "y": 2.7589385245901643 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3785860655737705, + "y": 0.8782274590163933 + }, + "prevControl": { + "x": 1.6053360124517428, + "y": 0.6884115261796728 + }, + "nextControl": { + "x": 1.16025, + "y": 1.060999999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8707500000000006, + "y": 2.855 + }, + "prevControl": { + "x": 3.65497131147541, + "y": 2.5912704918032783 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9092363054778103, + "rotationDegrees": 150.0 + }, + { + "waypointRelativePos": 1.87, + "rotationDegrees": 145.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.4737270875763637, + "maxWaypointRelativePos": 2.4219959266802538, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "Flying", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 0.path b/src/main/deploy/pathplanner/paths/top 0.path new file mode 100644 index 0000000..eb79083 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 0.path @@ -0,0 +1,77 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.97125, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 6.2595, + "y": 5.16575 + }, + "isLocked": false, + "linkedName": "top start" + }, + { + "anchor": { + "x": 5.09, + "y": 5.17 + }, + "prevControl": { + "x": 5.840250000000001, + "y": 5.1754999999999995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 0 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.296774193548388, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + }, + { + "name": "Auto Aim", + "waypointRelativePos": 0.7994598244429404, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Auto Aim" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 1.path b/src/main/deploy/pathplanner/paths/top 1.path new file mode 100644 index 0000000..6118a30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.09, + "y": 5.177 + }, + "prevControl": null, + "nextControl": { + "x": 4.869015617164109, + "y": 5.293901251244973 + }, + "isLocked": false, + "linkedName": "top 0 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.7644303278688527, + "y": 7.064922131147541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.2289205702647632, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 2.path b/src/main/deploy/pathplanner/paths/top 2.path new file mode 100644 index 0000000..cf69aa7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 2.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.5813739388568822, + "y": 7.018376768571864 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": { + "x": 3.026500264999205, + "y": 4.1882495760012715 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L2", + "waypointRelativePos": 0.7893333333333296, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L2" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.9, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 3.path b/src/main/deploy/pathplanner/paths/top 3.path new file mode 100644 index 0000000..d6030ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 3.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.159, + "y": 3.9762499999999994 + }, + "prevControl": null, + "nextControl": { + "x": 2.167053278688525, + "y": 4.659549180327868 + }, + "isLocked": false, + "linkedName": "top 2 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 1.9924754098360646, + "y": 6.9090819672131145 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.1702647657841135, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 4.path b/src/main/deploy/pathplanner/paths/top 4.path new file mode 100644 index 0000000..93b3c86 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 4.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.80375, + "y": 6.94025 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": { + "x": 3.500409836065574, + "y": 5.415573770491803 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 5 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.5482781904118879, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 5.path b/src/main/deploy/pathplanner/paths/top 5.path new file mode 100644 index 0000000..ecd8d9e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 5.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7635, + "y": 5.14625 + }, + "prevControl": null, + "nextControl": { + "x": 2.91301229508587, + "y": 5.907069672136539 + }, + "isLocked": false, + "linkedName": "top 5 end" + }, + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": { + "x": 2.001946721315378, + "y": 6.6982581967267025 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "High Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Start Intake L", + "waypointRelativePos": 0.12790224032586558, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Intake L" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.9, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.0 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top 6.path b/src/main/deploy/pathplanner/paths/top 6.path new file mode 100644 index 0000000..12e224b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top 6.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4527499999999998, + "y": 7.232749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 1.623322068747944, + "y": 7.0499791889741505 + }, + "isLocked": false, + "linkedName": "High Source" + }, + { + "anchor": { + "x": 3.4961116697126777, + "y": 4.966991612650144 + }, + "prevControl": { + "x": 3.2563575713520216, + "y": 5.2247272683878485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "top 6 end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.5725860904794094, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "non-processor autos", + "idealStartingState": { + "velocity": 0, + "rotation": 35.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..b0784a3 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,57 @@ +{ + "robotWidth": 0.94361, + "robotLength": 0.94361, + "holonomicMode": true, + "pathFolders": [ + "Bump auto", + "Example autos", + "Finn's testing autos", + "Flying", + "Chicken", + "Processor side 3 autos", + "Testing", + "bottom L3", + "middle Barge", + "middle autos", + "non-processor 3 autos", + "non-processor autos", + "processor side autos", + "top L3" + ], + "autoFolders": [ + "Flying Series", + "Killing Series", + "L3 autos", + "New Folder", + "New New Folder", + "Testing" + ], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 2.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 68.0, + "robotMOI": 6.9, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.0508, + "driveGearing": 6.75, + "maxDriveSpeed": 4.73, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 70.0, + "wheelCOF": 1.2, + "flModuleX": 0.307975, + "flModuleY": 0.307975, + "frModuleX": 0.307975, + "frModuleY": -0.307975, + "blModuleX": -0.307975, + "blModuleY": 0.307975, + "brModuleX": -0.307975, + "brModuleY": -0.307975, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.221,\"y\":-0.27},\"size\":{\"width\":0.4,\"length\":0.115},\"borderRadius\":0.01,\"strokeWidth\":0.015,\"filled\":false}}", + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.160086,\"y\":0.27},\"size\":{\"width\":0.4,\"length\":0.115},\"borderRadius\":0.01,\"strokeWidth\":0.015,\"filled\":false}}" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 639b994..c831e15 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,19 +4,39 @@ package frc.robot; -import com.revrobotics.spark.config.LimitSwitchConfig.Type; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; - +import com.pathplanner.lib.util.GeometryUtil; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.util.AllianceFlipUtil; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -37,17 +57,19 @@ public static class ControllerConstants { } public static class RobotConstants { - public static final double robotWidthMeters = Units.inchesToMeters(25.0); - public static final double robotLengthMeters = Units.inchesToMeters(25.0); - - // TODO: ############## REPLACE PLACEHOLDERS ############## - public static final double TOTAL_MASS_KG = 10; - public static final double MOMENT_OF_INERTIA = 1; + public static final double robotWidthMeters = Units.inchesToMeters(29.5); + public static final double robotLengthMeters = Units.inchesToMeters(29.5); + + // TODO: ############## REPLACE PLACEHOLDERS ############## + public static final double TOTAL_MASS_KG = 74.088; + public static final double MOMENT_OF_INERTIA = 6.883; } public static final class FieldConstants { public static final double GRAVITY = 9.81; - public static final double SPEAKER_HEIGHT = 2.05; // Meters + + public static final double FIELD_LENGTH = Units.inchesToMeters(690.876); // TODO: CHECK IF ACCURATE + public static final double FIELD_WIDTH = Units.inchesToMeters(317); public static Alliance getAlliance() { if (DriverStation.getAlliance().isPresent()) { @@ -56,80 +78,133 @@ public static Alliance getAlliance() { return Alliance.Blue; } + + public static ArrayList getSourcePoses() { + Pose2d leftCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + Pose2d rightCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + + leftCenterFace = AllianceFlipUtil.apply(leftCenterFace); + rightCenterFace = AllianceFlipUtil.apply(rightCenterFace); + + ArrayList poses = new ArrayList(); + + poses.add(leftCenterFace); + poses.add(rightCenterFace); + + return poses; + } + + public static ArrayList getSourceSidePoses() { + Pose2d leftCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176 + 300.0), + Rotation2d.fromDegrees(90 - 144.011)); + Pose2d rightCenterFace = new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824 - 300.0), + Rotation2d.fromDegrees(144.011 - 90)); + + leftCenterFace = AllianceFlipUtil.apply(leftCenterFace); + rightCenterFace = AllianceFlipUtil.apply(rightCenterFace); + + ArrayList poses = new ArrayList(); + + poses.add(leftCenterFace); + poses.add(rightCenterFace); + + return poses; + } + + public static Pose2d getReefPose() { + Pose2d reef = new Pose2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501), new Rotation2d()); + return AllianceFlipUtil.apply(reef); + } } public static class SwerveModuleConstants { - public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); + public static final double WHEEL_DIAMETER = Units.inchesToMeters(3.85); // ~4 in public static final double STEERING_GEAR_RATIO = 1.d / (150d / 7d); // This is for L2 modules with 16T pinions - public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d) * (16.f / 14.f); + public static final double DRIVE_GEAR_RATIO = (1.d / 6.75d); public static final double DRIVE_ROTATION_TO_METER = DRIVE_GEAR_RATIO * Math.PI * WHEEL_DIAMETER; public static final double STEER_ROTATION_TO_RADIANS = STEERING_GEAR_RATIO * Math.PI * 2d; public static final double DRIVE_METERS_PER_MINUTE = DRIVE_ROTATION_TO_METER / 60d; public static final double STEER_RADIANS_PER_MINUTE = STEER_ROTATION_TO_RADIANS / 60d; + public static final double STEER_MAX_RAD_SEC = 0.8 * STEERING_GEAR_RATIO * ((5880.f * 2.f * Math.PI) / 60.f); // TODO: ############## REPLACE PLACEHOLDERS ############## - public static final double WHEEL_FRICTION_COEFFICIENT = 1; + public static final double WHEEL_FRICTION_COEFFICIENT = 1.2; // Actual drive gains // public static final double MODULE_KP = 0.5; // public static final double MODULE_KD = 0.03; // NOTE: This may need additional tuning! - public static final double MODULE_KP = 0.46368;// 0.75628;// 0.7491; //.5; - public static final double MODULE_KD = 0.0066806;// 0.0057682; //0.0076954; + public static final double MODULE_KP = 0.46368;// goooood // 0.75628;// 0.7491; //.5; + public static final double MODULE_KD = 0.0050806;// gooood // 0.0057682; //0.0076954; // --------- Front Left Module --------- \\ - public static final int FL_DRIVE_ID = 4; + public static final int FL_DRIVE_ID = 34; public static final int FL_STEER_ID = 4; - public static final int FL_ABSOLUTE_ENCODER_PORT = 4; - public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(0.389893) + Math.PI * 0.5 + Math.PI; - public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = true; + public static final int FL_ABSOLUTE_ENCODER_PORT = 54; + public static final double FL_OFFSET_RADIANS = Units.rotationsToRadians(-0.310303); + public static final boolean FL_ABSOLUTE_ENCODER_REVERSED = false; public static final boolean FL_MOTOR_REVERSED = true; + public static final boolean FL_STEERING_MOTOR_REVERSED = true; // --------- Front Right Module --------- \\ - public static final int FR_DRIVE_ID = 1; + public static final int FR_DRIVE_ID = 31; public static final int FR_STEER_ID = 1; - public static final int FR_ABSOLUTE_ENCODER_PORT = 1; - public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(0.323730) + Math.PI * 0.5 + Math.PI; - public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = true; + public static final int FR_ABSOLUTE_ENCODER_PORT = 51; + public static final double FR_OFFSET_RADIANS = Units.rotationsToRadians(-0.253906); + public static final boolean FR_ABSOLUTE_ENCODER_REVERSED = false; public static final boolean FR_MOTOR_REVERSED = true; + public static final boolean FR_STEERING_MOTOR_REVERSED = true; // --------- Back Right Module --------- \\ - public static final int BR_DRIVE_ID = 2; + public static final int BR_DRIVE_ID = 32; public static final int BR_STEER_ID = 2; - public static final int BR_ABSOLUTE_ENCODER_PORT = 2; - public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(-0.360107) + Math.PI * 0.5 + Math.PI; - public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = true; + public static final int BR_ABSOLUTE_ENCODER_PORT = 52; + public static final double BR_OFFSET_RADIANS = Units.rotationsToRadians(0.353027); + public static final boolean BR_ABSOLUTE_ENCODER_REVERSED = false; public static final boolean BR_MOTOR_REVERSED = true; + public static final boolean BR_STEERING_MOTOR_REVERSED = true; // --------- Back Left Module --------- \\ - public static final int BL_DRIVE_ID = 3; + public static final int BL_DRIVE_ID = 33; public static final int BL_STEER_ID = 3; - public static final int BL_ABSOLUTE_ENCODER_PORT = 3; - public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(0.399902) + Math.PI * 0.5 + Math.PI; - public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = true; + public static final int BL_ABSOLUTE_ENCODER_PORT = 53; + public static final double BL_OFFSET_RADIANS = Units.rotationsToRadians(-0.134033); + public static final boolean BL_ABSOLUTE_ENCODER_REVERSED = false; public static final boolean BL_MOTOR_REVERSED = true; - + public static final boolean BL_STEERING_MOTOR_REVERSED = true; } public static class DriveConstants { // TODO: Make sure that this is correct - this is from the SDS website but needs // empirical verification - public static final double MAX_MODULE_VELOCITY = 5.21; - public static final double MAX_ROBOT_VELOCITY = 5.21; + public static final double MAX_MODULE_VELOCITY = 4.2; + public static final double MAX_ROBOT_VELOCITY = 4.2; public static final double MAX_ROBOT_RAD_VELOCITY = 12.0; // Approx. Measured rads/sec - // TODO: ############## REPLACE PLACEHOLDERS ############## - public static final double MAX_MODULE_CURRENT = 10; + public static final double MAX_MODULE_CURRENT = 70; + + // (((((65kg×1(m/s^2))/4)×(4in/2))/6.75)/(0.0194Nm/A))×(0.033ohm) + public static final double GLOBAL_kA = 0.135; // V/(m/ss) - public static final double TRACK_WIDTH = Units.inchesToMeters(19.75); - public static final double WHEEL_BASE = Units.inchesToMeters(19.75); - // TODO: Set this for FWERB V2 - public static final Rotation2d NAVX_ANGLE_OFFSET = Rotation2d.fromDegrees(-90); - // TODO: I'm not going to touch this... but it seems important! - public static final double DRIVE_BASE_RADIUS = Units.inchesToMeters(15); + public static final double TRACK_WIDTH = Units.inchesToMeters(19.675); + public static final double WHEEL_BASE = Units.inchesToMeters(19.675); + public static final double FULL_ROBOT_WIDTH = Units.inchesToMeters(37.50); + + public static final PIDConstants TRANSLATION_ASSIST = new PIDConstants(8, 0, 0.01); + public static final PIDConstants ROTATION_ASSIST = new PIDConstants(7.0, 0, 0.02); public static final class ModuleIndices { public static final int FRONT_LEFT = 0; @@ -150,71 +225,315 @@ public static final class ModuleIndices { public static class CommonConstants { public static final boolean LOG_INTO_FILE_ENABLED = true; + public static final boolean LOG_TO_NETWORKTABLES = true; } - public static class Elevator { - public static final int elevatorOnePort = 10; - public static final int elevatorTwoPort = 11; + public static class Climber { + public static final int MOTOR_PORT = 20; + public static final ProfiledPIDController PID = new ProfiledPIDController( + 1, + 0.0, + 0.0, + new TrapezoidProfile.Constraints(Double.MAX_VALUE, Double.MAX_VALUE)); - public static boolean elevatorOneInverted = true; - public static boolean elevatorTwoInverted = false; + public static boolean DBG_DISABLED = false; + public static double GEAR_RATIO = 45.0; - public static Type bottomLimitMode = Type.kNormallyOpen; + public static double DEPLOY_SOFT_LIMIT = -6.0; + public static double CLIMB_SOFT_LIMIT = -2.65; // 2.75 + } + public static class Coral { + public static boolean DEBUG_PIDS = false; + + public static class Pivot { + public static final int MOTOR_PORT = 14; + public static final int ENCODER_PORT = 28; + public static boolean DBG_DISABLED = false; + + public static final boolean ENCODER_INVERTED = false; + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(80); + public static final double ELEVATOR_BORDER_ANGLE = Units.degreesToRadians(10); + + // TODO: Tune in simulation + public static final ProfiledPIDController PID = new ProfiledPIDController( + 12.0, + 0.0, + 0.005, + new TrapezoidProfile.Constraints(7.0, 15.0)); + + // Updated with THEORETICAL values + public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward( + 0.0, + 0.0, // V + 0.0, // 1.0, // V*s/rad + 0.00// V*s^2/rad + ); + + public static class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeoVortex(1); + public static final double NET_REDUCTION = 96.0; + public static final double MASS_KG = 4.7727; + public static final double ARM_LENGTH_METERS = 0.510; + public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(23.0); + public static final double MOI = 0.2875548495; // Kg*m^2 + } + } - public static double motorTurnsPerMeter = 39.44; + public static class Roll { + public static final int MOTOR_PORT = 15; + public static final boolean MOTOR_INVERTED = false; + public static final boolean ENCODER_INVERTED = false; + public static final double ENCODER_SCORE_SIDE_VOLTS = 2.70; + public static final double ENCODER_INTAKE_SIDE_VOLTS = 1.0; // TODO: SET THIS!!! + + public static final double ENCODER_OFFSET_VOLTS = -(ENCODER_INTAKE_SIDE_VOLTS + ENCODER_SCORE_SIDE_VOLTS) / 2.0;// -1.86; + public static final double ENCODER_VOLTS_TO_RAD = Math.PI + / (ENCODER_SCORE_SIDE_VOLTS - ENCODER_INTAKE_SIDE_VOLTS); + + public static boolean DBG_DISABLED = false; + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 7.0, + 0.0, + 0.1, + new TrapezoidProfile.Constraints(13, 21.0)); + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(90 + 40.0); + + public static final double VELOCITY_FF = 0.0;// Volts per radian/second + + public static class PhysicalConstants { + public static DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double NET_REDUCTION = (54.0 / 12.0) * 20.0; + public static final double MASS_KG = 2.85; // Includes a coral + public static final double ARM_LENGTH_METERS = 0.083; + public static final double JOINT_LENGTH_METERS = 0.10; + public static final double MOI = 0.0403605447; // Kg*m^2 + } + } + + public static class Pitch { + public static boolean DEBUG_PIDS = false; + + public static final int MOTOR_PORT = 16; + public static final boolean MOTOR_INVERTED = true; + public static final boolean ENCODER_INVERTED = true; + public static final double ENCODER_OFFSET_VOLTS = -2.046; + public static boolean DBG_DISABLED = false; + + public static final double MAXIMUM_ANGLE = Units.degreesToRadians(115.0); + public static final double MINIMUM_ANGLE = Units.degreesToRadians(-20.0); + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 20.0, // was 7.0 + 0.0, + 0.065, + new TrapezoidProfile.Constraints(10.0, 30.0), 1.0 / 200.0); // Radians + + public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward(0.050, 0.21, 0.44, 0.005); + + public static class PhysicalConstants { + public static DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double NET_REDUCTION = 92.85714286; // Yeah this is cursed + public static final double MASS_KG = 2.16; // Includes a coral + public static final double ARM_LENGTH_METERS = 0.101; + public static final double JOINT_LENGTH_METERS = Units.inchesToMeters(13.875); + public static final double MOI = 0.0200055915; // Kg*m^2 + } + } + + public static class Intake { + public static final int MOTOR_PORT = 17; + public static boolean DBG_DISABLED = false; + public static boolean MOTOR_INVERTED = true; + + public static final double POSITIVE_RATE_LIMIT = 200.0; // Fast shoot + public static final double NEGATIVE_RATE_LIMIT = 100.0; // Slow intake + + public static final double SCORE_EXTRA_SECONDS = 1.0; + + public static final double CURRENT_LIMIT = 12.5; // Stator + + public static final class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getFalcon500(1); + public static final double MOI = 0.1; // J*KG / M^2 + public static final double GEARING = 1; + } + } + + public class Vision { + public static double CAM_FORWARDS = Units.inchesToMeters(1.141310); + public static double CAM_LEFT = Units.inchesToMeters(-8.606161); + public static double CAM_UP = Units.inchesToMeters(36.004011); + public static Rotation2d CAM_YAW = Rotation2d.fromDegrees(-85.0); + + public static Pose3d CAM_POSE = new Pose3d(CAM_FORWARDS, CAM_LEFT, CAM_UP, new Rotation3d(CAM_YAW)); + + public static double CAM_FOV_HORIZ = Units.degreesToRadians(58.51); + public static double CAM_FOV_VERT = Units.degreesToRadians(45.57); + public static double CAM_MAX_DIST = 2.0; // Meters + + public static Translation2d SCORING_POSITION = new Translation2d( + Units.inchesToMeters(8.5), // TODO: Forwards + Units.inchesToMeters(-18.75 - 11.0) // This includes the shift of reef base pole to scoring pole + ); + public static Translation2d SCORING_BUMPER_POINT = new Translation2d( + Units.inchesToMeters(8.5), // TODO: Forwards + Units.inchesToMeters(-18.75) // This includes the shift of reef base pole to scoring pole + ); + } + } + + // TODO: ##################### PLACEHOLDERS ##################### + public static final class Algae { + public static final boolean DEBUG_PIDS = false; + + public static final class Pivot { + public static final int MOTOR_PORT = 18; + public static final boolean MOTOR_INVERTED = true; + + public static final ProfiledPIDController PID = new ProfiledPIDController( + 1.0, + 0.0, + 0.0, + // RADIANS + new TrapezoidProfile.Constraints(20.f, 20.f)); + // public static final ArmFeedforward FEEDFORWARD = new ArmFeedforward( + // 1, + // 1, + // 1); + + public static final double RETRACTED_LIMIT = Units.degreesToRadians(0.0); + public static final double EXTENDED_LIMIT = Units.degreesToRadians(100.0); + + public static class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double NET_REDUCTION = 104.2285714; // Yeah this is cursed + public static final double MASS_KG = 1.36078; + public static final double ARM_LENGTH_METERS = 0.1620899476; + public static final double MOI = 0.0303284342; // Kg*m^2 + } + } + + public static final class Intake { + public static final int MOTOR_PORT = 19; + public static final boolean MOTOR_INVERTED = true; + public static final int LASERCAN_ID = 4; // NOTE: Beambreak will *probably* be a rockwell proximity sensor + // wired into the SPARK max + public static final double HOLDING_THRESHOLD = Units.inchesToMeters(5.5); + public static final double SHOT_THRESHOLD = Units.feetToMeters(1.4); + + public static final double POSITIVE_RATE_LIMIT = 5.0; + public static final double NEGATIVE_RATE_LIMIT = -5.0; + + public static final class PhysicalConstants { + public static final DCMotor MOTOR = DCMotor.getNeo550(1); + public static final double MOI = 0.1; // J*KG / M^2 + public static final double GEARING = 1; + } + } + } + + // See + // https://cad.onshape.com/documents/fa9a0365dfdf7e376f93f1b4/w/36bfb0cc9de95ef5933791e3/e/700ba3cf920578fe61d3ec24 + public static final class Elevator { + public static final boolean DEBUG_PIDS = false; + + public static final class Leader { + public static final int MOTOR_PORT = 10; + public static final boolean INVERTED = false; + } + + public static final class Follower { + public static final int MOTOR_PORT = 11; + public static final boolean INVERTED = true; + } + + public static boolean DBG_DISABLED = false; + + public static double MOTOR_REVOLUTIONS_PER_METER = 32.8083989501; + + // TODO: Tune! Use FWERB for now public static class PID { - public static double kP = 20.0; // 9.0; + public static double kP = 80.0;// 100.0; public static double kI = 0.0; - public static double kD = 0.5; // 4.0; - public static double MAX_VELOCITY = 2.8; - public static double MAX_ACCELERATION = 18.0; + public static double kD = 0.02; + public static double MAX_VELOCITY = 2.8;// 2.8;// 2.8; + // TODO: Needs empirical testing - analyze setpoint v/s state graphs to see if + // the elevator can make or exceed this + public static double MAX_ACCELERATION = 10.0;// 10.0;// 20.0; + public static double MAX_JERK = MAX_ACCELERATION * 10.0; } - // TODO: For the first testing, set these all to zero for safety reasons - // Remind me to pad the top and bottom of the elevator with poodles to make sure - // we don't damage it. - public static class Feedforward { + // TODO: PAD THE ELEVATOR!!!!!!! + public static class FeedforwardConstants { public static double Ks = 0.0; - public static double Kv = 4.0; - public static double Ka = 0.03; - public static double Kg = 0.1; + public static double Kv = 3.95; // Test: 2.40 was 2.45 + public static double Ka = 0.03; // Test: 0.11 was 0.1 + public static double Kg = 0.1; // Test 0.075 was 0.05 } + public static ElevatorFeedforward FEEDFORWARD = new ElevatorFeedforward( + FeedforwardConstants.Ks, + FeedforwardConstants.Kg, + FeedforwardConstants.Kv, + FeedforwardConstants.Ka); + public static class PhysicalParameters { - public static double gearReduction = 9.0 / 2.0; - public static double driveRadiusMeters = 0.0182; - public static double carriageMassKg = 1.5; - public static double elevatorHeightMeters = Units.inchesToMeters(50.0); - public static double elevatorBottomFromFloorMeters = Units.inchesToMeters(12.0); - public static double elevatorCarriageHeightMeters = Units.inchesToMeters(6.0); - public static double elevatorForwardsFromRobotCenterMeters = Units.inchesToMeters(25.0 / 2); - public static DCMotor simMotor = DCMotor.getNeoVortex(2); + public static final double GEARING = 5.0 / 2.0; + public static final double DRIVE_RADIUS_METERS = 0.0121; + public static final double CARRIAGE_MASS_KG = 2.00; // Load on the SECOND stage NOTE: This includes the + // weight + + public static final double MAX_TRAVEL = Units.inchesToMeters(59.5); + public static final double BOTTOM_TO_FLOOR = Units.inchesToMeters(3.0); // Relative to bottom of stage 2 + public static final double CARRIAGE_HEIGHT = Units.inchesToMeters(8.0); // Bottom to top of stage 2 + + public static final double CORAL_PIVOT_VERTICAL_OFFSET = Units.inchesToMeters(6.0); // From bottom of stage + // 2 to + // coral arm pivot axis + public static final double CORAL_PIVOT_HORIZONTAL_OFFSET = Units.inchesToMeters(7.5); // From bottom of + // stage 2 to + // coral arm pivot + // axis + public static final double ELEVATOR_FORWARDS_OFFSET = Units.inchesToMeters(1); // To mid-plane of elevator + + public static final DCMotor MOTOR = DCMotor.getKrakenX60(2); } } public static final class PathPlannerConstants { - public static final PIDConstants TRANSLATION_PID = new PIDConstants(5, 0, 0.2); - public static final PIDConstants ROTATION_PID = new PIDConstants(5, 0, 0.2); + // public static final PIDConstants TRANSLATION_PID = new PIDConstants(12, 0, + // 0.01); + // public static final PIDConstants ROTATION_PID = new PIDConstants(11.0, 0, + // 0.02); + public static final PIDConstants TRANSLATION_PID = new PIDConstants(15.0, 0, 0.005); + public static final PIDConstants ROTATION_PID = new PIDConstants(9.0, 0, 0.03); // was 10 and 9 public static final PPHolonomicDriveController HOLONOMIC_FOLLOWER_CONTROLLER = new PPHolonomicDriveController( - TRANSLATION_PID, - ROTATION_PID - ); + TRANSLATION_PID, + ROTATION_PID); public static final RobotConfig ROBOT_CONFIG = new RobotConfig( - RobotConstants.TOTAL_MASS_KG, - RobotConstants.MOMENT_OF_INERTIA, - new ModuleConfig( - SwerveModuleConstants.WHEEL_DIAMETER/2, - DriveConstants.MAX_MODULE_VELOCITY, - SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, // TODO: ############## REPLACE PLACEHOLDERS ############## - DCMotor.getKrakenX60(1), - DriveConstants.MAX_MODULE_CURRENT, // TODO: ############## REPLACE PLACEHOLDERS ############## - 4 - ) - ); + RobotConstants.TOTAL_MASS_KG, + RobotConstants.MOMENT_OF_INERTIA, + new ModuleConfig( + SwerveModuleConstants.WHEEL_DIAMETER / 2, + DriveConstants.MAX_MODULE_VELOCITY, + SwerveModuleConstants.WHEEL_FRICTION_COEFFICIENT, + DCMotor.getKrakenX60(1), + DriveConstants.MAX_MODULE_CURRENT, + 1), + DriveConstants.KINEMATICS.getModules()); + } + + public static final class AutoConstants { + public static final double SCORE_WAIT_BEFORE_SECONDS = 0.25; + public static final double SCORE_WAIT_AFTER_SECONDS = 0.15; + public static final double ALGAE_BARGE_SCORE_WAIT = 0.5; } public static final class PoseConstants { @@ -225,5 +544,16 @@ public static final class PoseConstants { public static final double kVisionStdDevY = 5; public static final double kVisionStdDevX = 5; public static final double kVisionStdDevTheta = 500; + + private static AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + public final static HashMap tagPoses = new HashMap() { + { + for (int i = 0; i < 22; ++i) { + if (tagLayout.getTagPose(i + 1).isPresent()) + put(i, tagLayout.getTagPose(i + 1).get().toPose2d()); + } + } + }; } } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 3f0b9af..854968f 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,10 +1,14 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) package frc.robot; +import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.LimelightHelpers.LimelightResults; +import frc.robot.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,6 +21,7 @@ import java.net.HttpURLConnection; import java.net.MalformedURLException; import java.net.URL; +import java.util.Map; import java.util.concurrent.CompletableFuture; import com.fasterxml.jackson.annotation.JsonFormat; @@ -25,9 +30,19 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") @@ -92,16 +107,22 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; + + @JsonProperty("ty") + public double ty; @JsonProperty("txp") public double tx_pixels; - @JsonProperty("ty") - public double ty; - @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -115,6 +136,9 @@ public LimelightTarget_Retro() { } + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ public static class LimelightTarget_Fiducial { @JsonProperty("fID") @@ -186,15 +210,21 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -207,10 +237,58 @@ public LimelightTarget_Fiducial() { } } + /** + * Represents a Barcode Target Result extracted from JSON Output + */ public static class LimelightTarget_Barcode { + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } } + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Classifier { @JsonProperty("class") @@ -241,6 +319,9 @@ public LimelightTarget_Classifier() { } } + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Detector { @JsonProperty("class") @@ -258,21 +339,32 @@ public static class LimelightTarget_Detector { @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + public LimelightTarget_Detector() { } } - public static class Results { - + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + @JsonProperty("pID") public double pipelineID; @@ -357,7 +449,7 @@ public Pose2d getBotPose2d_wpiBlue() { @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; - public Results() { + public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; @@ -369,30 +461,21 @@ public Results() { targets_Barcode = new LimelightTarget_Barcode[0]; } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } } + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { @@ -406,6 +489,47 @@ public RawFiducial(int id, double txnc, double tync, double ta, double distToCam } } + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -414,11 +538,28 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { this.pose = pose; this.timestampSeconds = timestampSeconds; @@ -428,9 +569,45 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; } + } + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; /** @@ -445,7 +622,13 @@ static final String sanitizeName(String name) { return name; } - private static Pose3d toPose3D(double[] inData){ + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -457,7 +640,14 @@ private static Pose3d toPose3D(double[] inData){ Units.degreesToRadians(inData[5]))); } - private static Pose2d toPose2D(double[] inData){ + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 2D Pose Data!"); @@ -468,7 +658,44 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } - private static double extractBotPoseEntry(double[] inData, int position){ + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ if(inData.length < position+1) { return 0; @@ -476,27 +703,35 @@ private static double extractBotPoseEntry(double[] inData, int position){ return inData[position]; } - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray,6); - int tagCount = (int)extractBotPoseEntry(poseArray,7); - double tagSpan = extractBotPoseEntry(poseArray,8); - double tagDist = extractBotPoseEntry(poseArray,9); - double tagArea = extractBotPoseEntry(poseArray,10); - //getlastchange() in microseconds, ll latency in milliseconds - var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); - - + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial*tagCount; - + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + if (poseArray.length != expectedTotalVals) { // Don't populate fiducials - } - else{ + } else { for(int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); int id = (int)poseArray[baseIndex]; @@ -509,11 +744,89 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; } - private static void printPoseEstimate(PoseEstimate pose) { + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; @@ -526,6 +839,7 @@ private static void printPoseEstimate(PoseEstimate pose) { System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); System.out.println(); if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { @@ -548,14 +862,30 @@ private static void printPoseEstimate(PoseEstimate pose) { } } + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } @@ -572,10 +902,16 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } + public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -590,30 +926,171 @@ public static URL getLimelightURLString(String tableName, String request) { ///// ///// + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ public static double getTX(String limelightName) { return getLimelightNTDouble(limelightName, "tx"); } + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ public static double getTY(String limelightName) { return getLimelightNTDouble(limelightName, "ty"); } + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ public static double getTA(String limelightName) { return getLimelightNTDouble(limelightName, "ta"); } + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ public static double getLatency_Capture(String limelightName) { return getLimelightNTDouble(limelightName, "cl"); } + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ public static double getCurrentPipelineIndex(String limelightName) { return getLimelightNTDouble(limelightName, "getpipe"); } + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } @@ -691,6 +1168,10 @@ public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + ///// ///// @@ -699,36 +1180,71 @@ public static Pose3d getBotPose3d(String limelightName) { return toPose3D(poseArray); } + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ public static Pose3d getBotPose3d_wpiRed(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); return toPose3D(poseArray); } + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ public static Pose3d getBotPose3d_wpiBlue(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); return toPose3D(poseArray); } + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ public static Pose3d getBotPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); return toPose3D(poseArray); @@ -748,25 +1264,24 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); } /** @@ -790,7 +1305,7 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); + return getBotPoseEstimate(limelightName, "botpose_wpired", false); } /** @@ -800,7 +1315,7 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); } /** @@ -816,9 +1331,21 @@ public static Pose2d getBotPose2d(String limelightName) { return toPose2D(result); } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); } ///// @@ -834,8 +1361,8 @@ public static void setPriorityTagID(String limelightName, int ID) { } /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera */ public static void setLEDMode_PipelineControl(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 0); @@ -853,29 +1380,38 @@ public static void setLEDMode_ForceOn(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 3); } + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_Standard(String limelightName) { setLimelightNTDouble(limelightName, "stream", 0); } + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPMain(String limelightName) { setLimelightNTDouble(limelightName, "stream", 1); } + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) */ public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { double[] entries = new double[4]; @@ -885,10 +1421,44 @@ public static void setCropWindow(String limelightName, double cropXMin, double c entries[3] = cropYMax; setLimelightNTDoubleArray(limelightName, "crop", entries); } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; @@ -898,8 +1468,48 @@ public static void SetRobotOrientation(String limelightName, double yaw, double entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); } + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { double[] validIDsDouble = new double[validIDs.length]; for (int i = 0; i < validIDs.length; i++) { @@ -907,7 +1517,50 @@ public static void SetFiducialIDFiltersOverride(String limelightName, int[] vali } setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -964,7 +1617,9 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) } /** - * Parses Limelight's JSON results dump into a LimelightResults Object + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data */ public static LimelightResults getLatestResults(String limelightName) { @@ -982,7 +1637,7 @@ public static LimelightResults getLatestResults(String limelightName) { long end = System.nanoTime(); double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; + results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d62800b..1698820 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,14 +4,36 @@ package frc.robot; +import org.littletonrobotics.urcl.URCL; + +import com.ctre.phoenix6.SignalLogger; +import com.fasterxml.jackson.databind.JsonSerializer; + +import au.grapplerobotics.CanBridge; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.epilogue.logging.FileBackend; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.net.WebServer; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.SwerveSubsystem.RotationStyle; /** * The VM is configured to automatically run this class, and to call the @@ -22,122 +44,187 @@ * build.gradle file in the * project. */ +@Logged(strategy = Strategy.OPT_IN) public class Robot extends TimedRobot { - - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - public static SendableChooser autoChooser = new SendableChooser<>(); - //LEDstripOne m_stripOne = new LEDstripOne(9); - - /** - * This function is run when the robot is first started up and should be used - * for any - * initialization code. - */ - @Override - public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our - // autonomous chooser on the dashboard. - // Shuffleboard.getTab("SmartDashboard").add(autoChooser); - SmartDashboard.putData(autoChooser); - - DataLogManager.start(); + + private Command m_autonomousCommand; + + @Logged + private RobotContainer m_robotContainer; + + public static SendableChooser autoChooser = new SendableChooser<>(); + + double lastLoopTime = Timer.getFPGATimestamp(); + @Logged + double loopTime = 0.02; + @Logged + double commandSchedulerTime = 0.02; + + DoublePublisher loopPub = NetworkTableInstance.getDefault() + .getDoubleTopic("loopTime").publish(); + DoublePublisher csTimePublisher = NetworkTableInstance.getDefault() + .getDoubleTopic("csTime").publish(); + + public Robot() { + DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); - m_robotContainer = new RobotContainer(); - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items - * like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

- * This runs after the mode specific periodic functions, but before LiveWindow - * and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled - // commands, running already-scheduled commands, removing finished or - // interrupted commands, - // and running subsystem periodic() methods. This must be called from the - // robot's periodic - // block in order for anything in the Command-based framework to work. - - CommandScheduler.getInstance().run(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() { - m_robotContainer.getSwerveSubsystem().stopDrive(); - } - - @Override - public void disabledPeriodic() { - } - - /** - * This autonomous runs the autonomous command selected by your - * {@link RobotContainer} class. - */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + // Pheonix 6 Signal Logging + SignalLogger.start(); + + // URCL (REV) Logging + if (Constants.CommonConstants.LOG_TO_NETWORKTABLES) { + URCL.start(); + } else { + URCL.start(DataLogManager.getLog()); + } + + Epilogue.configure(config -> { + config.backend = new FileBackend(DataLogManager.getLog()); + config.minimumImportance = Logged.Importance.DEBUG; + }); + Epilogue.bind(this); + } + + /** + * This function is run when the robot is first started up and should be used + * for any + * initialization code. + */ + @Override + public void robotInit() { + m_robotContainer = new RobotContainer(); + + // CanBridge.runTCP(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our + // autonomous chooser on the dashboard. + // Shuffleboard.getTab("SmartDashboard").add(autoChooser); + SmartDashboard.putData(autoChooser); + + // Put git/code version metadata on networktables + NetworkTable versionTable = NetworkTableInstance.getDefault().getTable("Version"); + versionTable.putValue("GIT_SHA", NetworkTableValue.makeString(BuildConstants.GIT_SHA)); + versionTable.putValue("BUILD_DATE", NetworkTableValue.makeString(BuildConstants.BUILD_DATE)); + versionTable.putValue("GIT_BRANCH", NetworkTableValue.makeString(BuildConstants.GIT_BRANCH)); + versionTable.putValue("DIRTY", NetworkTableValue.makeBoolean(BuildConstants.DIRTY != 0)); + + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); + + addPeriodic(new Runnable() { + @Override + public void run() { + m_robotContainer.periodic200Hz(); + } + }, 1.0 / 200.0); } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() { - } - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - m_robotContainer.getSwerveSubsystem().setRotationStyle(RotationStyle.Driver); - // m_robotContainer.resetShootake(); - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + + /** + * This function is called every 20 ms, no matter the mode. Use this for items + * like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic + // block in order for anything in the Command-based framework to work. + double startTime = Timer.getFPGATimestamp(); + + CommandScheduler.getInstance().run(); + + double currentTime = Timer.getFPGATimestamp(); + loopTime = currentTime - lastLoopTime; + commandSchedulerTime = currentTime - startTime; + lastLoopTime = currentTime; + + loopPub.set(loopTime); + csTimePublisher.set(commandSchedulerTime); + } + + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() { + Shuffleboard.selectTab("Autonomous"); + + m_robotContainer.getSwerveSubsystem().stopDrive(); + } + + @Override + public void disabledPeriodic() { + // m_robotContainer.zeroGyroAutoPrelim(); + } + + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + // m_robotContainer.resetShootake(); + Shuffleboard.selectTab("Teleoperated"); + + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + + m_robotContainer.operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + m_robotContainer.driverXbox.setRumble(RumbleType.kBothRumble, 0.0); + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() { + } + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() { + } + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() { + } - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() { - } - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() { - } - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() { - - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0bbe4cc..ad24544 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,22 +4,57 @@ package frc.robot; -import frc.robot.Constants.*; -import frc.robot.commands.*; -import frc.robot.commands.ElevatorCommand.ElevatorPresets; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.Supplier; import com.pathplanner.lib.auto.AutoBuilder; -import frc.robot.subsystems.*; - -import java.util.function.DoubleSupplier; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.ControllerConstants; +import frc.robot.commands.DriveCommand; +import frc.robot.commands.DriveCommand.DriveStyle; +import frc.robot.commands.algae.IntakeAlgaeCommand; +import frc.robot.commands.algae.PurgeAlgaeCommand; +import frc.robot.commands.algae.RemoveAlgaeCommand; +import frc.robot.commands.algae.ShootAlgaeBargeCommand; +import frc.robot.commands.algae.ShootAlgaeCommand; +import frc.robot.commands.coral.IntakeCoralCommand; +import frc.robot.commands.coral.PurgeCoralIntakeCommand; +import frc.robot.commands.coral.ScoreCoralCommand; +import frc.robot.commands.coral.motion.WristAlignAssist; +import frc.robot.commands.coral.motion.WristStowSafety; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.Limelight.LimelightType; +import frc.robot.subsystems.RobotMechanismLogger; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; +import frc.robot.subsystems.coral.CoralSubsystem.MirrorPresets; +import frc.robot.util.LimelightContainer; /** * This class is where the bulk of the robot should be declared. Since @@ -30,25 +65,55 @@ * the robot (including * subsystems, commands, and trigger mappings) should be declared here. */ +@Logged(strategy = Logged.Strategy.OPT_IN) public class RobotContainer { - private final CommandXboxController driverXbox = new CommandXboxController( + // private static final Limelight LL_BF = new Limelight(LimelightType.LL4, + // "limelight-bf", true, true); + private static final Limelight LL_BR = new Limelight(LimelightType.LL4, "limelight-br", true, true); + private static final Limelight LL_BL = new Limelight(LimelightType.LL4, "limelight-bl", true, true); + private static final Limelight LL_FR = new Limelight(LimelightType.LL4, "limelight-fr", true, true); + + @Logged + public static final LimelightContainer LLContainer = new LimelightContainer(LL_BR, LL_BL, LL_FR);// , LL_BF); + + // @Logged + public final CommandXboxController driverXbox = new CommandXboxController( ControllerConstants.DRIVER_CONTROLLER_PORT); - private final CommandXboxController operatorXbox = new CommandXboxController( + // @Logged + public final CommandXboxController operatorXbox = new CommandXboxController( ControllerConstants.OPERATOR_CONTROLLER_PORT); + // private final CommandXboxController debugXboxController = new + // CommandXboxController(3); + + private final UsbCamera climbCamera; // private final CommandXboxController debugXbox = new CommandXboxController(0); private final SendableChooser autoChooser; - private final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(); + @Logged + public final SwerveSubsystem swerveDriveSubsystem = new SwerveSubsystem(); + // private final LimeLightSubsystem limeLightSubsystem = new // LimeLightSubsystem(); - - private final UsbCamera intakeCam = CameraServer.startAutomaticCapture(); + @Logged private final DriveCommand normalDrive = new DriveCommand(swerveDriveSubsystem, driverXbox.getHID()); - private final ElevatorSubsystem elevator = new ElevatorSubsystem(); + // NOTE: Removed to prevent loop overruns while the robot does not have the + // algae manipulator installed. + @Logged + private final AlgaeSubsystem algaeSubsystem = new AlgaeSubsystem(); + + @Logged + private final CoralSubsystem coralSubsystem = new CoralSubsystem(swerveDriveSubsystem, operatorXbox.getHID(), + algaeSubsystem); + + @Logged + private final ClimberSubsystem climberSubsystem = new ClimberSubsystem(operatorXbox.getHID()); + + private final RobotMechanismLogger robotLogger = new RobotMechanismLogger(coralSubsystem, swerveDriveSubsystem, + algaeSubsystem); /* * The container for the robot. Contains subsystems, OI devices, and commands. @@ -60,54 +125,346 @@ public RobotContainer() { DataLogManager.logNetworkTables(true); DataLogManager.start(); - // NamedCommands.registerCommand("NoNote", new ( - // new WaitForCommand(1.0), - // new WaitUntilCommand(new BooleanSupplier() { - // @Override - // public boolean getAsBoolean() { - // return !intake.containsNote().getAsBoolean(); - // } - // }) - // )); + climbCamera = CameraServer.startAutomaticCapture(); - /* - * NamedCommands.registerCommand("Shoot Close", new SequentialCommandGroup( - * new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}), - * new WaitCommand(2), - * new AlignNoteCommand(intake, shooter), - * new PrepNoteCommand(shooter, intake), - * new PrepShooterCommand(intake, shooter, 0.8), - * new ShootCommand(shooter, intake) - * )); - * NamedCommands.registerCommand("Pickup", new SequentialCommandGroup( - * new InstantCommand(() -> {arm.setArmPreset(Presets.INTAKE);}), - * new IntakeCommand(intake))); - */ - // Test Auto Week 0 - // return new SequentialCommandGroup( - // new InstantCommand(() -> {arm.setArmPreset(Presets.SHOOT_HIGH);}), - // new WaitCommand(2), - // new AlignNoteCommand(intake, shooter), - // new PrepNoteCommand(shooter, intake), - // new PrepShooterCommand(intake, shooter, 0.8), - // new InstantCommand(() -> System.out.println("HELLLLLOOO")), - // new ShootCommand(shooter, intake) - // ); + swerveDriveSubsystem.setDefaultCommand(normalDrive); + + NamedCommands.registerCommand("L1", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_1); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + false))); + + // Normal scoring preset commands + NamedCommands.registerCommand("L2", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_2); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + false))); + + NamedCommands.registerCommand("L3", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_3); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + false))); + + NamedCommands.registerCommand("L4", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_4); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + false))); + + // Aim assist scoring preset commands + NamedCommands.registerCommand("L2 Auto", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_2); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + true))); + + NamedCommands.registerCommand("L3 Auto", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_3); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + true))); + + NamedCommands.registerCommand("L4 Auto", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.LEVEL_4); + }).andThen(coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, + true))); + + NamedCommands.registerCommand("Auto Aim", + new WristAlignAssist(coralSubsystem, operatorXbox.getHID(), swerveDriveSubsystem, false)); + + NamedCommands.registerCommand("Score", + new WaitCommand(Constants.AutoConstants.SCORE_WAIT_BEFORE_SECONDS).andThen(new ScoreCoralCommand( + coralSubsystem).withTimeout(Constants.AutoConstants.SCORE_WAIT_AFTER_SECONDS))); + + NamedCommands.registerCommand("Score Waitless", + new ScoreCoralCommand( + coralSubsystem).withTimeout(Constants.AutoConstants.SCORE_WAIT_AFTER_SECONDS)); + + NamedCommands.registerCommand("Intake", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.INTAKE); + }) + .andThen(coralSubsystem.getGoToLockedPresetFASTCommand(algaeSubsystem, + currentLockedPresetSupplier)) + .andThen(new IntakeCoralCommand(coralSubsystem)) + .andThen(getStowCommand())); + + NamedCommands.registerCommand("Start Intake", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.INTAKE); + }) + .andThen(coralSubsystem.getGoToLockedPresetFASTCommand(algaeSubsystem, + currentLockedPresetSupplier)) + .andThen(new InstantCommand(() -> { + CommandScheduler.getInstance().schedule(new IntakeCoralCommand(coralSubsystem)); + }))); + NamedCommands.registerCommand("Start Intake L", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.INTAKE); + }) + .andThen(coralSubsystem.getGoToLockedPresetSideFASTCommand(algaeSubsystem, + currentLockedPresetSupplier, + MirrorPresets.LEFT)) + .andThen(new InstantCommand(() -> { + CommandScheduler.getInstance().schedule(new IntakeCoralCommand(coralSubsystem)); + }))); + + NamedCommands.registerCommand("Start Intake R", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.INTAKE); + }) + .andThen(coralSubsystem.getGoToLockedPresetSideFASTCommand(algaeSubsystem, + currentLockedPresetSupplier, MirrorPresets.RIGHT)) + .andThen(new InstantCommand(() -> { + CommandScheduler.getInstance().schedule(new IntakeCoralCommand(coralSubsystem)); + }))); + + NamedCommands.registerCommand("Wait Intake", + new WaitUntilCommand(coralSubsystem.isHoldingSupplier()).andThen(new InstantCommand(() -> { + CommandScheduler.getInstance().schedule(getStowCommand()); + }))); + + NamedCommands.registerCommand("Stow", getStowCommand()); + + NamedCommands.registerCommand("Algae Low", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_REM_LOW); + }) + .andThen( + new ParallelCommandGroup( + new RemoveAlgaeCommand(algaeSubsystem), + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, + currentLockedPresetSupplier)))); + + NamedCommands.registerCommand("Algae High", + new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_REM_HIGH); + }) + .andThen( + new ParallelCommandGroup( + new RemoveAlgaeCommand(algaeSubsystem), + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, + currentLockedPresetSupplier)))); + + NamedCommands.registerCommand("Grab Low", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_ACQUIRE_LOW); + }).andThen((coralSubsystem + .getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier) + .alongWith(new IntakeAlgaeCommand(algaeSubsystem))))); + + NamedCommands.registerCommand("Grab High", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_ACQUIRE_HIGH); + }).andThen((coralSubsystem + .getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier) + .alongWith(new IntakeAlgaeCommand(algaeSubsystem))))); + + NamedCommands.registerCommand("Stow Bumper", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_STOW_BUMPER); + }).andThen((coralSubsystem + .getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier) + .alongWith(new InstantCommand(() -> { + algaeSubsystem.setAlgaeIntakePreset(AlgaeIntakePresets.HOLD); + algaeSubsystem.setAlgaePreset(AlgaePresets.HOLD_BUMPER); + }))))); + + NamedCommands.registerCommand("Intake Algae", new IntakeAlgaeCommand(algaeSubsystem)); + + NamedCommands.registerCommand("Go Barge", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_BARGE); + }).andThen((coralSubsystem + .getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier)))); + + NamedCommands.registerCommand("Go Processor", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_PROCESSOR); + }).andThen((coralSubsystem + .getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier)))); + + NamedCommands.registerCommand("Shoot Processor", + // new InstantCommand(() -> { + // CommandScheduler.getInstance().schedule( + (new ShootAlgaeCommand(algaeSubsystem).andThen(new WaitCommand(1.25))).withTimeout(1.0)); + + NamedCommands.registerCommand("Shoot Barge", new ScheduleCommand( + new ShootAlgaeBargeCommand(algaeSubsystem, false) + .withTimeout(AutoConstants.ALGAE_BARGE_SCORE_WAIT))); + + NamedCommands.registerCommand("Auto Barge", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_BARGE); + }).andThen((coralSubsystem + .getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier))).andThen( + new ShootAlgaeBargeCommand(algaeSubsystem, false) + .withTimeout(AutoConstants.ALGAE_BARGE_SCORE_WAIT))); + NamedCommands.registerCommand("Super Rem Low", new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_REM_LOW); + }).andThen(new ParallelCommandGroup( + new RemoveAlgaeCommand(algaeSubsystem), + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, + currentLockedPresetSupplier)))); + + swerveDriveSubsystem.configurePathplanner(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - swerveDriveSubsystem.setDefaultCommand(normalDrive); + autoChooser.onChange(new Consumer() { + @Override + public void accept(Command t) { + if (t instanceof PathPlannerAuto) { + PathPlannerAuto auto = (PathPlannerAuto) t; + swerveDriveSubsystem.setAutoStartingPose(auto.getStartingPose()); + } + } + }); + } + + private CoralPresets selectedScoringPreset = CoralPresets.STOW; + private int selectedLevel = 0; + private CoralPresets lockedPreset = CoralPresets.STOW; + private boolean isScoring = false; + private boolean opScoringLock = false; + + BooleanSupplier coralSafe = new BooleanSupplier() { + public boolean getAsBoolean() { + return !algaeSubsystem.isHolding(); + }; + }; + + BooleanSupplier algaeGrabSafe = new BooleanSupplier() { + public boolean getAsBoolean() { + return !coralSubsystem.isHolding(); + }; + }; + + Trigger coralAquisition = new Trigger(coralSubsystem.isHoldingSupplier()); + Trigger coralInPosition = new Trigger(new BooleanSupplier() { + public boolean getAsBoolean() { + return coralSubsystem.isSupposedToBeInPosition(); + }; + }); + + private void lockCoralArmPreset(CoralPresets preset) { + lockedPreset = preset; + SmartDashboard.putString("Locked Coral Preset", preset.toString()); + } + + private Supplier currentLockedPresetSupplier = new Supplier() { + public CoralPresets get() { + return lockedPreset; + }; + }; + + private Command getStowCommand() { + Command normalStow = new InstantCommand(() -> { + CoralPresets preset = CoralPresets.STOW; + // If holding algae, use the corresponding algae stow preset + if (algaeSubsystem.isHolding()) { + if (lockedPreset == CoralPresets.ALGAE_ACQUIRE_FLOOR || lockedPreset == CoralPresets.ALGAE_ACQUIRE_HIGH + || lockedPreset == CoralPresets.ALGAE_ACQUIRE_LOW + || lockedPreset == CoralPresets.ALGAE_ACQUIRE_LOLLIPOP) { + switch (lockedPreset) { + case ALGAE_ACQUIRE_HIGH: + preset = CoralPresets.ALGAE_STOW_HIGH; + break; + case ALGAE_ACQUIRE_LOW: + preset = CoralPresets.ALGAE_STOW_LOW; + break; + case ALGAE_ACQUIRE_FLOOR: + preset = CoralPresets.ALGAE_STOW_GROUND; + break; + case ALGAE_ACQUIRE_LOLLIPOP: + preset = CoralPresets.ALGAE_STOW_GROUND; + break; + default: + preset = CoralPresets.STOW; + break; + } + } else { + switch (selectedLevel) { + case 0: + preset = CoralPresets.ALGAE_STOW_GROUND; + break; + case 1: + preset = CoralPresets.ALGAE_STOW_GROUND; + break; + case 2: + preset = CoralPresets.ALGAE_STOW_LOW; + break; + case 3: + preset = CoralPresets.ALGAE_STOW_HIGH; + break; + default: + preset = CoralPresets.ALGAE_STOW_LOW; + break; + } + } + // if (lockedPreset == CoralPresets.ALGAE_ACQUIRE_HIGH || lockedPreset == + // CoralPresets.ALGAE_STOW_HIGH) { + // preset = CoralPresets.ALGAE_STOW_HIGH; + // } else { + // preset = CoralPresets.ALGAE_STOW_LOW; + // } + } + isScoring = false; + lockCoralArmPreset(preset); + algaeSubsystem.setAlgaePreset(algaeSubsystem.isHolding() ? AlgaePresets.HOLD : AlgaePresets.STOW); + }).andThen(new ConditionalCommand( + + coralSubsystem.getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier), + new WristStowSafety(coralSubsystem) + .andThen(coralSubsystem.getGoToLockedPresetFASTCommand(algaeSubsystem, + currentLockedPresetSupplier)), + algaeSubsystem.getIntake().getHoldingSupplier())); + + return normalStow; + } + + private Command getSuperStowCommand() { + Command algaeSupercycleCommand = new InstantCommand(() -> { + lockCoralArmPreset(selectedLevel == 4 ? CoralPresets.ALGAE_REM_HIGH : CoralPresets.ALGAE_REM_LOW); + }).andThen(new ParallelCommandGroup( + new RemoveAlgaeCommand(algaeSubsystem), + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, + currentLockedPresetSupplier))); + + return new ConditionalCommand(algaeSupercycleCommand, getStowCommand(), operatorXbox.leftBumper()); + } + + private Command getGoToCoralScoringPositionCommand() { + return new InstantCommand(() -> { + lockCoralArmPreset(selectedScoringPreset); + isScoring = true; + opScoringLock = true; + SmartDashboard.putString("Operator Control", "Going to Coral Scoring Preset: " + lockedPreset.toString()); + }).andThen( + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier) + + .andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 1.0); + }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + })))); } - // Command shootAction = - // Command alignAction = ; // Self-deadlines - // Command spoolAction = - // Command intakeAction = ; + private Command getGoToAlgaeScoringPositionCommand() { + return new InstantCommand(() -> { + lockCoralArmPreset(selectedLevel == 1 ? CoralPresets.ALGAE_PROCESSOR : CoralPresets.ALGAE_BARGE); + SmartDashboard.putString("Operator Control", "Going to Algae Scoring Preset: " + lockedPreset.toString()); + }).andThen(coralSubsystem.getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier)); + } - private ElevatorCommand elevatorToTop = new ElevatorCommand(elevator, ElevatorPresets.TOP, 0.0); - private ElevatorCommand elevatorToMiddle = new ElevatorCommand(elevator, ElevatorPresets.MIDDLE, 0.0); - private ElevatorCommand elevatorToStow = new ElevatorCommand(elevator, ElevatorPresets.STOW, 0.0); + private Command getScoreAlgaeCommand() { + return new ConditionalCommand(new ShootAlgaeBargeCommand(algaeSubsystem), new ShootAlgaeCommand(algaeSubsystem), + new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return currentLockedPresetSupplier.get() == CoralPresets.ALGAE_BARGE; + } + }); + } /** * Use this method to define your trigger->command mappings. Triggers can be @@ -124,28 +481,263 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - operatorXbox.a() - .onTrue(elevatorToStow); - operatorXbox.x() - .onTrue(elevatorToMiddle); - operatorXbox.y() - .onTrue(elevatorToTop); - - operatorXbox.b().whileTrue(new ElevatorFollowCommand(elevator, new DoubleSupplier() { + // Driver assist controls + driverXbox.leftTrigger(0.05).onTrue(new ConditionalCommand(new InstantCommand(() -> { + normalDrive.setDriveStyle(DriveStyle.REEF_ASSIST); + }), new InstantCommand(() -> { + normalDrive.setDriveStyle(DriveStyle.INTAKE_ASSIST); + }), new BooleanSupplier() { @Override - public double getAsDouble() { - return (operatorXbox.getLeftY() * -0.5 + 0.5) - * Constants.Elevator.PhysicalParameters.elevatorHeightMeters; + public boolean getAsBoolean() { + boolean isAlgaeRemove = coralSubsystem.getCurrentPreset() == CoralPresets.ALGAE_ACQUIRE_HIGH + || coralSubsystem.getCurrentPreset() == CoralPresets.ALGAE_ACQUIRE_LOW + || coralSubsystem.getCurrentPreset() == CoralPresets.ALGAE_REM_LOW + || coralSubsystem.getCurrentPreset() == CoralPresets.ALGAE_REM_HIGH; + return coralSubsystem.isHolding() || isAlgaeRemove; } + })).onFalse(new InstantCommand(() -> { + normalDrive.setDriveStyle(DriveStyle.FIELD_ORIENTED); })); - operatorXbox.povUp().debounce(0.02).onTrue(new InstantCommand(() -> { - elevator.setPosition(elevator.getGoalPosition() + 0.1); + driverXbox.leftBumper().onTrue(new InstantCommand(() -> { + normalDrive.setDriveStyle(DriveStyle.ROBOT_ORIENTED); + })).onFalse(new InstantCommand(() -> { + normalDrive.setDriveStyle(DriveStyle.FIELD_ORIENTED); + })); + + // L1 + operatorXbox.a().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_1; + selectedLevel = 1; + })); + // L2 + operatorXbox.x().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_2; + selectedLevel = 2; + })); + // L3 + operatorXbox.y().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_3; + selectedLevel = 3; + })); + // L4 + operatorXbox.b().onTrue(new InstantCommand(() -> { + selectedScoringPreset = CoralPresets.LEVEL_4; + selectedLevel = 4; + })); + + // Move arm to coral scoring preset + operatorXbox.rightTrigger().and(coralSafe) + .whileTrue(getGoToCoralScoringPositionCommand().onlyIf(coralSubsystem.isHoldingSupplier())); + // Stow arm + operatorXbox.rightTrigger().whileFalse(getSuperStowCommand().alongWith(new InstantCommand(() -> { + opScoringLock = false; + }))); + + // Driver scoring: + // - If holding coral, score coral + // - If not holding coral: + // -- If holding algae, score algae + // -- If not holding algae, spin coral intake + driverXbox.rightBumper().and(coralSubsystem.getIntake().getHoldingSupplier()) + .whileTrue(new ScoreCoralCommand(coralSubsystem)); + driverXbox.rightBumper().and(coralSubsystem.getIntake() + .getNotHoldingSupplier()) + .whileTrue(new ConditionalCommand(getScoreAlgaeCommand(), + new ScoreCoralCommand(coralSubsystem), algaeSubsystem.getIntake().getHoldingSupplier())); + + // Operator tap-to-stow + operatorXbox.rightBumper().whileFalse(getStowCommand()).onTrue(new InstantCommand(() -> { + selectedLevel = 0; + selectedScoringPreset = CoralPresets.STOW; + })); + driverXbox.rightBumper().whileFalse(getSuperStowCommand()); + + // Intake coral + operatorXbox.rightTrigger().and(coralSafe).and(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return !coralSubsystem.isHolding() && !isScoring && !opScoringLock; + } + }).whileTrue(new InstantCommand(() -> { + SmartDashboard.putString("Operator Control", "Intaking Coral"); + lockCoralArmPreset(CoralPresets.INTAKE); + }).andThen(coralSubsystem.getGoToLockedPresetFASTCommand(algaeSubsystem, + currentLockedPresetSupplier)).andThen(new IntakeCoralCommand(coralSubsystem)) + .andThen(getStowCommand())) + .whileFalse(new ConditionalCommand(getStowCommand(), new InstantCommand(), + coralSubsystem.isHoldingSupplier())); + + // Auto stow (after shooting!!!) + // coralAquisition.negate().and(operatorXbox.rightTrigger()).and(new + // BooleanSupplier() { + // @Override + // public boolean getAsBoolean() { + // return isScoring; + // } + // }).debounce(0.1).onTrue(new InstantCommand(() -> { + // isScoring = false; + // }).alongWith(getStowCommand())); + + // Purge gamepieces + operatorXbox.button(7).whileTrue(new ParallelCommandGroup(new PurgeCoralIntakeCommand(coralSubsystem), + new PurgeAlgaeCommand(algaeSubsystem))); + + // Reset climber deploy (allow to pull back in) + operatorXbox.button(8).onTrue(new InstantCommand(() -> { + climberSubsystem.resetClimberDeploy(); })); - operatorXbox.povDown().debounce(0.02).onTrue(new InstantCommand(() -> { - elevator.setPosition(elevator.getGoalPosition() - 0.1); + // Rumble on coral acquisition + coralAquisition.onChange(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 1.0); + driverXbox.setRumble(RumbleType.kBothRumble, 1.0); + }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + driverXbox.setRumble(RumbleType.kBothRumble, 0.0); + }))); + + new Trigger(algaeSubsystem.getIntake().getHoldingSupplier()).onChange(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 1.0); + driverXbox.setRumble(RumbleType.kBothRumble, 1.0); + }).andThen(new WaitCommand(0.1)).andThen(new InstantCommand(() -> { + operatorXbox.setRumble(RumbleType.kBothRumble, 0.0); + driverXbox.setRumble(RumbleType.kBothRumble, 0.0); + }))); + + // Algae removal + operatorXbox.leftBumper().and(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return (selectedLevel == 2 || selectedLevel == 3); + } + }).whileTrue( // .and(algaeManipReady) + new InstantCommand(() -> { + // algaeLock = true; + lockCoralArmPreset(selectedLevel == 2 ? CoralPresets.ALGAE_REM_LOW : CoralPresets.ALGAE_REM_HIGH); + }).andThen( + new ParallelCommandGroup( + new RemoveAlgaeCommand(algaeSubsystem), + coralSubsystem.getGoToLockedPresetCommandV2(algaeSubsystem, + currentLockedPresetSupplier)))); + // Stow + operatorXbox.leftBumper().onFalse(getStowCommand()); + + // TODO: Add proper lockout for intake and scoring like coral + // Algae intaking + operatorXbox.leftTrigger().and(algaeGrabSafe) + .and(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return (selectedLevel == 2 || selectedLevel == 3 || selectedLevel == 1 || selectedLevel == 0); + } + }).whileTrue( + (new InstantCommand(() -> { + lockCoralArmPreset( + selectedLevel == 2 ? CoralPresets.ALGAE_ACQUIRE_LOW + : (selectedLevel == 1 ? CoralPresets.ALGAE_ACQUIRE_LOLLIPOP + : (selectedLevel == 0 ? CoralPresets.ALGAE_ACQUIRE_FLOOR + : CoralPresets.ALGAE_ACQUIRE_HIGH))); + }).andThen(coralSubsystem + .getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier) + .alongWith(new ConditionalCommand(new IntakeAlgaeCommand( + algaeSubsystem, true), + new IntakeAlgaeCommand( + algaeSubsystem), + new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return selectedLevel == 0; + } + })))) + .onlyIf(algaeSubsystem.getIntake().getNotHoldingSupplier())); + // Stow + operatorXbox.leftTrigger().onFalse(getStowCommand()); + + // Algae scoring + operatorXbox.leftTrigger() + .and(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return selectedLevel == 1 || selectedLevel == 4; + } + }).whileTrue(getGoToAlgaeScoringPositionCommand().onlyIf( + algaeSubsystem.getIntake().getHoldingSupplier())); + operatorXbox.leftTrigger().whileFalse(getStowCommand()); + + // Driver elevator zeroing + driverXbox.button(7).onTrue(new InstantCommand(() -> { + // Move elevator down to zero + coralSubsystem.getElevator().startZeroElevator(); + // Re-zero wrist relative encoder!!! + coralSubsystem.getCoralArm().reset(); + })).onFalse(new InstantCommand(() -> { + coralSubsystem.getElevator().endZeroElevator(); })); + + /* + * coop + */ + // algae floor / shoot + // driverXbox.leftBumper().and(new BooleanSupplier() { + // @Override + // public boolean getAsBoolean() { + // return algaeSubsystem.isHolding(); + // } + // }).whileTrue(new ShootAlgaeCommand(algaeSubsystem)); + + /////////////////// DEBUGGING ////////////////// + // debugXboxController.a().onTrue(new InstantCommand(() -> { + // System.out.println("Going L4 Pitch"); + // coralSubsystem.setCoralPresetPitch(CoralPresets.LEVEL_4); + // })).onFalse(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetPitch(CoralPresets.STOW); + // })); + + // debugXboxController.b().onTrue(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetRoll(CoralPresets.LEVEL_4); + // })).onFalse(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetRoll(CoralPresets.STOW); + // })); + + // debugXboxController.x().onTrue(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetPivot(CoralPresets.LEVEL_4); + // })).onFalse(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetPivot(CoralPresets.STOW); + // })); + // debugXboxController.y().onTrue(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetElevator(CoralPresets.LEVEL_4); + // })).onFalse(new InstantCommand(() -> { + // coralSubsystem.setCoralPresetElevator(CoralPresets.STOW); + // })); + + // debugXboxController.rightBumper().whileTrue(new + // IntakeCoralCommand(coralSubsystem)); + // debugXboxController.leftBumper().whileTrue(new + // ScoreCoralCommand(coralSubsystem)); + + // // Algae debugging!!! + // debugXboxController.povUp().onTrue(new InstantCommand(() -> { + // algaeSubsystem.setAlgaePreset(AlgaePresets.REMOVE); + // })); + + // debugXboxController.povDown().onTrue(new InstantCommand(() -> { + // algaeSubsystem.setAlgaePreset(AlgaePresets.STOW); + // })); + + // debugXboxController.povLeft().whileTrue(new + // RemoveAlgaeCommand(algaeSubsystem)); + // debugXboxController.povRight().whileTrue(new + // IntakeAlgaeCommand(algaeSubsystem)); + + operatorXbox.povUp().onTrue(new InstantCommand(() -> { + lockCoralArmPreset(CoralPresets.ALGAE_STOW_BUMPER); + }).andThen((coralSubsystem + .getGoToLockedPresetAlgaeSafeCommand(algaeSubsystem, currentLockedPresetSupplier) + .alongWith(new InstantCommand(() -> { + algaeSubsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + algaeSubsystem.setAlgaePreset(AlgaePresets.HOLD_BUMPER); + }))))).onFalse(getStowCommand()); } /** @@ -154,9 +746,11 @@ public double getAsDouble() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + swerveDriveSubsystem.setGyroToEstimate(); + if (Robot.isSimulation()) { + coralSubsystem.simSetHolding(true); + } return autoChooser.getSelected(); - // return new PathPlannerAuto("4-close-middle"); - } public SwerveSubsystem getSwerveSubsystem() { @@ -170,4 +764,8 @@ public CommandXboxController getDriverXbox() { public CommandXboxController getOperatorXbox() { return operatorXbox; } + + public void periodic200Hz() { + coralSubsystem.getCoralArm().periodic200Hz(); + } } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index a7707f2..a63db20 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -1,40 +1,62 @@ package frc.robot.commands; +import java.util.ArrayList; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.*; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.*; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.CoralStation; +import frc.robot.util.Reef; +@Logged public class DriveCommand extends Command { + @NotLogged private final SwerveSubsystem swerveSubsystem; - private final XboxController xbox; + private final XboxController driverXbox; private SlewRateLimiter dsratelimiter = new SlewRateLimiter(4); - // Auto rotation pid/rate limiter - private PIDController rotationController = new PIDController(6, 0.0, 0.5); - private double DRIVE_MULT = 1.0; private final double SLOWMODE_MULT = 0.25; - private enum DriveState { - Free, - Locked, + public static enum DriveStyle { + FIELD_ORIENTED, + ROBOT_ORIENTED, + REEF_ASSIST, + INTAKE_ASSIST }; - private DriveState state = DriveState.Free; + private DriveStyle driveStyle = DriveStyle.FIELD_ORIENTED; + private PIDController rotationAssist = new PIDController( + DriveConstants.ROTATION_ASSIST.kP, + DriveConstants.ROTATION_ASSIST.kI, + DriveConstants.ROTATION_ASSIST.kD); + private PIDController translationAssist = new PIDController( + DriveConstants.TRANSLATION_ASSIST.kP, + DriveConstants.TRANSLATION_ASSIST.kI, + DriveConstants.TRANSLATION_ASSIST.kD); + + private boolean isXstance = false; public DriveCommand(SwerveSubsystem swerveSubsystem, XboxController xbox) { this.swerveSubsystem = swerveSubsystem; - this.xbox = xbox; - - rotationController.enableContinuousInput(-Math.PI, Math.PI); + this.driverXbox = xbox; + rotationAssist.enableContinuousInput(-Math.PI, Math.PI); dsratelimiter.reset(SLOWMODE_MULT); addRequirements(swerveSubsystem); @@ -65,120 +87,117 @@ public double DeadBand(double input, double deadband) { @Override public void execute() { - Translation2d xyRaw = new Translation2d(xbox.getLeftX(), xbox.getLeftY()); - Translation2d xySpeed = DeadBand(xyRaw, 0.15 / 2.f); - double zSpeed = DeadBand(xbox.getRightX(), 0.1); - double xSpeed = xySpeed.getX(); // xbox.getLeftX(); - double ySpeed = xySpeed.getY(); // xbox.getLeftY(); - - // System.out.println("DRIVE!!"); - - // double mag_xy = Math.sqrt(xSpeed*xSpeed + ySpeed*ySpeed); - - // xSpeed = mag_xy > 0.15 ? xSpeed : 0.0; - // ySpeed = mag_xy > 0.15 ? ySpeed : 0.0; - // zSpeed = Math.abs(zSpeed) > 0.15 ? zSpeed : 0.0; + Translation2d xyRaw = new Translation2d(-driverXbox.getLeftY(), -driverXbox.getLeftX()); + double zSpeed = -MathUtil.applyDeadband(driverXbox.getRightX(), 0.1); + double xSpeed = MathUtil.applyDeadband(xyRaw.getX(), 0.08); // xbox.getLeftX(); + double ySpeed = MathUtil.applyDeadband(xyRaw.getY(), 0.08); // xbox.getLeftY(); xSpeed *= DriveConstants.XY_SPEED_LIMIT * DriveConstants.MAX_ROBOT_VELOCITY; ySpeed *= DriveConstants.XY_SPEED_LIMIT * DriveConstants.MAX_ROBOT_VELOCITY; zSpeed *= DriveConstants.Z_SPEED_LIMIT * DriveConstants.MAX_ROBOT_RAD_VELOCITY; - // double dmult = dsratelimiter.calculate(xbox.getRightBumper() ? 1.0 : - // SLOWMODE_MULT); double dmult = dsratelimiter - .calculate((DRIVE_MULT - SLOWMODE_MULT) * xbox.getRightTriggerAxis() + SLOWMODE_MULT); + .calculate((DRIVE_MULT - SLOWMODE_MULT) * driverXbox.getRightTriggerAxis() + SLOWMODE_MULT); xSpeed *= dmult; ySpeed *= dmult; zSpeed *= dmult; - if (xbox.getXButton()) { + if (driverXbox.getStartButton()) { swerveSubsystem.zeroHeading(); - Translation2d pospose = swerveSubsystem.getPose().getTranslation(); - swerveSubsystem.odometry.resetPosition(swerveSubsystem.getRotation2d(), + Translation2d pospose = swerveSubsystem.getOdometryPose().getTranslation(); + swerveSubsystem.odometry.resetPosition(swerveSubsystem.getGyroRotation2d(), swerveSubsystem.getModulePositions(), new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() == Alliance.Blue ? 0.0 : Math.PI))); - // swerveSubsystem.resetOdometry(new Pose2d(1.38, 5.55, new Rotation2d())); - // swerveSubsystem.zeroHeading(); } - ChassisSpeeds speeds; - - // switch (swerveSubsystem.getRotationStyle()) { - // // case Driver: - - // // // do nothing special - // // break; - // // case AutoSpeaker: - // // // Rotation2d r = - // // // - // swerveSubsystem.getPose().getTranslation().minus(FieldConstants.getSpeakerPosition()).getAngle(); - // // // if (DriverStation.getAlliance().get() == Alliance.Red) - // // // r = r.rotateBy(new Rotation2d(Math.PI)); - // // // double calculatedAngle = targeting.getPhi(arm.getHorizOffset()); - // // // SmartDashboard.putNumber("Wanted Heading", calculatedAngle); - // // SmartDashboard.putNumberArray("Phi control", new double[] { - // // calculatedAngle, - // // swerveSubsystem.getHeading() - // // }); - // // zSpeed = - // MathUtil.clamp(-rotationController.calculate(swerveSubsystem.getPose().getRotation().getRadians() - // + ((FieldConstants.getAlliance() == Alliance.Red) ? Math.PI : 0.0), - // calculatedAngle), - // // -0.5 * DriveConstants.MAX_ROBOT_RAD_VELOCITY, 0.5 * - // DriveConstants.MAX_ROBOT_RAD_VELOCITY); - // // break; - // // case AutoShuttle: - // // double shuttleCalculatedAngle = targeting.getShuttlePhi(); - - // // zSpeed = - // MathUtil.clamp(-rotationController.calculate(swerveSubsystem.getPose().getRotation().getRadians() - // + ((FieldConstants.getAlliance() == Alliance.Red) ? Math.PI : 0.0), - // shuttleCalculatedAngle), - // // -0.5 * DriveConstants.MAX_ROBOT_RAD_VELOCITY, 0.5 * - // DriveConstants.MAX_ROBOT_RAD_VELOCITY); - // // break; - // } - - // Drive Non Field Oriented - if (xbox.getRightBumper()) { - speeds = new ChassisSpeeds(-xSpeed, ySpeed, -zSpeed); - - } else if (!xbox.getLeftBumper()) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(-ySpeed, xSpeed, zSpeed, - new Rotation2d( - -swerveSubsystem.getRotation2d().rotateBy(DriveConstants.NAVX_ANGLE_OFFSET).getRadians())); - - // speeds = ChassisSpeeds.fromFieldRelativeSpeeds(-ySpeed, xSpeed, - // zSpeed,swerveSubsystem.odometry.getEstimatedPosition().getRotation().rotateBy( - // new Rotation2d(FieldConstants.getAlliance() == Alliance.Red ? Math.PI : 0) - // )); - } else { - // Normal non-field oriented - speeds = new ChassisSpeeds(-xSpeed, -ySpeed, zSpeed); - } + double assistMixer = MathUtil.clamp(driverXbox.getLeftTriggerAxis(), 0.0, 1.0); + SmartDashboard.putNumber("Assist Mixer", assistMixer); + SmartDashboard.putString("Assist Mode", driveStyle.toString()); - // State transition logic - switch (state) { - case Free: - // state = xbox.getRightBumper() ? DriveState.Locked : DriveState.Free; + ChassisSpeeds speeds = new ChassisSpeeds(); + switch (driveStyle) { + case FIELD_ORIENTED: + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, zSpeed, + swerveSubsystem.getGyroRotation2d()); break; - case Locked: - state = ((xyRaw.getNorm() > 0.15) && !xbox.getBButton()) ? DriveState.Free : DriveState.Locked; + case ROBOT_ORIENTED: + speeds = ChassisSpeeds.fromRobotRelativeSpeeds( + xSpeed, ySpeed, zSpeed, + new Rotation2d(Math.PI)); + break; + case REEF_ASSIST: + Translation2d reefCenter = AllianceFlipUtil.apply(Reef.center); + double reefAngleRot = swerveSubsystem.getOdometryPose().getTranslation().minus( + reefCenter).getAngle().getRotations(); + + double targetAngle = MathUtil + .angleModulus(Units.rotationsToRadians(Math.floor((reefAngleRot + 1.0 / 12.0) * 6.0) / 6.0)) + - Math.PI / 2.0; + + SmartDashboard.putNumber("AutoRotate Target Angle", Units.radiansToDegrees(targetAngle)); + + SwerveModuleState rotationState = new SwerveModuleState(1.5, new Rotation2d(targetAngle)); + // NOTE: Remove this to dedicate a scoring side + // rotationState.optimize(swerveSubsystem.getOdometryPose().getRotation()); + + double zAssist = MathUtil + .clamp(rotationAssist.calculate(swerveSubsystem.getOdometryPose().getRotation().getRadians(), + rotationState.angle.getRadians()), -0.75 * DriveConstants.MAX_ROBOT_RAD_VELOCITY, + 0.75 + * DriveConstants.MAX_ROBOT_RAD_VELOCITY); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, zSpeed + zAssist * assistMixer, + swerveSubsystem.getGyroRotation2d()); + break; + case INTAKE_ASSIST: + // double zTarget = + // if (AllianceFlipUtil.shouldFlip()) + // zTarget += Math.PI; + Pose2d csRight = AllianceFlipUtil.apply(CoralStation.coralStations[0]); + Pose2d csLeft = AllianceFlipUtil.apply(CoralStation.coralStations[1]); + // CoralStation.putToShuffleboard(); + + Pose2d csTarget = (swerveSubsystem.getOdometryPose().getTranslation() + .getDistance(csRight.getTranslation()) < swerveSubsystem.getOdometryPose().getTranslation() + .getDistance(csLeft.getTranslation())) ? csRight : csLeft; + + // TODO: Optimize if need be + double targetRotation = csTarget.getRotation().rotateBy(Rotation2d.fromDegrees(90.0)).getRadians(); + + double zPid = MathUtil + .clamp(rotationAssist.calculate(swerveSubsystem.getOdometryPose().getRotation().getRadians(), + targetRotation), -0.75 * DriveConstants.MAX_ROBOT_RAD_VELOCITY, + 0.75 + * DriveConstants.MAX_ROBOT_RAD_VELOCITY); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, zSpeed + zPid + * assistMixer, + swerveSubsystem.getGyroRotation2d()); + + default: break; } + // State transition logic + isXstance = false; + if (isXstance) + isXstance = !((xyRaw.getNorm() > 0.08) && !driverXbox.getBButton()); + // Drive execution logic - switch (state) { - case Free: - SwerveModuleState[] calculatedModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(speeds); - swerveSubsystem.setModules(calculatedModuleStates); - break; - case Locked: - swerveSubsystem.setXstance(); - break; + + swerveSubsystem.zeroFeedforwards(); + if (isXstance) { + swerveSubsystem.setXstance(); + } else { + swerveSubsystem.setChassisSpeeds(speeds); } } + public void setDriveStyle(DriveStyle style) { + this.driveStyle = style; + } + @Override public void end(boolean interrupted) { swerveSubsystem.stopDrive(); diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java deleted file mode 100644 index 4018b4f..0000000 --- a/src/main/java/frc/robot/commands/ElevatorCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.subsystems.ElevatorSubsystem; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorCommand extends Command { - private final ElevatorSubsystem elevatorSub; - - public enum ElevatorPresets { - STOW(0.0), - MIDDLE(Constants.Elevator.PhysicalParameters.elevatorHeightMeters / 2), - TOP(Constants.Elevator.PhysicalParameters.elevatorHeightMeters); - - private ElevatorPresets(double pos_meters) { - this.position_m = pos_meters; - } - - private double position_m; - } - - private ElevatorPresets target = ElevatorPresets.STOW; - private double offset; - - public ElevatorCommand(ElevatorSubsystem elevatorSub, ElevatorPresets targetPosition, double targetOffset) { - this.elevatorSub = elevatorSub; - this.target = targetPosition; - this.offset = targetOffset; - addRequirements(elevatorSub); - } - - @Override - public void initialize() { - double tgt = target.position_m + offset; - elevatorSub.setGoal(MathUtil.clamp(tgt, 0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters)); - SmartDashboard.putString("Elevator Command", target.toString()); - } - - @Override - public boolean isFinished() { - return elevatorSub.isInPosition(); - } - - @Override - public void end(boolean interrupted) { - // NOTE: Don't disable (so it position-holds with feedforward) - // elevatorSub.disable(); - } -} diff --git a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java b/src/main/java/frc/robot/commands/ElevatorFollowCommand.java deleted file mode 100644 index 03a2aa1..0000000 --- a/src/main/java/frc/robot/commands/ElevatorFollowCommand.java +++ /dev/null @@ -1,55 +0,0 @@ -package frc.robot.commands; - -import frc.robot.Constants; -import frc.robot.subsystems.ElevatorSubsystem; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorFollowCommand extends Command { - private final ElevatorSubsystem elevatorSub; - private double lastTarget = 0; - private double lastTime = 0; - private DoubleSupplier target; - private SlewRateLimiter limiter = new SlewRateLimiter(Constants.Elevator.PID.MAX_ACCELERATION / 10.0); - - public ElevatorFollowCommand(ElevatorSubsystem elevatorSub, DoubleSupplier target) { - this.elevatorSub = elevatorSub; - this.target = target; - addRequirements(elevatorSub); - } - - @Override - public void initialize() { - elevatorSub.enable(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void execute() { - double tgt = target.getAsDouble(); - double dX = tgt - lastTarget; - double dt = Timer.getFPGATimestamp() - lastTime; - lastTime = Timer.getFPGATimestamp(); - lastTarget = tgt; - elevatorSub.setGoal(new TrapezoidProfile.State( - MathUtil.clamp(limiter.calculate(tgt), 0, - Constants.Elevator.PhysicalParameters.elevatorHeightMeters), - 0.0)); - } - - @Override - public void end(boolean interrupted) { - // NOTE: Don't disable (so it position-holds with feedforward) - // elevatorSub.disable(); - } -} diff --git a/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java new file mode 100644 index 0000000..e27ddef --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java @@ -0,0 +1,50 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; + +public class IntakeAlgaeCommand extends Command { + private final AlgaeSubsystem subsystem; + private boolean isGround = false; + + public IntakeAlgaeCommand(AlgaeSubsystem subsystem) { + this(subsystem, false); + } + + public IntakeAlgaeCommand(AlgaeSubsystem subsystem, boolean floor) { + this.subsystem = subsystem; + addRequirements(subsystem); + this.isGround = floor; + } + + @Override + public void initialize() { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.INTAKING); + subsystem.setAlgaePreset(isGround ? AlgaePresets.INTAKE_FLOOR : AlgaePresets.INTAKE); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Algae", true); + } + } + + @Override + public boolean isFinished() { + return subsystem.isHolding(); + } + + @Override + public void end(boolean interrupted) { + if (subsystem.isHolding() || DriverStation.isAutonomous()) { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.HOLD); + subsystem.setAlgaePreset(AlgaePresets.HOLD); + } else { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + subsystem.setAlgaePreset(AlgaePresets.STOW); + } + } +} diff --git a/src/main/java/frc/robot/commands/algae/PurgeAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/PurgeAlgaeCommand.java new file mode 100644 index 0000000..97a196b --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/PurgeAlgaeCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; + +public class PurgeAlgaeCommand extends Command { + private final AlgaeSubsystem subsystem; + + public PurgeAlgaeCommand(AlgaeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.SHOOT); + subsystem.setAlgaePreset(AlgaePresets.STOW); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Algae", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + subsystem.setAlgaePreset(AlgaePresets.STOW); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/algae/RemoveAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/RemoveAlgaeCommand.java new file mode 100644 index 0000000..38d080a --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/RemoveAlgaeCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; + +public class RemoveAlgaeCommand extends Command { + private final AlgaeSubsystem subsystem; + + public RemoveAlgaeCommand(AlgaeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setAlgaePreset(AlgaePresets.REMOVE); + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.REMOVAL); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + subsystem.setAlgaePreset(AlgaePresets.STOW); + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + } +} diff --git a/src/main/java/frc/robot/commands/algae/ShootAlgaeBargeCommand.java b/src/main/java/frc/robot/commands/algae/ShootAlgaeBargeCommand.java new file mode 100644 index 0000000..df93210 --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ShootAlgaeBargeCommand.java @@ -0,0 +1,46 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; + +public class ShootAlgaeBargeCommand extends Command { + private final AlgaeSubsystem subsystem; + private boolean autoExit = true;; + + public ShootAlgaeBargeCommand(AlgaeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + public ShootAlgaeBargeCommand(AlgaeSubsystem subsystem, boolean autoExit) { + this.subsystem = subsystem; + this.autoExit = autoExit; + // addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.SHOOT_BARGE); + subsystem.setAlgaePreset(AlgaePresets.BARGE); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Algae", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + // subsystem.setAlgaePreset(AlgaePresets.STOW); + } + + @Override + public boolean isFinished() { + return (subsystem.getIntake().getSensorDistance() > Constants.Algae.Intake.SHOT_THRESHOLD) && this.autoExit; + } +} diff --git a/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java new file mode 100644 index 0000000..51cf7c7 --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ShootAlgaeCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; + +public class ShootAlgaeCommand extends Command { + private final AlgaeSubsystem subsystem; + + public ShootAlgaeCommand(AlgaeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.SHOOT); + subsystem.setAlgaePreset(AlgaePresets.HOLD); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Algae", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setAlgaeIntakePreset(AlgaeIntakePresets.STOP); + // subsystem.setAlgaePreset(AlgaePresets.STOW); + } + + @Override + public boolean isFinished() { + return subsystem.getIntake().getSensorDistance() > Constants.Algae.Intake.SHOT_THRESHOLD; + } +} diff --git a/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java new file mode 100644 index 0000000..6d77952 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/IntakeCoralCommand.java @@ -0,0 +1,45 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; + +public class IntakeCoralCommand extends Command { + private final CoralSubsystem subsystem; + + public IntakeCoralCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + // addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setCoralIntakePreset(CoralIntakePresets.INTAKE); + SmartDashboard.putString("Intake Command", "Started"); + + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Coral", true); + } + } + + @Override + public void end(boolean interrupted) { + // if intaking failed + if (!subsystem.isHolding()) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + SmartDashboard.putString("Intake Command", "Stopped"); + } else { + SmartDashboard.putString("Intake Command", "Holding"); + + subsystem.setCoralIntakePreset(CoralIntakePresets.HOLD); + } + // else continue to hold the piece + } + + @Override + public boolean isFinished() { + return subsystem.isHolding(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java new file mode 100644 index 0000000..032ed93 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/PurgeCoralIntakeCommand.java @@ -0,0 +1,28 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; + +public class PurgeCoralIntakeCommand extends Command { + private final CoralSubsystem subsystem; + + public PurgeCoralIntakeCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + } + + @Override + public void initialize() { + subsystem.setCoralIntakePreset(CoralIntakePresets.PURGE); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Coral", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + } +} diff --git a/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java new file mode 100644 index 0000000..a409707 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/ScoreCoralCommand.java @@ -0,0 +1,51 @@ +package frc.robot.commands.coral; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralIntakePresets; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class ScoreCoralCommand extends Command { + + private final CoralSubsystem subsystem; + private double coralExitTime = 0; + + public ScoreCoralCommand(CoralSubsystem subsystem) { + this.subsystem = subsystem; + // addRequirements(subsystem); + } + + @Override + public void initialize() { + boolean isL1 = subsystem.getCurrentPreset() == CoralPresets.LEVEL_1; + subsystem.setCoralIntakePreset(isL1 ? CoralIntakePresets.SCORE_L1 : CoralIntakePresets.SCORE); + if (Robot.isSimulation()) { + SmartDashboard.putBoolean("[SIM] Holding Coral", false); + } + } + + @Override + public void end(boolean interrupted) { + subsystem.setCoralIntakePreset(CoralIntakePresets.STOP); + } + + @Override + public boolean isFinished() { + // if (!subsystem.isHolding() && coralExitTime == 0) { + // coralExitTime = Timer.getFPGATimestamp(); + // } + // return !subsystem.isHolding() + // && ((Timer.getFPGATimestamp() - coralExitTime) > + // Constants.Coral.Intake.SCORE_EXTRA_SECONDS); + + // Just keep going at it! + // For autos, might want a different command? + // return Robot.isSimulation(); + return false; + } + +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java new file mode 100644 index 0000000..13f16b2 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MoveElevator.java @@ -0,0 +1,37 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MoveElevator extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MoveElevator(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Elevator"); + SmartDashboard.putString("Move Elevator", "Started"); + coralSub.setCoralPresetElevator(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Elevator", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isElevatorSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePitch.java b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java new file mode 100644 index 0000000..5694dcc --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MovePitch.java @@ -0,0 +1,39 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MovePitch extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MovePitch(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + // HACK: This is much sketch + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Pitch"); + SmartDashboard.putString("Move Pitch", "Start"); + + coralSub.setCoralPresetPitch(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Pitch", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isPitchSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MovePivot.java b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java new file mode 100644 index 0000000..f4dddfc --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MovePivot.java @@ -0,0 +1,38 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MovePivot extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MovePivot(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + this.presetSupplier = presetSupplier; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Pivot"); + SmartDashboard.putString("Move Pivot", "Start"); + + coralSub.setCoralPresetPivot(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Pivot", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isPivotSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java new file mode 100644 index 0000000..e380020 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/MoveRoll.java @@ -0,0 +1,38 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class MoveRoll extends Command { + private CoralSubsystem coralSub; + private Supplier presetSupplier; + + public MoveRoll(CoralSubsystem coralSub, Supplier presetSupplier) { + this.coralSub = coralSub; + // addRequirements(coralSub); + this.presetSupplier = presetSupplier; + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Moving Roll"); + SmartDashboard.putString("Move Roll", "Start"); + + coralSub.setCoralPresetRoll(presetSupplier.get()); + } + + @Override + public void end(boolean interrupted) { + SmartDashboard.putString("Move Roll", "End"); + } + + @Override + public boolean isFinished() { + return coralSub.isRollSupposedToBeInPosition(); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/StowArm.java b/src/main/java/frc/robot/commands/coral/motion/StowArm.java new file mode 100644 index 0000000..f651d15 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/StowArm.java @@ -0,0 +1,30 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class StowArm extends Command { + private CoralSubsystem coralSub; + + public StowArm(CoralSubsystem coralSub) { + this.coralSub = coralSub; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("Arm Sequence", "Stowing Arm"); + coralSub.setCoralPresetPitch(CoralPresets.STOW); + coralSub.setCoralPresetRoll(CoralPresets.STOW); + coralSub.setCoralPresetPivot(CoralPresets.STOW); + } + + @Override + public boolean isFinished() { + // TODO: For the future... might want to wait? or not. + return true; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java new file mode 100644 index 0000000..1f53948 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitArmClearance.java @@ -0,0 +1,33 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitArmClearance extends Command { + private CoralSubsystem coralSub; + + public WaitArmClearance(CoralSubsystem coralSub) { + this.coralSub = coralSub; + // addRequirements(coralSub); + } + + @Override + public void initialize() { + SmartDashboard.putString("WaitArmClearance", "waiting"); + + } + + @Override + public boolean isFinished() { + boolean finished = Math.abs(coralSub.getPivotPositionDegrees()) > Units + .radiansToDegrees(Constants.Coral.Pivot.ELEVATOR_BORDER_ANGLE); + if (finished) { + SmartDashboard.putString("WaitArmClearance", "finished"); + } + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java new file mode 100644 index 0000000..6659fd8 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitElevatorApproach.java @@ -0,0 +1,33 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitElevatorApproach extends Command { + private CoralSubsystem coralSub; + private double metersBefore; + + public WaitElevatorApproach(CoralSubsystem coralSub, double metersBefore) { + this.coralSub = coralSub; + this.metersBefore = metersBefore; + } + + @Override + public void initialize() { + SmartDashboard.putString("WaitElevatorApproach", "waiting " + (Double.toString(metersBefore)) + "m"); + } + + @Override + public boolean isFinished() { + boolean finished = coralSub.getElevator() + .getPosition() > (coralSub.getElevator().getGoalPosition() - metersBefore); + if (finished) { + SmartDashboard.putString("WaitElevatorApproach", "finished"); + } + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitRollApproach.java b/src/main/java/frc/robot/commands/coral/motion/WaitRollApproach.java new file mode 100644 index 0000000..961b5cd --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitRollApproach.java @@ -0,0 +1,33 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitRollApproach extends Command { + private CoralSubsystem coralSub; + double degreesBefore; + + public WaitRollApproach(CoralSubsystem coralSub, double degreesBefore) { + this.coralSub = coralSub; + this.degreesBefore = degreesBefore; + } + + @Override + public void initialize() { + SmartDashboard.putString("WaitRollApproach", "waiting"); + } + + @Override + public boolean isFinished() { + boolean finished = (Math.abs(coralSub.getRollGoalDegrees()) > 10) && (Math + .abs(coralSub.getRollGoalDegrees() - coralSub.getCoralArm().getRollPositionDegrees()) < degreesBefore); + if (finished) { + SmartDashboard.putString("WaitRollApproach", "finished"); + } + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java new file mode 100644 index 0000000..ac08541 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WaitRollFinished.java @@ -0,0 +1,32 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WaitRollFinished extends Command { + private CoralSubsystem coralSub; + + public WaitRollFinished(CoralSubsystem coralSub) { + this.coralSub = coralSub; + // addRequirements(coralSub); + SmartDashboard.putString("WaitRollFinish", "waiting"); + } + + @Override + public void initialize() { + + } + + @Override + public boolean isFinished() { + boolean finished = (Math.abs(coralSub.getRollGoalDegrees()) > 10) && coralSub.isRollSupposedToBeInPosition(); + if (finished) { + SmartDashboard.putString("WaitRollFinish", "finished"); + } + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WristAlignAssist.java b/src/main/java/frc/robot/commands/coral/motion/WristAlignAssist.java new file mode 100644 index 0000000..835072f --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WristAlignAssist.java @@ -0,0 +1,104 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.coral.CoralReefVision; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; +import frc.robot.subsystems.coral.CoralSubsystem.MirrorPresets; + +public class WristAlignAssist extends Command { + CoralSubsystem coralSubsystem; + CoralPresets startingPreset; + CoralReefVision vision; + XboxController operatorController; + SwerveSubsystem swerveSubsystem; + boolean resetAfter = true; + + public WristAlignAssist(CoralSubsystem coralSub, XboxController operatorController, + SwerveSubsystem swerveSubsystem) { + coralSubsystem = coralSub; + vision = coralSub.getVisionSubsystem(); + this.operatorController = operatorController; + this.swerveSubsystem = swerveSubsystem; + } + + public WristAlignAssist(CoralSubsystem coralSub, XboxController operatorController, + SwerveSubsystem swerveSubsystem, boolean resetAfter) { + coralSubsystem = coralSub; + vision = coralSub.getVisionSubsystem(); + this.operatorController = operatorController; + this.swerveSubsystem = swerveSubsystem; + this.resetAfter = resetAfter; + } + + @Override + public void initialize() { + startingPreset = coralSubsystem.getCurrentPreset(); + } + + @Override + public void execute() { + double adjustCommandRight = operatorController.getRightX(); + + Rotation2d driverStationRight = Rotation2d.fromDegrees(-90); + if (FieldConstants.getAlliance() == Alliance.Red) + driverStationRight = driverStationRight.unaryMinus(); + + boolean directionFlip = (swerveSubsystem.getOdometryPose().getRotation().minus(driverStationRight)) + .getCos() > 0.0; + + double adjustCommandForwardsManual = -adjustCommandRight * (directionFlip ? -1.0 : 1.0); + + if (vision.hasValidTarget() && coralSubsystem.getMirror() == MirrorPresets.RIGHT + && !operatorController.getRightStickButton()) { + Translation2d error = vision.getSelectedTargetError(); + double autoAdjustDeg = Units + .radiansToDegrees(Math.atan2(error.getX(), /*-error.getY()*/ +Units.inchesToMeters(15.0))); + if (startingPreset == CoralPresets.LEVEL_2) { + autoAdjustDeg = Math.max(0.0, autoAdjustDeg); + } + + coralSubsystem.setCustomRollDegrees(MathUtil.clamp( + 90.0 - autoAdjustDeg + - Units.radiansToDegrees(Math.atan2(adjustCommandForwardsManual, 2.0)), + 90 - 30.0, + 90 + 30.0)); + + SmartDashboard.putNumber("Error X", error.getX()); + SmartDashboard.putNumber("Error Y", error.getY()); + // coralSubsystem.setCustomPitchDegrees(0); + } else { + coralSubsystem + .setCustomRollDegrees( + (coralSubsystem.getMirror() == MirrorPresets.LEFT ? -1.0 : 1.0) * MathUtil.clamp( + (90.0 - Units.radiansToDegrees( + Math.atan2( + adjustCommandForwardsManual, 2.0))), + 90 - 30.0, + 90 + 30.0)); + + } + } + + @Override + public boolean isFinished() { + return !(startingPreset == CoralPresets.LEVEL_4 || startingPreset == CoralPresets.LEVEL_2 + || startingPreset == CoralPresets.LEVEL_3); + } + + @Override + public void end(boolean interrupted) { + // Restore roll setting + if (resetAfter) + coralSubsystem.setCoralPresetRoll(startingPreset); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WristAlignAssistManual.java b/src/main/java/frc/robot/commands/coral/motion/WristAlignAssistManual.java new file mode 100644 index 0000000..21cc1da --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WristAlignAssistManual.java @@ -0,0 +1,69 @@ +package frc.robot.commands.coral.motion; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; +import frc.robot.subsystems.coral.CoralSubsystem.MirrorPresets; + +public class WristAlignAssistManual extends Command { + CoralSubsystem coralSubsystem; + CoralPresets startingPreset; + XboxController operatorController; + SwerveSubsystem swerveSubsystem; + + public WristAlignAssistManual(CoralSubsystem coralSub, XboxController operatorController, + SwerveSubsystem swerveSubsystem) { + coralSubsystem = coralSub; + this.operatorController = operatorController; + this.swerveSubsystem = swerveSubsystem; + } + + @Override + public void initialize() { + startingPreset = coralSubsystem.getCurrentPreset(); + } + + @Override + public void execute() { + // If doing "full manual" adjust, multiply by (coralSubsystem.getMirror() == + // MirrorPresets.LEFT ? 1.0 : -1.0) for consistent rotation direction + double adjustCommandRight = operatorController.getRightX(); + + Rotation2d driverStationRight = Rotation2d.fromDegrees(-90); + if (FieldConstants.getAlliance() == Alliance.Red) + driverStationRight = driverStationRight.unaryMinus(); + + boolean directionFlip = (swerveSubsystem.getOdometryPose().getRotation().minus(driverStationRight)) + .getCos() > 0.0; + + double adjustCommandForwards = -adjustCommandRight * (directionFlip ? -1.0 : 1.0); + + coralSubsystem + .setCustomRollDegrees( + (coralSubsystem.getMirror() == MirrorPresets.LEFT ? -1.0 : 1.0) * MathUtil.clamp( + (90.0 - Units.radiansToDegrees( + Math.atan2(adjustCommandForwards, 2.0))), + 90 - 30.0, + 90 + 30.0)); + } + + @Override + public boolean isFinished() { + return !(startingPreset == CoralPresets.LEVEL_4 || startingPreset == CoralPresets.LEVEL_2 + || startingPreset == CoralPresets.LEVEL_3); + } + + @Override + public void end(boolean interrupted) { + // Restore roll setting + coralSubsystem.setCoralPresetRoll(startingPreset); + } +} diff --git a/src/main/java/frc/robot/commands/coral/motion/WristStowSafety.java b/src/main/java/frc/robot/commands/coral/motion/WristStowSafety.java new file mode 100644 index 0000000..a334f63 --- /dev/null +++ b/src/main/java/frc/robot/commands/coral/motion/WristStowSafety.java @@ -0,0 +1,31 @@ +package frc.robot.commands.coral.motion; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +public class WristStowSafety extends Command { + private CoralSubsystem coralSub; + + public WristStowSafety(CoralSubsystem coralSub) { + this.coralSub = coralSub; + } + + @Override + public void initialize() { + coralSub.setCoralPresetRoll(CoralPresets.STOW); + } + + @Override + public void end(boolean interrupted) { + coralSub.setCoralPresetRoll(CoralPresets.STOW); + } + + @Override + public boolean isFinished() { + return Math.abs(coralSub.getCoralArm().getRollPositionDegrees()) < 90.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..039e9dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,107 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Climber; + +@Logged +public class ClimberSubsystem extends SubsystemBase { + private final SparkFlex climberMotor = new SparkFlex(Climber.MOTOR_PORT, MotorType.kBrushless); + private SparkFlexConfig climberConfig = new SparkFlexConfig(); + private RelativeEncoder climberEncoder; + @NotLogged + XboxController operator; + + boolean isTested = false; + + public ClimberSubsystem(XboxController operator) { + this.operator = operator; + climberMotor.configure(climberConfig.idleMode(IdleMode.kBrake) + .apply(new SoftLimitConfig().reverseSoftLimit(Constants.Climber.DEPLOY_SOFT_LIMIT) + .reverseSoftLimitEnabled(true).forwardSoftLimit(0.0) + .forwardSoftLimitEnabled(true)) + .apply( + new EncoderConfig() + .positionConversionFactor( + 1.0 / Constants.Climber.GEAR_RATIO) + .velocityConversionFactor( + 1.0 / Constants.Climber.GEAR_RATIO)), + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + + climberEncoder = climberMotor.getEncoder(); + } + + private double output = 0.0; + private boolean deployed = false; + + @Override + public void periodic() { + SmartDashboard.putNumber("Climber Encoder", climberEncoder.getPosition()); + if (climberEncoder.getPosition() < Constants.Climber.CLIMB_SOFT_LIMIT && + !deployed) { + deployed = true; + climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit( + Constants.Climber.CLIMB_SOFT_LIMIT)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + if (DriverStation.isTest() && !isTested) { + climberMotor.configure( + climberConfig.apply( + climberConfig.softLimit.forwardSoftLimitEnabled(false) + .reverseSoftLimitEnabled(false)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + isTested = true; + } else if (!DriverStation.isTest() && isTested) { + climberEncoder.setPosition(0.0); + climberMotor.configure( + climberConfig.apply( + climberConfig.softLimit.forwardSoftLimitEnabled(true) + .reverseSoftLimitEnabled(true)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + deployed = false; + isTested = false; + } + + if (!Constants.Climber.DBG_DISABLED) + climberMotor.set(operator.getLeftStickButton() + ? (MathUtil.applyDeadband( + operator.getLeftY(), 0.05) + * (operator.getLeftY() < 0.0 ? 1.0 : 0.7)) + : 0.0); + } + + public void resetClimberDeploy() { + deployed = false; + climberMotor.configure(climberConfig.apply(climberConfig.softLimit.forwardSoftLimit(0.0)), + ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + public void setOutput(double output) { + this.output = output; + } + + public double getOutput() { + return output; + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java deleted file mode 100644 index 26f9baa..0000000 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ /dev/null @@ -1,238 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkLimitSwitch; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.datalog.DoubleLogEntry; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import frc.robot.Constants; -import frc.robot.Robot; - -public class ElevatorSubsystem extends ProfiledPIDSubsystem { - // NOTE: Elevator motor one has both the encoder used for positioning, and the - // limit switch used for zeroing - private final SparkFlex elevatorMotorOne = new SparkFlex(Constants.Elevator.elevatorOnePort, - MotorType.kBrushless); - private final SparkFlex elevatorMotorTwo = new SparkFlex(Constants.Elevator.elevatorTwoPort, - MotorType.kBrushless); - private final ElevatorFeedforward feedForward = new ElevatorFeedforward( - Constants.Elevator.Feedforward.Ks, - Constants.Elevator.Feedforward.Kg, - Constants.Elevator.Feedforward.Kv, - Constants.Elevator.Feedforward.Ka - - ); - - private final SparkMaxConfig elevatorConfigOne = new SparkMaxConfig(); - private final SparkMaxConfig elevatorConfigTwo = new SparkMaxConfig(); - - /* - * DCMotor gearbox, - * double gearing, - * double carriageMassKg, - * double drumRadiusMeters, - * double minHeightMeters, - * double maxHeightMeters, - * boolean simulateGravity, - * double startingHeightMeters, - * Matrix measurementStdDevs - */ - private boolean isZeroed = false; - private final RelativeEncoder elevatorEncoderOne; - private final RelativeEncoder elevatorEncoderTwo; - private final SparkLimitSwitch bottomLimit; - - private final ElevatorSim simulation = new ElevatorSim( - Constants.Elevator.PhysicalParameters.simMotor, - Constants.Elevator.PhysicalParameters.gearReduction, - Constants.Elevator.PhysicalParameters.carriageMassKg, - Constants.Elevator.PhysicalParameters.driveRadiusMeters, - 0.0, - Constants.Elevator.PhysicalParameters.elevatorHeightMeters, - true, - 0.0, - 0.001, - 0.001 - ); - - private DoubleLogEntry elevatorTargetP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/position"); - private DoubleLogEntry elevatorTargetV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/target/velocity"); - private DoubleLogEntry elevatorP = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/position"); - private DoubleLogEntry elevatorV = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/state/velocity"); - private DoubleLogEntry elevatorOneOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output1"); - private DoubleLogEntry elevatorTwoOutput = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/output2"); - private DoubleLogEntry elevatorOneCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current1"); - private DoubleLogEntry elevatorTwoCurrent = new DoubleLogEntry(DataLogManager.getLog(), "Elevator/current2"); - - private Mechanism2d mechanism2D = new Mechanism2d(Constants.RobotConstants.robotLengthMeters, - Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters - + Constants.Elevator.PhysicalParameters.elevatorHeightMeters - + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0); - private MechanismRoot2d rootMechanism = mechanism2D.getRoot("climber", - Constants.RobotConstants.robotLengthMeters / 2.0 - + Constants.Elevator.PhysicalParameters.elevatorForwardsFromRobotCenterMeters, - Constants.Elevator.PhysicalParameters.elevatorBottomFromFloorMeters); - private MechanismLigament2d elevatorMechanism; - - public ElevatorSubsystem() { - super( - new ProfiledPIDController( - Constants.Elevator.PID.kP, - Constants.Elevator.PID.kI, - Constants.Elevator.PID.kD, - new TrapezoidProfile.Constraints( - Constants.Elevator.PID.MAX_VELOCITY, - Constants.Elevator.PID.MAX_ACCELERATION)), - 0.0); - - this.getController().setTolerance(Units.inchesToMeters(0.5)); - - elevatorConfigOne - .idleMode(IdleMode.kBrake) - .inverted(Constants.Elevator.elevatorOneInverted); - elevatorConfigOne.encoder - .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter) - .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter); - elevatorConfigOne.limitSwitch.reverseLimitSwitchType(Constants.Elevator.bottomLimitMode); - - elevatorConfigTwo - .idleMode(IdleMode.kBrake) - .inverted(Constants.Elevator.elevatorTwoInverted); - elevatorConfigTwo.encoder - .positionConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter) - .velocityConversionFactor(1.0 / Constants.Elevator.motorTurnsPerMeter); - - elevatorMotorOne.configure(elevatorConfigOne, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - elevatorMotorTwo.configure(elevatorConfigTwo, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - elevatorEncoderOne = elevatorMotorOne.getEncoder(); - elevatorEncoderTwo = elevatorMotorTwo.getEncoder(); - - elevatorMotorOne.getEncoder().setPosition(0); - elevatorMotorTwo.getEncoder().setPosition(0); - - bottomLimit = elevatorMotorOne.getReverseLimitSwitch(); - - this.enable(); - - this.elevatorMechanism = rootMechanism.append(new MechanismLigament2d("elevator", 0.0, 90)); - } - - public void setPosition(double positionMeters) { - setGoal(MathUtil.clamp(positionMeters, 0.0, Constants.Elevator.PhysicalParameters.elevatorHeightMeters)); - } - - public double getPosition() { - return getMeasurement(); - } - - public double getGoalPosition() { - return this.getController().getGoal().position; - } - - double lastVelocity = 0.0; - double lastTime = 0.0; - - @Override - public void useOutput(double output, TrapezoidProfile.State setpoint) { - double dv = setpoint.velocity - lastVelocity; - double dt = Timer.getFPGATimestamp() - lastTime; - lastTime = Timer.getFPGATimestamp(); - - lastVelocity = setpoint.velocity; - - double ff = feedForward.calculate(setpoint.velocity, dv / dt); - - if (isZeroed || Robot.isSimulation()) { - // Controller output voltage - double op = ff + output; - - elevatorMotorOne.setVoltage(op); - elevatorMotorTwo.setVoltage(op); - - elevatorTargetP.append(setpoint.position); - elevatorTargetV.append(setpoint.velocity); - - SmartDashboard.putNumber("Elevator/Current Position", getMeasurement()); - SmartDashboard.putNumber("Elevator/Current Velocity", elevatorEncoderOne.getVelocity()); - - SmartDashboard.putNumber("Elevator/Target Position", setpoint.position); - SmartDashboard.putNumber("Elevator/Target Velocity", setpoint.velocity); - - if (Robot.isSimulation()) { - simulation.setInputVoltage(MathUtil.clamp((output + ff), -12.0, 12.0)); - } - } else { - // Move down a little bit to zero - elevatorMotorOne.set(-0.05); - elevatorMotorTwo.set(-0.05); - } - } - - @Override - public double getMeasurement() { - return Robot.isSimulation() ? simulation.getPositionMeters() : elevatorEncoderOne.getPosition(); - } - - @Override - public void periodic() { - // TODO Auto-generated method stub - - if ((bottomLimit.isPressed()) && (!isZeroed)) { - elevatorEncoderOne.setPosition(0.0); - setGoal(0.0); - isZeroed = true; - } - - super.periodic(); - - elevatorP.append(getMeasurement()); - elevatorV.append(elevatorEncoderOne.getVelocity()); - elevatorOneOutput.append(elevatorMotorOne.getAppliedOutput()); - elevatorTwoOutput.append(elevatorMotorTwo.getAppliedOutput()); - elevatorOneCurrent.append(elevatorMotorOne.getOutputCurrent()); - elevatorTwoCurrent.append(elevatorMotorTwo.getOutputCurrent()); - this.elevatorMechanism - .setLength(getMeasurement() + Constants.Elevator.PhysicalParameters.elevatorCarriageHeightMeters / 2.0); - - // SmartDashboard.putNumber("Current Elevator Position", getMeasurement()); - // SmartDashboard.putNumber("Goal Elevator Position", - // this.getController().getGoal().position); - SmartDashboard.putBoolean("Elevator Bottom Limit", bottomLimit.isPressed()); - SmartDashboard.putData("Elevator Mechanism", mechanism2D); - } - - @Override - public void simulationPeriodic() { - // TODO Auto-generated method stub - super.simulationPeriodic(); - - simulation.update(0.020); - - RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(simulation.getCurrentDrawAmps())); - } - - public boolean isInPosition() { - return this.getController().atGoal(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java new file mode 100644 index 0000000..104112c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -0,0 +1,227 @@ +package frc.robot.subsystems; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; +import frc.robot.LimelightHelpers.RawFiducial; +import frc.robot.util.LimelightContainer; + +@Logged +public class Limelight extends SubsystemBase { + public enum LimelightType { + // LL1 doesn't have specs listed, so these could be incorrect + LL1(54, 41), + LL2(62.5, 48.9), + LL2Plus(62.5, 48.9), + LL3(62.5, 48.9), + LL3G(82, 56.2), + LL4(82, 56.2); + + private double HFOV; + private double VFOV; + + private LimelightType(double HFOV, double VFOV) { + this.HFOV = HFOV; + this.VFOV = VFOV; + } + } + + private final LimelightType limelightType; + private final String name; + private boolean isEnabled; + private boolean cropEnabled; + private double lastPoseEstimate = 0; + private int counter = 0; + private double lastFrame = 0; + + private final StructPublisher publisher; + + + public Limelight(LimelightType limelightType, String name, boolean isEnabled, boolean cropEnabled) { + this.limelightType = limelightType; + this.name = name; + this.isEnabled = isEnabled; + this.cropEnabled = cropEnabled; + + publisher = NetworkTableInstance.getDefault().getStructTopic(name, Pose2d.struct).publish(); + + LimelightHelpers.SetIMUMode(name, 1); + } + + @Override + public void periodic() { + if (isEnabled) { + if (cropEnabled) { + if (numTargets() > 0) { + smartCrop(); + } else { + restoreCrop(); + } + } + } + } + + public int numTargets() { + return LimelightHelpers.getRawFiducials(name).length; + } + + public void smartCrop() { + LimelightHelpers.PoseEstimate poseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + if(poseEstimate == null){return;} + if (lastPoseEstimate != poseEstimate.timestampSeconds){ + counter = ++counter % 50; + if(counter > 40){ // For every 5 frames, out of 50, check the entire screen for apriltags + restoreCrop(); // 45 will be cropped onto whichever limelights they find + return; + } + lastPoseEstimate = poseEstimate.timestampSeconds; //making sure we're only running this process on new frames + RawFiducial[] targets = LimelightHelpers.getRawFiducials(name); //get our targets + boolean useCrop = true; //we are currently using it, more changes will be implemented + Translation2d[] targetTranslations = new Translation2d[targets.length]; + double area = 0; + + for (int i = 0; i < targetTranslations.length; ++i) { + targetTranslations[i] = new Translation2d(targets[i].txnc, targets[i].tync); + area = Math.max(area, targets[i].ta); //Finding the largest area of the targets + } + + double xc = 0, yc = 0; + + for (int i = 0; i < targetTranslations.length; ++i) { // Add to the width, depending on the locations of the other targets + xc += targetTranslations[i].getX(); //If one is at (2, 1), the width of our box increases on each size by 2, and the height by 1 + yc += targetTranslations[i].getY(); //xc & xy increase per target + } + + xc /= targetTranslations.length; //Then, average depending on the number of targets + yc /= targetTranslations.length; // (2,1), (.05, .25), (-1, 2) -> xc =.35, yc= .75 + + xc = xc / (limelightType.HFOV / 2); // scales it to FOV + yc = yc / (limelightType.VFOV / 2); + + double borderx = 0, bordery = 0; + + if (targetTranslations.length > 1) { // for more than one tag + borderx = (area + 0.75) * 0.22 * targetTranslations.length + .2; // relatively randomly generated border + bordery = (area + 0.5) * 0.22 * targetTranslations.length; + } + + //todo optimize + + else { + if (area > 0.75){ + borderx = 1; + bordery = .5; + } + else if (area > 0.5) { //Remember that "area" only refers to the area of the largest apriltag + borderx = 0.8; + bordery = 0.5; + } + else if (area > .015){ + borderx = 1; + bordery = 0.25; + } + else if (area > .005){ + borderx = 1; + bordery = .5; + } + else { + borderx = 1; + bordery = .5; + } + } + + /*else { + if (area < 0.015) { + borderx = 0.5; + bordery = 0.25; + } + else if (area > .005){ + borderx = 1; + bordery = 0.5; + } + else { + borderx = 0.5; + bordery = 0.5; + } + } + */ + //borderx = 1; + //bordery = .25; + // Doesn't let borders extend beyond -1, 1 for x & y + double xlim = (xc - borderx < -1) ? -1 : xc - borderx; + double xlim2 = (xc - borderx > 1) ? 1 : xc + borderx; + double ylim = (yc - bordery < -1) ? -1 : yc - borderx; + double ylim2 = (yc - bordery > 1) ? 1 : xc + borderx; + + if (useCrop) { + LimelightHelpers.setCropWindow(name, xlim, xlim2, ylim, ylim2); + // Finds the center of the targets, tries to build a big enough box from there + } + } + } + + public void setIMUMode(int mode) { + LimelightHelpers.SetIMUMode(name, mode); + } + + public void restoreCrop() { + LimelightHelpers.setCropWindow(name, -1, 1, -1, 1); + } + + + public void setEnabled(boolean enabled) { + this.isEnabled = enabled; + } + + public void setCropEnabled(boolean enabled) { + this.cropEnabled = enabled; + } + + public boolean isEnabled() { + return isEnabled; + } + + public boolean isCropEnabled() { + return cropEnabled; + } + + public double getLastFrameTime(){ + return lastFrame; + } + + public void setLastFrame(double lastFrameTime){ + lastFrame = lastFrameTime; + } + + public double getVFOV() { + return limelightType.VFOV; + } + + public double getHFOV() { + return limelightType.HFOV; + } + + public LimelightType getLimelightType() { + return limelightType; + } + + public String getName() { + return name; + } + + /** + * Used to put a pose on Shuffleboard for debugging + * + * @param name Name of pose + * @param pose Pose2d pose + */ + public void pushPoseToShuffleboard(String name, Pose2d pose) { + publisher.set(pose); + } + +} diff --git a/src/main/java/frc/robot/subsystems/RobotMechanismLogger.java b/src/main/java/frc/robot/subsystems/RobotMechanismLogger.java new file mode 100644 index 0000000..ec150f6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RobotMechanismLogger.java @@ -0,0 +1,144 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Degrees; + +import java.util.HashSet; +import java.util.function.Consumer; + +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.Unit; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Elevator; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.coral.CoralArm; +import frc.robot.subsystems.coral.CoralSubsystem; + +public class RobotMechanismLogger extends SubsystemBase { + private final CoralSubsystem coralSubsystem; + private final AlgaeSubsystem algaeSubsystem; + private final SwerveSubsystem swerveSubsystem; + private final StructPublisher s1; + private final StructPublisher s2; + private final StructPublisher arm; + private final StructPublisher wrist1; + private final StructPublisher wrist2; + private final StructPublisher coral; + private final StructPublisher algae; + private final StructPublisher algaeArm; + + // Zeroed component poses + private Pose3d s1Pose = new Pose3d(0, 0, 0, new Rotation3d()); + private Pose3d s2Pose = new Pose3d(0, 0, 0, new Rotation3d()); + private Pose3d armPose = new Pose3d(0, 0, 0, new Rotation3d()); + private Pose3d wrist1Pose = new Pose3d(0, 0, 0, new Rotation3d()); + private Pose3d wrist2Pose = new Pose3d(0, 0, 0, new Rotation3d()); + private Pose3d coralPose = new Pose3d(); + private Pose3d algaeArmPose = new Pose3d(); + private Pose3d algaePose = new Pose3d(); + + public RobotMechanismLogger(CoralSubsystem coralSubsystem, SwerveSubsystem swerveSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.swerveSubsystem = swerveSubsystem; + this.coralSubsystem = coralSubsystem; + this.algaeSubsystem = algaeSubsystem; + s1 = NetworkTableInstance.getDefault().getStructTopic("0_ElevatorS1", Pose3d.struct).publish(); + s2 = NetworkTableInstance.getDefault().getStructTopic("1_ElevatorS2", Pose3d.struct).publish(); + arm = NetworkTableInstance.getDefault().getStructTopic("2_Arm", Pose3d.struct).publish(); + wrist1 = NetworkTableInstance.getDefault().getStructTopic("3_Wrist1", Pose3d.struct).publish(); + wrist2 = NetworkTableInstance.getDefault().getStructTopic("4_Wrist2", Pose3d.struct).publish(); + coral = NetworkTableInstance.getDefault().getStructTopic("Coral_Pose", Pose3d.struct).publish(); + algae = NetworkTableInstance.getDefault().getStructTopic("Algae_Pose", Pose3d.struct).publish(); + algaeArm = NetworkTableInstance.getDefault().getStructTopic("5_AlgaeArm", Pose3d.struct).publish(); + } + + @Override + public void periodic() { + updatePoses(); + updateAdvantageScope(); + } + + public void updateAdvantageScope() { + s1.set(s1Pose); + s2.set(s2Pose); + arm.set(armPose); + wrist1.set(wrist1Pose); + wrist2.set(wrist2Pose); + coral.set(coralPose); + algae.set(algaePose); + algaeArm.set(algaeArmPose); + } + + public void updatePoses() { + double elevatorHeight = coralSubsystem.getElevator().getPosition(); + double armRotation = coralSubsystem.getPivotPositionDegrees(); + + s1Pose = new Pose3d(0.026, 0, 0.055 + elevatorHeight / 2, new Rotation3d()); + s2Pose = new Pose3d(0, 0, 0.081 + elevatorHeight, new Rotation3d()); + armPose = new Pose3d(0.071, 0, 0.22 + elevatorHeight, + new Rotation3d(Units.degreesToRadians(armRotation), 0, 0)); + + wrist1Pose = armPose.transformBy(new Transform3d(0.11, 0, 0.74 - 0.22, + new Rotation3d(0, 0, -Units.degreesToRadians( + coralSubsystem.getCoralArm().getRollPositionDegrees())))); + + wrist2Pose = wrist1Pose.transformBy(new Transform3d(0.019050, + 0.0254, + 0.075057, + new Rotation3d(0, + Units.degreesToRadians( + coralSubsystem.getCoralArm().getPitchPositionDegrees()), + 0))); + + algaeArmPose = armPose.transformBy(new Transform3d( + 0.145, 0.0, 0.365, + new Rotation3d(-Units + .degreesToRadians(algaeSubsystem.getArm().getPositionDegrees() + 10.0), + 0.0, + 0.0))); + + if (coralSubsystem.isHolding()) { + // coralPose = new Pose3d(Units.inchesToMeters(2.0), 0, 0, Rotation3d.kZero) + // .relativeTo(wrist2Pose.relativeTo(new + // Pose3d(swerveSubsystem.getOdometryPose()))); + Pose3d coralHoldingPose = new Pose3d(0.0, 0, Units.inchesToMeters(2.5 + 11.875 + / 2.0), + new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0)); + coralPose = new Pose3d(swerveSubsystem.getOdometryPose()) + .transformBy(new Transform3d(wrist2Pose.getTranslation(), + wrist2Pose.getRotation())) + .transformBy(new Transform3d(coralHoldingPose.getTranslation(), + coralHoldingPose.getRotation())); + } else { + coralPose = Pose3d.kZero; + } + + if (algaeSubsystem.isHolding()) { + algaePose = new Pose3d(swerveSubsystem.getOdometryPose()) + .transformBy(new Transform3d(armPose.getTranslation(), armPose + .getRotation())) + .transformBy(new Transform3d(0.13, -Units.inchesToMeters(8.125), 0.55, + Rotation3d.kZero)); + } else { + algaePose = Pose3d.kZero; + } + + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 7da0d6c..acf434c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -1,19 +1,27 @@ package frc.robot.subsystems; +import org.opencv.core.Mat; + import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.google.flatbuffers.Constants; import com.revrobotics.*; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkLowLevel.PeriodicFrame; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; @@ -22,6 +30,7 @@ import frc.robot.Robot; import frc.robot.Constants.*; +@Logged public class SwerveModule { private final TalonFX driveMotor; // https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/api-usage/configuration.html @@ -34,28 +43,30 @@ public class SwerveModule { private final RelativeEncoder steerMotorEncoder; private double driveEncSim = 0; - private double steerEncSim = 0; + private double steerEncSim = -10.0; + double drive_command = 0; + double steer_command = 0; private final com.ctre.phoenix6.hardware.CANcoder absoluteEncoder; private final double motorOffsetRadians; private final boolean isAbsoluteEncoderReversed; - private final boolean motor_inv; private final PIDController steerPID; private static int moduleNumber = 0; int thisModuleNumber; - SlewRateLimiter turnratelimiter = new SlewRateLimiter(4.d); + SimpleMotorFeedforward steerFeedforward = new SimpleMotorFeedforward(0.425 / 12.0, 0.4184); - public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, double motorOffsetRadians, - boolean isAbsoluteEncoderReversed, boolean motorReversed) { + public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, double absEncoderOffsetRadians, + boolean isAbsoluteEncoderReversed, boolean motorReversed, boolean steerMotorReversed) { // driveMotor = new CANSparkMax(driveCanID, MotorType.kBrushless); driveMotor = new TalonFX(driveCanID); driveConfigurator = driveMotor.getConfigurator(); driveConfig = new MotorOutputConfigs(); - driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + driveConfig.Inverted = motorReversed ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; driveConfig.NeutralMode = NeutralModeValue.Brake; driveConfigurator.apply(driveConfig); @@ -64,13 +75,12 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou steerConfig = new SparkMaxConfig(); steerConfig .idleMode(IdleMode.kBrake) - .inverted(false); + .inverted(steerMotorReversed); steerConfig.encoder .positionConversionFactor(SwerveModuleConstants.STEER_ROTATION_TO_RADIANS) .velocityConversionFactor(SwerveModuleConstants.STEER_RADIANS_PER_MINUTE); steerMotor.configure(steerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - this.motor_inv = motorReversed; // driveMotorEncoder = driveMotor.get(); steerMotorEncoder = steerMotor.getEncoder(); @@ -82,7 +92,7 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou absoluteEncoder.getConfigurator().apply(cfg); // CANcoderConfigurator configurator = absoluteEncoder.getConfigurator(); - this.motorOffsetRadians = motorOffsetRadians; + this.motorOffsetRadians = absEncoderOffsetRadians; this.isAbsoluteEncoderReversed = isAbsoluteEncoderReversed; // driveMotorEncoder.setPositionConversionFactor(SwerveModuleConstants.DRIVE_ROTATION_TO_METER); @@ -100,8 +110,8 @@ public SwerveModule(int steerCanID, int driveCanID, int absoluteEncoderPort, dou } public void simulate_step() { - driveEncSim += 0.02 * driveMotor.get() * (DriveConstants.MAX_MODULE_VELOCITY); - steerEncSim += 0.02 * steerMotor.get() * (10.0); + driveEncSim += 0.02 * drive_command * (DriveConstants.MAX_MODULE_VELOCITY); + steerEncSim += 0.02 * steer_command * (SwerveModuleConstants.STEER_MAX_RAD_SEC); } public double getDrivePosition() { @@ -109,12 +119,13 @@ public double getDrivePosition() { return driveEncSim; // return driveMotorEncoder.getPosition(); // TODO: Do the conversion in the motor - return driveMotor.getPosition().getValueAsDouble()* SwerveModuleConstants.DRIVE_ROTATION_TO_METER; + return driveMotor.getPosition().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER; } public double getDriveVelocity() { // return driveMotorEncoder.getVelocity(); - return driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER; + return Robot.isSimulation() ? drive_command * (DriveConstants.MAX_MODULE_VELOCITY) + : driveMotor.getVelocity().getValueAsDouble() * SwerveModuleConstants.DRIVE_ROTATION_TO_METER; } public double getSteerPosition() { @@ -128,8 +139,7 @@ public double getSteerVelocity() { } public double getAbsoluteEncoderPosition() { - double angle = Units.rotationsToRadians(absoluteEncoder.getPosition().getValueAsDouble());// * (Math.PI / - // 180.d); + double angle = Units.rotationsToRadians(absoluteEncoder.getPosition().getValueAsDouble()); angle -= motorOffsetRadians; return angle * (isAbsoluteEncoderReversed ? -1.0 : 1.0); } @@ -138,35 +148,62 @@ public void resetEncoders() { // driveMotorEncoder.setPosition(0); driveMotor.setPosition(0.0); steerMotorEncoder.setPosition(getAbsoluteEncoderPosition()); + + if (Robot.isSimulation()) { + driveEncSim = 0.f; + steerEncSim = -10.f; + } } public SwerveModuleState getModuleState() { - return new SwerveModuleState(getDriveVelocity(), new Rotation2d(-getSteerPosition())); + // FIXME: Negative? + return new SwerveModuleState(getDriveVelocity(), getSteerRotation()); // Go back to old if broke } public SwerveModulePosition getModulePosition() { + // FIXME: Negative? return new SwerveModulePosition(getDrivePosition(), - new Rotation2d(-getSteerPosition()).rotateBy(DriveConstants.NAVX_ANGLE_OFFSET.times(-1))); + getSteerRotation()); // Go back to old if broke + } + + public Rotation2d getSteerRotation() { + return new Rotation2d(MathUtil.angleModulus(getSteerPosition())); + } + + double currentAcceleration = 0.0; + + public void setAcceleration(double m_ss_acc) { + currentAcceleration = m_ss_acc; } public void setModuleStateRaw(SwerveModuleState state) { - state.optimize(new Rotation2d(getSteerPosition())); - double drive_command = state.speedMetersPerSecond / DriveConstants.MAX_MODULE_VELOCITY; - // SmartDashboard.putNumber("Module " + Integer.toString(this.thisModuleNumber) + " Drive", drive_command); - driveMotor.set(drive_command * (motor_inv ? -1.0 : 1.0)); - - // This is stupid - // steerPID.setP(Constants.SwerveModuleConstants.MODULE_KP * - // Math.abs(drive_command)); - double steercmd = steerPID.calculate(getSteerPosition(), state.angle.getRadians()); + Rotation2d currentRotation = getSteerRotation(); + double orig_statevel = state.speedMetersPerSecond; + state.optimize(currentRotation); + state.cosineScale(currentRotation); + + double state_comp = state.speedMetersPerSecond / orig_statevel; + + drive_command = (state.speedMetersPerSecond / DriveConstants.MAX_MODULE_VELOCITY); + + driveMotor.setVoltage( + drive_command * 12.0 + + currentAcceleration * DriveConstants.GLOBAL_kA * state_comp); + if (Robot.isSimulation()) { - steerMotor.set(steercmd); + steer_command = steerPID.calculate(getSteerPosition(), + MathUtil.angleModulus(state.angle.getRadians())); } else { - steerMotor.setVoltage(12 * steercmd); + steer_command = steerPID.calculate(getSteerPosition(), + MathUtil.angleModulus(state.angle.getRadians())); + steer_command += Math.abs(steer_command) < 0.025 ? 0.0 + : Math.signum(steer_command) * steerFeedforward.getKs(); } - // SmartDashboard.putNumber("Abs" + thisModuleNumber, - // getAbsoluteEncoderPosition()); - SmartDashboard.putNumber("Drive" + thisModuleNumber, drive_command); + + steerMotor.setVoltage(12.0 * steer_command); + + // SmartDashboard.putNumber("Steer" + thisModuleNumber, getSteerPosition()); + // SmartDashboard.putNumber("Drive" + thisModuleNumber, drive_command); } public void setModuleState(SwerveModuleState state) { @@ -180,5 +217,7 @@ public void setModuleState(SwerveModuleState state) { public void stop() { driveMotor.set(0); steerMotor.set(0); + drive_command = 0.f; + steer_command = 0.f; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 45338f4..367ef72 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,206 +1,240 @@ package frc.robot.subsystems; +import java.util.ArrayList; +import java.util.Arrays; import java.util.List; -import com.studica.frc.AHRS; +import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; +import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; +import com.studica.frc.AHRS; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.kinematics.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; 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.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.LimelightHelpers; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.PathPlannerConstants; +import frc.robot.Constants.PoseConstants; +import frc.robot.Constants.SwerveModuleConstants; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.RobotContainer; import frc.robot.Robot; -import frc.robot.Constants.*; +@Logged public class SwerveSubsystem extends SubsystemBase { + + boolean isalliancereset = false; + + @NotLogged SwerveModule frontLeft = new SwerveModule(SwerveModuleConstants.FL_STEER_ID, SwerveModuleConstants.FL_DRIVE_ID, SwerveModuleConstants.FL_ABSOLUTE_ENCODER_PORT, SwerveModuleConstants.FL_OFFSET_RADIANS, SwerveModuleConstants.FL_ABSOLUTE_ENCODER_REVERSED, - SwerveModuleConstants.FL_MOTOR_REVERSED); - + SwerveModuleConstants.FL_MOTOR_REVERSED, + SwerveModuleConstants.FL_STEERING_MOTOR_REVERSED); + @NotLogged SwerveModule frontRight = new SwerveModule(SwerveModuleConstants.FR_STEER_ID, SwerveModuleConstants.FR_DRIVE_ID, SwerveModuleConstants.FR_ABSOLUTE_ENCODER_PORT, SwerveModuleConstants.FR_OFFSET_RADIANS, SwerveModuleConstants.FR_ABSOLUTE_ENCODER_REVERSED, - SwerveModuleConstants.FR_MOTOR_REVERSED); - + SwerveModuleConstants.FR_MOTOR_REVERSED, + SwerveModuleConstants.FR_STEERING_MOTOR_REVERSED); + @NotLogged SwerveModule backRight = new SwerveModule(SwerveModuleConstants.BR_STEER_ID, SwerveModuleConstants.BR_DRIVE_ID, SwerveModuleConstants.BR_ABSOLUTE_ENCODER_PORT, SwerveModuleConstants.BR_OFFSET_RADIANS, SwerveModuleConstants.BR_ABSOLUTE_ENCODER_REVERSED, - SwerveModuleConstants.BR_MOTOR_REVERSED); - + SwerveModuleConstants.BR_MOTOR_REVERSED, + SwerveModuleConstants.BR_STEERING_MOTOR_REVERSED); + @NotLogged SwerveModule backLeft = new SwerveModule(SwerveModuleConstants.BL_STEER_ID, SwerveModuleConstants.BL_DRIVE_ID, SwerveModuleConstants.BL_ABSOLUTE_ENCODER_PORT, SwerveModuleConstants.BL_OFFSET_RADIANS, SwerveModuleConstants.BL_ABSOLUTE_ENCODER_REVERSED, - SwerveModuleConstants.BL_MOTOR_REVERSED); - - private DoubleLogEntry chassisAccelX; - private DoubleLogEntry chassisAccelY; - private DoubleLogEntry chassisAccelZ; - private DoubleLogEntry chassisRotX; - private DoubleLogEntry chassisRotY; - private DoubleLogEntry chassisRotZ; - - PowerDistribution pdh = new PowerDistribution(1, ModuleType.kRev); - int[] pdh_channels = { - 18, 19, - 0, 1, - 16, 17, - 2, 3 - }; - - public enum RotationStyle { - Driver, - AutoSpeaker, - AutoShuttle - } - - private RotationStyle rotationStyle = RotationStyle.Driver; + SwerveModuleConstants.BL_MOTOR_REVERSED, + SwerveModuleConstants.BL_STEERING_MOTOR_REVERSED); - public final AHRS navX = new AHRS(AHRS.NavXComType.kMXP_SPI); + // public final AHRS navX = new AHRS(AHRS.NavXComType.kMXP_SPI, 50); private double navxSim; + public final Pigeon2 pigeon = new Pigeon2(11); + private double pigeonOffset = 0.0; private ChassisSpeeds lastChassisSpeeds = new ChassisSpeeds(); private Field2d field = new Field2d(); + private FieldObject2d fieldRobot = field.getRobotObject(); - boolean isalliancereset = false; + StructPublisher posePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Odometry Pose", Pose2d.struct).publish(); + StructArrayPublisher swerveStatesPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("Swerve States", SwerveModuleState.struct).publish(); + StructArrayPublisher swerveTargetStatesPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("Swerve Target States", SwerveModuleState.struct).publish(); // TODO: Properly set starting pose - public final SwerveDrivePoseEstimator odometry = new SwerveDrivePoseEstimator(DriveConstants.KINEMATICS, - getRotation2d(), - getModulePositions(), new Pose2d(), createStateStdDevs( - PoseConstants.kPositionStdDevX, - PoseConstants.kPositionStdDevY, - PoseConstants.kPositionStdDevTheta), - createVisionMeasurementStdDevs( - PoseConstants.kVisionStdDevX, - PoseConstants.kVisionStdDevY, - PoseConstants.kVisionStdDevTheta)); + public final SwerveDrivePoseEstimator odometry; + + private final SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; public SwerveSubsystem() { - // ! F - // zeroHeading() + odometry = new SwerveDrivePoseEstimator(DriveConstants.KINEMATICS, + getGyroRotation2d(), + getModulePositions(), new Pose2d(), createStateStdDevs( + PoseConstants.kPositionStdDevX, + PoseConstants.kPositionStdDevY, + PoseConstants.kPositionStdDevTheta), + createVisionMeasurementStdDevs( + PoseConstants.kVisionStdDevX, + PoseConstants.kVisionStdDevY, + PoseConstants.kVisionStdDevTheta)); // --------- Path Planner Init ---------- \\ + RobotConfig config = Constants.PathPlannerConstants.ROBOT_CONFIG; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } + + NamedCommands.registerCommand("namedCommand", new PrintCommand("Ran namedCommand")); + + setpointGenerator = new SwerveSetpointGenerator( + config, + Constants.SwerveModuleConstants.STEER_MAX_RAD_SEC); + + previousSetpoint = new SwerveSetpoint(getChassisSpeeds(), getModuleStates(), + DriveFeedforwards.zeros(config.numModules)); + + // navX.enableLogging(true); + } + + public void configurePathplanner() { + RobotConfig config = Constants.PathPlannerConstants.ROBOT_CONFIG; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) + this::getOdometryPose, // Robot pose supplier + this::resetOdometryAndGyro, // Method to reset odometry (will be called if your auto has a starting + // pose) this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, - PathPlannerConstants.ROBOT_CONFIG, + (speeds, feedforward) -> { + setChassisSpeeds(speeds); + setFeedforwards(feedforward); + }, // Method that will drive the robot given ROBOT + // RELATIVE ChassisSpeeds + Constants.PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, + // Constants.PathPlannerConstants.ROBOT_CONFIG, // The robot configuration + config, () -> { // Boolean supplier that controls when the path will be mirrored for the red // alliance // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { return alliance.get() == DriverStation.Alliance.Red; } - return false; }, this // Reference to this subsystem to set requirements ); - - NamedCommands.registerCommand("namedCommand", new PrintCommand("Ran namedCommand")); - - chassisAccelX = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/acceleration/x"); - chassisAccelY = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/acceleration/y"); - chassisAccelZ = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/acceleration/z"); - chassisRotX = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/rot_speed/x"); - chassisRotY = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/rot_speed/y"); - chassisRotZ = new DoubleLogEntry(DataLogManager.getLog(), "Chassis/rot_speed/z"); } @Override public void periodic() { + // if ((!isalliancereset && DriverStation.getAlliance().isPresent())) { + // RobotContainer.LLContainer.estimateMT1OdometryPrelim(odometry, + // lastChassisSpeeds, navX, getModulePositions()); + // SmartDashboard.putString("Prelim odometry position", + // odometry.getEstimatedPosition().toString()); + // isalliancereset = true; + // } - if (!isalliancereset && DriverStation.getAlliance().isPresent()) { - Translation2d pospose = getPose().getTranslation(); - odometry.resetPosition(getRotation2d(), getModulePositions(), - new Pose2d(pospose, new Rotation2d(FieldConstants.getAlliance() == Alliance.Blue ? 0.0 : Math.PI))); - isalliancereset = true; - } + if (DriverStation.isTeleop()) + RobotContainer.LLContainer.estimateMT1Odometry(odometry, lastChassisSpeeds); + else + RobotContainer.LLContainer.estimateMT1OdometryAuto(odometry, lastChassisSpeeds); - // TODO: Test - // WARNING: REMOVE IF USING TAG FOLLOW!!! - // updateVisionOdometry(); + odometry.update(getGyroRotation2d(), getModulePositions()); + // SmartDashboard.putString("Odometry current pos", + // getOdometryPose().toString()); - if (DriverStation.isTeleopEnabled()) { - updateMegaTagOdometry(); - } else { - updateVisionOdometry(); - } + fieldRobot.setPose(getOdometryPose()); - odometry.update(getRotation2d(), getModulePositions()); - // if (DriverStation.getAlliance().isPresent()) { - // switch (DriverStation.getAlliance().get()) { - // case Red: - // field.setRobotPose(new Pose2d(new Translation2d(16.5 - getPose().getX(), - // getPose().getY()), - // getPose().getRotation())); - // break; - - // case Blue: - // field.setRobotPose(getPose()); - // break; - // } - // } else { - // // If no alliance provided, just go with blue - field.setRobotPose(getPose()); - // } + posePublisher.set(getOdometryPose()); SmartDashboard.putData("Field", field); + swerveStatesPublisher.set(getModuleStates()); + + // SmartDashboard.putBoolean("NavX Connected", navX.isConnected()); + SmartDashboard.putBoolean("Pigeon 2 Connected", pigeon.isConnected()); - SmartDashboard.putString("Robot Pose", - getPose().toString()); - // double swerveCurrent = 0; - // for (int chan : pdh_channels) - // swerveCurrent += pdh.getCurrent(chan); - // SmartDashboard.putNumber("SwerveSubsystem Amps", swerveCurrent); - // SmartDashboard.putNumber("PDH Amps", pdh.getTotalCurrent()); - - SmartDashboard.putNumberArray("SwerveStates", new double[] { - frontLeft.getModuleState().angle.getDegrees() + 90, -frontLeft.getModuleState().speedMetersPerSecond, - frontRight.getModuleState().angle.getDegrees() + 90, -frontRight.getModuleState().speedMetersPerSecond, - backLeft.getModuleState().angle.getDegrees() + 90, -backLeft.getModuleState().speedMetersPerSecond, - backRight.getModuleState().angle.getDegrees() + 90, -backRight.getModuleState().speedMetersPerSecond - }); - - chassisAccelX.append(navX.getRawAccelX()); - chassisAccelY.append(navX.getRawAccelY()); - chassisAccelZ.append(navX.getRawAccelZ()); - chassisRotX.append(navX.getRawGyroX()); - chassisRotY.append(navX.getRawGyroY()); - chassisRotZ.append(navX.getRawGyroZ()); + // SmartDashboard.putNumber("NavX Heading", -navX.getAngle()); + SmartDashboard.putNumber("Pigeon 2 Heading", pigeon.getYaw().getValueAsDouble()); + } + + public void setFeedforwards(DriveFeedforwards ffs) { + double[] accs = ffs.accelerationsMPSSq(); + frontLeft.setAcceleration(accs[0]); + frontRight.setAcceleration(accs[1]); + backLeft.setAcceleration(accs[2]); + backRight.setAcceleration(accs[3]); + } + + public void zeroFeedforwards() { + frontLeft.setAcceleration(0.0); + frontRight.setAcceleration(0.0); + backLeft.setAcceleration(0.0); + backRight.setAcceleration(0.0); } public void zeroHeading() { @@ -214,32 +248,50 @@ public void setHeading(double deg) { // navX.reset(); // navX.setAngleAdjustment(deg); - double error = deg - navX.getAngle(); - double new_adjustment = navX.getAngleAdjustment() + error; - navX.setAngleAdjustment(new_adjustment); + // double error = deg - navX.getAngle(); + // double new_adjustment = navX.getAngleAdjustment() + error; + // navX.setAngleAdjustment(new_adjustment); + + pigeonOffset = deg - pigeon.getYaw().getValueAsDouble(); + } + + public void setGyroToEstimate() { + resetOdometryAndGyro(odometry.getEstimatedPosition()); } - public Pose2d getPose() { + public Pose2d getOdometryPose() { Pose2d p = odometry.getEstimatedPosition(); return p; } public void resetOdometry(Pose2d pose) { - // TODO: TEST - setHeading(Units.radiansToDegrees(pose.getRotation().times(-1.0).getRadians() - + (FieldConstants.getAlliance() == Alliance.Red ? Math.PI : 0.0))); + // setHeading(Units.radiansToDegrees(pose.getRotation().times(-1.0).getRadians() + // + (FieldConstants.getAlliance() == Alliance.Red ? Math.PI : 0.0))); + // SmartDashboard.putNumber("HEading reset to", getGyroHeading()); + // SmartDashboard.putBoolean("HASBEENREET", true); + odometry.resetPosition(getGyroRotation2d(), getModulePositions(), pose); + } - SmartDashboard.putNumber("HEading reset to", getHeading()); - SmartDashboard.putBoolean("HASBEENREET", true); - odometry.resetPosition(getRotation2d(), getModulePositions(), pose); + public void resetOdometryRotation(Rotation2d rot) { + odometry.resetRotation(rot); } - public double getHeading() { - return Robot.isSimulation() ? -navxSim : Units.degreesToRadians(Math.IEEEremainder(-navX.getAngle(), 360)); + public void resetOdometryAndGyro(Pose2d pose) { + setHeading(Units.radiansToDegrees(pose.getRotation().times(1.0).getRadians() + + (FieldConstants.getAlliance() == Alliance.Red ? Math.PI : 0.0))); + resetOdometry(pose); + } + + public double getGyroHeading() { + // return Robot.isSimulation() ? navxSim : + // Units.degreesToRadians(Math.IEEEremainder(-navX.getAngle(), 360)); + return Robot.isSimulation() ? navxSim + : Units.degreesToRadians(Math.IEEEremainder(pigeon.getYaw().getValueAsDouble() + pigeonOffset, + 360)); } - public Rotation2d getRotation2d() { - return new Rotation2d(getHeading()); + public Rotation2d getGyroRotation2d() { + return new Rotation2d(getGyroHeading()); } public void stopDrive() { @@ -253,22 +305,26 @@ public void setModules(SwerveModuleState[] states) { lastChassisSpeeds = DriveConstants.KINEMATICS.toChassisSpeeds(states); // Normalize speeds so they are all obtainable SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.MAX_MODULE_VELOCITY); - frontLeft.setModuleState(states[Constants.DriveConstants.ModuleIndices.FRONT_LEFT]); - frontRight.setModuleState(states[Constants.DriveConstants.ModuleIndices.FRONT_RIGHT]); - backRight.setModuleState(states[Constants.DriveConstants.ModuleIndices.REAR_RIGHT]); - backLeft.setModuleState(states[Constants.DriveConstants.ModuleIndices.REAR_LEFT]); + frontLeft.setModuleState(states[0]); + frontRight.setModuleState(states[1]); + backLeft.setModuleState(states[2]); + backRight.setModuleState(states[3]); + swerveTargetStatesPublisher.set(states); } - public void setChassisSpeedsAUTO(ChassisSpeeds speeds) { - double tmp = speeds.vxMetersPerSecond; - speeds.vxMetersPerSecond = speeds.vyMetersPerSecond; - speeds.vyMetersPerSecond = tmp; - tmp = speeds.omegaRadiansPerSecond; - speeds.omegaRadiansPerSecond *= -1; + public void setChassisSpeeds(ChassisSpeeds speeds) { SwerveModuleState[] states = DriveConstants.KINEMATICS.toSwerveModuleStates(speeds); setModules(states); } + public void setChassisSpeedsAuto(ChassisSpeeds speeds) { + previousSetpoint = setpointGenerator.generateSetpoint( + previousSetpoint, + speeds, + 0.02); + setModules(previousSetpoint.moduleStates()); + } + public void setXstance() { frontLeft.setModuleStateRaw(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); @@ -301,6 +357,17 @@ public SwerveModulePosition[] getModulePositions() { return states; } + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = { + frontLeft.getModuleState(), + frontRight.getModuleState(), + backLeft.getModuleState(), + backRight.getModuleState() + }; + + return states; + } + @Override public void simulationPeriodic() { frontLeft.simulate_step(); @@ -310,122 +377,12 @@ public void simulationPeriodic() { navxSim += 0.02 * lastChassisSpeeds.omegaRadiansPerSecond; } - public RotationStyle getRotationStyle() { - return rotationStyle; - } - - public void setRotationStyle(RotationStyle style) { - rotationStyle = style; - } - // ---------- Path Planner Methods ---------- \\ public Command loadPath(String name) { return new PathPlannerAuto(name); } - public Command followPathCommand(String pathName) { - try { - PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - - return new FollowPathCommand( - path, - this::getPose, // Robot pose supplier - this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforward) -> setChassisSpeedsAUTO(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - PathPlannerConstants.HOLONOMIC_FOLLOWER_CONTROLLER, - PathPlannerConstants.ROBOT_CONFIG, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } catch (Exception exception) { - return Commands.none(); - } - } - - public PathPlannerPath generateOTFPath(PathPoint... pathPoints) { - // Create the path using the bezier points created above - PathPlannerPath path = PathPlannerPath.fromPathPoints( - List.of(pathPoints), - new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a - // differential drivetrain, the angular - // constraints have no effect. - new GoalEndState(0.0, Rotation2d.fromDegrees(-90)) // Goal end state. You can set a holonomic rotation - // here. If using a differential drivetrain, the - // rotation will have no effect. - ); - - // Prevent the path from being flipped if the coordinates are already correct - path.preventFlipping = true; - - return path; - } - - public void updateVisionOdometry() { - boolean doRejectUpdate = false; - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); - - if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { - if (mt1.rawFiducials[0].ambiguity > .7) { - doRejectUpdate = true; - } - // if (mt1.rawFiducials[0].distToCamera > 3) { - if (mt1.rawFiducials[0].distToCamera > 5) { // TODO: TUNE!!! - - doRejectUpdate = true; - } - } - if (mt1.tagCount == 0) { - doRejectUpdate = true; - } - - if (!doRejectUpdate) { - // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); - odometry.setVisionMeasurementStdDevs(createVisionMeasurementStdDevs( - PoseConstants.kVisionStdDevX, - PoseConstants.kVisionStdDevY, - PoseConstants.kVisionStdDevTheta)); - odometry.addVisionMeasurement( - mt1.pose, - mt1.timestampSeconds); - } - } - - public void updateMegaTagOdometry() { - boolean doRejectUpdate = false; - LimelightHelpers.SetRobotOrientation("limelight", odometry.getEstimatedPosition().getRotation().getDegrees(), 0, - 0, 0, 0, 0); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); - if (Math.abs(navX.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore - // vision updates - { - doRejectUpdate = true; - } - - if (mt2.tagCount <= 0) { - doRejectUpdate = true; - } - if (!doRejectUpdate) { - // odometry.setVisionMeasurementStdDevs(VecBuilder.fill(2,2,2.0*PoseConstants.kVisionStdDevTheta)); - odometry.setVisionMeasurementStdDevs(VecBuilder.fill(2, 2, 9999999)); - - odometry.addVisionMeasurement( - mt2.pose, - mt2.timestampSeconds); - } - } - public Vector createStateStdDevs(double x, double y, double theta) { return VecBuilder.fill(x, y, Units.degreesToRadians(theta)); } @@ -433,4 +390,11 @@ public Vector createStateStdDevs(double x, double y, double theta) { public Vector createVisionMeasurementStdDevs(double x, double y, double theta) { return VecBuilder.fill(x, y, Units.degreesToRadians(theta)); } -} + + public void setAutoStartingPose(Pose2d pose) { + // Trajectory autoVizTraj = new Trajectory( + // Arrays.asList(new Trajectory.State(0.0, 0.0, 0.0, pose, 0.0))); + FieldObject2d obj = field.getObject("autoStart"); + obj.setPose(AllianceFlipUtil.apply(pose)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java new file mode 100644 index 0000000..c3dbf0d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeArm.java @@ -0,0 +1,140 @@ +package frc.robot.subsystems.algae; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.epilogue.Logged; +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.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Algae; + +@Logged +public class AlgaeArm extends SubsystemBase { + private final SparkMax pivotMotor = new SparkMax(Algae.Pivot.MOTOR_PORT, MotorType.kBrushless); + private RelativeEncoder pivotEncoder; + // sim motor + private final SparkMaxSim simPivotMotor = new SparkMaxSim(pivotMotor, Algae.Pivot.PhysicalConstants.MOTOR); + + // physics simulation + private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim( + Algae.Pivot.PhysicalConstants.MOTOR, + Algae.Pivot.PhysicalConstants.NET_REDUCTION, + Algae.Pivot.PhysicalConstants.MOI, + Algae.Pivot.PhysicalConstants.ARM_LENGTH_METERS, + Algae.Pivot.RETRACTED_LIMIT, + Algae.Pivot.EXTENDED_LIMIT, + true, + Algae.Pivot.RETRACTED_LIMIT); + + private final SparkMaxConfig pivotConfig = new SparkMaxConfig(); + + private final ProfiledPIDController pivotPID = Algae.Pivot.PID; + // private final ArmFeedforward pivotFeedforward = Algae.Pivot.FEEDFORWARD; + + private boolean testModeConfigured = false; + + public AlgaeArm() { + pivotConfig.idleMode(IdleMode.kBrake).inverted(Constants.Algae.Pivot.MOTOR_INVERTED).apply( + new EncoderConfig() + .positionConversionFactor( + (2. * Math.PI) / Constants.Algae.Pivot.PhysicalConstants.NET_REDUCTION) + .velocityConversionFactor( + (2. * Math.PI) / (60.0 * Constants.Algae.Pivot.PhysicalConstants.NET_REDUCTION))); + pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotEncoder = pivotMotor.getEncoder(); + pivotEncoder.setPosition(0.0f); // TODO: Add zeroing! + + if (Constants.Algae.DEBUG_PIDS) { + SmartDashboard.putNumber("Algae/Pivot/PID/P", pivotPID.getP()); + SmartDashboard.putNumber("Algae/Pivot/PID/I", pivotPID.getI()); + SmartDashboard.putNumber("Algae/Pivot/PID/D", pivotPID.getD()); + } + } + + @Override + public void periodic() { + + if (DriverStation.isTest() && !testModeConfigured) { + pivotMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + testModeConfigured = true; + } else if (!DriverStation.isTest() && testModeConfigured) { + pivotMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + testModeConfigured = false; + } + + if (Constants.Algae.DEBUG_PIDS) { + pivotPID.setP(SmartDashboard.getNumber("Algae/Pivot/PID/P", pivotPID.getP())); + pivotPID.setI(SmartDashboard.getNumber("Algae/Pivot/PID/I", pivotPID.getI())); + pivotPID.setD(SmartDashboard.getNumber("Algae/Pivot/PID/D", pivotPID.getD())); + } + + double output = pivotPID.calculate(Units.degreesToRadians(getPositionDegrees())); + + SmartDashboard.putNumber("Algae/Pivot/out", output); + SmartDashboard.putNumber("Algae/Pivot/position", pivotEncoder.getPosition()); + SmartDashboard.putNumber("Algae/Pivot/target", pivotPID.getSetpoint().position); + SmartDashboard.putNumber("Algae/Pivot/velocity_target", pivotPID.getSetpoint().velocity); + SmartDashboard.putNumber("Algae/Pivot/velocity", pivotEncoder.getVelocity()); + SmartDashboard.putNumber("Algae/Pivot/goal", pivotPID.getGoal().position); + + pivotMotor.set(output); + if (Robot.isSimulation()) + simPivotMotor.setAppliedOutput(output); + } + + @Override + public void simulationPeriodic() { + // update physics + simPivotPhysics.setInput(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage()); + simPivotPhysics.update(0.02); + + // update sim objects + simPivotMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simPivotPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + } + + public void setGoalDegrees(double goal) { + pivotPID.setGoal(MathUtil.clamp( + Units.degreesToRadians(goal), + Algae.Pivot.RETRACTED_LIMIT, + Algae.Pivot.EXTENDED_LIMIT)); + } + + @Logged + public double getGoalDegrees() { + return Units.radiansToDegrees(pivotPID.getGoal().position); + } + + @Logged + public double getPositionDegrees() { + return Robot.isReal() + ? Units.radiansToDegrees(pivotEncoder.getPosition()) + : Units.radiansToDegrees(simPivotPhysics.getAngleRads()); + } +} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java new file mode 100644 index 0000000..4a6616b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIntake.java @@ -0,0 +1,132 @@ +package frc.robot.subsystems.algae; + +import java.util.function.BooleanSupplier; + +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Algae; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeIntakePresets; +import frc.robot.Constants; +import frc.robot.Robot; + +@Logged +public class AlgaeIntake extends SubsystemBase { + private final SparkMax intakeMotor = new SparkMax(Algae.Intake.MOTOR_PORT, MotorType.kBrushless); + private SparkBaseConfig intakeMotorConfig = new SparkMaxConfig(); + // sim motor + private final SparkMaxSim simIntakeMotor = new SparkMaxSim(intakeMotor, Algae.Intake.PhysicalConstants.MOTOR); + + // physics simulation + private final FlywheelSim simIntakePhysics = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + Algae.Intake.PhysicalConstants.MOTOR, + Algae.Intake.PhysicalConstants.MOI, + Algae.Intake.PhysicalConstants.GEARING), + Algae.Intake.PhysicalConstants.MOTOR); + + // private final DigitalInput intakeBeambreak = new + // DigitalInput(Algae.Intake.BEAMBREAK_PORT); + private final LaserCan intakeSensor = new LaserCan(Constants.Algae.Intake.LASERCAN_ID); + + private AlgaeIntakePresets currentPreset = AlgaeIntakePresets.STOP; + + private boolean holding_internal = false; + + public AlgaeIntake() { + intakeMotorConfig = intakeMotorConfig.inverted(Constants.Algae.Intake.MOTOR_INVERTED); + intakeMotor.configure(intakeMotorConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + @Override + public void periodic() { + intakeMotor.set(currentPreset.outputPercentage); + if (Robot.isSimulation()) + simIntakeMotor.setAppliedOutput(currentPreset.outputPercentage); + + if (currentPreset == AlgaeIntakePresets.HOLD || currentPreset == AlgaeIntakePresets.INTAKING) { + holding_internal = getSensorDistance() < Constants.Algae.Intake.HOLDING_THRESHOLD; + } else { + holding_internal = false; + } + SmartDashboard.putBoolean("Holding Algae", isHolding()); + + } + + @Override + public void simulationPeriodic() { + // update physics + simIntakePhysics.setInput(simIntakeMotor.getAppliedOutput() * RoboRioSim.getVInVoltage()); + simIntakePhysics.update(0.02); + + // update sim objects + simIntakeMotor.iterate( + simIntakePhysics.getAngularVelocityRPM(), + RoboRioSim.getVInVoltage(), + 0.02); + } + + public void setIntakePreset(AlgaeIntakePresets preset) { + this.currentPreset = preset; + this.intakeMotorConfig = intakeMotorConfig.smartCurrentLimit(currentPreset.currentLimitA); + this.intakeMotor.configure(intakeMotorConfig, ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + } + + public AlgaeIntakePresets getIntakePresets() { + return currentPreset; + } + + public boolean isHolding() { + return Robot.isSimulation() ? SmartDashboard.getBoolean("[SIM] Holding Algae", false) : holding_internal; + } + + /* + * if (Robot.isSimulation()) { + * ; + * } + */ + + public BooleanSupplier getHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return isHolding(); + } + }; + } + + public BooleanSupplier getNotHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return !isHolding(); + } + }; + } + + public double getSensorDistance() { + Measurement meas = intakeSensor.getMeasurement(); + if (meas == null) { + return 0.0; + } else { + return meas.distance_mm / 1000.0; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java new file mode 100644 index 0000000..eec4e33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -0,0 +1,78 @@ +package frc.robot.subsystems.algae; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +@Logged +public class AlgaeSubsystem extends SubsystemBase { + + public enum AlgaePresets { + STOW(0.0), + REMOVE(90.0), + INTAKE(80.0), + INTAKE_FLOOR(95), + HOLD(80.0), + HOLD_BUMPER(55.0), + OUT_OF_THE_WAY(30.0), + BARGE(130.0); + + public double armAngle; + + private AlgaePresets(double armAngle) { + this.armAngle = armAngle; + } + } + + public enum AlgaeIntakePresets { + INTAKING(1.0, 20), + REMOVAL(-1.0, 20), + PURGE(-1, 20), + SHOOT_BARGE(-1, 20), + SHOOT(-0.5, 20), + HOLD(0.8, 14), // Used to be 0.5 percent + STOP(0, 20); + + public double outputPercentage; + public int currentLimitA; + + AlgaeIntakePresets(double outputPercentage, int current) { + this.outputPercentage = outputPercentage; + this.currentLimitA = current; + } + } + + private final AlgaeArm arm = new AlgaeArm(); + private final AlgaeIntake intake = new AlgaeIntake(); + + public AlgaeArm getArm() { + return arm; + } + + public AlgaeIntake getIntake() { + return intake; + } + + private AlgaePresets currentPreset = AlgaePresets.STOW; + private AlgaeIntakePresets currentIntakePreset = AlgaeIntakePresets.STOP; + + public void setAlgaePreset(AlgaePresets preset) { + SmartDashboard.putString("Algae Preset", preset.toString()); + if (preset != currentPreset) { + arm.setGoalDegrees(preset.armAngle); + currentPreset = preset; + } + } + + public void setAlgaeIntakePreset(AlgaeIntakePresets preset) { + SmartDashboard.putString("Algae Intake Preset", preset.toString()); + if (preset != currentIntakePreset) { + intake.setIntakePreset(preset); + currentIntakePreset = preset; + } + } + + public boolean isHolding() { + return intake.isHolding(); + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralArm.java b/src/main/java/frc/robot/subsystems/coral/CoralArm.java new file mode 100644 index 0000000..ac96b17 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralArm.java @@ -0,0 +1,601 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.sim.SparkAnalogSensorSim; +import com.revrobotics.sim.SparkFlexSim; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.sim.SparkRelativeEncoderSim; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.AnalogSensorConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SignalsConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +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.filter.LinearFilter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Coral; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +@Logged +public class CoralArm extends SubsystemBase { + private final SparkFlex pivotMotor = new SparkFlex(Coral.Pivot.MOTOR_PORT, MotorType.kBrushless); + private final SparkMax rollMotor = new SparkMax(Coral.Roll.MOTOR_PORT, MotorType.kBrushless); + private final SparkMax pitchMotor = new SparkMax(Coral.Pitch.MOTOR_PORT, MotorType.kBrushless); + // sim motors + private final SparkFlexSim simPivotMotor = new SparkFlexSim(pivotMotor, Coral.Pivot.PhysicalConstants.MOTOR); + private final SparkMaxSim simRollMotor = new SparkMaxSim(rollMotor, Coral.Roll.PhysicalConstants.MOTOR); + private final SparkMaxSim simPitchMotor = new SparkMaxSim(pitchMotor, Coral.Pitch.PhysicalConstants.MOTOR); + + private final CANcoder pivotEncoder = new CANcoder(Coral.Pivot.ENCODER_PORT); + private final SparkAnalogSensor rollEncoder = rollMotor.getAnalog(); + private final SparkAnalogSensor pitchEncoder = pitchMotor.getAnalog(); + private RelativeEncoder rollRelEncoder; + // sim encoders + private final SparkAnalogSensorSim simRollEncoder = new SparkAnalogSensorSim(rollMotor); + private final SparkRelativeEncoderSim simRollRelEncoder = new SparkRelativeEncoderSim(rollMotor); + private final SparkAnalogSensorSim simPitchEncoder = new SparkAnalogSensorSim(pitchMotor); + private final SparkRelativeEncoderSim simPivotEncoder = new SparkRelativeEncoderSim(pivotMotor); + + private boolean testModeConfigured = false; + + // physics simulations + private final SingleJointedArmSim simPivotPhysics = new SingleJointedArmSim( + Coral.Pivot.PhysicalConstants.MOTOR, + Coral.Pivot.PhysicalConstants.NET_REDUCTION, + Coral.Pivot.PhysicalConstants.MOI, + Coral.Pivot.PhysicalConstants.ARM_LENGTH_METERS, + 0.5 * Math.PI - Coral.Pivot.MAXIMUM_ANGLE, + 0.5 * Math.PI + Coral.Pivot.MAXIMUM_ANGLE, true, 0.5 * Math.PI); + private final SingleJointedArmSim simRollPhysics = new SingleJointedArmSim( + Coral.Roll.PhysicalConstants.MOTOR, + Coral.Roll.PhysicalConstants.NET_REDUCTION, + Coral.Roll.PhysicalConstants.MOI, + Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS, + Coral.Roll.MAXIMUM_ANGLE * -1, + Coral.Roll.MAXIMUM_ANGLE, + false, + 0); + private final SingleJointedArmSim simPitchPhysics = new SingleJointedArmSim( + Coral.Pitch.PhysicalConstants.MOTOR, + Coral.Pitch.PhysicalConstants.NET_REDUCTION, + Coral.Pitch.PhysicalConstants.MOI, + Coral.Pitch.PhysicalConstants.ARM_LENGTH_METERS, + Coral.Pitch.MAXIMUM_ANGLE * -1, + Coral.Pitch.MAXIMUM_ANGLE, + false, + 0); + + private final SparkFlexConfig pivotConfig = new SparkFlexConfig(); + private final SparkMaxConfig rollConfig = new SparkMaxConfig(); + private final SparkMaxConfig pitchConfig = new SparkMaxConfig(); + + private final ProfiledPIDController pivotPID = Coral.Pivot.PID; + + public ProfiledPIDController getPivotPID() { + return pivotPID; + } + + public ProfiledPIDController getRollPID() { + return rollPID; + } + + public ProfiledPIDController getPitchPID() { + return pitchPID; + } + + private final ArmFeedforward pivotFeedforward = Coral.Pivot.FEEDFORWARD; + private final ProfiledPIDController rollPID = Coral.Roll.PID; + private final ProfiledPIDController pitchPID = Coral.Pitch.PID; + + private double pivotGoal = 0.0; // Rads + private double rollGoal = 0.0; // Rads + private double pitchGoal = 0.0; // Rads + + // LinearFilter pitchEncFilter = LinearFilter.movingAverage(3); + // LinearFilter rollEncFilter = LinearFilter.movingAverage(3); + LinearFilter pitchEncFilter = LinearFilter.singlePoleIIR((0.02 / 2.0) * 4.0, (1.0 / 200.0)); + LinearFilter rollEncFilter = LinearFilter.singlePoleIIR(0.02 * 4.0, 0.02); + + @NotLogged + AlgaeSubsystem algaeSub; + + public CoralArm(AlgaeSubsystem algae) { + this.algaeSub = algae; + AnalogSensorConfig wristEncConfig = new AnalogSensorConfig(); + + pivotMotor.configure( + pivotConfig.idleMode(IdleMode.kBrake).apply( + new EncoderConfig() + .positionConversionFactor( + (2.0 * Math.PI) + / Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION) + .velocityConversionFactor( + (2.0 * Math.PI) + / (Constants.Coral.Pivot.PhysicalConstants.NET_REDUCTION + * 60.0f))), // Encoder + // -> + // Rotations & + // Seconds + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rollMotor.configure( + rollConfig.idleMode(IdleMode.kBrake) + .apply(wristEncConfig.inverted(Constants.Coral.Roll.ENCODER_INVERTED)) + .apply(new EncoderConfig().positionConversionFactor((2.0 * Math.PI) + / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION) + .velocityConversionFactor((2.0 * Math.PI) + / (60.0 * Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION))) + .apply(new SparkMaxConfig().inverted( + Constants.Coral.Pitch.MOTOR_INVERTED)), + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + rollRelEncoder = rollMotor.getEncoder(); + + pitchMotor.configure(pitchConfig + .idleMode(IdleMode.kBrake).apply(wristEncConfig + .inverted(Constants.Coral.Pitch.ENCODER_INVERTED)) + .apply(new EncoderConfig().positionConversionFactor((2.0 * Math.PI) + / Constants.Coral.Pitch.PhysicalConstants.NET_REDUCTION) + .velocityConversionFactor((2.0 * Math.PI) + / (60.0 * Constants.Coral.Pitch.PhysicalConstants.NET_REDUCTION))) + .apply(new SignalsConfig().analogPositionPeriodMs(1000 / 200)) + .apply(new SparkMaxConfig().inverted(Constants.Coral.Pitch.MOTOR_INVERTED)), + ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + pivotMotor.getEncoder().setPosition(readPivotEncoderPosition()); + + /* + * .apply(new EncoderConfig() + * .positionConversionFactor( + * (1.0 * 2.0 * Math.PI) + * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION) + * .velocityConversionFactor((60.0 * 2.0 * Math.PI) + * / Constants.Coral.Roll.PhysicalConstants.NET_REDUCTION)), + */ + // Not sure how to get + + // Disabling this for now because we don't want the wrist wire bundle to explode + // rollPID.enableContinuousInput(-180, 180); + + // TODO: Are these reasonable? who knows. + pivotPID.setTolerance(Units.degreesToRadians(3.0)); + rollPID.setTolerance(Units.degreesToRadians(3.0)); + pitchPID.setTolerance(Units.degreesToRadians(3.0)); + + if (Constants.Coral.DEBUG_PIDS) { + SmartDashboard.putNumber("Coral/Pivot/PID/P", pivotPID.getP()); + SmartDashboard.putNumber("Coral/Pivot/PID/I", pivotPID.getI()); + SmartDashboard.putNumber("Coral/Pivot/PID/D", pivotPID.getD()); + + SmartDashboard.putNumber("Coral/Roll/PID/P", rollPID.getP()); + SmartDashboard.putNumber("Coral/Roll/PID/I", rollPID.getI()); + SmartDashboard.putNumber("Coral/Roll/PID/D", rollPID.getD()); + + SmartDashboard.putNumber("Coral/Pitch/PID/P", pitchPID.getP()); + SmartDashboard.putNumber("Coral/Pitch/PID/I", pitchPID.getI()); + SmartDashboard.putNumber("Coral/Pitch/PID/D", pitchPID.getD()); + } + + // Warm up the filters + for (int i = 0; i < 10; ++i) { + readPitchEncoderPositionUpdate(); + readRollEncoderPositionUpdate(); + } + + // Prep for 200Hz running + pitchMotor.setControlFramePeriodMs(1000 / 200); + } + + double lastPitchReading = 0.0; + double lastRollReading = 0.0; + boolean rollreset = false; + double lastPivotReading = 0.0; + + @Override + public void periodic() { + double rollPosition = readRollEncoderPositionUpdate();// readRollEncoderPosition(); + + if (!rollreset) { + rollRelEncoder.setPosition(rollPosition); + rollreset = true; + } + + // Put motors in coast mode for testing!!! + if (DriverStation.isTest() && !testModeConfigured) { + pitchMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + pivotMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + rollMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + testModeConfigured = true; + } else if (!DriverStation.isTest() && testModeConfigured) { + pitchMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + pivotMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + rollMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters); + testModeConfigured = false; + } + + // if entering the frame border + // && wrist is at neutral + // if ((pivotPID.getGoal().position != pivotGoal) + // && (rollPID.atGoal() && pitchPID.atGoal())) { + // // allow arm to enter frame border + // pivotPID.setGoal(pivotGoal); + // } else if ( // if exiting the frame border && has exited the frame border + // ((rollGoal != rollPID.getGoal().position) || (pitchGoal != + // pitchPID.getGoal().position)) + // && (Math.abs(this.getPitchPositionDegrees()) > + // Coral.Pivot.ELEVATOR_BORDER_ANGLE)) { + // // set roll and pitch back to their goals + // rollPID.setGoal(rollGoal); + // pitchPID.setGoal(pitchGoal); + // } + + double dRollDt = (rollPosition - lastRollReading) / 0.02; + lastRollReading = rollPosition; + + if (Constants.Coral.DEBUG_PIDS) { + pivotPID.setP(SmartDashboard.getNumber("Coral/Pivot/PID/P", pivotPID.getP())); + pivotPID.setI(SmartDashboard.getNumber("Coral/Pivot/PID/I", pivotPID.getI())); + pivotPID.setD(SmartDashboard.getNumber("Coral/Pivot/PID/D", pivotPID.getD())); + + rollPID.setP(SmartDashboard.getNumber("Coral/Roll/PID/P", rollPID.getP())); + rollPID.setI(SmartDashboard.getNumber("Coral/Roll/PID/I", rollPID.getI())); + rollPID.setD(SmartDashboard.getNumber("Coral/Roll/PID/D", rollPID.getD())); + } + /* + * run the motors + */ + + // double pivotPosition = Robot.isSimulation() ? readPivotEncoderPosition() + // : pivotMotor.getEncoder().getPosition();// readPivotEncoderPosition(); + double pivotPosition = readPivotEncoderPosition(); + lastPivotReading = pivotPosition; + + double pivotFFout = pivotFeedforward.calculate( + Math.PI * 0.5 + pivotPosition, + pivotPID.getSetpoint().velocity); + double pivotPIDout = pivotPID.calculate(pivotPosition); + SmartDashboard.putNumber("Coral/Pivot/pid_out", pivotPIDout); + SmartDashboard.putNumber("Coral/Pivot/ff_out", pivotFFout); + SmartDashboard.putNumber("Coral/Pivot/out", pivotFFout + pivotPIDout); + SmartDashboard.putNumber("Coral/Pivot/position", pivotPosition); + SmartDashboard.putNumber("Coral/Pivot/target", pivotPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Pivot/velocity_target", pivotPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Pivot/velocity", readPivotEncoderVelocity()); + SmartDashboard.putNumber("Coral/Pivot/goal", pivotPID.getGoal().position); + if (!Constants.Coral.Pivot.DBG_DISABLED) + pivotMotor.setVoltage( + (Math.abs(pivotGoal) < Units.degreesToRadians(3)) ? MathUtil.applyDeadband( + pivotPIDout + pivotFFout, 0.6) : pivotPIDout + pivotFFout); + if (Robot.isSimulation()) + simPivotMotor.setAppliedOutput((pivotPIDout + pivotFFout) / 12.0); + + // TODO: Swap to roll motor relative encoder? + // NEW: Use motor encoder + // double rollPIDout = rollPID.calculate(rollRelEncoder.getPosition()); + // OLD: + double rollPIDout = rollPID.calculate(rollPosition); + + double rollFFout = 0.0;// Constants.Coral.Roll.FEEDFORWARD.calculate(rollPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Roll/position", rollPosition); + SmartDashboard.putNumber("Coral/Roll/motor_position", rollRelEncoder.getPosition()); + SmartDashboard.putNumber("Coral/Roll/velocity", dRollDt); + SmartDashboard.putNumber("Coral/Roll/target", rollPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Roll/velocity_target", rollPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Roll/goal", rollPID.getGoal().position); + SmartDashboard.putNumber("Coral/Roll/pid_out", rollPIDout); + SmartDashboard.putNumber("Coral/Roll/ff_out", rollPIDout); + SmartDashboard.putNumber("Coral/Roll/out", rollPIDout + rollFFout); + if (!Constants.Coral.Roll.DBG_DISABLED) + rollMotor.setVoltage(rollPIDout + rollFFout); + if (Robot.isSimulation()) + simRollMotor.setAppliedOutput(rollPIDout / 12.0); + + } + + double lastPitchVelTgt = 0.0; + + public void periodic200Hz() { + if (Constants.Coral.Pitch.DEBUG_PIDS) { + pitchPID.setP(SmartDashboard.getNumber("Coral/Pitch/PID/P", pitchPID.getP())); + pitchPID.setI(SmartDashboard.getNumber("Coral/Pitch/PID/I", pitchPID.getI())); + pitchPID.setD(SmartDashboard.getNumber("Coral/Pitch/PID/D", pitchPID.getD())); + } + + double pitchPosition = readPitchEncoderPositionUpdate(); + double dPitchDt = (pitchPosition - lastPitchReading) / (1.0 / 200.0); + lastPitchReading = pitchPosition; + + double dVelDt = (pitchPID.getSetpoint().velocity - lastPitchVelTgt) / (1.0 / 200.0); + lastPitchVelTgt = pitchPID.getSetpoint().velocity; + + // Pitch in here + double pitchPIDout = pitchPID.calculate(pitchPosition); + + // Feedforwardsing + // 0 is flat to the ground, pi/2 is straight up. + double pitchGlobalPosition = ((pitchPosition + Math.PI / 2.0) - Math.abs(lastPivotReading)); + double pitchFFout = Constants.Coral.Pitch.FEEDFORWARD + .calculate(pitchGlobalPosition, pitchPID.getSetpoint().velocity, dVelDt); + // double pitchFFout = 0.0; + + SmartDashboard.putNumber("Coral/Pitch/position", pitchPosition); + SmartDashboard.putNumber("Coral/Pitch/velocity", dPitchDt); + SmartDashboard.putNumber("Coral/Pitch/target", pitchPID.getSetpoint().position); + SmartDashboard.putNumber("Coral/Pitch/velocity_target", pitchPID.getSetpoint().velocity); + SmartDashboard.putNumber("Coral/Pitch/goal", pitchPID.getGoal().position); + SmartDashboard.putNumber("Coral/Pitch/out", pitchPIDout + pitchFFout); + SmartDashboard.putNumber("Coral/Pitch/pid_out", pitchPIDout); + SmartDashboard.putNumber("Coral/Pitch/ff_out", pitchFFout); + if (!Constants.Coral.Pitch.DBG_DISABLED) + if (algaeSub.isHolding()) { + pitchMotor.setVoltage(pitchPIDout * 0.25 + pitchFFout * 0.75); + } else { + pitchMotor.setVoltage(pitchPIDout + pitchFFout); + } + + if (Robot.isSimulation()) + simPitchMotor.setAppliedOutput(pitchPIDout / 12.0); + } + + @Override + public void simulationPeriodic() { + // update physics + simPivotPhysics + .setInput(MathUtil.clamp(simPivotMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + simRollPhysics + .setInput(MathUtil.clamp(simRollMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + simPitchPhysics + .setInput(MathUtil.clamp(simPitchMotor.getAppliedOutput() * RoboRioSim.getVInVoltage(), + -12.0, 12.0)); + + SmartDashboard.putNumber("Coral/Roll/sim_mechanism_position", simRollPhysics.getAngleRads()); + SmartDashboard.putNumber("Coral/Pitch/sim_mechanism_position", simPitchPhysics.getAngleRads()); + SmartDashboard.putNumber("Coral/Pivot/sim_mechanism_position", simPivotPhysics.getAngleRads()); + + simPivotPhysics.update(0.02); + simRollPhysics.update(0.02); + simPitchPhysics.update(0.02); + + // update sim objects + simPivotMotor.iterate( + simPivotPhysics.getVelocityRadPerSec(), + RoboRioSim.getVInVoltage(), + 0.02); + simRollMotor.iterate( + simRollPhysics.getVelocityRadPerSec(), + RoboRioSim.getVInVoltage(), + 0.02); + simPitchMotor.iterate( + Units.radiansPerSecondToRotationsPerMinute(simPitchPhysics.getVelocityRadPerSec()), + RoboRioSim.getVInVoltage(), + 0.02); + + // Roll encoder simulation + simRollEncoder.setPosition(Units.radiansToRotations(simRollPhysics.getAngleRads()) * 3.3 + - Constants.Coral.Roll.ENCODER_OFFSET_VOLTS); + simRollEncoder.iterate( + Units.radiansToRotations(simRollPhysics.getVelocityRadPerSec()) * 3.3, + 0.02); + simRollRelEncoder.setPosition(simRollPhysics.getAngleRads()); + simRollRelEncoder.iterate(simRollPhysics.getVelocityRadPerSec(), 0.02); + + // Pitch encoder simulation + simPitchEncoder.setPosition(Units.radiansToRotations(simPitchPhysics.getAngleRads()) * 3.3 + - Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS); + simPitchEncoder.iterate( + Units.radiansToRotations(simPitchPhysics.getVelocityRadPerSec()) * 3.3, + 0.02); + + // Pivot encoder simulation + simPivotEncoder.setPosition(simPivotPhysics.getAngleRads() - Math.PI * 0.5); + simPivotEncoder.iterate(simPivotPhysics.getVelocityRadPerSec(), 0.02); + } + + // this should be relative to straight upwards. + // i.e. 0 should be straight vertical, + // 20 would be to the right, -20 to the left + public void setPivotGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + pivotGoal = MathUtil.clamp(goalRads, + -Coral.Pivot.MAXIMUM_ANGLE, Coral.Pivot.MAXIMUM_ANGLE); + // if passing through the frame border + /* + * if (((this.getPivotPositionDegrees() < 0) == (pivotGoal < 0)) + * && (Math.abs(this.getPivotPositionDegrees()) < + * Coral.Pivot.FRAME_BORDER_ANGLE)) { + * // set the pivot to stop at the frame border to allow the wrist to move + * // neutral + * pivotPID.setGoal( + * Coral.Pivot.FRAME_BORDER_ANGLE + * (pivotGoal < 0 ? -1 : 1)); + * rollPID.setGoal(0); + * pitchPID.setGoal(0); + * } else { + * pivotPID.setGoal(pivotGoal); + * } + */ + + pivotPID.setGoal(pivotGoal); + } + + // similarly, this ranges from -180 to 180 + public void setRollGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + rollGoal = MathUtil.clamp(goalRads, Coral.Roll.MAXIMUM_ANGLE * -1, Coral.Roll.MAXIMUM_ANGLE); + /* + * // if outside of frame border + * if (Math.abs(this.getPivotPositionDegrees()) > + * Coral.Pivot.FRAME_BORDER_ANGLE) { + * // start moving to goal + * rollPID.setGoal(rollGoal); + * } + */ + rollPID.setGoal(rollGoal); + } + + public void setPitchGoalDegrees(double goal) { + double goalRads = Units.degreesToRadians(goal); + pitchGoal = MathUtil.clamp(goalRads, Coral.Pitch.MINIMUM_ANGLE, Coral.Pitch.MAXIMUM_ANGLE); + /* + * // if outside the frame border + * if (Math.abs(this.getPivotPositionDegrees()) > + * Coral.Pivot.FRAME_BORDER_ANGLE) { + * // start moving to goal + * pitchPID.setGoal(pitchGoal); + * } + */ + pitchPID.setGoal(pitchGoal); + } + + public double getPivotGoalDegrees() { + return Units.radiansToDegrees(pivotGoal); + } + + public double getRollGoalDegrees() { + return Units.radiansToDegrees(rollGoal); + } + + public double getPitchGoalDegrees() { + return Units.radiansToDegrees(pitchGoal); + } + + public double getPivotPositionDegrees() { + return Units.radiansToDegrees(readPivotEncoderPosition()); + } + + public double getRollPositionDegrees() { + return Units.radiansToDegrees(readRollEncoderPosition()); + } + + public double getPitchPositionDegrees() { + return Units.radiansToDegrees(readPitchEncoderPosition()); + } + + public boolean isPitchInPosition() { + return pitchPID.atGoal(); + } + + public boolean isRollInPosition() { + return rollPID.atGoal(); + } + + public boolean isPivotInPosition() { + return pivotPID.atGoal(); + } + + public double readPitchEncoderPositionUpdate() { + return MathUtil + .angleModulus(((pitchEncFilter + .calculate(pitchEncoder.getPosition()) + + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS) + / 3.3) + * 2 * Math.PI); + } + + public double readPitchEncoderPosition() { + return MathUtil + .angleModulus(((pitchEncFilter.lastValue() + + Constants.Coral.Pitch.ENCODER_OFFSET_VOLTS) + / 3.3) + * 2 * Math.PI); + } + + // public double readPitchEncoderVelocity() { + // return MathUtil + // .angleModulus(((pitchEncFilter + // .calculate(Robot.isSimulation() ? simPitchEncoder.getVelocity() + // : pitchEncoder.getVelocity())) + // / 3.3) + // * 2 * Math.PI); + // } + + public double readRollEncoderPosition() { + return MathUtil + .angleModulus((rollEncFilter.lastValue() + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) + * Constants.Coral.Roll.ENCODER_VOLTS_TO_RAD); + } + + public double readRollEncoderPositionUpdate() { + return MathUtil + .angleModulus((rollEncFilter + .calculate(rollEncoder.getPosition()) + + Constants.Coral.Roll.ENCODER_OFFSET_VOLTS) * Constants.Coral.Roll.ENCODER_VOLTS_TO_RAD); + } + + // public double readRollEncoderVelocity() { + // return MathUtil + // .angleModulus(((rollEncFilter + // .calculate(Robot.isSimulation() ? simRollEncoder.getVelocity() + // : rollEncoder.getVelocity())) + // / 3.3) + // * 2 * Math.PI); + // } + + public double readPivotEncoderPosition() { + return Robot.isSimulation() ? (simPivotPhysics.getAngleRads() - Math.PI * 0.5) + : Units.rotationsToRadians((pivotEncoder.getPosition().getValueAsDouble() + * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))); + } + + public double readPivotEncoderVelocity() { + return Robot.isSimulation() ? (simPivotPhysics.getVelocityRadPerSec()) + : Units.rotationsToRadians((pivotEncoder.getVelocity().getValueAsDouble() + * (Constants.Coral.Pivot.ENCODER_INVERTED ? -1.0 : 1.0))); + } + + public boolean isPitchSupposedToBeInPosition() { + // return MathUtil.isNear(this.pitchPID.getSetpoint().position, + // this.pitchPID.getGoal().position, 0.01); + return MathUtil.isNear(this.pitchPID.getSetpoint().position, pitchGoal, 0.01); + } + + public boolean isPivotSupposedToBeInPosition() { + // return MathUtil.isNear(this.pivotPID.getSetpoint().position, + // this.pivotPID.getGoal().position, 0.01); + return MathUtil.isNear(this.pivotPID.getSetpoint().position, pivotGoal, 0.01); + } + + public boolean isRollSupposedToBeInPosition() { + // return MathUtil.isNear(this.rollPID.getSetpoint().position, + // this.rollPID.getGoal().position, 0.01); + return MathUtil.isNear(this.rollPID.getSetpoint().position, rollGoal, 0.01); + } + + public void reset() { + rollreset = false; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralElevator.java b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java new file mode 100644 index 0000000..412fa2c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralElevator.java @@ -0,0 +1,186 @@ +package frc.robot.subsystems.coral; + +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +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 com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Elevator; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; + +@Logged(strategy = Strategy.OPT_IN) +public class CoralElevator extends SubsystemBase { + private final TalonFX leader; + private final TalonFX follower; + + TalonFXConfiguration elevatorConfig = new TalonFXConfiguration(); + MotionMagicVoltage mmReq = new MotionMagicVoltage(0.0); + + @Logged + double positionGoal = 0.0; + @Logged + double currentPosition = 0.0; + + boolean zeroing = false; + + private final ElevatorSim simElevator = new ElevatorSim( + Elevator.PhysicalParameters.MOTOR, + Elevator.PhysicalParameters.GEARING, + Elevator.PhysicalParameters.CARRIAGE_MASS_KG, + Elevator.PhysicalParameters.DRIVE_RADIUS_METERS, + 0, + Elevator.PhysicalParameters.MAX_TRAVEL, + true, + 0); + + public CoralElevator() { + leader = new TalonFX(Constants.Elevator.Leader.MOTOR_PORT); + follower = new TalonFX(Constants.Elevator.Follower.MOTOR_PORT); + + FeedbackConfigs fbc = elevatorConfig.Feedback; + fbc.SensorToMechanismRatio = Constants.Elevator.MOTOR_REVOLUTIONS_PER_METER; + + MotionMagicConfigs mmc = elevatorConfig.MotionMagic; + mmc.MotionMagicAcceleration = Constants.Elevator.PID.MAX_ACCELERATION; + mmc.MotionMagicCruiseVelocity = Constants.Elevator.PID.MAX_VELOCITY; + mmc.MotionMagicJerk = Constants.Elevator.PID.MAX_JERK; + + Slot0Configs s0c = elevatorConfig.Slot0; + s0c.GravityType = GravityTypeValue.Elevator_Static; + s0c.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + s0c.kA = Constants.Elevator.FeedforwardConstants.Ka; + s0c.kV = Constants.Elevator.FeedforwardConstants.Kv; + s0c.kS = Constants.Elevator.FeedforwardConstants.Ks; + s0c.kG = Constants.Elevator.FeedforwardConstants.Kg; + + s0c.kP = Constants.Elevator.PID.kP; + s0c.kI = Constants.Elevator.PID.kI; + s0c.kD = Constants.Elevator.PID.kD; + + leader.getConfigurator().apply(elevatorConfig); + leader.getConfigurator() + .apply(new MotorOutputConfigs() + .withInverted(Constants.Elevator.Leader.INVERTED + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + follower.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)); + follower.setControl(new Follower(Constants.Elevator.Leader.MOTOR_PORT, true)); + + leader.setPosition(0.0); + follower.setPosition(0.0); + + // Soft limits + leader.getConfigurator().apply( + new SoftwareLimitSwitchConfigs().withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(0.03) + .withForwardSoftLimitThreshold( + Constants.Elevator.PhysicalParameters.MAX_TRAVEL + - Units.inchesToMeters(1.0)) + .withForwardSoftLimitEnable(true)); + // follower.getConfigurator().apply( + // new SoftwareLimitSwitchConfigs().withReverseSoftLimitEnable(false) + // .withReverseSoftLimitThreshold(0.03) + // .withForwardSoftLimitThreshold( + // Constants.Elevator.PhysicalParameters.MAX_TRAVEL + // - Units.inchesToMeters(1.0)) + // .withForwardSoftLimitEnable(false)); + } + + @Override + public void periodic() { + if (zeroing) { + leader.setControl(new DutyCycleOut(-0.05)); + } else { + leader.setControl(mmReq.withPosition(positionGoal).withSlot(0)); + } + + currentPosition = leader.getPosition().getValueAsDouble(); + SmartDashboard.putNumber("Elevator/goal", positionGoal); + SmartDashboard.putNumber("Elevator/position", currentPosition); + } + + @Override + public void simulationPeriodic() { + var leaderSimState = leader.getSimState(); + leaderSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + simElevator.setInputVoltage(leaderSimState.getMotorVoltage()); + simElevator.update(0.02); + leaderSimState + .setRawRotorPosition(simElevator.getPositionMeters() + * Constants.Elevator.MOTOR_REVOLUTIONS_PER_METER); + leaderSimState.setRotorVelocity( + simElevator.getVelocityMetersPerSecond() + * Constants.Elevator.MOTOR_REVOLUTIONS_PER_METER); + } + + public void setGoalPosition(double targetMeters) { + positionGoal = targetMeters; + } + + public double getPosition() { + return leader.getPosition().getValueAsDouble(); + } + + public double getGoalPosition() { + return positionGoal; + } + + public boolean isInPosition() { + return MathUtil.isNear(getPosition(), getGoalPosition(), Units.inchesToMeters(0.75)); + } + + public void startZeroElevator() { + if ((getPosition() < 0.25)) { + zeroing = true; + leader.getConfigurator().apply( + new SoftwareLimitSwitchConfigs().withReverseSoftLimitEnable(false) + .withReverseSoftLimitThreshold(0.03) + .withForwardSoftLimitThreshold( + Constants.Elevator.PhysicalParameters.MAX_TRAVEL + - Units.inchesToMeters(1.0)) + .withForwardSoftLimitEnable(false)); + } else { + zeroing = false; + } + } + + public void endZeroElevator() { + if (zeroing) { + leader.getConfigurator().apply( + new SoftwareLimitSwitchConfigs().withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(0.03) + .withForwardSoftLimitThreshold( + Constants.Elevator.PhysicalParameters.MAX_TRAVEL + - Units.inchesToMeters(1.0)) + .withForwardSoftLimitEnable(true)); + leader.setPosition(0.0); + follower.setPosition(0.0); + zeroing = false; + } + } + + // TODO: Add zeroing!!! +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralIntake.java b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java new file mode 100644 index 0000000..162518f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralIntake.java @@ -0,0 +1,128 @@ +package frc.robot.subsystems.coral; + +import java.util.function.BooleanSupplier; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Coral; + +@Logged +public class CoralIntake extends SubsystemBase { + private final TalonFX intakeMotor = new TalonFX(Coral.Intake.MOTOR_PORT); + // sim motor + private TalonFXSimState simIntakeMotor; + + // physics simulation + private final FlywheelSim simIntakePhysics = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + Coral.Intake.PhysicalConstants.MOTOR, + Coral.Intake.PhysicalConstants.MOI, + Coral.Intake.PhysicalConstants.GEARING), + Coral.Intake.PhysicalConstants.MOTOR); + + private final SlewRateLimiter intakeProfile = new SlewRateLimiter( + Coral.Intake.POSITIVE_RATE_LIMIT, + -Coral.Intake.NEGATIVE_RATE_LIMIT, + 0); + + public CoralIntake() { + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + intakeMotor.getConfigurator() + .apply(new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Coral.Intake.CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true)); + intakeMotor.getConfigurator() + .apply(new MotorOutputConfigs() + .withInverted(Constants.Coral.Intake.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive)); + intakeMotor.getConfigurator() + .apply(new HardwareLimitSwitchConfigs().withForwardLimitEnable(false).withReverseLimitEnable(false)); + } + + private double outputPercentage = 0.0; + // private boolean lastHolding = false; + + @Override + public void periodic() { + boolean curHolding = isHolding(); + // if no coral + if (!curHolding) { + double output = outputPercentage;// intakeProfile.calculate(outputPercentage); + if (!Constants.Coral.Intake.DBG_DISABLED) + intakeMotor.set(output); + } else { + // hold, negative is out so intake a bit. + // Set to 0.1 to be good + if (!Constants.Coral.Intake.DBG_DISABLED) + intakeMotor.set(Math.min(0.1, outputPercentage)); + } + + SmartDashboard.putBoolean("Holding Coral", curHolding); + } + + @Override + public void simulationPeriodic() { + simIntakeMotor = intakeMotor.getSimState(); + simIntakeMotor.setSupplyVoltage(RoboRioSim.getVInVoltage()); + + // update physics + simIntakePhysics.setInput(simIntakeMotor.getMotorVoltage() * RoboRioSim.getVInVoltage()); + simIntakePhysics.update(0.02); + + // update sim objects + // rpm to rps + simIntakeMotor.setRotorVelocity(simIntakePhysics.getAngularVelocityRPM() / 60); + } + + public void setOutputPercentage(double outputPercentage) { + this.outputPercentage = MathUtil.clamp(outputPercentage, -1, 1); + } + + public void setStatorLimit(double amps) { + intakeMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(amps)); + } + + public double getOutputPercentage() { + return outputPercentage; + } + + public boolean isHolding() { + return Robot.isReal() + ? intakeMotor.getForwardLimit().getValue().value == 0 + : false; + } + + public BooleanSupplier getHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return isHolding(); + } + }; + } + + public BooleanSupplier getNotHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return !isHolding(); + } + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralReefVision.java b/src/main/java/frc/robot/subsystems/coral/CoralReefVision.java new file mode 100644 index 0000000..ecb18e8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralReefVision.java @@ -0,0 +1,234 @@ +package frc.robot.subsystems.coral; + +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.function.Predicate; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.Constants.Elevator; +import frc.robot.Constants.Coral.Vision; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; +import frc.robot.util.CoralReefVisionSim; +import frc.robot.util.Reef; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.IntegerArraySubscriber; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +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; + +@Logged(strategy = Strategy.OPT_IN) +public class CoralReefVision extends SubsystemBase { + + // Vision targets in robot space + private final StructArrayPublisher visionTargetPublisher; + // Camera pose in robot space + private final StructPublisher cameraPublisher; + + // Debug outputs + private StructPublisher primaryVisionTargetFieldSpace; + private StructArrayPublisher visionTargetsFieldSpace; + private StructPublisher scoringPoseFieldSpace; + private StructPublisher cameraPoseFieldSpace; + private BooleanPublisher hasTargetPublisher; + + // Inputs from vision coprocessor + NetworkTable visionRawTable = NetworkTableInstance.getDefault().getTable("CoralVision/raw"); + private final DoubleArraySubscriber inputAngles; + private final DoubleArraySubscriber inputDistances; + private final IntegerSubscriber inputFrame; + + // private DatagramSocket visionDataRecever; + // private Alert visionAlert = new Alert("Vision Error", AlertType.kError); + // private byte[] dataBuf = new byte[1024]; + + @Logged + private ArrayList visionTargets = new ArrayList(); + private int selectedTargetIndex = -1; + private long lastFrame = 0; + + private CoralReefVisionSim sim; + + public CoralReefVision() { + visionRawTable = NetworkTableInstance.getDefault().getTable("CoralVision/raw"); + inputAngles = visionRawTable.getDoubleArrayTopic("angles").subscribe(new double[] {}); + inputDistances = visionRawTable.getDoubleArrayTopic("distances").subscribe(new double[] {}); + inputFrame = visionRawTable.getIntegerTopic("frame").subscribe(0); + + visionTargetPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("CoralVision/targets", Translation3d.struct).publish(); + cameraPublisher = NetworkTableInstance.getDefault() + .getStructTopic("CoralVision/cameraPose", Pose3d.struct).publish(); + + // Debug publishers + primaryVisionTargetFieldSpace = NetworkTableInstance.getDefault() + .getStructTopic("CoralVision/selectedTargetPose", + Pose3d.struct) + .publish(); + visionTargetsFieldSpace = NetworkTableInstance.getDefault() + .getStructArrayTopic("CoralVision/targetPoses", + Pose3d.struct) + .publish(); + scoringPoseFieldSpace = NetworkTableInstance.getDefault().getStructTopic("CoralVision/scoringPose", + Pose2d.struct).publish(); + cameraPoseFieldSpace = NetworkTableInstance.getDefault().getStructTopic("CoralVision/cameraPoseFieldSpace", + Pose3d.struct).publish(); + + hasTargetPublisher = NetworkTableInstance.getDefault().getBooleanTopic("CoralVision/hasTarget").publish(); + + // Simulator for testing/debugging + if (Robot.isSimulation()) { + sim = new CoralReefVisionSim(); + } + } + + @Override + public void periodic() { + + long frame = inputFrame.getAsLong(); + double[] angles = inputAngles.get(); + double[] distances = inputDistances.get(); + + boolean dataUpdated = true;// frame != lastFrame + if (dataUpdated) { + lastFrame = frame; + + visionTargets.clear(); + + if (angles.length == distances.length) { + visionTargets.clear(); + // Got good data from coprocessor + for (int i = 0; i < angles.length; i++) { + double r = distances[i]; + double theta = angles[i] + Constants.Coral.Vision.CAM_YAW.getRadians(); // Radians!!! + + // Calculate vision target positions + Translation3d targetPos = Constants.Coral.Vision.CAM_POSE.getTranslation().plus( + new Translation3d(r * Math.cos(theta), r * Math.sin(theta), 0.0)); + visionTargets.add(targetPos); + } + } + + // Filter out invalid vision targets + visionTargets.removeIf(new Predicate() { + @Override + public boolean test(Translation3d t) { + // TODO Auto-generated method stub + double camDist = t.getDistance(Constants.Coral.Vision.CAM_POSE.getTranslation()); + if (camDist > 2.0 || camDist < 0.25) + return true; + return false; + } + }); + + // Select a target + double minDistance = Double.MAX_VALUE; + if (visionTargets.size() == 0) { + selectedTargetIndex = -1; + } else { + for (int i = 0; i < visionTargets.size(); ++i) { + double dist = visionTargets.get(i).toTranslation2d() + .getDistance(Constants.Coral.Vision.SCORING_BUMPER_POINT); + if (dist < minDistance) { + minDistance = dist; + selectedTargetIndex = i; + } + } + } + + // Filter out too far away targets (1 meter) + if (minDistance >= Reef.faceOffset * 2.5 + Units.inchesToMeters(10.0)) { + selectedTargetIndex = -1; + } + + cameraPublisher.set(Constants.Coral.Vision.CAM_POSE); + visionTargetPublisher.set(visionTargets.toArray(new Translation3d[] {})); + } + + hasTargetPublisher.set(hasValidTarget()); + } + + @Override + public void simulationPeriodic() { + + } + + public Translation2d getSelectedTargetError() { + if (selectedTargetIndex == -1 || selectedTargetIndex >= visionTargets.size()) { + return new Translation2d(); + } else { + return visionTargets.get(selectedTargetIndex).toTranslation2d() + .minus(Constants.Coral.Vision.SCORING_POSITION); + } + } + + public Pose2d getSelectedTargetPose(SwerveSubsystem swerveSub) { + if (selectedTargetIndex == -1 || selectedTargetIndex >= visionTargets.size()) { + return new Pose2d(); + } else { + return swerveSub.getOdometryPose() + .transformBy(new Transform2d( + visionTargets.get(selectedTargetIndex).toTranslation2d(), + Rotation2d.kZero)); + } + } + + public Pose3d getCameraPoseFieldSpace(SwerveSubsystem swerveSub) { + return new Pose3d(swerveSub.getOdometryPose()) + .transformBy(new Transform3d(Pose3d.kZero, Constants.Coral.Vision.CAM_POSE)); + } + + public void publishDebugData(SwerveSubsystem swerveSubsystem) { + scoringPoseFieldSpace.set(swerveSubsystem.getOdometryPose().transformBy(new Transform2d( + Constants.Coral.Vision.SCORING_POSITION, + Rotation2d.kZero))); + cameraPoseFieldSpace.set(getCameraPoseFieldSpace(swerveSubsystem)); + + // Transform local vision targets to field space + Pose3d primaryTargetPose3d = new Pose3d(swerveSubsystem.getOdometryPose()); + Pose3d[] targetsField = new Pose3d[visionTargets.size()]; + for (int i = 0; i < visionTargets.size(); i++) { + targetsField[i] = new Pose3d(swerveSubsystem.getOdometryPose()) + .transformBy(new Transform3d(visionTargets.get(i), Rotation3d.kZero)); + if (i == selectedTargetIndex) + primaryTargetPose3d = targetsField[i]; + } + visionTargetsFieldSpace.set(targetsField); + primaryVisionTargetFieldSpace.set(primaryTargetPose3d); + + } + + public boolean hasValidTarget() { + return selectedTargetIndex != -1; + } +} diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java new file mode 100644 index 0000000..d5a9452 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java @@ -0,0 +1,585 @@ +package frc.robot.subsystems.coral; + +import java.lang.reflect.Field; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +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.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Ultrasonic; +import edu.wpi.first.wpilibj.XboxController; +// import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.util.LimelightAssistance; +import frc.robot.RobotContainer; +import frc.robot.commands.coral.motion.MoveElevator; +import frc.robot.commands.coral.motion.MovePitch; +import frc.robot.commands.coral.motion.MovePivot; +import frc.robot.commands.coral.motion.MoveRoll; +import frc.robot.commands.coral.motion.StowArm; +import frc.robot.commands.coral.motion.WaitArmClearance; +import frc.robot.commands.coral.motion.WaitElevatorApproach; +import frc.robot.commands.coral.motion.WaitRollApproach; +import frc.robot.commands.coral.motion.WaitRollFinished; +import frc.robot.commands.coral.motion.WristAlignAssist; +import frc.robot.commands.coral.motion.WristAlignAssistManual; +import frc.robot.commands.coral.motion.WristStowSafety; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.Limelight; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaePresets; +import frc.robot.subsystems.coral.CoralSubsystem.CoralPresets; +import frc.robot.util.LimelightAssistance; + +import frc.robot.util.LimelightContainer; +import frc.robot.util.Reef; + +@Logged +public class CoralSubsystem extends SubsystemBase { + + private final CoralArm arm; + private final CoralIntake intake = new CoralIntake(); + + private final CoralElevator elevator = new CoralElevator(); + + private final CoralReefVision vision = new CoralReefVision(); + + // Members for aim assisting and other automation + private final XboxController operatorController; + @NotLogged + private final SwerveSubsystem swerveSubsystem; + + private final Mechanism2d coralMechanism = new Mechanism2d(2, 3); + private final MechanismRoot2d rootMechanism = coralMechanism.getRoot("Coral", 1.0, 0.0); + private final MechanismLigament2d elevatorMechanism = rootMechanism.append( + new MechanismLigament2d("Elevator", Constants.Elevator.PhysicalParameters.BOTTOM_TO_FLOOR, 90)); + private final MechanismLigament2d pivotMechanism = elevatorMechanism.append( + new MechanismLigament2d("Coral", Constants.Coral.Pivot.PhysicalConstants.JOINT_LENGTH_METERS, 0)); + private final MechanismLigament2d pitchMechanism = pivotMechanism.append( + new MechanismLigament2d("Pitch", Constants.Coral.Pitch.PhysicalConstants.JOINT_LENGTH_METERS, 0)); + private final MechanismLigament2d rollMechanism = pivotMechanism.append( + new MechanismLigament2d("Roll", Constants.Coral.Roll.PhysicalConstants.ARM_LENGTH_METERS, 90)); + + StructPublisher leftPosePub = NetworkTableInstance.getDefault().getStructTopic("Debug/Left", Pose2d.struct) + .publish(); + StructPublisher rightPosePub = NetworkTableInstance.getDefault() + .getStructTopic("Debug/Right", Pose2d.struct) + .publish(); + + public enum CoralPresets { + LEVEL_1(0.05, Units.radiansToDegrees(0.662), 65, Units.radiansToDegrees(1.41), true), + LEVEL_2(0.247 - 0.085 - 0.003, 15 - 0.7, 90, 98.0 + 0.7, true, true), + LEVEL_3(0.650 - 0.085 - 0.003, 15 - 0.7, 90, 98.0 + + 0.7, true, + true), + LEVEL_4(1.342 - 0.02, 19.5, 90, 114.5, true, + true), + INTAKE(0.097, 18.5, 90, 50, true), + STOW(0.05, 0.0, 0.0, 0.0, true), + ZERO(0.0, 0.0, 0.0, 0.0, false), + + ALGAE_REM_LOW(0.62, 32.0, 0.0, 0.0, false), + ALGAE_REM_HIGH(1.05, 32.0, 0.0, 0.0, false), + + ALGAE_STOW_GROUND(0.05, + 32.0, 90.0, 42.0, + false), + ALGAE_STOW_LOW(0.44, + 32.0, 90.0, 42.0, + false), + ALGAE_STOW_HIGH(0.822, + 32.0, 90.0, 42.0, + false), + + // ALGAE_STOW_BARGE(0.9, -20.0, 90.0, 42.0, + // false), + + ALGAE_ACQUIRE_LOW(0.452, 33.5, 90.0, 42.0, false), + ALGAE_ACQUIRE_LOLLIPOP(0.05, 39.0, 90.0, 42.0, false), + ALGAE_ACQUIRE_FLOOR(0.03, 65.0, 90.0, 25.0, false), + ALGAE_ACQUIRE_HIGH(0.832, + 33.5, 90.0, 42.0, false), + + ALGAE_PROCESSOR(0.03, 38.0, 90.0, -20.0, false), + ALGAE_BARGE(1.45, -10.0, 90.0, 42.0, false), + // ALGAE_BARGE( + // 1.44, -45.0, 90.0, -10.0, false), + + ALGAE_STOW_BUMPER(0.03, 43.0, 90.0, 50.0, false), + + CUSTOM(Double.NaN, Double.NaN, Double.NaN, Double.NaN, false); + + double elevatorHeightM; // Elevator height (relative to bottom of elevator/fully retracted) + double pivotAngleDeg; // Looking at the robot from the FRONT (algae intake side), positive to the + // right, and negative to the left (positive=CW) + double rollAngleDeg; // Wrist 1 angle, degrees from pointing at the bumpers on the CORAL ARM side of + // the robot. positive=CCW + double pitchAngleDeg; // Wrist 2 angle, degrees from pointing straight up (max: 115deg) + boolean allowMirror; + boolean allowAimAssist; + + private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle, + boolean allowMirror) { + this.elevatorHeightM = elevatorHeight; + this.pivotAngleDeg = pivotAngle; + this.rollAngleDeg = rollAngle; + this.pitchAngleDeg = pitchAngle; + this.allowMirror = allowMirror; + this.allowAimAssist = false; + } + + private CoralPresets(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle, + boolean allowMirror, boolean allowAimAssist) { + this(elevatorHeight, pivotAngle, rollAngle, pitchAngle, allowMirror); + this.allowAimAssist = allowAimAssist; + } + } + + @NotLogged + AlgaeSubsystem algaeSub; + + public CoralSubsystem(SwerveSubsystem swerveSubsystem, XboxController operatorController, AlgaeSubsystem algae) { + this.swerveSubsystem = swerveSubsystem; + this.operatorController = operatorController; + this.algaeSub = algae; + arm = new CoralArm(this.algaeSub); + } + + public enum MirrorPresets { + RIGHT(false), + LEFT(true); + + boolean isMirrored; + + private MirrorPresets(boolean isMirrored) { + this.isMirrored = isMirrored; + } + } + + public enum CoralIntakePresets { + INTAKE(1, 40.0), + HOLD(0.4, 12.5), + PURGE(-1, 40.0), + SCORE(-1, 30.0), + SCORE_L1(-0.2, 30.0), + STOP(0, 12.5), + + CUSTOM(Double.NaN, 40.0); + + double intakePercentage; + double intakeCurrent; + + private CoralIntakePresets(double intakePercentage, double intakeCurrent) { + this.intakePercentage = intakePercentage; + this.intakeCurrent = intakeCurrent; + } + } + + @Override + public void periodic() { + // i have no idea what any of the getPositions output + elevatorMechanism + .setLength(elevator.getPosition() + Constants.Elevator.PhysicalParameters.CORAL_PIVOT_VERTICAL_OFFSET); + pivotMechanism.setAngle(arm.getPivotPositionDegrees()); + pitchMechanism.setAngle(arm.getPitchPositionDegrees()); + rollMechanism.setLength(Math.cos(Units.degreesToRadians(arm.getRollPositionDegrees())) + * Constants.Coral.Roll.PhysicalConstants.JOINT_LENGTH_METERS); + + // SmartDashboard.putData("Coral Mechanism", coralMechanism); + // SmartDashboard.putBoolean("Elevator in position", isElevatorInPosition()); + // SmartDashboard.putBoolean("Roll in position", isRollInPosition()); + // SmartDashboard.putBoolean("Pitch in position", isPitchInPosition()); + // SmartDashboard.putBoolean("Pivot in position", isPivotInPosition()); + + // SmartDashboard.putBoolean("Elevator SUPPOSED to be in position", + // isElevatorSupposedToBeInPosition()); + // SmartDashboard.putBoolean("Roll SUPPOSED to be in position", + // isRollSupposedToBeInPosition()); + // SmartDashboard.putBoolean("Pitch SUPPOSED to be in position", + // isPitchSupposedToBeInPosition()); + // SmartDashboard.putBoolean("Pivot SUPPOSED to be in position", + // isPivotSupposedToBeInPosition()); + + vision.publishDebugData(swerveSubsystem); + + Pose2d left = swerveSubsystem.getOdometryPose().transformBy(new Transform2d(0, 0.2, new Rotation2d())); + Pose2d right = swerveSubsystem.getOdometryPose().transformBy(new Transform2d(0, -0.2, new Rotation2d())); + leftPosePub.set(left); + rightPosePub.set(right); + // Reef.putToShuffleboard(); + + } + + private CoralPresets currentPreset = CoralPresets.STOW; + private MirrorPresets mirrorSetting = MirrorPresets.RIGHT; + private CoralIntakePresets currentIntakePreset = CoralIntakePresets.STOP; + + public void setCoralPresetDIRECT(CoralPresets preset) { + if (preset == CoralPresets.CUSTOM) { + // uhhh i don't now how to throw an exception and i don't feel like figuring it + // out + } else if (preset != currentPreset) { + elevator.setGoalPosition(preset.elevatorHeightM); + arm.setPivotGoalDegrees( + preset.pivotAngleDeg + * (preset.allowMirror ? (mirrorSetting.isMirrored ? -1.0 : 1.0) : 1.0)); + arm.setRollGoalDegrees(preset.rollAngleDeg + * (preset.allowMirror ? (mirrorSetting.isMirrored ? -1.0 : 1.0) : 1.0)); + arm.setPitchGoalDegrees(preset.pitchAngleDeg); + + currentPreset = preset; + } + } + + public void setCoralPresetElevator(CoralPresets preset) { + elevator.setGoalPosition(preset.elevatorHeightM); + currentPreset = preset; + } + + public CoralPresets getCurrentPreset() { + return currentPreset; + } + + public boolean isElevatorInPosition() { + return elevator.isInPosition(); + } + + public void setCoralPresetPivot(CoralPresets preset) { + SmartDashboard.putNumber("Pivot Pre", preset.pivotAngleDeg); + arm.setPivotGoalDegrees( + preset.pivotAngleDeg + * (preset.allowMirror ? (mirrorSetting.isMirrored ? -1 : 1) : 1.0)); + currentPreset = preset; + } + + public boolean isPivotInPosition() { + return arm.isPivotInPosition(); + } + + public void setCoralPresetPitch(CoralPresets preset) { + arm.setPitchGoalDegrees( + preset.pitchAngleDeg); + currentPreset = preset; + } + + public boolean isPitchInPosition() { + return arm.isPitchInPosition(); + } + + public void setCoralPresetRoll(CoralPresets preset) { + arm.setRollGoalDegrees( + preset.rollAngleDeg + * (preset.allowMirror ? (mirrorSetting.isMirrored ? -1 : 1) : 1.0)); + currentPreset = preset; + } + + public boolean isRollInPosition() { + return arm.isRollInPosition(); + } + + public double getPivotGoalDegrees() { + return arm.getPivotGoalDegrees(); + } + + public double getRollGoalDegrees() { + return arm.getRollGoalDegrees(); + } + + public double getPitchGoalDegrees() { + return arm.getPitchGoalDegrees(); + } + + public boolean isHolding() { + return Robot.isSimulation() ? SmartDashboard.getBoolean("[SIM] Holding Coral", false) : intake.isHolding(); + } + + public BooleanSupplier isHoldingSupplier() { + return new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return isHolding(); + } + }; + } + + public void setCustomPosition(double elevatorHeight, double pivotAngle, double rollAngle, double pitchAngle) { + currentPreset = CoralPresets.CUSTOM; + + elevator.setGoalPosition(elevatorHeight); + arm.setPivotGoalDegrees(pivotAngle); + arm.setRollGoalDegrees(rollAngle); + arm.setPitchGoalDegrees(pitchAngle); + } + + public void setCustomElevatorMeters(double elevatorHeight) { + currentPreset = CoralPresets.CUSTOM; + elevator.setGoalPosition(elevatorHeight); + } + + public void setCustomPivotDegrees(double pivotAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setPivotGoalDegrees(pivotAngle); + } + + public void setCustomRollDegrees(double rollAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setRollGoalDegrees(rollAngle); + } + + public void setCustomPitchDegrees(double pitchAngle) { + currentPreset = CoralPresets.CUSTOM; + arm.setPitchGoalDegrees(pitchAngle); + } + + public void mirrorArm() { + if (mirrorSetting == MirrorPresets.LEFT) { + mirrorSetting = MirrorPresets.RIGHT; + } else + mirrorSetting = MirrorPresets.LEFT; + } + + public void mirrorArm(MirrorPresets preset) { + mirrorSetting = preset; + } + + public MirrorPresets getMirror() { + return mirrorSetting; + } + + public void autoSetMirrorIntake() { + Pose2d robotPose = swerveSubsystem.getOdometryPose(); + Pose2d closestSource = robotPose.nearest(FieldConstants.getSourceSidePoses()); + Pose2d left = robotPose.transformBy(new Transform2d(0, 0.2, new Rotation2d())); + Pose2d right = robotPose.transformBy(new Transform2d(0, -0.2, new Rotation2d())); + leftPosePub.set(left); + rightPosePub.set(right); + + this.mirrorSetting = left.getTranslation().getDistance(closestSource.getTranslation()) < right.getTranslation() + .getDistance(closestSource.getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT; + + SmartDashboard.putString("Mirror Side", mirrorSetting.name()); + + // this.mirrorSetting = (this.leftUltrasonic.get() < this.rightUltrasonic.get()) + // ? MirrorPresets.LEFT + // : MirrorPresets.RIGHT; + } + + public void autoSetMirrorScoring() { + Pose2d robotPose = swerveSubsystem.getOdometryPose(); + Pose2d left = robotPose.transformBy(new Transform2d(0, 0.2, new Rotation2d())); + Pose2d right = robotPose.transformBy(new Transform2d(0, -0.2, new Rotation2d())); + leftPosePub.set(left); + rightPosePub.set(right); + + this.mirrorSetting = left.getTranslation().getDistance(FieldConstants.getReefPose().getTranslation()) < right + .getTranslation() + .getDistance(FieldConstants.getReefPose().getTranslation()) ? MirrorPresets.LEFT : MirrorPresets.RIGHT; + + SmartDashboard.putString("Mirror Side", mirrorSetting.name()); + } + + public void setCoralIntakePreset(CoralIntakePresets preset) { + SmartDashboard.putString("Coral Intake Preset", preset.toString()); + if (preset != currentIntakePreset) { + intake.setOutputPercentage(preset.intakePercentage); + intake.setStatorLimit(preset.intakeCurrent); + currentIntakePreset = preset; + } + } + + public void setCustomIntakePercent(double percentage) { + currentIntakePreset = CoralIntakePresets.CUSTOM; + intake.setOutputPercentage(percentage); + intake.setStatorLimit(currentIntakePreset.intakeCurrent); + } + + public double getPivotPositionDegrees() { + return arm.getPivotPositionDegrees(); + } + + public boolean isElevatorSupposedToBeInPosition() { + // return elevator.isSupposedToBeInPosition(); + return elevator.isInPosition(); + } + + public boolean isPitchSupposedToBeInPosition() { + return arm.isPitchSupposedToBeInPosition(); + } + + public boolean isRollSupposedToBeInPosition() { + return arm.isRollSupposedToBeInPosition(); + } + + public boolean isPivotSupposedToBeInPosition() { + return arm.isPivotSupposedToBeInPosition(); + } + + public boolean isSupposedToBeInPosition() { + return isPivotSupposedToBeInPosition() && isRollSupposedToBeInPosition() && isElevatorSupposedToBeInPosition() + && isPitchSupposedToBeInPosition(); + } + + public CoralArm getCoralArm() { + return arm; + } + + public CoralIntake getIntake() { + return intake; + } + + public CoralElevator getElevator() { + return elevator; + } + + public void simSetHolding(boolean holding) { + SmartDashboard.putBoolean("[SIM] Holding Coral", holding); + } + + public Command getGoToLockedPresetCommandV2(AlgaeSubsystem algaeSubsystem, + Supplier currentLockedPresetSupplier) { + return getGoToLockedPresetCommandV2(algaeSubsystem, currentLockedPresetSupplier, true); + } + + public Command getGoToLockedPresetCommandV2(AlgaeSubsystem algaeSubsystem, + Supplier currentLockedPresetSupplier, boolean autoAlignEnable) { + return new InstantCommand(() -> { + if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE) { + this.autoSetMirrorIntake(); + // if (this.mirrorSetting.isMirrored) + // algaeSubsystem.setAlgaePreset(AlgaePresets.OUT_OF_THE_WAY); + } else { + this.autoSetMirrorScoring(); + } + + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm( + this)) + .andThen(new ParallelCommandGroup( + new MoveElevator( + this, currentLockedPresetSupplier), + new MovePivot( + this, currentLockedPresetSupplier), + new MoveRoll( + this, currentLockedPresetSupplier) + .andThen(new WristAlignAssist(this, operatorController, swerveSubsystem) + .onlyIf(new BooleanSupplier() { + @Override + public boolean getAsBoolean() { + return currentLockedPresetSupplier.get().allowAimAssist + && autoAlignEnable; + } + })), + new WaitRollApproach(this, 40.0).andThen( + new WaitElevatorApproach( + this, 0.4)) + .andThen(new MovePitch( + this, currentLockedPresetSupplier).andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", + currentLockedPresetSupplier.get().toString() + " - Done"); + }))))); + } + + public Command getGoToLockedPresetSideFASTCommand(AlgaeSubsystem algaeSubsystem, + Supplier currentLockedPresetSupplier, MirrorPresets mirrorSide) { + return new InstantCommand(() -> { + + this.mirrorArm(mirrorSide); + // if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE && + // this.mirrorSetting.isMirrored) + // algaeSubsystem.setAlgaePreset(AlgaePresets.OUT_OF_THE_WAY); + + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm( + this)) + .andThen(new MoveElevator( + this, currentLockedPresetSupplier)) + .andThen(new ParallelCommandGroup( + new MovePivot( + this, currentLockedPresetSupplier), + new MoveRoll( + this, currentLockedPresetSupplier), + new MovePitch( + this, currentLockedPresetSupplier))) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } + + // Goes to a preset more quickly by moving pitch+pivot+roll at the same time, + // but can throw coral. Good for intaking + public Command getGoToLockedPresetFASTCommand(AlgaeSubsystem algaeSubsystem, + Supplier currentLockedPresetSupplier) { + return new InstantCommand(() -> { + + if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE) { + + this.autoSetMirrorIntake(); + // if (!this.mirrorSetting.isMirrored) + // algaeSubsystem.setAlgaePreset(AlgaePresets.OUT_OF_THE_WAY); + } else { + this.autoSetMirrorScoring(); + } + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }).andThen(new StowArm(this)) + .andThen(new MoveElevator( + this, currentLockedPresetSupplier)) + .andThen(new ParallelCommandGroup( + new MovePivot( + this, currentLockedPresetSupplier), + new MoveRoll( + this, currentLockedPresetSupplier), + new MovePitch( + this, currentLockedPresetSupplier))) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } + + public Command getGoToLockedPresetAlgaeSafeCommand(AlgaeSubsystem algaeSubsystem, + Supplier currentLockedPresetSupplier) { + return new InstantCommand(() -> { + if (currentLockedPresetSupplier.get() == CoralPresets.INTAKE) { + algaeSubsystem.setAlgaePreset(AlgaePresets.OUT_OF_THE_WAY); + + this.autoSetMirrorIntake(); + } else { + this.autoSetMirrorScoring(); + } + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString()); + }) + .andThen(new MoveElevator( + this, currentLockedPresetSupplier) + .alongWith(new MovePivot(this, currentLockedPresetSupplier))) + .andThen(new ParallelCommandGroup( + new MoveRoll( + this, currentLockedPresetSupplier), + new MovePitch( + this, currentLockedPresetSupplier))) + .andThen(new InstantCommand(() -> { + SmartDashboard.putString("Going to", currentLockedPresetSupplier.get().toString() + " - Done"); + })); + } + + public CoralReefVision getVisionSubsystem() { + return vision; + } +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..43d9a53 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,69 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants.FieldConstants; + +public class AllianceFlipUtil +{ + + public static double applyX(double x) + { + return shouldFlip() ? FieldConstants.FIELD_LENGTH - x : x; + } + + public static double applyY(double y) + { + return shouldFlip() ? FieldConstants.FIELD_WIDTH - y : y; + } + + public static Translation2d apply(Translation2d translation) + { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) + { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) + { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Pose2d flip(Pose2d pose) + { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static ArrayList flip(List poses) { + ArrayList flippedPoses = new ArrayList(); + + for (Pose2d pose : poses) { + flippedPoses.add(flip(pose)); + } + + return flippedPoses; + } + + public static boolean shouldFlip() + { + return (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/CoralReefVisionSim.java b/src/main/java/frc/robot/util/CoralReefVisionSim.java new file mode 100644 index 0000000..63aa7e3 --- /dev/null +++ b/src/main/java/frc/robot/util/CoralReefVisionSim.java @@ -0,0 +1,107 @@ +package frc.robot.util; + +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructSubscriber; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class CoralReefVisionSim extends SubsystemBase { + + NetworkTable visionRawTable = NetworkTableInstance.getDefault().getTable("CoralVision/raw"); + private final DoubleArrayPublisher simDistPub; + private final DoubleArrayPublisher simAnglePub; + private final IntegerPublisher simFramePub; + + long simFrame = 0; + + // Drivetrain input for simulating reef pole positions + StructSubscriber botPoseSubscriber = NetworkTableInstance.getDefault() + .getStructTopic("Odometry Pose", Pose2d.struct).subscribe(Pose2d.kZero); + + StructArrayPublisher debugPub = NetworkTableInstance.getDefault() + .getStructArrayTopic("Camera Debug Pose", Translation2d.struct).publish(); + + public CoralReefVisionSim() { + simDistPub = visionRawTable.getDoubleArrayTopic("distances").publish(); + simAnglePub = visionRawTable.getDoubleArrayTopic("angles").publish(); + simFramePub = visionRawTable.getIntegerTopic("frame").publish(); + + // for (int i = 0; i < numSimTargets; ++i) { + // SmartDashboard.putNumber("CoralVisionSim/angle" + Integer.toString(i), 0.0); + // SmartDashboard.putNumber("CoralVisionSim/distance" + Integer.toString(i), + // 0.0); + // } + } + + @Override + public void simulationPeriodic() { + ArrayList reefPoles = new ArrayList<>(); + for (Translation2d translation2d : Reef.baseTranslations.values()) { + reefPoles.add(AllianceFlipUtil.apply(translation2d)); + } + + // Bot pose from odometry (NT) + Pose3d botPose = new Pose3d(botPoseSubscriber.get()); + // Reef pole translations in the camera flat view plane (XY plane, +X camera + // front) + ArrayList reefPolesCameraSpace = new ArrayList<>(); + // Camera pose in field space + Pose3d cameraPoseFieldSpace = botPose + .transformBy(new Transform3d(Pose3d.kZero, Constants.Coral.Vision.CAM_POSE)); + + for (Translation2d pole : reefPoles) { + Pose3d polePoseCameraSpace = new Pose3d(new Translation3d(pole), Rotation3d.kZero) + .relativeTo(cameraPoseFieldSpace); + reefPolesCameraSpace.add(polePoseCameraSpace.getTranslation().toTranslation2d()); + } + + debugPub.set(reefPolesCameraSpace.toArray(new Translation2d[] {})); + + double[] anglesPrelim = new double[reefPolesCameraSpace.size()]; + double[] distancesPrelim = new double[reefPolesCameraSpace.size()]; + for (int i = 0; i < reefPolesCameraSpace.size(); ++i) { + anglesPrelim[i] = reefPolesCameraSpace.get(i).getAngle().getRadians(); + distancesPrelim[i] = reefPolesCameraSpace.get(i).getNorm(); + } + + // Filter to what would actually be visible by the camera + ArrayList anglesFinal = new ArrayList<>(); + ArrayList distancesFinal = new ArrayList<>(); + for (int i = 0; i < anglesPrelim.length; ++i) { + if (Math.abs(anglesPrelim[i]) <= Constants.Coral.Vision.CAM_FOV_HORIZ / 2.0 + && distancesPrelim[i] < 2.0 && distancesPrelim[i] > 0.0) { + distancesFinal.add(distancesPrelim[i]); + anglesFinal.add(anglesPrelim[i]); + } + } + + double[] distances = new double[distancesFinal.size()]; + double[] angles = new double[anglesFinal.size()]; + for (int i = 0; i < angles.length; ++i) { + angles[i] = anglesFinal.get(i); + distances[i] = distancesFinal.get(i); + } + + simDistPub.set(distances); + simAnglePub.set(angles); + simFramePub.set(simFrame++); + + Reef.putToShuffleboard(); + } +} diff --git a/src/main/java/frc/robot/util/CoralStation.java b/src/main/java/frc/robot/util/CoralStation.java new file mode 100644 index 0000000..e3cbb63 --- /dev/null +++ b/src/main/java/frc/robot/util/CoralStation.java @@ -0,0 +1,29 @@ +package frc.robot.util; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; + +public class CoralStation { + public static final Pose2d[] coralStations = new Pose2d[] { + new Pose2d( + 0.85, + 0.65, + Rotation2d.fromDegrees(144.011392 - 90.0)), + new Pose2d( + 0.85, + 8.020 - 0.65, + Rotation2d.fromDegrees(-144.011392 + 90.0)), + }; + + public static void putToShuffleboard() { + NetworkTableInstance.getDefault() + .getStructTopic("Coral Station Right", Pose2d.struct).publish().set(coralStations[0]); + NetworkTableInstance.getDefault() + .getStructTopic("Coral Station Left", Pose2d.struct).publish().set(coralStations[1]); + + } +} diff --git a/src/main/java/frc/robot/util/LimelightAssistance.java b/src/main/java/frc/robot/util/LimelightAssistance.java new file mode 100644 index 0000000..b9ec3c8 --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightAssistance.java @@ -0,0 +1,40 @@ +package frc.robot.util; + +import frc.robot.RobotContainer; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.Constants.PoseConstants; +import frc.robot.subsystems.SwerveSubsystem; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +@Logged +public class LimelightAssistance { + + @NotLogged + SwerveSubsystem swerveSub = null; + + public LimelightAssistance(SwerveSubsystem swerveSub) { + this.swerveSub = swerveSub; + } + + public boolean isTagOnRight() { + + SmartDashboard.putString("Recieved position to isTagOnLeft: ", + swerveSub.odometry.getEstimatedPosition().toString()); + int nearestTag = RobotContainer.LLContainer.findNearestTagPos(swerveSub.odometry); + SmartDashboard.putNumber("Recieved nearest tag to isTagOnLeft: ", nearestTag); + SmartDashboard.putString("Tag angle, robot angle: ", + "" + PoseConstants.tagPoses.get(nearestTag).getRotation().getDegrees() + + swerveSub.odometry.getEstimatedPosition().getRotation().getDegrees()); + if (Math.abs(PoseConstants.tagPoses.get(nearestTag).getRotation().getDegrees() + - swerveSub.odometry.getEstimatedPosition().getRotation().getDegrees()) > 180) { + SmartDashboard.putBoolean("Tag is on left: ", true); + return true; + } else { + SmartDashboard.putBoolean("Tag is on left: ", false); + return false; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/LimelightContainer.java b/src/main/java/frc/robot/util/LimelightContainer.java new file mode 100644 index 0000000..d38b73d --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightContainer.java @@ -0,0 +1,236 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import java.util.ArrayList; + +import com.studica.frc.AHRS; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.LimelightHelpers; +import frc.robot.subsystems.Limelight; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.Constants.FieldConstants; +import frc.robot.Constants.PoseConstants; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +@Logged +public class LimelightContainer { + static int SIMCOUNTER = 0; + static int RLCOUNTER = 0; + static int RLCountermt1 = 0; + private static ArrayList limelights = new ArrayList(); + + public LimelightContainer(Limelight... limelights) { + int[] validIDs = { 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22 }; + /* + * Red coral source: 1, 2 + * Blue coral source: 12, 13 + * Blue processor: 3 + * Red processor: 16 + * Blue-side barge tags: 14, 15 + * Red-side barge tags: 4, 5 + * Red reef tags: 6, 7, ... 11 + * Blue reef tags: 17, 18, ... 22 + */ + for (Limelight limelight : limelights) { + LimelightContainer.limelights.add(limelight); + LimelightHelpers.SetFiducialIDFiltersOverride(limelight.getName(), validIDs); + LimelightHelpers.SetIMUMode(limelight.getName(), 0); + } + enableLimelights(true); + + } + + public void enableLimelights(boolean enable) { + for (Limelight limelight : limelights) { + limelight.setEnabled(enable); + } + } + + public static void estimateSimOdometry() { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers + .getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + if (mt2 == null) { // in case not all limelights are connected + continue; + } + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + if (!doRejectUpdate) { + SmartDashboard.putString("Simulated Pos", mt2.pose.toString() + SIMCOUNTER); + SIMCOUNTER++; + } + } + } + + public void estimateMT1OdometryPrelim(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx, + SwerveModulePosition[] swerveModulePositions) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + + if (mt1 == null) { + continue; + } + + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (Math.abs(navx.getRate()) > 720) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + + odometry.resetPosition(mt1.pose.getRotation(), swerveModulePositions, mt1.pose); + + // SmartDashboard.putString("Pos MT1 prelim: ", mt1.pose.toString() + " " + + // RLCountermt1); + } + + RLCountermt1++; + } + } + + public void estimateMT1Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + if (mt1 == null) { + continue; + } + + if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { + if (mt1.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + if (mt1.rawFiducials[0].distToCamera > 3) { + doRejectUpdate = true; + } + } + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + odometry.setVisionMeasurementStdDevs(VecBuilder.fill(3, 3, 10)); + odometry.addVisionMeasurement( + mt1.pose, + mt1.timestampSeconds); + limelight.pushPoseToShuffleboard(limelight.getName(), mt1.pose); + } else { + limelight.pushPoseToShuffleboard(limelight.getName(), new Pose2d()); + } + + } + } + + public void estimateMT1OdometryAuto(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName()); + if (mt1 == null) { + continue; + } + + if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { + if (mt1.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + if (mt1.rawFiducials[0].distToCamera > 1.85) { + doRejectUpdate = true; + } + } + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + odometry.setVisionMeasurementStdDevs( + VecBuilder.fill(mt1.avgTagDist * 0.7, mt1.avgTagDist * 0.7, 9999999)); + odometry.addVisionMeasurement( + mt1.pose, + mt1.timestampSeconds); + limelight.pushPoseToShuffleboard(limelight.getName(), mt1.pose); + } else { + limelight.pushPoseToShuffleboard(limelight.getName(), new Pose2d()); + } + + } + } + + public void estimateMT2Odometry(SwerveDrivePoseEstimator odometry, ChassisSpeeds speeds, AHRS navx) { + for (Limelight limelight : limelights) { + boolean doRejectUpdate = false; + LimelightHelpers.SetRobotOrientation(limelight.getName(), + -navx.getAngle() + (FieldConstants.getAlliance() == Alliance.Red ? 180 : 0.0), -navx.getRate(), 0, + 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers + .getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName()); + if (Math.abs(navx.getRate()) > 720) + doRejectUpdate = true;// if our angular velocity is greater than 720 degrees per second, ignore + // vision updates + if (mt2 == null) { + continue; + } + + if (mt2.tagCount == 1 && mt2.rawFiducials.length == 1) { + if (mt2.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + + if (mt2.rawFiducials[0].distToCamera > 6) { + doRejectUpdate = true; + } + } + + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + odometry.setVisionMeasurementStdDevs(VecBuilder.fill(5, 5, 9999999)); + odometry.addVisionMeasurement( + mt2.pose, + mt2.timestampSeconds); + limelight.pushPoseToShuffleboard(limelight.getName(), mt2.pose); + + } else { + limelight.pushPoseToShuffleboard(limelight.getName(), new Pose2d()); + } + } + + } + + public int findNearestTagPos(SwerveDrivePoseEstimator odometry) { + double min = 43490824; + int toReturn = 0; + for (int i = 0; i < 22; i++) { + double thing = Math + .sqrt(Math.abs(PoseConstants.tagPoses.get(i).getX() - odometry.getEstimatedPosition().getX())); + if (thing < min) { + min = thing; + toReturn = i + 1; + } + } + return toReturn; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Reef.java b/src/main/java/frc/robot/util/Reef.java new file mode 100644 index 0000000..4add2e0 --- /dev/null +++ b/src/main/java/frc/robot/util/Reef.java @@ -0,0 +1,154 @@ +package frc.robot.util; + +import java.util.HashMap; +import java.util.Map; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; + +public class Reef { + public enum ReefBranch { + A, B, C, D, E, F, G, H, I, J, K, L; + } + + private static final Pose2d[] centerFaces = new Pose2d[] { + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)), + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)), + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)), + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)), + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(300)), + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(240)) + }; + + public static final Translation2d center = new Translation2d( + 4.485, + 4.025); + // Starting off facing DS wall + public static final double centerOffset = Units.inchesToMeters(32); + public static final double faceOffset = Units.inchesToMeters(6.469); + public static final double poleInset = Units.inchesToMeters(2.0); + public static final double baseInset = Units.inchesToMeters(12.0); + + public static final Map robotBranchPoses = new HashMap() { + { + ReefBranch[] branchName = ReefBranch.values(); + for (int i = 0; i < 12; i += 2) { + Pose2d face = centerFaces[i / 2]; + put(branchName[i], face.transformBy( + new Transform2d(DriveConstants.FULL_ROBOT_WIDTH / 2.0, faceOffset, + new Rotation2d()))); + put(branchName[i + 1], face.transformBy( + new Transform2d(DriveConstants.FULL_ROBOT_WIDTH / 2.0, -faceOffset, + new Rotation2d()))); + } + } + }; + + public static final Map branchTranslations = new HashMap() { + { + ReefBranch[] branchName = ReefBranch.values(); + for (int i = 0; i < 12; i += 2) { + Pose2d face = centerFaces[i / 2]; + put(branchName[i], face.transformBy( + new Transform2d(-poleInset, faceOffset, + new Rotation2d())) + .getTranslation()); + put(branchName[i + 1], face.transformBy( + new Transform2d(-poleInset, -faceOffset, + new Rotation2d())) + .getTranslation()); + } + } + }; + + public static final Map baseTranslations = new HashMap() { + { + ReefBranch[] branchName = ReefBranch.values(); + for (int i = 0; i < 12; i += 2) { + Pose2d face = centerFaces[i / 2]; + put(branchName[i], face.transformBy( + new Transform2d(-baseInset, faceOffset, + new Rotation2d())) + .getTranslation()); + put(branchName[i + 1], face.transformBy( + new Transform2d(-baseInset, -faceOffset, + new Rotation2d())) + .getTranslation()); + } + } + }; + + public static Pose2d getBranchPose2d(ReefBranch branch) { + return robotBranchPoses.get(branch); + } + + /** + * Used to put a pose on Shuffleboard for debugging - Don't repeatadly call + * this! + * + * @param name Name of pose + * @param pose Pose2d pose + */ + public static void pushPoseToShuffleboard(String name, Pose2d pose) { + StructPublisher publisher = NetworkTableInstance.getDefault() + .getStructTopic(name, Pose2d.struct) + .publish(); + publisher.set(pose); + } + + static StructArrayPublisher botPosePublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("Reef Robot Poses", Pose2d.struct).publish(); + static StructArrayPublisher branchPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("Reef Branches", Translation2d.struct).publish(); + static StructArrayPublisher basePublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("Reef Bases", Translation2d.struct).publish(); + + /** + * Just for visualization for poses + */ + public static void putToShuffleboard() { + // for (ReefBranch branch : branches.keySet()) { + // System.out.println(branch.name()); + // Pose2d branchPosition = branches.get(branch); + + // StructPublisher publisher = NetworkTableInstance.getDefault() + // .getStructTopic(branch.name(), Pose2d.struct).publish(); + + // publisher.set(branchPosition); + // } + + botPosePublisher.set(robotBranchPoses.values().toArray(new Pose2d[] {})); + branchPublisher.set(branchTranslations.values().toArray(new Translation2d[] {})); + basePublisher.set(baseTranslations.values().toArray(new Translation2d[] {})); + } +} \ No newline at end of file diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 1fa7c03..bef4a15 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0", + "version": "4.1.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,21 +12,22 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0" + "version": "4.1.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0", + "version": "4.1.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ "linuxathena", - "windowsx86-64", "linuxx86-64", - "osxuniversal" + "linuxarm64", + "osxuniversal", + "windowsx86-64" ] } ], diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.6.json index 6322388..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.3.2.json similarity index 80% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.3.2.json index 473f6a8..29187a8 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.3.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.3.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.3.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.3.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +365,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.3.2", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.3.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.2", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib.json index cde6011..ac62be8 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json similarity index 89% rename from vendordeps/Studica-2025.0.0.json rename to vendordeps/Studica-2025.0.1.json index ddb0e44..5010be0 100644 --- a/vendordeps/Studica-2025.0.0.json +++ b/vendordeps/Studica-2025.0.1.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.0.0.json", + "fileName": "Studica-2025.0.1.json", "name": "Studica", - "version": "2025.0.0", + "version": "2025.0.1", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.0.0" + "version": "2025.0.1" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.0.0" + "version": "2025.0.1" } ] } \ No newline at end of file diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..2310eee --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/main/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2025.0.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2025.0.1", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2025.0.1", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..a8884ff --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.0.8", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.0.8" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.0.8", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file