From ec0aba408121396e05f423e686493ecfb69a8e07 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Sat, 12 Jul 2025 21:50:08 -0500 Subject: [PATCH 1/2] delete some unused/outdated files that we probably will never need --- Zen_Logging_Layout_lakeSuperior.json | 679 ------------------ Zen_Logging_Layout_weekZero.json | 462 ------------ docs/TUNING.md | 3 - shuffleboard_layout_comp.json | 273 ------- simgui.txt | 398 ---------- .../commands/SwerveControllerCommand.java | 237 ------ .../team1816/lib/events/EventAggregator.java | 29 - .../com/team1816/lib/events/EventBase.java | 41 -- .../lib/events/EventSubscription.java | 36 - .../team1816/lib/events/IEventAggregator.java | 11 - .../lib/events/IEventSubscription.java | 14 - .../team1816/lib/events/PubSubConsumer.java | 28 - .../team1816/lib/events/PubSubRunnable.java | 17 - .../configurations/PeriodicStatusFrame.java | 41 -- .../team1816/lib/util/GreenDriveHelper.java | 167 ----- .../java/com/team1816/lib/util/MotorUtil.java | 44 -- .../com/team1816/lib/util/VectorUtil.java | 42 -- .../simUtil/SingleJointedElevatorArmSim.java | 310 -------- .../util/visionUtil/GreenPhotonCamera.java | 233 ------ .../util/visionUtil/GreenSimPhotonCamera.java | 212 ------ .../util/visionUtil/GreenSimVisionSystem.java | 263 ------- .../util/visionUtil/GreenSimVisionTarget.java | 62 -- .../lib/util/visionUtil/VisionPoint.java | 42 -- .../team1816/lib/variableInputs/Numeric.java | 77 -- .../lib/variableInputs/VariableInput.java | 17 - .../resources/yaml/cheeze-curd.config.yml | 85 --- src/main/resources/yaml/zen.config.yml | 271 ------- src/main/resources/yaml/zingi.config.yml | 286 -------- .../lib/events/EventAggregatorTests.java | 68 -- 29 files changed, 4448 deletions(-) delete mode 100644 Zen_Logging_Layout_lakeSuperior.json delete mode 100644 Zen_Logging_Layout_weekZero.json delete mode 100644 docs/TUNING.md delete mode 100644 shuffleboard_layout_comp.json delete mode 100644 simgui.txt delete mode 100644 src/main/java/com/team1816/lib/auto/commands/SwerveControllerCommand.java delete mode 100644 src/main/java/com/team1816/lib/events/EventAggregator.java delete mode 100644 src/main/java/com/team1816/lib/events/EventBase.java delete mode 100644 src/main/java/com/team1816/lib/events/EventSubscription.java delete mode 100644 src/main/java/com/team1816/lib/events/IEventAggregator.java delete mode 100644 src/main/java/com/team1816/lib/events/IEventSubscription.java delete mode 100644 src/main/java/com/team1816/lib/events/PubSubConsumer.java delete mode 100644 src/main/java/com/team1816/lib/events/PubSubRunnable.java delete mode 100644 src/main/java/com/team1816/lib/hardware/components/motor/configurations/PeriodicStatusFrame.java delete mode 100644 src/main/java/com/team1816/lib/util/GreenDriveHelper.java delete mode 100644 src/main/java/com/team1816/lib/util/MotorUtil.java delete mode 100644 src/main/java/com/team1816/lib/util/VectorUtil.java delete mode 100644 src/main/java/com/team1816/lib/util/simUtil/SingleJointedElevatorArmSim.java delete mode 100644 src/main/java/com/team1816/lib/util/visionUtil/GreenPhotonCamera.java delete mode 100644 src/main/java/com/team1816/lib/util/visionUtil/GreenSimPhotonCamera.java delete mode 100644 src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionSystem.java delete mode 100644 src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionTarget.java delete mode 100644 src/main/java/com/team1816/lib/util/visionUtil/VisionPoint.java delete mode 100644 src/main/java/com/team1816/lib/variableInputs/Numeric.java delete mode 100644 src/main/java/com/team1816/lib/variableInputs/VariableInput.java delete mode 100644 src/main/resources/yaml/cheeze-curd.config.yml delete mode 100644 src/main/resources/yaml/zen.config.yml delete mode 100644 src/main/resources/yaml/zingi.config.yml delete mode 100644 src/test/java/com/team1816/lib/events/EventAggregatorTests.java diff --git a/Zen_Logging_Layout_lakeSuperior.json b/Zen_Logging_Layout_lakeSuperior.json deleted file mode 100644 index 6894dc83..00000000 --- a/Zen_Logging_Layout_lakeSuperior.json +++ /dev/null @@ -1,679 +0,0 @@ -{ - "hubs": [ - { - "x": -8, - "y": -8, - "width": 1936, - "height": 1096, - "state": { - "sidebar": { - "width": 300, - "expanded": [ - "/Drivetrain/Swerve", - "/Drivetrain/Pose", - "/GreenLogs", - "/GreenLogs/TrajectoryDesPose", - "/Shooter/Roller", - "/NT", - "/NT/CameraPublisher/Arducam_OV9281_USB_Camera-processed", - "/NT/CameraPublisher/Arducam_OV9281_USB_Camera-processed/modes", - "/NT/CameraPublisher/Arducam_OV9281_USB_Camera-processed/streams", - "/NT/LiveWindow", - "/NT/Shuffleboard", - "/NT/Shuffleboard/.metadata", - "/NT/Shuffleboard/.recording", - "/NT/SmartDashboard", - "/NT/SmartDashboard/Mech2d", - "/NTConnection", - "/Drivetrain" - ] - }, - "tabs": { - "selected": 17, - "tabs": [ - { - "type": 0, - "path": "../docs/INDEX.md" - }, - { - "type": 3, - "field": "messages", - "title": "Console" - }, - { - "type": 5, - "uuid": "x4swhsks7s7q9zfa26tzirbnz15pdasx", - "fields": [], - "listFields": [ - [ - { - "type": "Robot", - "key": "Drivetrain/Pose", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "game": "2024 Field", - "unitDistance": "meters", - "unitRotation": "degrees", - "origin": "right", - "size": 0.889, - "allianceBumpers": "auto", - "allianceOrigin": "blue", - "orientation": "blue left, red right" - }, - "configHidden": false, - "visualizer": null, - "title": "Odometry" - }, - { - "type": 6, - "uuid": "m3am0oevj1v6vgltwctpt5k54w8d28ft", - "fields": [], - "listFields": [ - [], - [ - { - "type": "Robot", - "key": "Drivetrain/Pose", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Trajectory", - "key": "NT:/SmartDashboard/Field/Trajectory", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "field": "2024 Field", - "alliance": "blue", - "robot": "Zen", - "unitDistance": "meters", - "unitRotation": "degrees" - }, - "configHidden": false, - "visualizer": { - "cameraIndex": -1, - "orbitFov": 50, - "cameraPosition": [ - -9.085676329178565, - 5.036645649119814, - 1.0950997936323528 - ], - "cameraTarget": [ - 0, - 0.5, - 0 - ] - }, - "title": "3D Field" - }, - { - "type": 9, - "uuid": "k61fvho2dir2y1cgbmddy78a14q4rhnt", - "fields": [ - { - "key": "Drivetrain/Swerve/ActualStateStruct", - "sourceTypeIndex": 1, - "sourceType": "SwerveModuleState[]" - }, - { - "key": "Drivetrain/Swerve/DesiredStateStruct", - "sourceTypeIndex": 1, - "sourceType": "SwerveModuleState[]" - }, - { - "key": "Drivetrain/Pose/2", - "sourceTypeIndex": 0, - "sourceType": 2 - } - ], - "listFields": [], - "options": { - "maxSpeed": 4.5, - "rotationUnits": "radians", - "arrangement": "0,1,2,3", - "sizeLeftRight": 0.65, - "sizeFrontBack": 0.65, - "forwardDirection": "up" - }, - "configHidden": false, - "visualizer": null, - "title": "Swerve" - }, - { - "type": 10, - "uuid": "kwx0q8kvvoms472kfl2ubr87zwozsxa7", - "fields": [ - { - "key": "NT:/SmartDashboard/Mech2d", - "sourceTypeIndex": 0, - "sourceType": "Mechanism2d" - }, - null, - null - ], - "listFields": [], - "options": {}, - "configHidden": false, - "visualizer": null, - "title": "Pivot Mech2d" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Pivot/actualPivotPosition", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Pivot/desiredPivotPosition", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Pivot/pivotMotorCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Pivot" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Roller/actualRollerVelocity", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Roller/desiredRollerVelocity", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Roller/rollerMotorCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Roller" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Feeder/actualFeederVelocity", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Feeder/desiredFeederVelocity", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "Shooter/Feeder/beamBreakTriggered", - "color": "#af2437", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Feeder/feederMotorCurrentDraw", - "color": "#80588e", - "show": true - } - ] - } - }, - "title": "Feeder" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Collector/actualIntakePower", - "color": "#2b66a2", - "show": true - }, - { - "key": "Collector/desiredIntakePower", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Collector/intakeCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Collector" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Climber/actualClimberOutput", - "color": "#2b66a2", - "show": true - }, - { - "key": "Climber/desiredClimberOutput", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "Climber/controlMode", - "color": "#80588e", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Climber/climberCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Climber" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/DesiredTrajectoryPose/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "Drivetrain/DesiredTrajectoryPose/1", - "color": "#e5b31b", - "show": true - }, - { - "key": "Drivetrain/DesiredTrajectoryPose/2", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Pose/0", - "color": "#80588e", - "show": true - }, - { - "key": "Drivetrain/Pose/1", - "color": "#e48b32", - "show": true - }, - { - "key": "Drivetrain/Pose/2", - "color": "#aacaee", - "show": true - } - ] - } - }, - "title": "Auto Pose" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/ChassisSpeeds/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "Drivetrain/ChassisSpeeds/1", - "color": "#e5b31b", - "show": true - }, - { - "key": "Drivetrain/ChassisSpeeds/2", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "Drivetrain/Swerve/ControlRequest", - "color": "#c0b487", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/DesiredSpeeds/0", - "color": "#80588e", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/1", - "color": "#e48b32", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/2", - "color": "#aacaee", - "show": true - } - ] - } - }, - "title": "ChassisSpeeds" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/Inputs/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "Drivetrain/Swerve/Inputs/1", - "color": "#e5b31b", - "show": true - }, - { - "key": "Drivetrain/Swerve/Inputs/2", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Inputs" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/moduleTemps", - "color": "#2b66a2", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "ModuleTemps" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "CAN/highSpeedUtilization", - "color": "#2b66a2", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "CAN/lowSpeedUtilization", - "color": "#e5b31b", - "show": true - } - ] - } - }, - "title": "CAN Utilization" - }, - { - "type": 1, - "legendHeight": 0.3, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Timings/Robot", - "color": "#2b66a2", - "show": true - }, - { - "key": "Timings/RobotState", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Looptimes" - }, - { - "type": 2, - "fields": [ - "NT:/SmartDashboard/Git Hash" - ], - "title": "Git hash" - } - ] - } - } - } - ], - "satellites": [], - "version": "3.2.0" -} diff --git a/Zen_Logging_Layout_weekZero.json b/Zen_Logging_Layout_weekZero.json deleted file mode 100644 index f64c50c4..00000000 --- a/Zen_Logging_Layout_weekZero.json +++ /dev/null @@ -1,462 +0,0 @@ -{ - "version": "3.0.1", - "layout": [ - { - "type": 5, - "fields": [], - "listFields": [ - [ - { - "type": "Robot", - "key": "Drivetrain/Pose", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Trajectory", - "key": "NT:/SmartDashboard/Field/Trajectory", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "game": "2024 Field", - "unitDistance": "meters", - "unitRotation": "degrees", - "origin": "right", - "size": 0.889, - "allianceBumpers": "auto", - "allianceOrigin": "blue", - "orientation": "blue left, red right" - }, - "configHidden": false, - "title": "Odometry" - }, - { - "type": 6, - "fields": [], - "listFields": [ - [], - [ - { - "type": "Robot", - "key": "Drivetrain/Pose", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Trajectory", - "key": "NT:/SmartDashboard/Field/Trajectory", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "field": "2024 Field", - "alliance": "blue", - "robot": "Zen", - "unitDistance": "meters", - "unitRotation": "degrees", - "cameraIndex": -1, - "fov": 50 - }, - "configHidden": false, - "title": "3D Odometry" - }, - { - "type": 2, - "fields": [ - "NT:/SmartDashboard/Git Hash" - ], - "title": "Table" - }, - { - "type": 9, - "fields": [ - { - "key": "Drivetrain/Swerve/ActualStateStruct", - "sourceTypeIndex": 1, - "sourceType": "SwerveModuleState[]" - }, - { - "key": "Drivetrain/Swerve/DesiredStateStruct", - "sourceTypeIndex": 1, - "sourceType": "SwerveModuleState[]" - }, - { - "key": "Drivetrain/Pose/2", - "sourceTypeIndex": 0, - "sourceType": 2 - } - ], - "listFields": [], - "options": { - "maxSpeed": 4.9, - "rotationUnits": "degrees", - "arrangement": "0,1,2,3", - "sizeLeftRight": 0.889, - "sizeFrontBack": 0.889, - "forwardDirection": "right" - }, - "configHidden": false, - "title": "Swerve" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/ChassisSpeeds/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "Drivetrain/ChassisSpeeds/1", - "color": "#e5b31b", - "show": true - }, - { - "key": "Drivetrain/ChassisSpeeds/2", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "Drivetrain/Swerve/ControlRequest", - "color": "#80588e", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/DesiredSpeeds/0", - "color": "#e48b32", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/1", - "color": "#aacaee", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/2", - "color": "#c0b487", - "show": true - } - ] - } - }, - "title": "ChassisSpeeds" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "CAN/highSpeedUtilization", - "color": "#2b66a2", - "show": true - }, - { - "key": "CAN/lowSpeedUtilization", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "CAN Utilization" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Collector/actualIntakeVelocity", - "color": "#2b66a2", - "show": true - }, - { - "key": "Collector/desiredIntakeVelocity", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Collector/intakeCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Collector" - }, - { - "type": 8, - "fields": [], - "listFields": [], - "options": { - "ids": [ - 0, - 1, - 2 - ], - "layouts": [ - "Xbox Controller (White)", - "Xbox Controller (White)", - "None" - ] - }, - "configHidden": false, - "title": "Joysticks" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/Inputs/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "Drivetrain/Swerve/Inputs/1", - "color": "#e5b31b", - "show": true - }, - { - "key": "Drivetrain/Swerve/Inputs/2", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Drivetrain/Swerve/DesiredSpeeds/0", - "color": "#80588e", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/1", - "color": "#e48b32", - "show": true - }, - { - "key": "Drivetrain/Swerve/DesiredSpeeds/2", - "color": "#aacaee", - "show": true - } - ] - } - }, - "title": "Inputs" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Roller/actualRollerVelocity", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Roller/desiredRollerVelocity", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Roller/rollerMotorCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Roller" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Feeder/actualFeederVelocity", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Roller/desiredFeederVelocity", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Feeder/feederMotorCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Feeder" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Pivot/desiredPivotPosition", - "color": "#2b66a2", - "show": true - }, - { - "key": "Shooter/Pivot/actualPivotPosition", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "Shooter/Pivot/pivotMotorCurrentDraw", - "color": "#af2437", - "show": true - } - ] - } - }, - "title": "Pivot" - }, - { - "type": 10, - "fields": [ - null, - null, - null - ], - "listFields": [], - "options": {}, - "configHidden": false, - "title": "Pivot Mechanism" - } - ] -} diff --git a/docs/TUNING.md b/docs/TUNING.md deleted file mode 100644 index e2b63fe9..00000000 --- a/docs/TUNING.md +++ /dev/null @@ -1,3 +0,0 @@ -# Tuning Guide - -A guide on how to tune things on the robot. diff --git a/shuffleboard_layout_comp.json b/shuffleboard_layout_comp.json deleted file mode 100644 index bf3ff8d6..00000000 --- a/shuffleboard_layout_comp.json +++ /dev/null @@ -1,273 +0,0 @@ -{ - "HALProvider": { - "Other Devices": { - "Compressor[8]": { - "header": { - "open": true - } - }, - "window": { - "visible": false - } - } - }, - "NTProvider": { - "/SmartDashboard/Starting Position": { - "window": { - "visible": true - } - }, - "types": { - "/FMSInfo": "FMSInfo", - "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", - "/LiveWindow/Ungrouped/DigitalOutput[1]": "Digital Output", - "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[12]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[13]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[14]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[15]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[16]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[17]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[18]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[19]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[20]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[21]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[22]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[23]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[24]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[25]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[26]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[27]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[28]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[29]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[30]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[31]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[32]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[33]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[34]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[35]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[36]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[37]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[38]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[39]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[40]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[41]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[42]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[43]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[44]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[45]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[46]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[47]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[48]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[49]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[50]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[51]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[52]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[53]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[54]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/LiveWindow/drivetrain": "Gyro", - "/SmartDashboard/Alliance color": "String Chooser", - "/SmartDashboard/Auto mode": "String Chooser", - "/SmartDashboard/Controller layout": "String Chooser", - "/SmartDashboard/Demo Mode": "String Chooser", - "/SmartDashboard/Dynamic Start": "String Chooser", - "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/First collected note": "String Chooser", - "/SmartDashboard/First shooting position": "String Chooser", - "/SmartDashboard/Mech 2D": "Mechanism2d", - "/SmartDashboard/Mech2d": "Mechanism2d", - "/SmartDashboard/Playlist": "String Chooser", - "/SmartDashboard/Robot color": "String Chooser", - "/SmartDashboard/Scram Or Not": "String Chooser", - "/SmartDashboard/Second collected note": "String Chooser", - "/SmartDashboard/Second shooting position": "String Chooser", - "/SmartDashboard/SendableChooser[0]": "String Chooser", - "/SmartDashboard/SendableChooser[2]": "String Chooser", - "/SmartDashboard/Starting Position": "String Chooser", - "/SmartDashboard/Third collected note": "String Chooser", - "/SmartDashboard/Turret Sim": "Mechanism2d", - "/SmartDashboard/VisionSystemSim-SimVision/Sim Field": "Field2d", - "/SmartDashboard/VisionSystemSim-photonvision/Sim Field": "Field2d" - }, - "windows": { - "/SmartDashboard/Auto mode": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Controller layout": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Dynamic Start": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Field": { - "April Tags": { - "arrows": false, - "color": [ - 0.800000011920929, - 0.20000000298023224, - 1.0, - 255.0 - ], - "length": 0.05000000074505806, - "style": "Box/Image", - "weight": 2.5, - "width": 0.3499999940395355 - }, - "EstimatedRobot": { - "arrowColor": [ - 0.5980392098426819, - 0.5247501134872437, - 0.5247501134872437, - 55.0 - ], - "arrowWeight": 0.20000000298023224, - "weight": 0.20000000298023224 - }, - "Robot": { - "arrowColor": [ - 0.5980392098426819, - 0.5247501134872437, - 0.5247501134872437, - 255.0 - ], - "arrowWeight": 2.0, - "length": 0.8889999985694885, - "weight": 2.0, - "width": 0.8889999985694885 - }, - "Trajectory": { - "arrows": false, - "selectable": false, - "style": "Track", - "weight": 0.8889999985694885, - "width": 0.8889999985694885 - }, - "bottom": 1476, - "camera": { - "arrowSize": 0, - "arrows": false, - "color": [ - 0.30379724502563477, - 0.0, - 1.0, - 55.0 - ], - "length": 0.010000001639127731, - "width": 0.010000001639127731 - }, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "turret": { - "arrowWeight": 3.0, - "style": "Line" - }, - "width": 16.541748046875, - "window": { - "visible": true - } - }, - "/SmartDashboard/First collected note": { - "window": { - "visible": true - } - }, - "/SmartDashboard/First shooting position": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Mech2d": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Robot color": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Scram Or Not": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Second collected note": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Second shooting position": { - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "LiveWindow": { - "drivetrain": { - "open": true - } - }, - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, - "Shuffleboard": { - "Shuffleboard": { - "open": true - }, - "Simulation": { - "open": true - }, - "open": true - }, - "SmartDashboard": { - "Drive": { - "open": true - }, - "Field": { - "open": true - }, - "Starting Position": { - "open": true - }, - "open": true - }, - "[...]##v_$serversub": { - "open": true - }, - "combined": true, - "photonvision": { - "open": true, - "zed": { - "open": true - } - }, - "special": true - }, - "NetworkTables Info": { - "visible": true - } -} diff --git a/simgui.txt b/simgui.txt deleted file mode 100644 index 724b57de..00000000 --- a/simgui.txt +++ /dev/null @@ -1,398 +0,0 @@ -{ - "HALProvider": { - "Other Devices": { - "Compressor[8]": { - "header": { - "open": true - } - }, - "window": { - "visible": false - } - } - }, - "NTProvider": { - "/SmartDashboard/Starting Position": { - "window": { - "visible": true - } - }, - "types": { - "/FMSInfo": "FMSInfo", - "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", - "/LiveWindow/Ungrouped/DigitalOutput[1]": "Digital Output", - "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[12]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[13]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[14]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[15]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[16]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[17]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[18]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[19]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[20]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[21]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[22]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[23]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[24]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[25]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[26]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[27]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[28]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[29]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[30]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[31]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[32]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[33]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[34]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[35]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[36]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[37]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[38]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[39]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[40]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[41]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[42]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[43]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[44]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[45]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[46]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[47]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[48]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[49]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[50]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[51]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[52]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[53]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[54]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/LiveWindow/drivetrain": "Gyro", - "/SmartDashboard/Alliance color": "String Chooser", - "/SmartDashboard/Auto mode": "String Chooser", - "/SmartDashboard/Controller layout": "String Chooser", - "/SmartDashboard/Demo Mode": "String Chooser", - "/SmartDashboard/Dynamic Start": "String Chooser", - "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/First collected note": "String Chooser", - "/SmartDashboard/First shooting position": "String Chooser", - "/SmartDashboard/Mech 2D": "Mechanism2d", - "/SmartDashboard/Mech2d": "Mechanism2d", - "/SmartDashboard/Playlist": "String Chooser", - "/SmartDashboard/Robot color": "String Chooser", - "/SmartDashboard/Scram Or Not": "String Chooser", - "/SmartDashboard/Second collected note": "String Chooser", - "/SmartDashboard/Second shooting position": "String Chooser", - "/SmartDashboard/SendableChooser[0]": "String Chooser", - "/SmartDashboard/SendableChooser[2]": "String Chooser", - "/SmartDashboard/Starting Position": "String Chooser", - "/SmartDashboard/Third collected note": "String Chooser", - "/SmartDashboard/Turret Sim": "Mechanism2d", - "/SmartDashboard/Velocity Multiplier": "String Chooser", - "/SmartDashboard/VisionSystemSim-SimVision/Sim Field": "Field2d", - "/SmartDashboard/VisionSystemSim-photonvision/Sim Field": "Field2d" - }, - "windows": { - "/SmartDashboard/Auto mode": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Controller layout": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Dynamic Start": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Field": { - "April Tags": { - "arrows": false, - "color": [ - 0.800000011920929, - 0.20000000298023224, - 1.0, - 255.0 - ], - "length": 0.05000000074505806, - "style": "Box/Image", - "weight": 2.5, - "width": 0.3499999940395355 - }, - "AutopathFailPoints": { - "arrows": false, - "length": 0.05000000074505806, - "weight": 0.0, - "width": 0.05000000074505806 - }, - "AutopathSuccessfulPoints": { - "arrows": false, - "color": [ - 0.0, - 0.0, - 256.0, - 256.0 - ], - "length": 0.05000000074505806, - "weight": 0.0, - "width": 0.05000000074505806 - }, - "AutopathTrajectory": { - "arrows": false, - "color": [ - 0.0, - 256.0, - 0.0, - 256.0 - ], - "length": 0.009999999776482582, - "style": "Line", - "weight": 0.0, - "width": 0.009999999776482582 - }, - "AutopathTrajectory: 57035734": { - "arrows": false - }, - "AutopathWayoints": { - "width": 0.10000000149011612 - }, - "AutopathWaypoints": { - "arrows": false, - "color": [ - 0.0, - 256.0, - 0.0, - 256.0 - ], - "length": 0.10000000149011612, - "weight": 5.0, - "width": 0.10000000149011612 - }, - "EndCollisionPoints": { - "arrows": false, - "color": [ - 0.5661484599113464, - 0.0, - 0.894514799118042, - 255.0 - ], - "length": 0.10000000149011612, - "weight": 5.0, - "width": 0.10000000149011612 - }, - "EstimatedRobot": { - "arrowColor": [ - 0.5980392098426819, - 0.5247501134872437, - 0.5247501134872437, - 55.0 - ], - "arrowWeight": 0.20000000298023224, - "weight": 0.20000000298023224 - }, - "ExpandedObstacles": { - "arrows": false, - "color": [ - 256.0, - 256.0, - 0.0, - 256.0 - ], - "length": 0.009999999776482582, - "weight": 0.0, - "width": 0.009999999776482582 - }, - "Obstacles": { - "arrows": false, - "color": [ - 256.0, - 0.0, - 0.0, - 256.0 - ], - "length": 0.009999999776482582, - "weight": 0.0, - "width": 0.009999999776482582 - }, - "Robot": { - "arrowColor": [ - 0.5980392098426819, - 0.5247501134872437, - 0.5247501134872437, - 255.0 - ], - "arrowWeight": 2.0, - "length": 0.8889999985694885, - "weight": 2.0, - "width": 0.8889999985694885 - }, - "StartCollisionPoints": { - "arrows": false, - "color": [ - 0.0, - 0.07050158083438873, - 0.9282700419425964, - 255.0 - ], - "length": 0.10000000149011612, - "style": "Box/Image", - "weight": 6.0, - "width": 0.10000000149011612 - }, - "Trajectory": { - "arrows": false, - "selectable": false, - "style": "Track", - "weight": 0.8889999985694885, - "width": 0.8889999985694885 - }, - "bottom": 1476, - "camera": { - "arrowSize": 0, - "arrows": false, - "color": [ - 0.30379724502563477, - 0.0, - 1.0, - 55.0 - ], - "length": 0.010000001639127731, - "width": 0.010000001639127731 - }, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "trajectory": { - "arrows": false, - "style": "Line" - }, - "turret": { - "arrowWeight": 3.0, - "style": "Line" - }, - "width": 16.541748046875, - "window": { - "visible": true - } - }, - "/SmartDashboard/First collected note": { - "window": { - "visible": true - } - }, - "/SmartDashboard/First shooting position": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Mech2d": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Robot color": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Scram Or Not": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Second collected note": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Second shooting position": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Velocity Multiplier": { - "bottom": 1476, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "width": 16.541748046875, - "window": { - "visible": true - } - }, - "/SmartDashboard/VisionSystemSim-SimVision/Sim Field": { - "bottom": 1476, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "width": 16.541748046875, - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "LiveWindow": { - "drivetrain": { - "open": true - } - }, - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, - "Shuffleboard": { - "Shuffleboard": { - "open": true - }, - "Simulation": { - "open": true - }, - "open": true - }, - "SmartDashboard": { - "Drive": { - "open": true - }, - "Field": { - "open": true - }, - "Starting Position": { - "open": true - }, - "open": true - }, - "[...]##v_$serversub": { - "open": true - }, - "combined": true, - "photonvision": { - "open": true, - "zed": { - "open": true - } - }, - "special": true - }, - "NetworkTables Info": { - "visible": true - } -} diff --git a/src/main/java/com/team1816/lib/auto/commands/SwerveControllerCommand.java b/src/main/java/com/team1816/lib/auto/commands/SwerveControllerCommand.java deleted file mode 100644 index 8c92969e..00000000 --- a/src/main/java/com/team1816/lib/auto/commands/SwerveControllerCommand.java +++ /dev/null @@ -1,237 +0,0 @@ -package com.team1816.lib.auto.commands; - -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; - -import java.util.function.Consumer; -import java.util.function.Supplier; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -/** - * A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController - * ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a swerve drive. - * - *

This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) in an - * array. The desired wheel and module rotation velocities should be taken from those and used in - * velocity PIDs. - * - *

The robot angle controller does not follow the angle given by the trajectory but rather goes - * to the angle given in the final state of the trajectory. - * - *

This class is provided by the NewCommands VendorDep - */ -public class SwerveControllerCommand extends Command { - private final Timer m_timer = new Timer(); - private final Trajectory m_trajectory; - private final Supplier m_pose; - private final SwerveDriveKinematics m_kinematics; - private final HolonomicDriveController m_controller; - private final Consumer m_outputModuleStates; - private final Supplier m_desiredRotation; - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the provided - * trajectory. This command will not return output voltages but rather raw module states from the - * position controllers which need to be put into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path. - * This is left to the user to do since it is not appropriate for paths with nonstationary - * endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller for the robot's x position. - * @param yController The Trajectory Tracker PID controller for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller for angle for the robot. - * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each - * time step. - * @param outputModuleStates The raw output module states from the position controllers. - * @param requirements The subsystems to require. - */ - public SwerveControllerCommand( - Trajectory trajectory, - Supplier pose, - SwerveDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Supplier desiredRotation, - Consumer outputModuleStates, - Subsystem... requirements) { - this( - trajectory, - pose, - kinematics, - new HolonomicDriveController( - requireNonNullParam(xController, "xController", "SwerveControllerCommand"), - requireNonNullParam(yController, "yController", "SwerveControllerCommand"), - requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")), - desiredRotation, - outputModuleStates, - requirements); - } - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the provided - * trajectory. This command will not return output voltages but rather raw module states from the - * position controllers which need to be put into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path. - * This is left to the user since it is not appropriate for paths with nonstationary endstates. - * - *

Note 2: The final rotation of the robot will be set to the rotation of the final pose in the - * trajectory. The robot will not follow the rotations from the poses at each timestep. If - * alternate rotation behavior is desired, the other constructor with a supplier for rotation - * should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param xController The Trajectory Tracker PID controller for the robot's x position. - * @param yController The Trajectory Tracker PID controller for the robot's y position. - * @param thetaController The Trajectory Tracker PID controller for angle for the robot. - * @param outputModuleStates The raw output module states from the position controllers. - * @param requirements The subsystems to require. - */ - public SwerveControllerCommand( - Trajectory trajectory, - Supplier pose, - SwerveDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Consumer outputModuleStates, - Subsystem... requirements) { - this( - trajectory, - pose, - kinematics, - xController, - yController, - thetaController, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), - outputModuleStates, - requirements); - } - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the provided - * trajectory. This command will not return output voltages but rather raw module states from the - * position controllers which need to be put into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path- - * this is left to the user, since it is not appropriate for paths with nonstationary endstates. - * - *

Note 2: The final rotation of the robot will be set to the rotation of the final pose in the - * trajectory. The robot will not follow the rotations from the poses at each timestep. If - * alternate rotation behavior is desired, the other constructor with a supplier for rotation - * should be used. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param outputModuleStates The raw output module states from the position controllers. - * @param requirements The subsystems to require. - */ - public SwerveControllerCommand( - Trajectory trajectory, - Supplier pose, - SwerveDriveKinematics kinematics, - HolonomicDriveController controller, - Consumer outputModuleStates, - Subsystem... requirements) { - this( - trajectory, - pose, - kinematics, - controller, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), - outputModuleStates, - requirements); - } - - /** - * Constructs a new SwerveControllerCommand that when executed will follow the provided - * trajectory. This command will not return output voltages but rather raw module states from the - * position controllers which need to be put into a velocity PID. - * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path- - * this is left to the user, since it is not appropriate for paths with nonstationary endstates. - * - * @param trajectory The trajectory to follow. - * @param pose A function that supplies the robot pose - use one of the odometry classes to - * provide this. - * @param kinematics The kinematics for the robot drivetrain. - * @param controller The HolonomicDriveController for the drivetrain. - * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each - * time step. - * @param outputModuleStates The raw output module states from the position controllers. - * @param requirements The subsystems to require. - */ - @SuppressWarnings("this-escape") - public SwerveControllerCommand( - Trajectory trajectory, - Supplier pose, - SwerveDriveKinematics kinematics, - HolonomicDriveController controller, - Supplier desiredRotation, - Consumer outputModuleStates, - Subsystem... requirements) { - m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand"); - m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand"); - m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand"); - m_controller = requireNonNullParam(controller, "controller", "SwerveControllerCommand"); - - m_desiredRotation = - requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand"); - - m_outputModuleStates = - requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand"); - - addRequirements(requirements); - } - - @Override - public void initialize() { - m_timer.restart(); - } - - @Override - public void execute() { - double curTime = m_timer.get(); - var desiredState = m_trajectory.sample(curTime); - - var targetChassisSpeeds = - m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get()); - var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds); - - m_outputModuleStates.accept(targetModuleStates); - } - - @Override - public void end(boolean interrupted) { - m_timer.stop(); - } - - @Override - public boolean isFinished() { - return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); - } -} diff --git a/src/main/java/com/team1816/lib/events/EventAggregator.java b/src/main/java/com/team1816/lib/events/EventAggregator.java deleted file mode 100644 index 805c1a38..00000000 --- a/src/main/java/com/team1816/lib/events/EventAggregator.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.team1816.lib.events; - -import com.team1816.lib.util.logUtil.GreenLogger; - -import java.util.HashMap; - -/** - * A class the aggregates events into a registry of events that can then be called upon to trigger an event - * - * @see IEventAggregator - */ -public class EventAggregator implements IEventAggregator { - - private final HashMap _events = new HashMap<>(); - - @Override - public TEventType GetEvent(Class type) { - if (_events.containsKey(type)) return type.cast(_events.get(type)); - TEventType newEvent; - try { - newEvent = type.getDeclaredConstructor().newInstance(); - } catch (Exception exp) { - GreenLogger.log(exp.getMessage()); - return null; - } - _events.put(type, newEvent); - return newEvent; - } -} diff --git a/src/main/java/com/team1816/lib/events/EventBase.java b/src/main/java/com/team1816/lib/events/EventBase.java deleted file mode 100644 index 97b5d57e..00000000 --- a/src/main/java/com/team1816/lib/events/EventBase.java +++ /dev/null @@ -1,41 +0,0 @@ -package com.team1816.lib.events; - -import java.util.ArrayList; - -/** - * The base event that all events extend from - * - * @see PubSubRunnable - * @see PubSubConsumer - */ -public abstract class EventBase { - - /** - * Event subscriptions - */ - protected ArrayList Subscriptions = new ArrayList<>(); - - /** - * Adds subscription - * - * @param subscription to be invoked when {@link #InternalPublish(Object)} is called - */ - protected void InternalSubscribe(IEventSubscription subscription) { - Subscriptions.add(subscription); - } - - /** - * Publishes date to subscribers - * - * @param parameter the value to pass to the {@link #Subscriptions} - */ - protected void InternalPublish(Object parameter) { - for (var sub : Subscriptions) { - if (sub.IsRunnable()) { - sub.GetRunnable().run(); - } else { - sub.GetConsumer().accept(parameter); - } - } - } -} diff --git a/src/main/java/com/team1816/lib/events/EventSubscription.java b/src/main/java/com/team1816/lib/events/EventSubscription.java deleted file mode 100644 index c98b003f..00000000 --- a/src/main/java/com/team1816/lib/events/EventSubscription.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.team1816.lib.events; - -import java.util.function.Consumer; - -/** - * Implementation of an event that sends data sed to store the Consumer (action) to be called - * - * @param this is the type of data that publish will fire - */ -public class EventSubscription implements IEventSubscription { - - private Consumer _consumerAction = null; - private Runnable _runnableAction = null; - - public EventSubscription(Consumer action) { - _consumerAction = action; - } - - public EventSubscription(Runnable action) { - _runnableAction = action; - } - - @Override - public Consumer GetConsumer() { - return _consumerAction; - } - - @Override - public Runnable GetRunnable() { - return _runnableAction; - } - - public boolean IsRunnable() { - return _consumerAction == null; - } -} diff --git a/src/main/java/com/team1816/lib/events/IEventAggregator.java b/src/main/java/com/team1816/lib/events/IEventAggregator.java deleted file mode 100644 index 75068717..00000000 --- a/src/main/java/com/team1816/lib/events/IEventAggregator.java +++ /dev/null @@ -1,11 +0,0 @@ -package com.team1816.lib.events; - -import java.lang.reflect.InvocationTargetException; - -/** - * Interface for event aggregator - */ -public interface IEventAggregator { - TEventType GetEvent(Class type) - throws NoSuchMethodException, InvocationTargetException, InstantiationException, IllegalAccessException; -} diff --git a/src/main/java/com/team1816/lib/events/IEventSubscription.java b/src/main/java/com/team1816/lib/events/IEventSubscription.java deleted file mode 100644 index 69cf61c5..00000000 --- a/src/main/java/com/team1816/lib/events/IEventSubscription.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.team1816.lib.events; - -import java.util.function.Consumer; - -/** - * Interface for event subscriptions - */ -public interface IEventSubscription { - Consumer GetConsumer(); - - Runnable GetRunnable(); - - boolean IsRunnable(); -} diff --git a/src/main/java/com/team1816/lib/events/PubSubConsumer.java b/src/main/java/com/team1816/lib/events/PubSubConsumer.java deleted file mode 100644 index cc14d37a..00000000 --- a/src/main/java/com/team1816/lib/events/PubSubConsumer.java +++ /dev/null @@ -1,28 +0,0 @@ -package com.team1816.lib.events; - -import java.util.function.Consumer; - -/** - * Base class that all events that send data extend from - * - * @param T is the type of data that is sent - * @see EventBase - */ -public class PubSubConsumer extends EventBase { - - /** - * @param action this is the consumer ( the method that listens to the event) - */ - public void Subscribe(Consumer action) { - InternalSubscribe(new EventSubscription<>(action)); - } - - /** - * Sends an event to the registered consumers - * - * @param parameter this is the data to publish to the subscribers - */ - public void Publish(T parameter) { - InternalPublish(parameter); - } -} diff --git a/src/main/java/com/team1816/lib/events/PubSubRunnable.java b/src/main/java/com/team1816/lib/events/PubSubRunnable.java deleted file mode 100644 index 207bc28a..00000000 --- a/src/main/java/com/team1816/lib/events/PubSubRunnable.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.team1816.lib.events; - -/** - * PubSub event that does not pass any data - * - * @see EventBase - */ -public class PubSubRunnable extends EventBase { - - public void Subscribe(Runnable action) { - InternalSubscribe(new EventSubscription<>(action)); - } - - public void Publish() { - InternalPublish(null); - } -} diff --git a/src/main/java/com/team1816/lib/hardware/components/motor/configurations/PeriodicStatusFrame.java b/src/main/java/com/team1816/lib/hardware/components/motor/configurations/PeriodicStatusFrame.java deleted file mode 100644 index ee842377..00000000 --- a/src/main/java/com/team1816/lib/hardware/components/motor/configurations/PeriodicStatusFrame.java +++ /dev/null @@ -1,41 +0,0 @@ -package com.team1816.lib.hardware.components.motor.configurations; - -/** - * Enum containing all status frame types - * - * @see com.ctre.phoenix.motorcontrol.StatusFrameEnhanced - * @see com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame - */ -public enum PeriodicStatusFrame { - // SparkMax Exclusive - STATUS_0, - STATUS_5, // No, we do not know why CTRE doesn't have a status 5. - - // Talon & SparkMax - STATUS_3, // Talon: Quadrature sensor - - // Universal - STATUS_1, // CTRE: General Status - STATUS_2, // CTRE: Feedback for selected sensor on primary PID[0]. - STATUS_4, // CTRE: Analog sensor, motor controller temperature, and voltage at input leads - STATUS_6, // CTRE: Miscellaneous signals - - // Talon Exclusive - STATUS_8_PULSEWIDTH, - STATUS_11_UARTGADGETEER, - STATUS_21_FEEDBACKINTEGRATED, - STATUS_BRUSHLESS_CURRENT, - - // Victor Exclusive - STATUS_17_TARGETS1, - - // CTRE universal - STATUS_7_COMMSTATUS, - STATUS_9_MOTPROFBUFFER, - STATUS_10_TARGETS, - STATUS_12_FEEDBACK1, - STATUS_13_BASE_PIDF0, - STATUS_14_TURN_PIDF1, - STATUS_15_FIRMWAREAPISTATUS, - -} diff --git a/src/main/java/com/team1816/lib/util/GreenDriveHelper.java b/src/main/java/com/team1816/lib/util/GreenDriveHelper.java deleted file mode 100644 index 80279b75..00000000 --- a/src/main/java/com/team1816/lib/util/GreenDriveHelper.java +++ /dev/null @@ -1,167 +0,0 @@ -package com.team1816.lib.util; - -import com.team1816.lib.util.team254.DriveSignal; - -/** - * Helper class to implement "Cheesy Drive". "Cheesy Drive" simply means that the "turning" stick controls the curvature - * of the robot's path rather than its rate of heading change. This helps make the robot more controllable at high - * speeds. Also handles the robot's quick turn functionality - "quick turn" overrides constant-curvature turning for - * turn-in-place maneuvers. - *

- * This is based off of the team 254 code with 2 changes. Added auto quick turn when not commanding throttle - * and fixed the quickTurn code to not exceed += 1.0 - * - * @see com.team1816.lib.util.team254.DriveHelper - * @see com.team1816.lib.util.team254.CheesyDriveHelper - */ -public class GreenDriveHelper { - - private static final double kThrottleDeadband = 0.02; - private static final double kWheelDeadband = 0.02; - - // These factor determine how fast the wheel traverses the "non linear" sine curve. - private static final double kHighWheelNonLinearity = 0.65; - private static final double kLowWheelNonLinearity = 0.5; - - private static final double kHighNegInertiaScalar = 4.0; - - private static final double kLowNegInertiaThreshold = 0.65; - private static final double kLowNegInertiaTurnScalar = 3.5; - private static final double kLowNegInertiaCloseScalar = 4.0; - private static final double kLowNegInertiaFarScalar = 5.0; - - private static final double kHighSensitivity = 0.6; - private static final double kLowSensitiity = 1.0; - - private static final double kQuickStopDeadband = 0.5; - private static final double kQuickStopWeight = 0.1; - private static final double kQuickStopScalar = 5.0; - - private double mOldWheel = 0.0; - private double mQuickStopAccumlator = 0.0; - private double mNegInertiaAccumlator = 0.0; - - public DriveSignal cheesyDrive( - double throttle, - double wheel, - boolean isQuickTurn, - boolean isHighGear - ) { - wheel = handleDeadband(wheel, kWheelDeadband); - throttle = handleDeadband(throttle, kThrottleDeadband); - - // Auto quickTurn - isQuickTurn = isQuickTurn || Math.abs(throttle) < kThrottleDeadband; - - double negInertia = wheel - mOldWheel; - mOldWheel = wheel; - - double wheelNonLinearity; - if (isHighGear) { - wheelNonLinearity = kHighWheelNonLinearity; - final double denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity); - // Apply a sin function that's scaled to make it feel better. - wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator; - wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator; - } else { - wheelNonLinearity = kLowWheelNonLinearity; - final double denominator = Math.sin(Math.PI / 2.0 * wheelNonLinearity); - // Apply a sin function that's scaled to make it feel better. - wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator; - wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator; - wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / denominator; - } - - double leftPwm, rightPwm, overPower; - double sensitivity; - - double angularPower; - double linearPower; - - // Negative inertia! - double negInertiaScalar; - if (isHighGear) { - negInertiaScalar = kHighNegInertiaScalar; - sensitivity = kHighSensitivity; - } else { - if (wheel * negInertia > 0) { - // If we are moving away from 0.0, aka, trying to get more wheel. - negInertiaScalar = kLowNegInertiaTurnScalar; - } else { - // Otherwise, we are attempting to go back to 0.0. - if (Math.abs(wheel) > kLowNegInertiaThreshold) { - negInertiaScalar = kLowNegInertiaFarScalar; - } else { - negInertiaScalar = kLowNegInertiaCloseScalar; - } - } - sensitivity = kLowSensitiity; - } - double negInertiaPower = negInertia * negInertiaScalar; - mNegInertiaAccumlator += negInertiaPower; - - wheel = wheel + mNegInertiaAccumlator; - if (mNegInertiaAccumlator > 1) { - mNegInertiaAccumlator -= 1; - } else if (mNegInertiaAccumlator < -1) { - mNegInertiaAccumlator += 1; - } else { - mNegInertiaAccumlator = 0; - } - linearPower = throttle; - - // Quickturn! - if (isQuickTurn) { - if (Math.abs(linearPower) < kQuickStopDeadband) { - double alpha = kQuickStopWeight; - mQuickStopAccumlator = - (1 - alpha) * - mQuickStopAccumlator + - alpha * - Util.limit(wheel, 1.0) * - kQuickStopScalar; - } - overPower = 1.0; - angularPower = wheel; - } else { - overPower = 0.0; - angularPower = - Math.abs(throttle) * wheel * sensitivity - mQuickStopAccumlator; - if (mQuickStopAccumlator > 1) { - mQuickStopAccumlator -= 1; - } else if (mQuickStopAccumlator < -1) { - mQuickStopAccumlator += 1; - } else { - mQuickStopAccumlator = 0.0; - } - } - - rightPwm = leftPwm = linearPower; - leftPwm += angularPower; - rightPwm -= angularPower; - - double ratio; - if (leftPwm > 1.0) { - ratio = 1.0 / leftPwm; - leftPwm = 1.0; - rightPwm = rightPwm * ratio; - } else if (rightPwm > 1.0) { - ratio = 1.0 / rightPwm; - rightPwm = 1.0; - leftPwm = leftPwm * ratio; - } else if (leftPwm < -1.0) { - ratio = -1.0 / leftPwm; - leftPwm = -1.0; - rightPwm = rightPwm * ratio; - } else if (rightPwm < -1.0) { - ratio = -1.0 / rightPwm; - rightPwm = -1.0; - leftPwm = leftPwm * ratio; - } - return new DriveSignal(leftPwm, rightPwm); - } - - public double handleDeadband(double val, double deadband) { - return (Math.abs(val) > Math.abs(deadband)) ? val : 0.0; - } -} diff --git a/src/main/java/com/team1816/lib/util/MotorUtil.java b/src/main/java/com/team1816/lib/util/MotorUtil.java deleted file mode 100644 index 84883919..00000000 --- a/src/main/java/com/team1816/lib/util/MotorUtil.java +++ /dev/null @@ -1,44 +0,0 @@ -package com.team1816.lib.util; - -import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.spark.SparkMax; -import com.team1816.lib.hardware.components.motor.IGreenMotor; -import edu.wpi.first.wpilibj.DriverStation; - -/** - * A simple motor utility for retrieving properties - */ -public class MotorUtil { - - /** - * Checks the specified error code for issues - * - * @param errorCode error code - * @param message message to print if error happens - */ - public static void checkError(ErrorCode errorCode, String message) { - if (errorCode != ErrorCode.OK) { - DriverStation.reportError(message + errorCode, false); - } - } - - /** - * Returns the supplied current to the motor - * - * @param motor IGreenMotor - * @return supplied current - */ - public static double getSupplyCurrent(IGreenMotor motor) { - // If only CTRE had these methods in the interface... - if (motor instanceof TalonFX) { - return ((TalonFX) motor).getSupplyCurrent().getValueAsDouble(); - } else if (motor instanceof TalonSRX) { - return ((TalonSRX) motor).getSupplyCurrent(); - } else if (motor instanceof SparkMax) { - return ((SparkMax) motor).getOutputCurrent(); - } - return 0; - } -} diff --git a/src/main/java/com/team1816/lib/util/VectorUtil.java b/src/main/java/com/team1816/lib/util/VectorUtil.java deleted file mode 100644 index ea3eee58..00000000 --- a/src/main/java/com/team1816/lib/util/VectorUtil.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.team1816.lib.util; - -import edu.wpi.first.math.geometry.Translation2d; - -/** - * A simple vector utility for basic products - */ -public class VectorUtil { - - /** - * Returns the angle between two translations - * - * @param a Translation2d - * @param b Translation2d - * @return angle (radians) - */ - public static double getAngleBetween(Translation2d a, Translation2d b) { - double dot = (a.getNorm() * b.getNorm() == 0) - ? 0 - : Math.acos( - (a.getX() * b.getX() + a.getY() * b.getY()) / (a.getNorm() * b.getNorm()) - ); - double cross = crossProduct(a, b); - if (cross > 0) { - dot *= -1; - } - return dot; - } - - /** - * Simple cross product of two translations - * - * @param a Translation2d - * @param b Translation2d - * @return product - */ - private static double crossProduct(Translation2d a, Translation2d b) { - double[] vect_A = {a.getX(), a.getY(), 0}; - double[] vect_B = {b.getX(), b.getY(), 0}; - return vect_A[0] * vect_B[1] - vect_A[1] * vect_B[0]; - } -} diff --git a/src/main/java/com/team1816/lib/util/simUtil/SingleJointedElevatorArmSim.java b/src/main/java/com/team1816/lib/util/simUtil/SingleJointedElevatorArmSim.java deleted file mode 100644 index 888d1acc..00000000 --- a/src/main/java/com/team1816/lib/util/simUtil/SingleJointedElevatorArmSim.java +++ /dev/null @@ -1,310 +0,0 @@ -//// Copyright (c) FIRST and other WPILib contributors. -//// Open Source Software; you can modify and/or share it under the terms of -//// the WPILib BSD license file in the root directory of this project. -// -//package com.team1816.lib.util.simUtil; -// -//import edu.wpi.first.math.Matrix; -//import edu.wpi.first.math.VecBuilder; -//import edu.wpi.first.math.numbers.N1; -//import edu.wpi.first.math.numbers.N2; -//import edu.wpi.first.math.system.LinearSystem; -//import edu.wpi.first.math.system.NumericalIntegration; -//import edu.wpi.first.math.system.plant.DCMotor; -//import edu.wpi.first.math.system.plant.LinearSystemId; -//import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -// -///** -// * Represents a simulated single jointed arm mechanism. -// */ -//public class SingleJointedElevatorArmSim extends LinearSystemSim { -// // The gearbox for the arm. -// private final DCMotor m_gearbox; -// -// // The gearing between the motors and the output. -// private final double m_gearing; -// -// // The length of the arm. -// private double m_armLenMeters; -// -// // The minimum angle that the arm is capable of. -// private final double m_minAngle; -// -// // The maximum angle that the arm is capable of. -// private final double m_maxAngle; -// -// // Whether the simulator should simulate gravity. -// private final boolean m_simulateGravity; -// -// /** -// * Creates a simulated arm mechanism. -// * -// * @param plant The linear system that represents the arm. -// * @param gearbox The type of and number of motors in the arm gearbox. -// * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). -// * @param armLengthMeters The length of the arm. -// * @param minAngleRads The minimum angle that the arm is capable of. -// * @param maxAngleRads The maximum angle that the arm is capable of. -// * @param simulateGravity Whether gravity should be simulated or not. -// */ -// public SingleJointedElevatorArmSim( -// LinearSystem plant, -// DCMotor gearbox, -// double gearing, -// double armLengthMeters, -// double minAngleRads, -// double maxAngleRads, -// boolean simulateGravity) { -// this( -// plant, -// gearbox, -// gearing, -// armLengthMeters, -// minAngleRads, -// maxAngleRads, -// simulateGravity, -// null); -// } -// -// /** -// * Creates a simulated arm mechanism. -// * -// * @param plant The linear system that represents the arm. -// * @param gearbox The type of and number of motors in the arm gearbox. -// * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). -// * @param armLengthMeters The length of the arm. -// * @param minAngleRads The minimum angle that the arm is capable of. -// * @param maxAngleRads The maximum angle that the arm is capable of. -// * @param simulateGravity Whether gravity should be simulated or not. -// * @param measurementStdDevs The standard deviations of the measurements. -// */ -// public SingleJointedElevatorArmSim( -// LinearSystem plant, -// DCMotor gearbox, -// double gearing, -// double armLengthMeters, -// double minAngleRads, -// double maxAngleRads, -// boolean simulateGravity, -// Matrix measurementStdDevs) { -// super(plant, measurementStdDevs); -// m_gearbox = gearbox; -// m_gearing = gearing; -// m_armLenMeters = armLengthMeters; -// m_minAngle = minAngleRads; -// m_maxAngle = maxAngleRads; -// m_simulateGravity = simulateGravity; -// } -// -// /** -// * Creates a simulated arm mechanism. -// * -// * @param gearbox The type of and number of motors in the arm gearbox. -// * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). -// * @param jKgMetersSquared The moment of inertia of the arm, can be calculated from CAD software. -// * @param armLengthMeters The length of the arm. -// * @param minAngleRads The minimum angle that the arm is capable of. -// * @param maxAngleRads The maximum angle that the arm is capable of. -// * @param simulateGravity Whether gravity should be simulated or not. -// */ -// public SingleJointedElevatorArmSim( -// DCMotor gearbox, -// double gearing, -// double jKgMetersSquared, -// double armLengthMeters, -// double minAngleRads, -// double maxAngleRads, -// boolean simulateGravity) { -// this( -// gearbox, -// gearing, -// jKgMetersSquared, -// armLengthMeters, -// minAngleRads, -// maxAngleRads, -// simulateGravity, -// null); -// } -// -// /** -// * Creates a simulated arm mechanism. -// * -// * @param gearbox The type of and number of motors in the arm gearbox. -// * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). -// * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software. -// * @param armLengthMeters The length of the arm. -// * @param minAngleRads The minimum angle that the arm is capable of. -// * @param maxAngleRads The maximum angle that the arm is capable of. -// * @param simulateGravity Whether gravity should be simulated or not. -// * @param measurementStdDevs The standard deviations of the measurements. -// */ -// public SingleJointedElevatorArmSim( -// DCMotor gearbox, -// double gearing, -// double jKgMetersSquared, -// double armLengthMeters, -// double minAngleRads, -// double maxAngleRads, -// boolean simulateGravity, -// Matrix measurementStdDevs) { -// super( -// LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), -// measurementStdDevs); -// m_gearbox = gearbox; -// m_gearing = gearing; -// m_armLenMeters = armLengthMeters; -// m_minAngle = minAngleRads; -// m_maxAngle = maxAngleRads; -// m_simulateGravity = simulateGravity; -// } -// -// /** -// * Returns whether the arm would hit the lower limit. -// * -// * @param currentAngleRads The current arm height. -// * @return Whether the arm would hit the lower limit. -// */ -// public boolean wouldHitLowerLimit(double currentAngleRads) { -// return currentAngleRads <= this.m_minAngle; -// } -// -// /** -// * Returns whether the arm would hit the upper limit. -// * -// * @param currentAngleRads The current arm height. -// * @return Whether the arm would hit the upper limit. -// */ -// public boolean wouldHitUpperLimit(double currentAngleRads) { -// return currentAngleRads >= this.m_maxAngle; -// } -// -// /** -// * Returns whether the arm has hit the lower limit. -// * -// * @return Whether the arm has hit the lower limit. -// */ -// public boolean hasHitLowerLimit() { -// return wouldHitLowerLimit(getAngleRads()); -// } -// -// /** -// * Returns whether the arm has hit the upper limit. -// * -// * @return Whether the arm has hit the upper limit. -// */ -// public boolean hasHitUpperLimit() { -// return wouldHitUpperLimit(getAngleRads()); -// } -// -// /** -// * Returns the current arm angle. -// * -// * @return The current arm angle. -// */ -// public double getAngleRads() { -// return m_y.get(0, 0); -// } -// -// /** -// * Returns the current arm velocity. -// * -// * @return The current arm velocity. -// */ -// public double getVelocityRadPerSec() { -// return m_x.get(1, 0); -// } -// -// /** -// * Returns the arm current draw. -// * -// * @return The aram current draw. -// */ -// @Override -// public double getCurrentDrawAmps() { -// // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is -// // spinning 10x faster than the output -// var motorVelocity = m_x.get(1, 0) * m_gearing; -// return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); -// } -// -// /** -// * Sets the input voltage for the arm. -// * -// * @param volts The input voltage. -// */ -// public void setInputVoltage(double volts) { -// setInput(volts); -// } -// -// /** -// * Calculates a rough estimate of the moment of inertia of an arm given its length and mass. -// * -// * @param lengthMeters The length of the arm. -// * @param massKg The mass of the arm. -// * @return The calculated moment of inertia. -// */ -// public static double estimateMOI(double lengthMeters, double massKg) { -// return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters; -// } -// -// public void setM_armLenMeters(double m_armLenMeters) { -// this.m_armLenMeters = m_armLenMeters; -// } -// -// /** -// * Updates the state of the arm. -// * -// * @param currentXhat The current state estimate. -// * @param u The system inputs (voltage). -// * @param dtSeconds The time difference between controller updates. -// */ -// @Override -// protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { -// // The torque on the arm is given by τ = F⋅r, where F is the force applied by -// // gravity and r the distance from pivot to center of mass. Recall from -// // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is -// // torque on the arm, J is the mass-moment of inertia about the pivot axis, -// // and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J -// // -// // We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal: -// // -// // α = (m⋅g⋅cos(θ))⋅r/J -// // -// // Multiply RHS by cos(θ) to account for the arm angle. Further, we know the -// // arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as a -// // rod rotating about it's end, where L is the overall rod length. The mass -// // distribution is assumed to be uniform. Substitute r=L/2 to find: -// // -// // α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²) -// // α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²) -// // α = 3/2⋅g⋅cos(θ)/L -// // -// // This acceleration is next added to the linear system dynamics ẋ=Ax+Bu -// // -// // f(x, u) = Ax + Bu + [0 α]ᵀ -// // f(x, u) = Ax + Bu + [0 3/2⋅g⋅cos(θ)/L]ᵀ -// -// Matrix updatedXhat = -// NumericalIntegration.rkdp( -// (Matrix x, Matrix _u) -> { -// Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); -// if (m_simulateGravity) { -// double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters; -// xdot = xdot.plus(VecBuilder.fill(0, alphaGrav)); -// } -// return xdot; -// }, -// currentXhat, -// u, -// dtSeconds); -// -// // We check for collision after updating xhat -// if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { -// return VecBuilder.fill(m_minAngle, 0); -// } -// if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { -// return VecBuilder.fill(m_maxAngle, 0); -// } -// return updatedXhat; -// } -//} diff --git a/src/main/java/com/team1816/lib/util/visionUtil/GreenPhotonCamera.java b/src/main/java/com/team1816/lib/util/visionUtil/GreenPhotonCamera.java deleted file mode 100644 index 6e9aa5eb..00000000 --- a/src/main/java/com/team1816/lib/util/visionUtil/GreenPhotonCamera.java +++ /dev/null @@ -1,233 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2022 PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package com.team1816.lib.util.visionUtil; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import org.photonvision.PhotonVersion; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; - -/** - * Represents a camera that is connected to PhotonVision. - */ -public class GreenPhotonCamera { - - protected final NetworkTable rootTable; - final NetworkTableEntry rawBytesEntry; - final NetworkTableEntry driverModeEntry; - final NetworkTableEntry inputSaveImgEntry; - final NetworkTableEntry outputSaveImgEntry; - final NetworkTableEntry pipelineIndexEntry; - final NetworkTableEntry ledModeEntry; - final NetworkTableEntry versionEntry; - - private final String path; - - private static boolean VERSION_CHECK_ENABLED = true; - - public static void setVersionCheckEnabled(boolean enabled) { - VERSION_CHECK_ENABLED = enabled; - } - - Packet packet = new Packet(1); - - /** - * Constructs a PhotonCamera from a root table. - * - * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in - * simulation, but should *usually* be the default NTInstance from - * NetworkTableInstance::getDefault - * @param cameraName The name of the camera, as seen in the UI. - */ - public GreenPhotonCamera(NetworkTableInstance instance, String cameraName) { - var mainTable = instance.getTable("photonvision"); - this.rootTable = mainTable.getSubTable(cameraName); - path = rootTable.getPath(); - rawBytesEntry = rootTable.getEntry("rawBytes"); - driverModeEntry = rootTable.getEntry("driverMode"); - inputSaveImgEntry = rootTable.getEntry("inputSaveImgCmd"); - outputSaveImgEntry = rootTable.getEntry("outputSaveImgCmd"); - pipelineIndexEntry = rootTable.getEntry("pipelineIndex"); - ledModeEntry = mainTable.getEntry("ledMode"); - versionEntry = mainTable.getEntry("version"); - } - - /** - * Constructs a PhotonCamera from the name of the camera. - * - * @param cameraName The nickname of the camera (found in the PhotonVision UI). - */ - public GreenPhotonCamera(String cameraName) { - this(NetworkTableInstance.getDefault(), cameraName); - } - - /** - * Returns the latest pipeline result. - * - * @return The latest pipeline result. - */ - public PhotonPipelineResult getLatestResult() { - verifyVersion(); - - // Clear the packet. - packet.clear(); - - // Create latest result. - var ret = new PhotonPipelineResult(); - - // Populate packet and create result. - packet.setData(rawBytesEntry.getRaw(new byte[]{})); - if (packet.getSize() < 1) return ret; -// ret.createFromPacket(packet); FIXME during sim vision rework (this class is prob getting deleted anyways) - - // Return result. - return ret; - } - - /** - * Returns whether the camera is in driver mode. - * - * @return Whether the camera is in driver mode. - */ - public boolean getDriverMode() { - return driverModeEntry.getBoolean(false); - } - - /** - * Toggles driver mode. - * - * @param driverMode Whether to set driver mode. - */ - public void setDriverMode(boolean driverMode) { - driverModeEntry.setBoolean(driverMode); - } - - /** - * Request the camera to save a new image file from the input camera stream with overlays. Images - * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk - * space and eventually cause the system to stop working. Clear out images in - * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. - */ - public void takeInputSnapshot() { - inputSaveImgEntry.setBoolean(true); - } - - /** - * Request the camera to save a new image file from the output stream with overlays. Images take - * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space - * and eventually cause the system to stop working. Clear out images in - * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. - */ - public void takeOutputSnapshot() { - outputSaveImgEntry.setBoolean(true); - } - - /** - * Returns the active pipeline index. - * - * @return The active pipeline index. - */ - public int getPipelineIndex() { - return pipelineIndexEntry.getNumber(0).intValue(); - } - - /** - * Allows the user to select the active pipeline index. - * - * @param index The active pipeline index. - */ - public void setPipelineIndex(int index) { - pipelineIndexEntry.setNumber(index); - } - - /** - * Returns the current LED mode. - * - * @return The current LED mode. - */ - public VisionLEDMode getLEDMode() { - int value = ledModeEntry.getNumber(-1).intValue(); - switch (value) { - case 0: - return VisionLEDMode.kOff; - case 1: - return VisionLEDMode.kOn; - case 2: - return VisionLEDMode.kBlink; - case -1: - default: - return VisionLEDMode.kDefault; - } - } - - /** - * Sets the LED mode. - * - * @param led The mode to set to. - */ - public void setLED(VisionLEDMode led) { - ledModeEntry.setNumber(led.value); - } - - /** - * Returns whether the latest target result has targets. - * - *

This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should be used instead. - * - * @return Whether the latest target result has targets. - * @deprecated This method should be replaced with {@link PhotonPipelineResult#hasTargets()} - */ - @Deprecated - public boolean hasTargets() { - return getLatestResult().hasTargets(); - } - - private void verifyVersion() { - if (!VERSION_CHECK_ENABLED) return; - - String versionString = versionEntry.getString(""); - if (versionString.equals("")) { - DriverStation.reportError( - "PhotonVision coprocessor at path " + - path + - " not found on NetworkTables!", - true - ); - } else if (!PhotonVersion.versionMatches(versionString)) { - DriverStation.reportError( - "Photon version " + - PhotonVersion.versionString + - " does not match coprocessor version " + - versionString + - "!", - true - ); - } - } -} diff --git a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimPhotonCamera.java b/src/main/java/com/team1816/lib/util/visionUtil/GreenSimPhotonCamera.java deleted file mode 100644 index 5066eae7..00000000 --- a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimPhotonCamera.java +++ /dev/null @@ -1,212 +0,0 @@ -///* -// * MIT License -// * -// * Copyright (c) 2022 PhotonVision -// * -// * Permission is hereby granted, free of charge, to any person obtaining a copy -// * of this software and associated documentation files (the "Software"), to deal -// * in the Software without restriction, including without limitation the rights -// * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// * copies of the Software, and to permit persons to whom the Software is -// * furnished to do so, subject to the following conditions: -// * -// * The above copyright notice and this permission notice shall be included in all -// * copies or substantial portions of the Software. -// * -// * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// * SOFTWARE. -// */ -// -//package com.team1816.lib.util.visionUtil; -// -//import edu.wpi.first.networktables.NetworkTableEntry; -//import edu.wpi.first.networktables.NetworkTableInstance; -//import org.photonvision.PhotonTargetSortMode; -//import org.photonvision.PhotonVersion; -//import org.photonvision.common.dataflow.structures.Packet; -//import org.photonvision.targeting.PhotonPipelineResult; -//import org.photonvision.targeting.PhotonTrackedTarget; -// -//import java.util.ArrayList; -//import java.util.Arrays; -//import java.util.List; -// -//@SuppressWarnings("unused") -//public class GreenSimPhotonCamera extends GreenPhotonCamera { -// -// public List targetList = new ArrayList(); -// -// public NetworkTableEntry targetListEntry1; //pain -// public NetworkTableEntry targetListEntry2; -// public NetworkTableEntry targetListEntry3; -// public NetworkTableEntry targetListEntry4; -// public NetworkTableEntry targetListEntry5; -// public NetworkTableEntry targetListEntry6; -// public NetworkTableEntry targetListEntry7; -// public NetworkTableEntry targetListEntry8; -// public NetworkTableEntry targetListEntry9; -// -// private final NetworkTableEntry versionEntry; -// private final NetworkTableEntry latencyMillisEntry; -// private final NetworkTableEntry hasTargetEntry; -// -// /** -// * Constructs a Simulated PhotonCamera from a root table. -// * -// * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in -// * simulation, but should *usually* be the default NTInstance from -// * NetworkTableInstance::getDefault -// * @param cameraName The name of the camera, as seen in the UI. -// */ -// public GreenSimPhotonCamera(NetworkTableInstance instance, String cameraName) { -// super(instance, cameraName); -// latencyMillisEntry = rootTable.getEntry("latencyMillis"); -// hasTargetEntry = rootTable.getEntry("hasTargetEntry"); -// -// targetListEntry1 = rootTable.getEntry("targetListEntry1"); -// targetListEntry2 = rootTable.getEntry("targetListEntry2"); -// targetListEntry3 = rootTable.getEntry("targetListEntry3"); -// targetListEntry4 = rootTable.getEntry("targetListEntry4"); -// targetListEntry5 = rootTable.getEntry("targetListEntry5"); -// targetListEntry6 = rootTable.getEntry("targetListEntry6"); -// targetListEntry7 = rootTable.getEntry("targetListEntry7"); -// targetListEntry8 = rootTable.getEntry("targetListEntry8"); -// targetListEntry9 = rootTable.getEntry("targetListEntry9"); -// -// targetList.add(targetListEntry1); -// targetList.add(targetListEntry2); -// targetList.add(targetListEntry3); -// targetList.add(targetListEntry4); -// targetList.add(targetListEntry5); -// targetList.add(targetListEntry6); -// targetList.add(targetListEntry7); -// targetList.add(targetListEntry8); -// targetList.add(targetListEntry9); -// -// versionEntry = rootTable.getEntry("versionEntry"); -// // Sets the version string so that it will always match the current version -// versionEntry.setString(PhotonVersion.versionString); -// } -// -// /** -// * Constructs a Simulated PhotonCamera from the name of the camera. -// * -// * @param cameraName The nickname of the camera (found in the PhotonVision UI). -// */ -// public GreenSimPhotonCamera(String cameraName) { -// this(NetworkTableInstance.getDefault(), cameraName); -// } -// -// /** -// * Simulate one processed frame of vision data, putting one result to NT. -// * -// * @param latencyMillis Latency of the provided frame -// * @param targets Each target detected -// */ -// public void submitProcessedFrame( -// double latencyMillis, -// PhotonTrackedTarget... targets -// ) { -// submitProcessedFrame(latencyMillis, Arrays.asList(targets)); -// } -// -// /** -// * Simulate one processed frame of vision data, putting one result to NT. -// * -// * @param latencyMillis Latency of the provided frame -// * @param sortMode Order in which to sort targets -// * @param targets Each target detected -// */ -// public void submitProcessedFrame( -// double latencyMillis, -// PhotonTargetSortMode sortMode, -// PhotonTrackedTarget... targets -// ) { -// submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets)); -// } -// -// /** -// * Simulate one processed frame of vision data, putting one result to NT. -// * -// * @param latencyMillis Latency of the provided frame -// * @param targetList List of targets detected -// */ -// public void submitProcessedFrame( -// double latencyMillis, -// List targetList -// ) { -// submitProcessedFrame(latencyMillis, null, targetList); -// } -// -// /** -// * Simulate one processed frame of vision data, putting one result to NT. -// * -// * @param latencyMillis Latency of the provided frame -// * @param sortMode Order in which to sort targets -// * @param submittedTargetList List of targets detected -// */ -// public void submitProcessedFrame( -// double latencyMillis, -// PhotonTargetSortMode sortMode, -// List submittedTargetList -// ) { -// latencyMillisEntry.setDouble(latencyMillis); -// -// if (sortMode != null) { -// submittedTargetList.sort(sortMode.getComparator()); -// } -// -// PhotonPipelineResult newResult = new PhotonPipelineResult( -// latencyMillis, -// submittedTargetList -// ); -// var newPacket = new Packet(newResult.getPacketSize()); -// // newResult.populatePacket(newPacket); FIXME during sim vision rework -// rawBytesEntry.setRaw(newPacket.getData()); -// -// boolean hasTargets = newResult.hasTargets(); -// hasTargetEntry.setBoolean(hasTargets); -// if (!hasTargets) { -// for (int i = 0; i < targetList.size(); i++) { -// targetList.get(i).setString("None"); -// } -// } else { -// var targets = newResult.getTargets(); -// -// for (int i = 0; i < 9; i++) { -// if (i >= targets.size() - 1) { -// targetList.get(i).setString("None"); -// } else { -// targetList -// .get(i) -// .setString(PhotonTrackTargetToString(targets.get(i))); -// } -// } -// } -// } -// -// private static String PhotonTrackTargetToString(PhotonTrackedTarget t) { -// return ( -// "Id: " + -// t.getFiducialId() + -// // "; yaw= " + -// // (int) t.getYaw() + // If yaw seems weird that's because it's an int -// "; X: " + -// roundAvoid(t.getBestCameraToTarget().getX(), 3) + -// "; Y: " + -// roundAvoid(t.getBestCameraToTarget().getY(), 3) + -// "; Z: " + -// roundAvoid(t.getBestCameraToTarget().getZ(), 3) -// ); -// } -// -// public static double roundAvoid(double value, int places) { -// double scale = Math.pow(10, places); -// return Math.round(value * scale) / scale; -// } -//} diff --git a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionSystem.java b/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionSystem.java deleted file mode 100644 index da9c9cbd..00000000 --- a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionSystem.java +++ /dev/null @@ -1,263 +0,0 @@ -///* -// * MIT License -// * -// * Copyright (c) 2022 PhotonVision -// * -// * Permission is hereby granted, free of charge, to any person obtaining a copy -// * of this software and associated documentation files (the "Software"), to deal -// * in the Software without restriction, including without limitation the rights -// * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// * copies of the Software, and to permit persons to whom the Software is -// * furnished to do so, subject to the following conditions: -// * -// * The above copyright notice and this permission notice shall be included in all -// * copies or substantial portions of the Software. -// * -// * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// * SOFTWARE. -// */ -// -//package com.team1816.lib.util.visionUtil; -// -//import edu.wpi.first.math.geometry.*; -//import edu.wpi.first.math.util.Units; -//import org.photonvision.PhotonTargetSortMode; -//import org.photonvision.targeting.PhotonTrackedTarget; -//import org.photonvision.targeting.TargetCorner; -// -//import java.util.ArrayList; -//import java.util.List; -// -//public class GreenSimVisionSystem { -// -// GreenSimPhotonCamera cam; -// -// double camHorizFOVDegrees; -// double camVertFOVDegrees; -// double cameraHeightOffGroundMeters; -// double maxLEDRangeMeters; -// double camPitchDegrees; -// int cameraResWidth; -// int cameraResHeight; -// double minTargetArea; -// Transform2d cameraToRobot; -// -// ArrayList targetList; -// -// /** -// * Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot -// * running PhotonVision, detecting one or more targets scattered around the field. This assumes a -// * fairly simple and distortion-less pinhole camera model. -// * -// * @param camName Name of the PhotonVision camera to create. Align it with the settings you use in -// * the PhotonVision GUI. -// * @param camVertFOVDegrees Vertical Field of View of the camera used. Align it with the -// * manufacturer specifications, and/or whatever is configured in the PhotonVision Setting -// * page. -// * @param camHorizFOVDegrees Horizontal Field of View of the camera used. Align it with the -// * manufacturer specifications, and/or whatever is configured in the PhotonVision Setting -// * page. -// * @param camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same -// * as whatever is configured in the PhotonVision Setting page. -// * @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's -// * position -// * @param cameraHeightOffGroundMeters Height of the camera off the ground in meters -// * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and -// * make it visible. Set to 9000 or more if your vision system does not rely on LED's. -// * @param cameraResWidth Width of your camera's image sensor in pixels -// * @param cameraResHeight Height of your camera's image sensor in pixels -// * @param minTargetArea Minimum area that the target should be before it's recognized as a -// * target by the camera. Match this with your contour filtering settings in the PhotonVision -// * GUI. -// */ -// public GreenSimVisionSystem( // not using diag FOV to calculate horiz/vert FOV b/c calculated numbers don't match official camera properties -// String camName, -// double camHorizFOVDegrees, -// double camVertFOVDegrees, -// double camPitchDegrees, -// Transform2d cameraToRobot, -// double cameraHeightOffGroundMeters, -// double maxLEDRangeMeters, -// int cameraResWidth, -// int cameraResHeight, -// double minTargetArea -// ) { -// this.camPitchDegrees = camPitchDegrees; -// this.cameraToRobot = cameraToRobot; -// this.cameraHeightOffGroundMeters = cameraHeightOffGroundMeters; -// this.maxLEDRangeMeters = maxLEDRangeMeters; -// this.cameraResWidth = cameraResWidth; -// this.cameraResHeight = cameraResHeight; -// this.minTargetArea = minTargetArea; -// -// // Calculate horizontal/vertical FOV by similar triangles -// double hypotPixels = Math.hypot(cameraResWidth, cameraResHeight); -// this.camHorizFOVDegrees = camHorizFOVDegrees; -// this.camVertFOVDegrees = camVertFOVDegrees; -// -// cam = new GreenSimPhotonCamera(camName); -// targetList = new ArrayList<>(); -// } -// -// /** -// * Add a target on the field which your vision system is designed to detect. The PhotonCamera from -// * this system will report the location of the robot relative to the subset of these targets which -// * are visible from the given robot position. -// * -// * @param target Target to add to the simulated field -// */ -// public void addSimVisionTarget(GreenSimVisionTarget target) { -// targetList.add(target); -// } -// -// /** -// * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or -// * turret or some other mobile platform. -// * -// * @param newCameraToRobot New Transform from the robot to the camera -// * @param newCamHeightMeters New height of the camera off the floor -// * @param newCamPitchDegrees New pitch of the camera axis back from horizontal -// */ -// public void moveCamera( -// Transform2d newCameraToRobot, -// double newCamHeightMeters, -// double newCamPitchDegrees -// ) { -// this.cameraToRobot = newCameraToRobot; -// this.cameraHeightOffGroundMeters = newCamHeightMeters; -// this.camPitchDegrees = newCamPitchDegrees; -// } -// -// /** -// * Periodic update. Call this once per frame of image data you wish to process and send to -// * NetworkTables -// * -// * @param robotPoseMeters current pose of the robot on the field. Will be used to calculate which -// * targets are actually in view, where they are at relative to the robot, and relevant -// * PhotonVision parameters. -// */ -// public void processFrame(Pose2d robotPoseMeters) { -// Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse()); -// ArrayList visibleTgtList = new ArrayList<>( -// targetList.size() -// ); -// -// targetList.forEach( -// tgt -> { -// var camToTargetTransform = new Transform3d( -// new Pose3d( -// cameraPos.getX(), -// cameraPos.getY(), -// cameraHeightOffGroundMeters, -// new Rotation3d( -// 0, -// Units.degreesToRadians(camPitchDegrees), -// cameraPos.getRotation().getRadians() -// ) -// ), -// new Pose3d( -// tgt.targetPos.getX(), -// tgt.targetPos.getY(), -// tgt.targetHeightMeters, -// new Rotation3d() -// ) -// ); -// double distAlongGroundMeters = camToTargetTransform -// .getTranslation() -// .getNorm(); -// double distVerticalMeters = -// tgt.targetHeightAboveGroundMeters - this.cameraHeightOffGroundMeters; -// double distMeters = Math.hypot(distAlongGroundMeters, distVerticalMeters); -// -// double area = tgt.tgtAreaMeters2 / getM2PerPx(distAlongGroundMeters); -// -// // 2D yaw mode considers the target as a point, and should ignore target rotation. -// // Photon reports it in the correct robot reference frame. -// // IE: targets to the left of the image should report negative yaw. -// double yawDegrees = -// -1.0 * -// Units.radiansToDegrees( -// Math.atan2( -// camToTargetTransform.getTranslation().getY(), -// camToTargetTransform.getTranslation().getX() -// ) -// ); -// double pitchDegrees = -// Units.radiansToDegrees( -// Math.atan2(distVerticalMeters, distAlongGroundMeters) -// ) - -// this.camPitchDegrees; -// -// if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) { -// visibleTgtList.add( -// new PhotonTrackedTarget( -// yawDegrees, -// pitchDegrees, -// area, -// 0.0, -// tgt.fiducialID, -// new Transform3d( -// new Translation3d( -// camToTargetTransform.getX(), -// camToTargetTransform.getY(), -// distVerticalMeters -// ), -// new Rotation3d() -// ), -// new Transform3d( -// new Translation3d( -// camToTargetTransform.getX(), -// camToTargetTransform.getY(), -// distVerticalMeters -// ), -// new Rotation3d() -// ), -// 0.25, -// List.of( // not utilizing target corners -// new TargetCorner(0, 0), -// new TargetCorner(0, 0), -// new TargetCorner(0, 0), -// new TargetCorner(0, 0) -// ), -// List.of( -// new TargetCorner(0, 0), -// new TargetCorner(0, 0), -// new TargetCorner(0, 0), -// new TargetCorner(0, 0) -// ) -// ) -// ); -// } -// } -// ); -// -// cam.submitProcessedFrame(0.0, PhotonTargetSortMode.Largest, visibleTgtList); -// } -// -// double getM2PerPx(double dist) { -// double widthMPerPx = -// 2 * -// dist * -// Math.tan(Units.degreesToRadians(this.camHorizFOVDegrees) / 2) / -// cameraResWidth; -// double heightMPerPx = -// 2 * -// dist * -// Math.tan(Units.degreesToRadians(this.camVertFOVDegrees) / 2) / -// cameraResHeight; -// return widthMPerPx * heightMPerPx; -// } -// -// boolean camCanSeeTarget(double distMeters, double yaw, double pitch, double area) { -// boolean inRange = (distMeters < this.maxLEDRangeMeters); -// boolean inHorizAngle = Math.abs(yaw) < (this.camHorizFOVDegrees / 2); -// boolean inVertAngle = Math.abs(pitch) < (this.camVertFOVDegrees / 2); -// boolean targetBigEnough = area > this.minTargetArea; -// return (inRange && inHorizAngle && inVertAngle && targetBigEnough); -// } -//} diff --git a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionTarget.java b/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionTarget.java deleted file mode 100644 index 912ec97e..00000000 --- a/src/main/java/com/team1816/lib/util/visionUtil/GreenSimVisionTarget.java +++ /dev/null @@ -1,62 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2022 PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package com.team1816.lib.util.visionUtil; - -import edu.wpi.first.math.geometry.Pose2d; - -public class GreenSimVisionTarget { - - Pose2d targetPos; - double targetWidthMeters; - double targetHeightMeters; - double targetHeightAboveGroundMeters; - double tgtAreaMeters2; - int fiducialID; - - /** - * Describes a vision target located somewhere on the field that your SimVisionSystem can detect. - * - * @param targetPos Pose2d of the target on the field. Define it such that, if you are standing in - * the middle of the field facing the target, the Y axis points to your left, and the X axis - * points away from you. - * @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters. - * @param targetWidthMeters Width of the outer bounding box of the target in meters. - * @param targetHeightMeters Pair Height of the outer bounding box of the target in meters. - */ - public GreenSimVisionTarget( - Pose2d targetPos, - double targetHeightAboveGroundMeters, - double targetWidthMeters, - double targetHeightMeters, - int fiducialID - ) { - this.targetPos = targetPos; - this.targetHeightAboveGroundMeters = targetHeightAboveGroundMeters; - this.targetWidthMeters = targetWidthMeters; - this.targetHeightMeters = targetHeightMeters; - this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters; - this.fiducialID = fiducialID; - } -} diff --git a/src/main/java/com/team1816/lib/util/visionUtil/VisionPoint.java b/src/main/java/com/team1816/lib/util/visionUtil/VisionPoint.java deleted file mode 100644 index 387333e3..00000000 --- a/src/main/java/com/team1816/lib/util/visionUtil/VisionPoint.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.team1816.lib.util.visionUtil; - -import edu.wpi.first.math.geometry.Transform3d; - -/** - * A lightweight fiducial marker identification utility - */ - -public class VisionPoint { - - public int id; // -2 if not detected - public Transform3d cameraToTarget; - public double weight; - - public VisionPoint() { - id = 0; - cameraToTarget = new Transform3d(); - weight = 0; - } - - public VisionPoint(int i, Transform3d targetTransform) { - id = i; - cameraToTarget = targetTransform; - weight = 0; - } - - public double getX() { - return cameraToTarget.getX(); - } - - public double getY() { - return cameraToTarget.getY(); - } - - public double getZ() { - return cameraToTarget.getZ(); - } - - public String toString() { - return "id: " + id + " camera to target: " + cameraToTarget; - } -} diff --git a/src/main/java/com/team1816/lib/variableInputs/Numeric.java b/src/main/java/com/team1816/lib/variableInputs/Numeric.java deleted file mode 100644 index 1f220bcc..00000000 --- a/src/main/java/com/team1816/lib/variableInputs/Numeric.java +++ /dev/null @@ -1,77 +0,0 @@ -package com.team1816.lib.variableInputs; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.jetbrains.annotations.NotNull; - -import java.lang.constant.Constable; -import java.util.Optional; - - -public class Numeric extends Number - implements VariableInput, Comparable, Constable { - - //I'm so cooked - - private final String smartDashboardReference; - private final double defaultValue; - - protected Numeric(String smartDashboardReference, double defaultValue) { - this.smartDashboardReference = smartDashboardReference; - this.defaultValue = defaultValue; - SmartDashboard.putNumber(smartDashboardReference, defaultValue); - } - - private double getValue() { - return SmartDashboard.getNumber(smartDashboardReference, defaultValue); //TODO maybe add an FMS connectivity check. - } - - @Override - public int intValue() { - return (int) getValue(); - } - - @Override - public long longValue() { - return (long) getValue(); - } - - @Override - public float floatValue() { - return (float) getValue(); - } - - @Override - public double doubleValue() { - return getValue(); - } - - @Override - public int compareTo(@NotNull Double anotherDouble) { - return Double.compare(getValue(), anotherDouble); - } - - public int compareTo(@NotNull Numeric anotherNumeric) { - return Double.compare(getValue(), anotherNumeric.getValue()); - } - - @Override - public Optional describeConstable() { - return Optional.of(this.getValue()); - } - - @Override - public double toNumber() { - return getValue(); - } - - @Override - public String toString() { - return Double.toString(getValue()); - } - - @Override - public String toDescriptiveString() { - return String.format("Numeric(Reference: %s, DefaultValue: %f, CurrentValue: %b)", - smartDashboardReference, defaultValue, getValue()) ; - } -} diff --git a/src/main/java/com/team1816/lib/variableInputs/VariableInput.java b/src/main/java/com/team1816/lib/variableInputs/VariableInput.java deleted file mode 100644 index 59584bf1..00000000 --- a/src/main/java/com/team1816/lib/variableInputs/VariableInput.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.team1816.lib.variableInputs; - - -public interface VariableInput { - static Numeric number(String smartDashboardReference, double defaultValue) { - return new Numeric(smartDashboardReference, defaultValue); - } - - // NOTE: Not for use in the smartdashboard. - static Numeric number(double value) { - return new Numeric(null, value); - } - - double toNumber(); - - String toDescriptiveString(); -} diff --git a/src/main/resources/yaml/cheeze-curd.config.yml b/src/main/resources/yaml/cheeze-curd.config.yml deleted file mode 100644 index 6c7b0792..00000000 --- a/src/main/resources/yaml/cheeze-curd.config.yml +++ /dev/null @@ -1,85 +0,0 @@ -# TODO: Will need to be updated once "REEFSCAPE" robot, Zone is present. Currently, is not accurate -subsystems: - drivetrain: - implemented: true - motors: - rightMain: - motorType: TalonFX - motorName: rightMain - id: 2 - invertMotor: false - leftMain: - motorType: TalonFX - motorName: leftMain - id: 1 - invertMotor: false - pidConfig: - slot0: - kP: 0.1 # 0.18 - kI: 0.0001 # 0.018 - kD: 4.0 # 3.6 - kF: 0.0475813 - constants: - ## Positional Configurations ## - openLoopRampRate: 0.1 - isSwerve: 0 - isCTRSwerve: 0 #TODO IF ALL ELSE FAILS - wheelDiameter: 3.81 #inches - kTrackScrubFactor: 1.0 - ## Velocity Constraints ## - maxRotVel: 0.8 # Pi radians/sec - maxRotVelClosedLoop: 2 # Pi radians/sec - trackWidth: 22.75 # inches - wheelbaseLength: 22.75 # inches - maxVelOpenLoop: 3 # meters/s - maxVel12VMPS: 5.2 - maxVelPathFollowing: 4 # meters/s - maxAccel: 2 # meters/s^2 - isDemoMode: 0 - ## Swerve Characterization ## - driveGearRatio: 6.75 # L2 - azimuthGearRatio: 12.8 # MK4 - azimuthEncPPR: 0 - ## Control Configurations ## - slowModeScalar: 0.4 - turboModeScalar: 1.73 - driveDeadband: 0.1 - rotationalDeadband: 0.1 - inputDeadband: 0.15 # Deadband of the normalized input - snapDivisor: 30 - camera: - implemented: false - constants: - useMultiTargetOdometry: 0 -infrastructure: - canBusName: highspeed - ## Power Distribution ## - pdId: 1 - pdIsRev: true - ## Pneumatics Control ## - compressorEnabled: false - pcmIsRev: false - pcmId: -1 - ## Pigeon ## - pigeonId: 5 - isPigeon2: true -inputHandler: drivercentric # the default input handler -constants: - zeroingButton: 9 # channel of DIO port - enableMusic: 0 - ## Drivetrain ## - minAllowablePoseError: 0.025 - maxAllowablePoseError: 5 - soundOnConfig: 0 #If this is false, It will still beep on robot bootup. - rotationToleranceClosedLoop: 0.5 - ## Logging ## - logRobot: 1 # 0 or 1 - logDrivetrain: 1 # 0 or 1 - configStatusFrames: 0 # 0 or 1 - ## General ## - usingVision: 0 - teleopFieldCentric: 1 # 0 or 1 - kLooperDt: .025 # seconds - isProLicensed: 0 - hasCanivore: 1 # 0 or 1 - resetFactoryDefaults: 0 # whether motors get reset to factory default \ No newline at end of file diff --git a/src/main/resources/yaml/zen.config.yml b/src/main/resources/yaml/zen.config.yml deleted file mode 100644 index ddf47530..00000000 --- a/src/main/resources/yaml/zen.config.yml +++ /dev/null @@ -1,271 +0,0 @@ -subsystems: - drivetrain: - implemented: true - motors: - FLDr: - motorType: TalonFX - motorName: FLDr - id: 7 - invertMotor: false - FRDr: - motorType: TalonFX - motorName: FRDr - id: 2 - invertMotor: true - RLDr: - motorType: TalonFX - motorName: BLDr - id: 8 - invertMotor: true - RRDr: - motorType: TalonFX - motorName: BRDr - id: 1 - invertMotor: true - FLAz: - motorType: TalonFX - motorName: FLAz - id: 12 - invertMotor: false - FRAz: - motorType: TalonFX - motorName: FRAz - id: 3 - invertMotor: false - RLAz: - motorType: TalonFX - motorName: BLAz - id: 9 - invertMotor: false - RRAz: - motorType: TalonFX - motorName: BRAz - id: 0 - invertMotor: false - canCoders: - FLEnc: 16 - FREnc: 15 - RLEnc: 17 - RREnc: 14 - swerveModules: - modules: - frontLeft: - drive: FLDr - azimuth: FLAz - canCoder: FLEnc - xCoord: 7.125 - yCoord: 11.375 - constants: - encoderOffset: -0.34228515625 - couplingRatio: 3.576 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - frontRight: - drive: FRDr - azimuth: FRAz - canCoder: FREnc - xCoord: 7.125 - yCoord: -11.375 - constants: - encoderOffset: -0.014404296875 - couplingRatio: 3.579 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - backLeft: - drive: RLDr - azimuth: RLAz - canCoder: RLEnc - xCoord: -11.375 - yCoord: 11.375 - constants: - encoderOffset: 0.23779296875 - couplingRatio: 3.572 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - backRight: - drive: RRDr - azimuth: RRAz - canCoder: RREnc - xCoord: -11.375 - yCoord: -11.375 - constants: - encoderOffset: 0.3076171875 - couplingRatio: 3.589 - freeSpin12VRPS: 104 - invertSensorPhase: 1 - drivePID: - slot0: - kP: 0.0065 - kI: 0 - kD: 0.00005 - kF: 0.00925 - azimuthPID: - slot0: - kP: 75 - kI: 0 - kD: 0 - kF: 0 - allowableError: 5 - constants: - ## Positional Configurations ## - openLoopRampRate: 0.1 - isSwerve: 1 - isCTRSwerve: 1 #TODO IF ALL ELSE FAILS - wheelDiameter: 3.81 #inches - kTrackScrubFactor: 1.0 - ## Velocity Constraints ## - maxRotVel: 0.8 # Pi radians/sec - maxRotVelClosedLoop: 2 # Pi radians/sec - trackWidth: 22.75 # inches - wheelbaseLength: 22.75 # inches - maxVelOpenLoop: 3 # meters/s - maxVel12VMPS: 5.2 - maxVelPathFollowing: 4 # meters/s - maxAccel: 3 # meters/s^2 - isDemoMode: 0 - ## Swerve Characterization ## - driveGearRatio: 6.12 # L2 - azimuthGearRatio: 12.8 # MK4 - ## Control Configurations ## - slowModeScalar: 0.4 - turboModeScalar: 1.73 - driveDeadband: 0.1 - rotationalDeadband: 0.1 - inputDeadband: 0.15 # Deadband of the normalized input - snapDivisor: 30 - camera: - implemented: false - constants: - useMultiTargetOdometry: 0 - collector: - implemented: true - motors: - intakeMotor: - motorType: TalonFX - motorName: intakeMotor - id: 6 - invertMotor: false - pidConfig: - slot0: - kP: 0.043 - Ki: 0 - kD: 0.00001 - kF: 0.0099 - constants: - intakeSpeed: -0.75 - outtakeSpeed: 0.3 - shooter: - implemented: true - motors: - rollerMotor: - motorType: TalonFX - motorName: rollerMotor - id: 13 - invertMotor: false - pivotMotor: - motorType: TalonFX - motorName: pivotMotor - id: 10 - invertMotor: false - currentLimit: 20 - motionMagic: - expoKV: 0.035 - expoKA: 0.065 - pivotFollowMotor: - motorType: TalonFX - motorName: pivotFollowMotor - id: 4 - invertMotor: false # ? - currentLimit: 20 - feederMotor: - motorType: TalonFX - motorName: feederMotor - id: 11 - invertMotor: true - pidConfig: - slot0: - kP: 0.03 - Ki: 0 - kD: 0 - kF: 0.0105 - slot1: - kP: 0.00025 - Ki: 0 - kD: 0 - kF: 0.0105 - slot2: - kP: 0.6 - ki: 0 - kD: 0.02 - kF: 0 - gravityType: cosine - canCoders: - Pivot: 25 - constants: - noteSensorChannel: 0 - rollerSpeakerShootSpeed: 71 - rollerAmpShootSpeed: 30 - rollerEjectShootSpeed: 65 - feederSpeakerShootSpeed: 30 - feederAmpShootSpeed: 11 - feederEjectShootSpeed: 28 - feederIntakeSpeed: 10.5 - velocityErrorMargin: 0.05 - velocityErrorMarginAutoAim: 0.003 - autoAimDegreeTolerance: .1 - pivotNeutralPosition: 1.5 - pivotAmpShootPosition: 33 - pivotDistanceShootPosition: 8.675 - invertFollowerMotor: 1 - canCoderOffset: 0.306 - climber: - implemented: false - motors: - climbMotor: - motorType: TalonFX - motorName: climbMotor - id: -14 - invertMotor: true - currentLimit: 30 - enableCurrentLimit: true - pidConfig: - slot0: - kP: 0.02 - constants: - slowClimbPower: 0.3 - fastClimbPower: 0.8 - reSpoolPower: -0.5 - climbPosition: 194 - climbThreshold: 187 -infrastructure: - canBusName: highSpeed - ## Power Distribution ## - pdId: 1 - pdIsRev: true - ## Pneumatics Control ## - compressorEnabled: false - pcmIsRev: false - pcmId: -1 - ## Pigeon ## - pigeonId: 5 - isPigeon2: true -inputHandler: drivercentric # the default input handler -constants: - zeroingButton: 9 # channel of DIO port - enableMusic: 0 - ## Drivetrain ## - minAllowablePoseError: 0.025 - maxAllowablePoseError: 5 - soundOnConfig: 0 #If this is false, It will still beep on robot bootup. - rotationToleranceClosedLoop: 0.5 - ## Logging ## - logRobot: 1 # 0 or 1 - logDrivetrain: 1 # 0 or 1 - configStatusFrames: 0 # 0 or 1 - ## General ## - usingVision: 0 - teleopFieldCentric: 1 # 0 or 1 - kLooperDt: .025 # seconds - isProLicensed: 0 - hasCanivore: 1 # 0 or 1 - resetFactoryDefaults: 1 # whether motors get reset to factory default \ No newline at end of file diff --git a/src/main/resources/yaml/zingi.config.yml b/src/main/resources/yaml/zingi.config.yml deleted file mode 100644 index 94916d07..00000000 --- a/src/main/resources/yaml/zingi.config.yml +++ /dev/null @@ -1,286 +0,0 @@ -subsystems: - drivetrain: - implemented: true - motors: - FLDr: - motorType: TalonFX - motorName: FLDr - id: 7 - invertMotor: false - FRDr: - motorType: TalonFX - motorName: FRDr - id: 2 - invertMotor: true - RLDr: - motorType: TalonFX - motorName: BLDr - id: 8 - invertMotor: true - RRDr: - motorType: TalonFX - motorName: BRDr - id: 1 - invertMotor: true - FLAz: - motorType: TalonFX - motorName: FLAz - id: 12 - invertMotor: false - FRAz: - motorType: TalonFX - motorName: FRAz - id: 3 - invertMotor: false - RLAz: - motorType: TalonFX - motorName: BLAz - id: 9 - invertMotor: false - RRAz: - motorType: TalonFX - motorName: BRAz - id: 0 - invertMotor: false - canCoders: - FLEnc: 16 - FREnc: 15 - RLEnc: 17 - RREnc: 14 - swerveModules: - modules: - frontLeft: - drive: FLDr - azimuth: FLAz - canCoder: FLEnc - xCoord: 7.125 - yCoord: 11.375 - constants: - encoderOffset: -0.34228515625 - couplingRatio: 3.576 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - frontRight: - drive: FRDr - azimuth: FRAz - canCoder: FREnc - xCoord: 7.125 - yCoord: -11.375 - constants: - encoderOffset: -0.014404296875 - couplingRatio: 3.579 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - backLeft: - drive: RLDr - azimuth: RLAz - canCoder: RLEnc - xCoord: -11.375 - yCoord: 11.375 - constants: - encoderOffset: 0.23779296875 - couplingRatio: 3.572 - freeSpin12VRPS: 101 - invertSensorPhase: 1 - backRight: - drive: RRDr - azimuth: RRAz - canCoder: RREnc - xCoord: -11.375 - yCoord: -11.375 - constants: - encoderOffset: 0.3076171875 - couplingRatio: 3.589 - freeSpin12VRPS: 104 - invertSensorPhase: 1 - drivePID: - slot0: - kP: 0.0065 - kI: 0 - kD: 0.00005 - kV: 0.00925 - azimuthPID: - slot0: - kP: 75 - kI: 0 - kD: 0 - kV: 0 - allowableError: 5 - constants: - ## Positional Configurations ## - deepCageHangerEthanPosition: 0.1 - deepCageHangerLiaoPosition: 0.1 - openLoopRampRate: 0.1 - isSwerve: 1 - isCTRSwerve: 0 #TODO IF ALL ELSE FAILS - wheelDiameter: 3.81 #inches - kTrackScrubFactor: 1.0 - ## Velocity Constraints ## - maxRotVel: 0.8 # Pi radians/sec - maxRotVelClosedLoop: 2 # Pi radians/sec - trackWidth: 22.75 # inches - wheelbaseLength: 22.75 # inches - maxVelOpenLoop: 3 # meters/s - maxVel12VMPS: 5.2 - maxVelPathFollowing: 4 # meters/s - maxAccel: 3 # meters/s^2 - isDemoMode: 0 - ## Swerve Characterization ## - driveGearRatio: 6.12 # L2 - azimuthGearRatio: 12.8 # MK4 - ## Control Configurations ## - slowModeScalar: 0.4 - turboModeScalar: 1.73 - driveDeadband: 0.1 - rotationalDeadband: 0.1 - inputDeadband: 0.15 # Deadband of the normalized input - snapDivisor: 30 - pneumatic: - implemented: false - solenoids: - pneumatic: 6 - constants: - pneumaticOn: 1.0 - pneumaticOff: 0.0 - camera: - implemented: false - constants: - useMultiTargetOdometry: 0 - algaeCatcher: - #TODO tune these stuffs - implemented: false - motors: - algaeCatcherLeadMotor: - motorType: TalonFX - motorName: leadMotor - id: -324092 - invertMotor: false - algaeCatcherFollowerMotor: - motorType: TalonFX - motorName: followerMotor - id: -314159 - invertMotor: false - algaeCatcherPositionMotor: - motorType: TalonFX - motorName: positionMotor - id: -314159 - invertMotor: false - pidConfig: - slot0: - kP: 0.043 - Ki: 0 - kD: 0.00001 - kV: 0.0099 - constants: - algaeCollectSpeed: -0.5 - algaeHoldSpeed: -0.1 - algaeReleaseSpeed: 0.25 - - neutralPosition: 1 - intakePosition: 1 - holdPosition: 1 - outtakePosition: 1 - coralArm: - implemented: false - motors: - coralPivotMotor: - motorType: TalonFX - motorName: pivotMotor - id: -1 - invertMotor: false - coralIntakeMotor: - motorType: SparkMax - motorName: intakeMotor - id: -1 - invertMotor: false - pidConfig: - slot1: - kP: 0 - kI: 0 - kD: 0 - kV: 0 - slot2: - kP: 0 - kI: 0 - kD: 0 - kV: 0 - constants: # TODO: all of these - intakeSpeed: 1 - outtakeSpeed: -1 - holdSpeed: 0 - - coralSensorChannel: 0 - - coralArmL1Position: 1 - coralArmL2Position: 2 - coralArmL3Position: 3 - coralArmL4Position: 4 - coralArmFeederPosition: -1 - coralArmRestPosition: -1 - elevator: - implemented: false - motors: - elevatorMotor: - motorType: TalonFX - motorName: elevatorMotor - id: 10 - invertMotor: false - currentLimit: 20 - motionMagic: - expoKV: 0.035 - expoKA: 0.065 - constants: - elevatorRestPosition: 0.1 - elevatorFeederPosition: 0.1 - elevatorL1Position: 0.1 - elevatorL2Position: 0.1 - elevatorL3Position: 0.1 - elevatorL4Position: 0.1 - ethan: - deepCageHangerMotor: - motorType: TalonFX - motorName: deepCageHangerMotor - id: 10 - invertMotor: false - currentLimit: 20 - motionMagic: - expoKV: 0.035 - expoKA: 0.065 - deepCageHangerFollowMotor: - motorType: TalonFX - motorName: deepCageHangerFollowMotor - id: 4 - invertMotor: false - currentLimit: 20 -infrastructure: - canBusName: highSpeed - ## Power Distribution ## - pdId: 1 - pdIsRev: true - ## Pneumatics Control ## - compressorEnabled: false - pcmIsRev: false - pcmId: -1 - ## Pigeon ## - pigeonId: 5 - isPigeon2: true -inputHandler: drivercentric # the default input handler -constants: - zeroingButton: 9 # channel of DIO port - enableMusic: 0 - ## Drivetrain ## - minAllowablePoseError: 0.025 - maxAllowablePoseError: 5 - soundOnConfig: 0 #If this is false, It will still beep on robot bootup. - rotationToleranceClosedLoop: 0.5 - ## Logging ## - logRobot: 1 # 0 or 1 - logDrivetrain: 1 # 0 or 1 - configStatusFrames: 0 # 0 or 1 - ## General ## - usingVision: 1 - teleopFieldCentric: 1 # 0 or 1 - kLooperDt: .025 # seconds - isProLicensed: 0 - hasCanivore: 1 # 0 or 1 - resetFactoryDefaults: 1 # whether motors get reset to factory default \ No newline at end of file diff --git a/src/test/java/com/team1816/lib/events/EventAggregatorTests.java b/src/test/java/com/team1816/lib/events/EventAggregatorTests.java deleted file mode 100644 index d8741811..00000000 --- a/src/test/java/com/team1816/lib/events/EventAggregatorTests.java +++ /dev/null @@ -1,68 +0,0 @@ -package com.team1816.lib.events; - -import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -public class EventAggregatorTests { - - private String _eventText = ""; - - protected static class StringEvent extends PubSubConsumer { - } - - protected static class StringEvent2 extends PubSubConsumer { - } - - protected static class RunnableEvent extends PubSubRunnable { - } - - private EventAggregator _target; - - @BeforeEach - public void TestInit() { - _target = new EventAggregator(); - _eventText = ""; - } - - @Test - public void GetEventTest() { - var evt = _target.GetEvent(StringEvent.class); - // verify same class gets returned - Assertions.assertEquals( - evt.hashCode(), - _target.GetEvent(StringEvent.class).hashCode(), - "New class created" - ); - evt.Subscribe(this::EventFired); - evt.Publish("foobar"); - Assertions.assertEquals("foobar", _eventText); - } - - @Test - public void GetEventTestNoData() { - var evt = _target.GetEvent(RunnableEvent.class); - evt.Subscribe( - () -> { - _eventText = "noData"; - } - ); - evt.Publish(); - Assertions.assertEquals("noData", _eventText); - } - - @Test - public void GetEventTestSingle() { - //Ensure that event system only fires on the published event - var evt2 = _target.GetEvent(StringEvent2.class); - evt2.Subscribe(this::EventFired); - var evt = _target.GetEvent(StringEvent.class); - evt.Subscribe(this::EventFired); - evt.Publish("single"); - Assertions.assertEquals("single", _eventText); - } - - private void EventFired(String arg) { - _eventText += arg; - } -} From 92dd32059a5cf229797220201ec5ddcdb714afd7 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Sat, 12 Jul 2025 21:54:32 -0500 Subject: [PATCH 2/2] commit some changes that I failed to commit in the last commit --- src/main/java/com/team1816/core/states/Orchestrator.java | 1 - src/main/java/com/team1816/core/states/RobotState.java | 6 ------ 2 files changed, 7 deletions(-) diff --git a/src/main/java/com/team1816/core/states/Orchestrator.java b/src/main/java/com/team1816/core/states/Orchestrator.java index 1b6db769..a0f18cfb 100644 --- a/src/main/java/com/team1816/core/states/Orchestrator.java +++ b/src/main/java/com/team1816/core/states/Orchestrator.java @@ -12,7 +12,6 @@ import com.team1816.lib.subsystems.drive.Drive; import com.team1816.lib.subsystems.vision.Camera; import com.team1816.lib.util.logUtil.GreenLogger; -import com.team1816.lib.util.visionUtil.VisionPoint; import com.team1816.season.subsystems.CoralArm; import com.team1816.season.subsystems.Elevator; import com.team1816.season.subsystems.Ramp; diff --git a/src/main/java/com/team1816/core/states/RobotState.java b/src/main/java/com/team1816/core/states/RobotState.java index 4aed4888..53eb12b6 100644 --- a/src/main/java/com/team1816/core/states/RobotState.java +++ b/src/main/java/com/team1816/core/states/RobotState.java @@ -6,7 +6,6 @@ import com.team1816.lib.auto.Color; import com.team1816.lib.subsystems.drive.SwerveDrive; import com.team1816.lib.subsystems.drive.TankDrive; -import com.team1816.lib.util.visionUtil.VisionPoint; import com.team1816.season.subsystems.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -108,9 +107,6 @@ public class RobotState { public boolean isElevatorInRange = false; public boolean isCoralArmPivotInRange = false; - public VisionPoint superlativeTarget = new VisionPoint(); - public List visibleTargets = new ArrayList<>(); - public final Mechanism2d rampMech2d = new Mechanism2d(2, 2); public final MechanismRoot2d rampMech2dRoot = rampMech2d.getRoot("root", 1, 1); @@ -190,8 +186,6 @@ public synchronized void resetAllStates() { actualPneumaticState = Pneumatic.PNEUMATIC_STATE.OFF; isPoseUpdated = true; - superlativeTarget = new VisionPoint(); - visibleTargets = new ArrayList<>(); drivetrainTemp = 0; vehicleToFloorProximityCentimeters = 0; }