diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 0000000..727b697 --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,871 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Match", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Scheduler", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 896.0, + "type": "Scheduler", + "properties": { + "topic": "/SmartDashboard/Scheduler", + "period": 0.06 + } + }, + { + "title": "FMSInfo", + "x": 384.0, + "y": 0.0, + "width": 512.0, + "height": 128.0, + "type": "FMSInfo", + "properties": { + "topic": "/FMSInfo", + "period": 0.06 + } + }, + { + "title": "APPLIED STATE", + "x": 896.0, + "y": 0.0, + "width": 512.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/APPLIED STATE", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Alerts", + "x": 1408.0, + "y": 384.0, + "width": 384.0, + "height": 512.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/Alerts", + "period": 0.06 + } + }, + { + "title": "Field2d", + "x": 384.0, + "y": 128.0, + "width": 1024.0, + "height": 512.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field2d", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": false, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Auto Chooser", + "x": 384.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Initialized", + "x": 384.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Initialized", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "InShift", + "x": 1152.0, + "y": 640.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Match Data/InShift", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "Checkmark", + "false_icon": "X" + } + }, + { + "title": "TimeLeftInShift", + "x": 896.0, + "y": 640.0, + "width": 256.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Data/TimeLeftInShift", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "MatchTime", + "x": 640.0, + "y": 640.0, + "width": 256.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Data/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Offset", + "x": 1408.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "Radial Gauge", + "properties": { + "topic": "/Robot/ShotCalc/Offset", + "period": 0.033, + "data_type": "double", + "start_angle": -140.0, + "end_angle": 140.0, + "min_value": -5.0, + "max_value": 5.0, + "number_of_labels": 10, + "wrap_value": false, + "show_pointer": true, + "show_ticks": true + } + } + ] + } + }, + { + "name": "Diagnostic", + "grid_layout": { + "layouts": [ + { + "title": "Swerve Velocities", + "x": 1408.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Front Left Velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve Drive/Front Left Velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Front Right Velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve Drive/Front Right Velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Back Left Velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve Drive/Back Left Velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Back Right Velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve Drive/Back Right Velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Robot Angle", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve Drive/Robot Angle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "ClimbTop", + "x": 256.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ClimbTop/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ClimbTop/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position Degrees", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ClimbTop/Position Degrees", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position Rotations", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ClimbTop/Position Rotations", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ClimbTop/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "ElevatorFront", + "x": 768.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ElevatorFront/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "MotorVoltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ElevatorFront/MotorVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Rotations", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ElevatorFront/Rotations", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ElevatorFront/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "Intake", + "x": 1024.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Velocity RPM", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Velocity RPM", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "Twist", + "x": 1280.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Twist/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Degrees", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Twist/Degrees", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Twist/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Twist/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "Shoulder", + "x": 1536.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shoulder/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "MotorVoltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shoulder/MotorVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position Degrees", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shoulder/Position Degrees", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shoulder/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "Swerve", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Pose X", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve/Pose X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pose Y", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Swerve/Pose Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "IntakePivot", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentCommand", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/IntakePivot/CurrentCommand", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "MotorVoltage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/IntakePivot/MotorVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position Degrees", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/IntakePivot/Position Degrees", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StatorCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/IntakePivot/StatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "Scheduler", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 1024.0, + "type": "Scheduler", + "properties": { + "topic": "/SmartDashboard/Scheduler", + "period": 0.06 + } + }, + { + "title": "Swerve Drive", + "x": 1024.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "SwerveDrive", + "properties": { + "topic": "/SmartDashboard/Swerve Drive", + "period": 0.06, + "show_robot_rotation": true, + "rotation_unit": "Radians" + } + }, + { + "title": "Alerts", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/Alerts", + "period": 0.06 + } + }, + { + "title": "States", + "x": 256.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/States", + "period": 0.06 + } + } + ] + } + }, + { + "name": "Vision", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "limelight-back", + "x": 1408.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-back", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "limelight-front", + "x": 0.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-front", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "limelight-top", + "x": 512.0, + "y": 0.0, + "width": 896.0, + "height": 640.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-top", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Field", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field2d", + "x": 0.0, + "y": 0.0, + "width": 1792.0, + "height": 1024.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field2d", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": false, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/rebuilt/ShotCalculator.java b/src/main/java/frc/rebuilt/ShotCalculator.java index b0e5d00..11dc52b 100644 --- a/src/main/java/frc/rebuilt/ShotCalculator.java +++ b/src/main/java/frc/rebuilt/ShotCalculator.java @@ -38,6 +38,28 @@ public record ShootingParameters( private static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); + public static final double STARTING_FLYWHEEL_SPEED_OFFSET = 0; + public static double FLYWHEEL_SPEED_OFFSET = STARTING_FLYWHEEL_SPEED_OFFSET; + + public static final double STARTING_TURRET_ANGLE_OFFSET_DEGREES = 0; + public static double TURRET_ANGLE_OFFSET_DEGREES = STARTING_TURRET_ANGLE_OFFSET_DEGREES; + + public static void increaseFlywheelSpeedOffset() { + FLYWHEEL_SPEED_OFFSET += 1; + } + + public static void decreaseFlywheelSpeedOffset() { + FLYWHEEL_SPEED_OFFSET -= 1; + } + + public static void increaseTurretAngleOffsetDegrees() { + TURRET_ANGLE_OFFSET_DEGREES += 1; + } + + public static void decreaseTurretAngleOffsetDegrees() { + TURRET_ANGLE_OFFSET_DEGREES -= 1; + } + static { phaseDelay = 0.03; @@ -110,6 +132,8 @@ public ShootingParameters getParameters() { // Calculate parameters accounted for imparted velocity Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); double flywheelSpeed = shotFlywheelSpeedMap.get(lookaheadTurretToTargetDistance); + turretAngle = turretAngle.plus(Rotation2d.fromDegrees(TURRET_ANGLE_OFFSET_DEGREES)); + flywheelSpeed += flywheelSpeed * (FLYWHEEL_SPEED_OFFSET / 100.0); latestParameters = new ShootingParameters( turretAngle, @@ -120,6 +144,8 @@ public ShootingParameters getParameters() { DogLog.log("ShotCalc/TurretAngleDeg", Double.toString(turretAngle.getDegrees())); DogLog.log("ShotCalc/FlywheelSpeed", Double.toString(flywheelSpeed)); DogLog.log("ShotCalc/TargetPose", new Pose2d(target.getX(), target.getY(), new Rotation2d())); + DogLog.log("ShotCalc/FlywheelSpeedOffset", FLYWHEEL_SPEED_OFFSET); + DogLog.log("ShotCalc/TurretAngleOffsetDegrees", TURRET_ANGLE_OFFSET_DEGREES); return latestParameters; } diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index 36630de..7643a2a 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -31,6 +31,11 @@ public class Pilot extends Gamepad { public final Trigger RB = rightBumper.and(teleop); + public final Trigger dpadUp = upDpad.and(teleop); + public final Trigger dpadDown = downDpad.and(teleop); + public final Trigger dpadLeft = leftDpad.and(teleop); + public final Trigger dpadRight = rightDpad.and(teleop); + // Vision Triggers public final Trigger tagsInView = new Trigger(() -> Robot.getVision().tagsInView()); diff --git a/src/main/java/frc/robot/pilot/PilotStates.java b/src/main/java/frc/robot/pilot/PilotStates.java index 1ee789b..dc82fa1 100644 --- a/src/main/java/frc/robot/pilot/PilotStates.java +++ b/src/main/java/frc/robot/pilot/PilotStates.java @@ -2,7 +2,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.rebuilt.ShotCalculator; import frc.robot.Robot; import frc.robot.RobotSim; import frc.robot.vision.VisionStates; @@ -24,8 +26,12 @@ public static void setStates() { pilot.visionPoseReset_LB_Select.onTrue(VisionStates.resetVisionPose()); pilot.AButton.whileTrue(RobotSim.mapleSimIntakeFuel()); - pilot.YButton.whileTrue(RobotSim.mapleSimLaunchFuel()); + + pilot.dpadDown.onTrue(log(new InstantCommand(ShotCalculator::decreaseFlywheelSpeedOffset))); + pilot.dpadUp.onTrue(log(new InstantCommand(ShotCalculator::increaseFlywheelSpeedOffset))); + pilot.dpadRight.onTrue(log(new InstantCommand(ShotCalculator::decreaseTurretAngleOffsetDegrees))); + pilot.dpadLeft.onTrue(log(new InstantCommand(ShotCalculator::increaseTurretAngleOffsetDegrees))); // Rumble whenever we reorient pilot.upReorient