diff --git a/aslayout.json b/aslayout.json index 4fcc8ac..70c092f 100644 --- a/aslayout.json +++ b/aslayout.json @@ -1,21 +1,41 @@ { "hubs": [ { - "x": 345, - "y": 164, - "width": 1363, - "height": 791, + "x": -2568, + "y": -190, + "width": 2576, + "height": 1408, "state": { "sidebar": { - "width": 300, + "width": 410, "expanded": [ - "/SwerveDrive/PoseEstimatorPose", "/SwerveDrive/PoseEstimatorPose/rotation", - "/PathPlanner" + "/SwerveDrive/CurrentStates/0", + "/SwerveDrive/DesiredStates/0", + "/SwerveDrive/CurrentPositions/0", + "/SwerveDrive/CurrentPositions/0/angle", + "/SwerveDrive/CurrentPositions/1", + "/SwerveDrive/CurrentPositions/1/angle", + "/SwerveDrive/CurrentPositions/2", + "/SwerveDrive/CurrentPositions/2/angle", + "/SwerveDrive/CurrentPositions/3", + "/SwerveDrive/CurrentPositions/3/angle", + "/SwerveDrive/DesiredStates/0/angle", + "/SwerveDrive/CurrentStates/0/angle", + "/photonvision/fl_cam", + "/SwerveDrive/PoseEstimatorPose", + "/Vision", + "/Vision/fl_camPoseEstimation", + "/Vision/fl_camPoseEstimation/translation", + "/Vision/fl_camPoseEstimation/rotation", + "/NoteVisualizer/LaunchedNotes", + "/SwerveDrive", + "/SwerveDrive/CurrentStates", + "/SwerveDrive/DesiredStates" ] }, "tabs": { - "selected": 2, + "selected": 5, "tabs": [ { "type": 0, @@ -31,7 +51,18 @@ "type": null, "factor": 1 }, - "fields": [] + "fields": [ + { + "key": "NT:/SwerveDrive/CurrentStates/0/speed", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/SwerveDrive/DesiredStates/0/speed", + "color": "#e5b31b", + "show": true + } + ] }, "discrete": { "fields": [] @@ -42,17 +73,65 @@ "type": null, "factor": 1 }, - "fields": [] + "fields": [ + { + "key": "NT:/SwerveDrive/CurrentStates/0/angle/value", + "color": "#af2437", + "show": true + }, + { + "key": "NT:/SwerveDrive/DesiredStates/0/angle/value", + "color": "#80588e", + "show": true + } + ] } }, "title": "Line Graph" }, + { + "type": 4, + "fields": [ + null, + null, + null + ], + "selectionType": "full", + "selectionRangeMin": 0, + "selectionRangeMax": 10, + "measurementType": "independent", + "measurementSampling": "fixed", + "measurementSamplingPeriod": 20, + "histogramMin": 0, + "histogramMax": 10, + "histogramStep": 1, + "title": "Statistics" + }, { "type": 6, "uuid": "7azj8gpne5ee1gxp8gnulcpeg1hzrw9d", "fields": [], "listFields": [ - [], + [ + { + "type": "Game Piece 0", + "key": "NT:/NoteVisualizer/StagedNotes", + "sourceTypeIndex": 2, + "sourceType": "Pose3d[]" + }, + { + "type": "Game Piece 0", + "key": "NT:/NoteVisualizer/LaunchedNotes", + "sourceTypeIndex": 2, + "sourceType": "Pose3d[]" + }, + { + "type": "Game Piece 0", + "key": "NT:/NoteVisualizer/RobotNote", + "sourceTypeIndex": 1, + "sourceType": "Pose3d" + } + ], [ { "type": "Robot", @@ -95,33 +174,79 @@ "key": "NT:/PathPlanner/activePath", "sourceTypeIndex": 0, "sourceType": 5 + }, + { + "type": "Trajectory", + "key": "NT:/SwerveDrive/CurrentChoreoTrajectory", + "sourceTypeIndex": 2, + "sourceType": "Pose2d[]" } ] ], "options": { "field": "2024 Field", "alliance": "blue", - "robot": "20532024", + "robot": "LynkClone", "unitDistance": "meters", "unitRotation": "radians" }, "configHidden": false, "visualizer": { - "cameraIndex": -1, - "orbitFov": 50, + "cameraIndex": -2, + "orbitFov": 75, "cameraPosition": [ - -3.5853041954079066, - 3.1117141052284785, - 6.420683499878898 + 0.0948801836520725, + 3.2238861086067025, + 0.2469499751943985 ], "cameraTarget": [ - -4.27671200193653, - 0.19080418080003858, - 2.121466287955874 + 0.094882990675214, + 0.41078572113049105, + 0.24694978849037907 ] }, "title": "3D Field" }, + { + "type": 5, + "uuid": "surohaj5wfsq7ktr2nxzab22oc0ynqe9", + "fields": [], + "listFields": [ + [ + { + "type": "Trajectory", + "key": "NT:/SwerveDrive/CurrentChoreoTrajectory", + "sourceTypeIndex": 2, + "sourceType": "Pose2d[]" + }, + { + "type": "Robot", + "key": "NT:/SwerveDrive/PoseEstimatorPose", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + }, + { + "type": "Ghost", + "key": "NT:/Vision/fl_camPoseEstimation", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + } + ] + ], + "options": { + "game": "2024 Field", + "unitDistance": "meters", + "unitRotation": "radians", + "origin": "right", + "size": 0.868426, + "allianceBumpers": "auto", + "allianceOrigin": "blue", + "orientation": "blue left, red right" + }, + "configHidden": false, + "visualizer": null, + "title": "Odometry" + }, { "type": 9, "uuid": "ldza8g2yvu4juu4c201rjah0f42x3bq4", @@ -144,16 +269,42 @@ ], "listFields": [], "options": { - "maxSpeed": 5.7, + "maxSpeed": 6.037576989785339, "rotationUnits": "radians", "arrangement": "0,1,2,3", - "sizeLeftRight": 0.79375, - "sizeFrontBack": 0.79375, + "sizeLeftRight": 0.55245, + "sizeFrontBack": 0.40005, "forwardDirection": "up" }, "configHidden": false, "visualizer": null, "title": "Swerve" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" } ] } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 9ce7b97..47112e5 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -11,8 +11,11 @@ #include "frc/Alert.h" #include "str/DataUtils.h" +#include "frc/Threads.h" void Robot::RobotInit() { + //DANGEROUS MAKE SURE CODE DOESN'T BLOCK!!! + frc::SetCurrentThreadPriority(true, 15); str::DataUtils::LogGitInfo(); frc2::CommandScheduler::GetInstance().SetPeriod(consts::LOOP_PERIOD); frc::DataLogManager::Start(); diff --git a/src/main/deploy/branch.txt b/src/main/deploy/branch.txt index c8769c9..88d050b 100644 --- a/src/main/deploy/branch.txt +++ b/src/main/deploy/branch.txt @@ -1 +1 @@ -shooter \ No newline at end of file +main \ No newline at end of file diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 0b8977a..454be0e 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -a411e2f \ No newline at end of file +6a1814f \ No newline at end of file diff --git a/src/main/include/constants/SwerveConstants.h b/src/main/include/constants/SwerveConstants.h index 9f67d49..ab31e9e 100644 --- a/src/main/include/constants/SwerveConstants.h +++ b/src/main/include/constants/SwerveConstants.h @@ -42,6 +42,7 @@ inline constexpr units::ampere_t SLIP_CURRENT_LIMIT = 180_A; namespace physical { +#ifdef __FRC_ROBORIO__ inline constexpr units::radian_t IMU_ROLL_OFFSET = -0.4168_deg; inline constexpr units::radian_t IMU_PITCH_OFFSET = -89.879_deg; inline constexpr units::radian_t IMU_YAW_OFFSET = 89.9885_deg; @@ -60,6 +61,27 @@ inline constexpr bool FL_DRIVE_INVERT = false; inline constexpr bool FR_DRIVE_INVERT = true; inline constexpr bool BL_DRIVE_INVERT = false; inline constexpr bool BR_DRIVE_INVERT = true; +#else +inline constexpr units::radian_t IMU_ROLL_OFFSET = 0_deg; +inline constexpr units::radian_t IMU_PITCH_OFFSET = 0_deg; +inline constexpr units::radian_t IMU_YAW_OFFSET = 0_deg; + +inline constexpr units::turn_t FL_ENC_OFFSET = .25_tr; +inline constexpr units::turn_t FR_ENC_OFFSET = .25_tr; +inline constexpr units::turn_t BL_ENC_OFFSET = .25_tr; +inline constexpr units::turn_t BR_ENC_OFFSET = .25_tr; + +inline constexpr bool FL_STEER_INVERT = false; +inline constexpr bool FR_STEER_INVERT = false; +inline constexpr bool BL_STEER_INVERT = false; +inline constexpr bool BR_STEER_INVERT = false; + +inline constexpr bool FL_DRIVE_INVERT = false; +inline constexpr bool FR_DRIVE_INVERT = false; +inline constexpr bool BL_DRIVE_INVERT = false; +inline constexpr bool BR_DRIVE_INVERT = false; +#endif + inline constexpr units::scalar_t STEER_GEARING_MK4I = (50.0 / 14.0) * (60.0 / 10.0);