From e3d92ef0af7f107cdf62dd6e0558a22dfaef366d Mon Sep 17 00:00:00 2001 From: Max Nargang Date: Thu, 23 May 2024 12:28:25 -0400 Subject: [PATCH] Format entire repository --- .../deploy/swerve/controllerproperties.json | 14 +- src/main/deploy/swerve/modules/backleft.json | 53 +- src/main/deploy/swerve/modules/backright.json | 52 +- src/main/deploy/swerve/modules/frontleft.json | 52 +- .../deploy/swerve/modules/frontright.json | 52 +- .../swerve/modules/physicalproperties.json | 30 +- .../deploy/swerve/modules/pidfproperties.json | 30 +- src/main/deploy/swerve/swervedrive.json | 26 +- src/main/java/frc/robot/Constants.java | 80 +- src/main/java/frc/robot/Main.java | 6 +- src/main/java/frc/robot/Robot.java | 84 +- src/main/java/frc/robot/RobotContainer.java | 89 +- .../frc/robot/commands/HapticCommands.java | 8 +- .../frc/robot/commands/MechanismCommands.java | 110 +- .../commands/drivetrain/LockWheelsState.java | 59 +- .../robot/commands/drivetrain/TurnToTag.java | 29 +- .../frc/robot/shuffleboard/ClimberTab.java | 37 +- .../java/frc/robot/shuffleboard/HeadTab.java | 2 +- .../java/frc/robot/shuffleboard/LEDTab.java | 13 +- .../java/frc/robot/shuffleboard/MotorTab.java | 59 +- .../robot/shuffleboard/ShuffleboardInfo.java | 42 +- .../shuffleboard/ShuffleboardTabBase.java | 6 +- src/main/java/frc/robot/subsystems/Arm.java | 57 +- .../java/frc/robot/subsystems/Climber.java | 20 +- .../java/frc/robot/subsystems/Drivetrain.java | 137 +- src/main/java/frc/robot/subsystems/Head.java | 10 +- src/main/java/frc/robot/subsystems/LED.java | 20 +- src/main/java/frc/robot/util/Alert.java | 258 +-- .../java/frc/robot/util/LimelightHelpers.java | 1892 ++++++++--------- src/test/java/ConstantsTest.java | 6 +- 30 files changed, 1624 insertions(+), 1709 deletions(-) diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index a09b771..3c4c129 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { - "angleJoystickRadiusDeadband": 0.05, - "heading": { - "p": 0.8, - "i": 0, - "d": 0.01 - } -} \ No newline at end of file + "angleJoystickRadiusDeadband": 0.05, + "heading": { + "p": 0.8, + "i": 0, + "d": 0.01 + } +} diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index bb0044d..fcdcbd7 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -1,28 +1,27 @@ { - "drive": { - "type": "sparkmax", - "id": 13, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 3, - "canbus": null - }, - "encoder": { - "type": "canandcoder", - "id": 12, - "canbus": null - }, - "absoluteEncoderInverted": true, - "inverted": { - "drive": true, - "angle": true - }, - "absoluteEncoderOffset": 0.0, - - "location": { - "front": -10.5, - "left": 10.5 - } -} \ No newline at end of file + "drive": { + "type": "sparkmax", + "id": 13, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 3, + "canbus": null + }, + "encoder": { + "type": "canandcoder", + "id": 12, + "canbus": null + }, + "absoluteEncoderInverted": true, + "inverted": { + "drive": true, + "angle": true + }, + "absoluteEncoderOffset": 0.0, + "location": { + "front": -10.5, + "left": 10.5 + } +} diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 208f2ee..97f0950 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -1,27 +1,27 @@ { - "drive": { - "type": "sparkmax", - "id": 12, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 2, - "canbus": null - }, - "encoder": { - "type": "canandcoder", - "id": 11, - "canbus": null - }, - "inverted": { - "drive": true, - "angle": true - }, - "absoluteEncoderOffset": 0, - "absoluteEncoderInverted": true, - "location": { - "front": -10.5, - "left": -10.5 - } -} \ No newline at end of file + "drive": { + "type": "sparkmax", + "id": 12, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 2, + "canbus": null + }, + "encoder": { + "type": "canandcoder", + "id": 11, + "canbus": null + }, + "inverted": { + "drive": true, + "angle": true + }, + "absoluteEncoderOffset": 0, + "absoluteEncoderInverted": true, + "location": { + "front": -10.5, + "left": -10.5 + } +} diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 18e58b5..0f1f403 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -1,27 +1,27 @@ { - "drive": { - "type": "sparkmax", - "id": 14, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 4, - "canbus": null - }, - "encoder": { - "type": "canandcoder", - "id": 9, - "canbus": null - }, - "inverted": { - "drive": true, - "angle": true - }, - "absoluteEncoderOffset": 0, - "absoluteEncoderInverted": true, - "location": { - "front": 10.5, - "left": 10.5 - } -} \ No newline at end of file + "drive": { + "type": "sparkmax", + "id": 14, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 4, + "canbus": null + }, + "encoder": { + "type": "canandcoder", + "id": 9, + "canbus": null + }, + "inverted": { + "drive": true, + "angle": true + }, + "absoluteEncoderOffset": 0, + "absoluteEncoderInverted": true, + "location": { + "front": 10.5, + "left": 10.5 + } +} diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 91a5409..dff81df 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -1,27 +1,27 @@ { - "drive": { - "type": "sparkmax", - "id": 11, - "canbus": null - }, - "angle": { - "type": "sparkmax", - "id": 1, - "canbus": null - }, - "encoder": { - "type": "canandcoder", - "id": 10, - "canbus": null - }, - "inverted": { - "drive": true, - "angle": true - }, - "absoluteEncoderOffset": 0, - "absoluteEncoderInverted": true, - "location": { - "front": 10.5, - "left": -10.5 - } -} \ No newline at end of file + "drive": { + "type": "sparkmax", + "id": 11, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 1, + "canbus": null + }, + "encoder": { + "type": "canandcoder", + "id": 10, + "canbus": null + }, + "inverted": { + "drive": true, + "angle": true + }, + "absoluteEncoderOffset": 0, + "absoluteEncoderInverted": true, + "location": { + "front": 10.5, + "left": -10.5 + } +} diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 3a33df5..eb8c23e 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -1,16 +1,16 @@ { - "conversionFactor": { - "drive": 0.04808969374, - "angle": 360 - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - }, - "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12 -} \ No newline at end of file + "conversionFactor": { + "drive": 0.04808969374, + "angle": 360 + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "wheelGripCoefficientOfFriction": 1.19, + "optimalVoltage": 12 +} diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json index 24a37ec..b0b0869 100644 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -1,16 +1,16 @@ { - "drive": { - "p": 0.0011261, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - }, - "angle": { - "p": 0.01, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - } -} \ No newline at end of file + "drive": { + "p": 0.0011261, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.01, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json index 860ad8d..8770990 100644 --- a/src/main/deploy/swerve/swervedrive.json +++ b/src/main/deploy/swerve/swervedrive.json @@ -1,14 +1,14 @@ { - "imu": { - "type": "pigeon2", - "id": 1, - "canbus": "" - }, - "invertedIMU": false, - "modules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] -} \ No newline at end of file + "imu": { + "type": "pigeon2", + "id": 1, + "canbus": "" + }, + "invertedIMU": false, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7a5f445..6b112a0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,87 +25,82 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public final static double ENABLED_BATTERY_WARNING_VOLTAGE = 9.9; private final static double THE_NUMBER_THREE = 7; public final static double DISABLED_BATTERY_WARNING_VOLTAGE = 12.2; /** in meters */ public final static double PODIUM_DISTANCE = 5.0; - public static final int BRAKE_TOGGLE_BUTTON_DIO = 7; - + public static final int BRAKE_TOGGLE_BUTTON_DIO = 7; + // public static class TestNumber{ - // public static int number = 5; + // public static int number = 5; // } - + public static class ShootingConstants { public enum ShootingPosition { - //TODO: Rename DBOT to MID_STAGE to be more descriptive - AMP(1500.0, 84.5, ElevatorConstants.MAX_HEIGHT), - SUBWOOFER(5500.0, 14.0, ElevatorConstants.MAX_HEIGHT), - PODIUM(7000.0, 32.0, ElevatorConstants.MIN_HEIGHT), // todo where should elevator be? + // TODO: Rename DBOT to MID_STAGE to be more descriptive + AMP(1500.0, 84.5, ElevatorConstants.MAX_HEIGHT), SUBWOOFER(5500.0, 14.0, ElevatorConstants.MAX_HEIGHT), PODIUM(7000.0, 32.0, ElevatorConstants.MIN_HEIGHT), // todo where should elevator be? DBOT(8000.0, 37.520, ElevatorConstants.MIN_HEIGHT); // todo where should elevator be? - + ShootingPosition(double rpm, double arm_angle, double elevator_target) { this.rpm = rpm; this.arm_angle = arm_angle; this.elevator_target = elevator_target; } - + private final double rpm; private final double arm_angle; private final double elevator_target; - - public double rpm(){ + + public double rpm() { return rpm; } - - public double arm_angle(){ + + public double arm_angle() { return arm_angle; } - - public double elevator_target(){ + + public double elevator_target() { return elevator_target; } } - + public static final int AUTO_SHOOT_SPEED = 6000; public static final int VARIABLE_DISTANCE_SHOT = 6750; } - + public static class ArmConstants { - public static final int RIGHT_MOTOR_ID = 25; + public static final int RIGHT_MOTOR_ID = 25; public static final int LEFT_MOTOR_ID = 26; public static final int MAX_AMPERAGE = 40; public static final double ARM_OFFSET = 236.04; - + // Some relavant numbers // arm mass: 35-40 lbs // center of mass distance // when retracted: 14.04112 // when extended: 17.64693 - + // stow values // arm: 20.4 // elevator: retracted - - - + // all arm angle targets must be ~ 4 degrees more then what it should actually be public static final double MAX_ANGLE = 90; public static final double MIN_ANGLE = 2.5; - public static final double SOURCE_ANGLE = 64.5; //64 to compensate for change of ABEncoder offset hack; + public static final double SOURCE_ANGLE = 64.5; // 64 to compensate for change of ABEncoder offset hack; public static final double FLOOR_PICKUP = 3.75; public static final double STOW_ANGLE = 20.0; /** the mininum angle the arm can be where the elevator can pass over the bumper */ public static final double MIN_ABOVE_PASS_ANGLE = 18.5; - + // Constants for extended state - public static final double EXTENDED_KS = 0.17; //these values are based on the test arm + public static final double EXTENDED_KS = 0.17; // these values are based on the test arm public static final double EXTENDED_KG = 0.28; // .5 while extended public static final double EXTENDED_KV = 0.2; - + // Constants for retracted state - public static final double RETRACTED_KS = 0.17; //these values are based on calculator + public static final double RETRACTED_KS = 0.17; // these values are based on calculator public static final double RETRACTED_KG = 0.3; public static final double RETRACTED_KV = 0.1; public static final double KP = 0.028; // was 0.015 @@ -118,9 +113,8 @@ public static class ArmConstants { public static final double MAX_MANNUAL_ARM_SPEED = 45.0; public static final double ARM_VELOCITY_DEADBAND = 0.75; public static final double ARM_AT_TARGET_DEADBAND = 1.5; - } - + public static class ElevatorConstants { public static final boolean KILL_IT_ALL = false; public static final int RIGHT_MOTOR_ID = 28; @@ -134,7 +128,6 @@ public static class ElevatorConstants { public static final double KD = 0.0; public static final double kMinOutput = -.35; public static final double kMaxOutput = 0.60; - /** the maximum height the elevator can safely reach */ public static final double MAX_HEIGHT = 18.6; @@ -145,9 +138,8 @@ public static class ElevatorConstants { /** the maximum height the elevator can be, but still be safely inside the bumber of the robot */ public static final double MAX_MANUAL_SPEED = 20; public static final double ELEVATOR_AT_TARGET_DEADBAND = 2.0; - } - + public static class OperatorConstants { public static final double DRIVER_JOYSTICK_DEADBAND = 0.075; public static final double OPERATOR_JOYSTICK_DEADBAND = 0.125; @@ -199,12 +191,11 @@ public static class IntakeConstants { public static final int NOTE_SENSOR_DIO = 9; public static final int NOTE_ALIGNMENT_SENSOR_DIO = 6; - public static final double INTAKE_SPEED = 0.75; //0.95 - public static final double ALIGMNMENT_SPEED = 0.2; //0.08 + public static final double INTAKE_SPEED = 0.75; // 0.95 + public static final double ALIGMNMENT_SPEED = 0.2; // 0.08 public static final double OUTAKE_SPEED = -0.25; public static final double FEEDER_SPEED = 0.25; // What speed should a note be fed into the shooter at? } - public static class ShooterConstants { public static final int MOTOR_BOTTOM_CAN_ID = 23; @@ -216,7 +207,7 @@ public static class ShooterConstants { public static final double BOTTOM_kF = 0.004; public static final double BOTTOM_kMinOutput = -1; public static final double BOTTOM_kMaxOutput = 1; - + public static final double TOP_kP = 0.00026; public static final double TOP_kI = 0; public static final double TOP_kD = 0.0; @@ -224,11 +215,10 @@ public static class ShooterConstants { public static final double TOP_kMinOutput = -1; public static final double TOP_kMaxOutput = 1; - public static final double VELOCITY_MINIMUM = 0.5; public static final double VELOCITY_MAXIMUM = 2.0; } - + public static class ClimberConstants { public static final int LEFT_CLIMBER_PORT = 30; public static final int RIGHT_CLIMBER_PORT = 31; @@ -239,12 +229,12 @@ public static class ClimberConstants { public static final double CLIMB_HEIGHT = 60; public static final double CLIMB_RATE = .6; } - + public static final int NUMBER_OF_MOTORS = 10; public static class VisionConstants { - public static final double MAX_DETECTION_RANGE = 5.5;//TODO 3.2 - public static final Transform3d INTAKE_CAMERA_POSITION = new Transform3d(Units.inchesToMeters(10.0 + 5.0 / 8.0), -Units.inchesToMeters(-(11.0 + 3.0/6.0)), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(-45), 0)); - public static final Transform3d SHOOTER_CAMERA_POSITION = new Transform3d(-Units.inchesToMeters(-(10.0 + 5.0 / 8.0)), Units.inchesToMeters(11.0 + 3.0/6.0), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(45), Units.degreesToRadians(180))); + public static final double MAX_DETECTION_RANGE = 5.5;// TODO 3.2 + public static final Transform3d INTAKE_CAMERA_POSITION = new Transform3d(Units.inchesToMeters(10.0 + 5.0 / 8.0), -Units.inchesToMeters(-(11.0 + 3.0 / 6.0)), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(-45), 0)); + public static final Transform3d SHOOTER_CAMERA_POSITION = new Transform3d(-Units.inchesToMeters(-(10.0 + 5.0 / 8.0)), Units.inchesToMeters(11.0 + 3.0 / 6.0), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(45), Units.degreesToRadians(180))); } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 76c1b6f..65f99b8 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -14,12 +14,10 @@ * call. */ public final class Main { - private Main() { - } - + private Main() {} + /** * Main initialization function. Do not perform any initialization here. - * *

* If you change your main robot class, change the parameter type. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8a2f163..b03bd9d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,11 +23,11 @@ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; - + private RobotContainer m_robotContainer; - + private Timer disabledTimer; - + /** * This function is run when the robot is first started up and should be used * for any @@ -39,25 +39,23 @@ public void robotInit() { // and put our // autonomous chooser on the dashboard. DataLogManager.start(); - + DriverStation.startDataLog(DataLogManager.getLog()); m_robotContainer = new RobotContainer(); - - // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop - // immediately when disabled, but then also let it be pushed more - disabledTimer = new Timer(); - - for (int port = 5800; port <= 5807; port++) { - PortForwarder.add(port, "limelight.local", port); - } - + + // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop + // immediately when disabled, but then also let it be pushed more + disabledTimer = new Timer(); + + for (int port = 5800; port <= 5807; port++) { + PortForwarder.add(port, "limelight.local", port); + } } - + /** * This function is called every 20 ms, no matter the mode. Use this for items * like diagnostics * that you want ran during disabled, autonomous, teleoperated and test. - * *

* This runs after the mode specific periodic functions, but before LiveWindow * and @@ -74,7 +72,7 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } - + /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { @@ -82,17 +80,18 @@ public void disabledInit() { disabledTimer.reset(); disabledTimer.start(); } - + @Override public void disabledPeriodic() { - /* if (disabledTimer.hasElapsed(Constants.SwerveConstants.BRAKE_TIMER_DURATION)) - { - m_robotContainer.setMotorBrake(false); - disabledTimer.stop(); - } - */ + /* + * if (disabledTimer.hasElapsed(Constants.SwerveConstants.BRAKE_TIMER_DURATION)) + * { + * m_robotContainer.setMotorBrake(false); + * disabledTimer.stop(); + * } + */ } - + /** * This autonomous runs the autonomous command selected by your * {@link RobotContainer} class. @@ -101,21 +100,17 @@ public void disabledPeriodic() { public void autonomousInit() { m_robotContainer.doVisionUpdates(false); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - + // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } - - - } - + /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - } - + public void autonomousPeriodic() {} + @Override public void teleopInit() { // This makes sure that the autonomous stops running when @@ -125,38 +120,33 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - + m_robotContainer.StopShooter(); m_robotContainer.doVisionUpdates(true); m_robotContainer.setMotorBrake(true); // m_robotContainer.teleopInit(); - } - + /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } - + public void teleopPeriodic() {} + @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } - + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() { - } - + public void testPeriodic() {} + /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() { - } - + public void simulationInit() {} + /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() { - } + public void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78ff612..a074e64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,11 +84,12 @@ public class RobotContainer { private final Command absoluteDrive = drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> processJoystickAngular(driverControllerCommands.getRightX()), () -> processJoystickAngular(driverControllerCommands.getRightY())); private final Command rotationDrive = drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> processJoystickAngularButFree(driverControllerCommands.getRightX())); - - private final Command rotationDriveFast = drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> processJoystickAngularButFree(driverControllerCommands.getRightX()*SwerveConstants.FAST_TURN_TIME)); + + private final Command rotationDriveFast = drivetrain.driveCommand(() -> processJoystickVelocity(driverControllerCommands.getLeftY()), () -> processJoystickVelocity(driverControllerCommands.getLeftX()), () -> processJoystickAngularButFree(driverControllerCommands.getRightX() * SwerveConstants.FAST_TURN_TIME)); private final DigitalInput brakeToggleButton = new DigitalInput(Constants.BRAKE_TOGGLE_BUTTON_DIO); private boolean isClimbMode = false; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -99,14 +100,14 @@ public RobotContainer() { NamedCommands.registerCommand("turnToSpeaker", turnToSpeaker()); NamedCommands.registerCommand("turnTo0", pointAwayFromSpeaker()); NamedCommands.registerCommand("IntakeGround", MechanismCommands.IntakeGround(arm, head, false)); - NamedCommands.registerCommand("ShootSpeaker", MechanismCommands.AutonomousShoot(arm,head, drivetrain)); + NamedCommands.registerCommand("ShootSpeaker", MechanismCommands.AutonomousShoot(arm, head, drivetrain)); NamedCommands.registerCommand("shootAmp", MechanismCommands.AutonomousShoot(arm, head, ShootingPosition.AMP)); NamedCommands.registerCommand("Stow", MechanismCommands.AutoStowAndStopIntake(arm, head)); NamedCommands.registerCommand("TurnUp", turnSideways()); NamedCommands.registerCommand("StopShooter", head.SpinDownShooter()); NamedCommands.registerCommand("TurnDown", turnAwayFromAmp()); - NamedCommands.registerCommand("TurnAndShoot", Commands.sequence(turnToSpeaker(),MechanismCommands.AutonomousShoot(arm, head, drivetrain))); - NamedCommands.registerCommand("variableShoot", MechanismCommands.PrepareShoot(arm, head, ()->drivetrain.getDistanceToSpeaker()).andThen(MechanismCommands.Shoot(arm, head))); + NamedCommands.registerCommand("TurnAndShoot", Commands.sequence(turnToSpeaker(), MechanismCommands.AutonomousShoot(arm, head, drivetrain))); + NamedCommands.registerCommand("variableShoot", MechanismCommands.PrepareShoot(arm, head, () -> drivetrain.getDistanceToSpeaker()).andThen(MechanismCommands.Shoot(arm, head))); autoChooser = AutoBuilder.buildAutoChooser("mid start 2 piece"); @@ -132,7 +133,7 @@ public RobotContainer() { // STOP HERE shuffleboard.addTabs(tabs); - + Trigger brakeToggleTrigger = new Trigger(() -> brakeToggleButton.get()); brakeToggleTrigger.onTrue(arm.ToggleBrakeModes()); brakeToggleTrigger.onTrue(head.ToggleBreakModes()); @@ -142,24 +143,23 @@ public RobotContainer() { head.EnableBrakeMode(); })); } - - private void configureDefaultCommands(){ + + private void configureDefaultCommands() { drivetrain.setDefaultCommand(absoluteDrive); arm.setDefaultCommand(arm.ArmDefaultCommand(() -> Math.abs(operatorController.getRightY()) > OperatorConstants.OPERATOR_JOYSTICK_DEADBAND ? -operatorController.getRightY() * ArmConstants.MAX_MANNUAL_ARM_SPEED : 0, () -> Math.abs(operatorController.getLeftY()) > OperatorConstants.OPERATOR_JOYSTICK_DEADBAND ? -operatorController.getLeftY() * ElevatorConstants.MAX_MANUAL_SPEED : 0)); - } - - private void configureDriverBindings(){ + + private void configureDriverBindings() { driverControllerCommands.povRight().whileTrue(turnToSpeaker(() -> processJoystickVelocity(driverController.getLeftY()), () -> processJoystickVelocity(driverController.getLeftX()))); driverControllerCommands.leftBumper() .onTrue(new ScheduleCommand(rotationDrive)) .onFalse(Commands.runOnce(() -> rotationDrive.cancel())); - + driverControllerCommands.leftTrigger() .onTrue(new ScheduleCommand(rotationDriveFast)) .onFalse(Commands.runOnce(() -> rotationDriveFast.cancel())); - + driverControllerCommands.rightBumper() .onTrue(Commands.runOnce(() -> speedMultiplier = Math.max(.1, speedMultiplier - SwerveConstants.SLOW_SPEED_DECREMENT))) .onFalse(Commands.runOnce(() -> speedMultiplier += SwerveConstants.SLOW_SPEED_DECREMENT)); @@ -181,19 +181,22 @@ private void configureDriverBindings(){ driverControllerCommands.start().onTrue(Commands.runOnce(() -> drivetrain.zeroGyro())); driverControllerCommands.back().onTrue(Commands.runOnce(() -> drivetrain.doVisionUpdates(false))); - //TODO: Shoot should not need the position passed in - //TODO: Rename DBOT to MID_STAGE to be more descriptive - driverControllerCommands.a().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.DBOT)) + // TODO: Shoot should not need the position passed in + // TODO: Rename DBOT to MID_STAGE to be more descriptive + driverControllerCommands.a() + .onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.DBOT)) .onFalse(MechanismCommands.Shoot(driverController, operatorController, arm, head)); - - //TODO: This can be turned into a drive to source function - /*driverControllerCommands.b().onTrue(drivetrain.driveToPose( - new Pose2d(new Translation2d(2.9,4.11), new Rotation2d( Units.degreesToRadians(-60)))) - .andThen(Commands.runOnce(() ->drivetrain.resetLastAngeScalar())));*/ + + // TODO: This can be turned into a drive to source function + /* + * driverControllerCommands.b().onTrue(drivetrain.driveToPose( + * new Pose2d(new Translation2d(2.9,4.11), new Rotation2d( Units.degreesToRadians(-60)))) + * .andThen(Commands.runOnce(() ->drivetrain.resetLastAngeScalar()))); + */ // driverControllerCommands.b().onTrue(MechanismCommands.AutonomousShoot(arm,head,drivetrain)); } - - private void configureOperatorBindings(){ + + private void configureOperatorBindings() { operatorControllerCommands.x().and(() -> !isClimbMode).onTrue(arm.Stow()); operatorControllerCommands.y().and(() -> !isClimbMode).whileTrue(head.StartOutake()).onFalse(head.StopIntake()); operatorControllerCommands.a().and(() -> !isClimbMode).onTrue(MechanismCommands.IntakeGround(driverController, operatorController, arm, head).andThen(arm.Stow())); @@ -202,14 +205,14 @@ private void configureOperatorBindings(){ operatorControllerCommands.leftTrigger().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.AMP)); operatorControllerCommands.leftBumper().onTrue(MechanismCommands.Shoot(driverController, operatorController, arm, head)); - operatorControllerCommands.rightTrigger().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker)) - .onFalse(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker).andThen(MechanismCommands.Shoot(arm, head)).andThen(arm.Stow())); - + operatorControllerCommands.rightTrigger() + .onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker)) + .onFalse(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker).andThen(MechanismCommands.Shoot(arm, head)).andThen(arm.Stow())); + operatorControllerCommands.rightBumper().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.SUBWOOFER)).onFalse(MechanismCommands.Shoot(driverController, operatorController, arm, head)); operatorControllerCommands.povLeft().onTrue(head.StopIntake().andThen(head.SpinDownShooter())); operatorControllerCommands.povRight().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.PODIUM)).onFalse(MechanismCommands.Shoot(driverController, operatorController, arm, head)); - operatorControllerCommands.povUp().onTrue(Commands.runOnce(() -> climber.setSpeed(ClimberConstants.CLIMB_RATE, ClimberConstants.CLIMB_RATE), climber)).onFalse(Commands.runOnce(() -> climber.setSpeed(0, 0), climber)); operatorControllerCommands.povDown().onTrue(Commands.runOnce(() -> climber.setSpeed(-ClimberConstants.CLIMB_RATE, -ClimberConstants.CLIMB_RATE), climber)).onFalse(Commands.runOnce(() -> climber.setSpeed(0, 0), climber)); @@ -236,8 +239,8 @@ private void configureOperatorBindings(){ isClimbMode = !isClimbMode; })); } - - public String outputValues(Supplier distance, Supplier armAngle){ + + public String outputValues(Supplier distance, Supplier armAngle) { return "distance: " + distance.get() + "\n arm angle: " + armAngle.get(); } @@ -259,11 +262,10 @@ private double processJoystickAngular(double joystickInput) { private double processJoystickAngularButFree(double joystickInput) { return checkAllianceColors(Alliance.Blue) ? Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3) : Math.pow(-MathUtil.applyDeadband(joystickInput, OperatorConstants.DRIVER_JOYSTICK_DEADBAND), 3); } - - public void doVisionUpdates(boolean doVisionUpdates){ - drivetrain.doVisionUpdates(doVisionUpdates); - } + public void doVisionUpdates(boolean doVisionUpdates) { + drivetrain.doVisionUpdates(doVisionUpdates); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -302,29 +304,26 @@ public void teleopInit() { */ public Command pointAwayFromSpeaker() { if (checkAllianceColors(Alliance.Red)) { - return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(180)),Commands.waitSeconds(0.5)); + return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(180)), Commands.waitSeconds(0.5)); } - return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(0)),Commands.waitSeconds(1)); + return new ParallelRaceGroup(drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(0)), Commands.waitSeconds(1)); } - - public Command turnSideways(){ + + public Command turnSideways() { if (checkAllianceColors(Alliance.Red)) { return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(-90)); } return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(90)); } - - public Command turnAwayFromAmp(){ + + public Command turnAwayFromAmp() { if (checkAllianceColors(Alliance.Red)) { return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(90)); } return drivetrain.turnToAngleCommand(Rotation2d.fromDegrees(-90)); } - - public void StopShooter() - { + + public void StopShooter() { head.stopShooter(); } - - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/HapticCommands.java b/src/main/java/frc/robot/commands/HapticCommands.java index 5f58d3f..8d917ea 100644 --- a/src/main/java/frc/robot/commands/HapticCommands.java +++ b/src/main/java/frc/robot/commands/HapticCommands.java @@ -10,13 +10,13 @@ public class HapticCommands { * Trigger rumble on a controller for a specified amount of time. * * @param controller - * controller to rumble + * controller to rumble * @param type - * rumble type + * rumble type * @param strength - * rumble strength (0 to 1) + * rumble strength (0 to 1) * @param length - * rumble time (seconds) + * rumble time (seconds) */ public static Command HapticTap(XboxController controller, RumbleType type, double strength, double length) { return Commands.runOnce(() -> controller.setRumble(type, strength)) diff --git a/src/main/java/frc/robot/commands/MechanismCommands.java b/src/main/java/frc/robot/commands/MechanismCommands.java index 8f20445..98ff32c 100644 --- a/src/main/java/frc/robot/commands/MechanismCommands.java +++ b/src/main/java/frc/robot/commands/MechanismCommands.java @@ -20,9 +20,9 @@ public class MechanismCommands { * will finish after piece has been intaken * * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @return Command */ public static Command IntakeSource(Arm arm, Head head) { @@ -36,13 +36,13 @@ public static Command IntakeSource(Arm arm, Head head) { * will finish after piece has been intaken * * @param driverController - * for haptics + * for haptics * @param operatorController - * for haptics + * for haptics * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @return Command */ public static Command IntakeSource(XboxController driverController, XboxController operatorController, Arm arm, Head head) { @@ -55,30 +55,29 @@ public static Command IntakeSource(XboxController driverController, XboxControll * will finish after piece has been intaken * * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @return Command */ public static Command IntakeGround(Arm arm, Head head, boolean stopShooter) { - return - Commands.either(head.SpinDownShooter(), Commands.none(), () ->stopShooter) + return Commands.either(head.SpinDownShooter(), Commands.none(), () -> stopShooter) .andThen(() -> arm.setArmTarget(ArmConstants.FLOOR_PICKUP)) .andThen(() -> arm.setElevatorTarget(ElevatorConstants.MAX_HEIGHT)) .andThen(head.IntakePiece()); } - + /** * will finish after piece has been intaken * * @param driverController - * for haptics + * for haptics * @param operatorController - * for haptics + * for haptics * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @return Command */ public static Command IntakeGround(XboxController driverController, XboxController operatorController, Arm arm, Head head) { @@ -91,9 +90,9 @@ public static Command IntakeGround(XboxController driverController, XboxControll * cancel shoot and intake and stow * * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @return */ public static Command StowStopIntakeAndShooter(Arm arm, Head head) { @@ -101,18 +100,18 @@ public static Command StowStopIntakeAndShooter(Arm arm, Head head) { .andThen(head.StopIntake()) .andThen(head.SpinDownShooter()); } - - //THIS ISNT CODE DUPLICATION IT DOES A FUNDAMENTALLY DIFFERENT THING!!!!! - //TODO: I see that this is different, but when would it be used? Should be renamed to better describe - //what it does. Why doesn't it stop the shooter? Will it be used for auto? + + // THIS ISNT CODE DUPLICATION IT DOES A FUNDAMENTALLY DIFFERENT THING!!!!! + // TODO: I see that this is different, but when would it be used? Should be renamed to better describe + // what it does. Why doesn't it stop the shooter? Will it be used for auto? public static Command AutoStowAndStopIntake(Arm arm, Head head) { return arm.Stow() .andThen(head.StopIntake()); } - /** * will finish when ready + * * @param arm * @param head * @param position @@ -124,6 +123,7 @@ public static Command PrepareShoot(Arm arm, Head head, ShootingPosition position /** * will finish when ready + * * @param arm * @param head * @param position @@ -132,14 +132,15 @@ public static Command PrepareShoot(Arm arm, Head head, Supplier distance return arm.SetTargets(distance) .andThen(head.SpinUpShooter(ShootingConstants.VARIABLE_DISTANCE_SHOT)); } - + public static Command AutonomousPrepareShoot(Arm arm, Head head, Supplier distance) { return arm.SetTargetsAuto(distance) - .andThen(head.SpinUpShooter(ShootingConstants.AUTO_SHOOT_SPEED)); + .andThen(head.SpinUpShooter(ShootingConstants.AUTO_SHOOT_SPEED)); } /** * will finish when ready + * * @param operatorController * @param arm * @param head @@ -150,9 +151,10 @@ public static Command PrepareShoot(XboxController operatorController, Arm arm, H return PrepareShoot(arm, head, position) .andThen(new ScheduleCommand(HapticCommands.HapticTap(operatorController, RumbleType.kBothRumble, 0.3, 0.3))); } - + /** * will finish when ready + * * @param operatorController * @param arm * @param head @@ -163,67 +165,57 @@ public static Command PrepareShoot(XboxController operatorController, Arm arm, H return PrepareShoot(arm, head, distance) .andThen(new ScheduleCommand(HapticCommands.HapticTap(operatorController, RumbleType.kBothRumble, 0.3, 0.3))); } - + /** * will finish after piece has been shot * * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @param position - * position to shoot from + * position to shoot from * @return Command */ public static Command Shoot(Arm arm, Head head) { return Shoot(arm, head, true); } - - public static Command Shoot(Arm arm, Head head, boolean stopShooter) - { + + public static Command Shoot(Arm arm, Head head, boolean stopShooter) { return Commands.waitUntil(() -> arm.areArmAndElevatorAtTarget()) .andThen(head.Shoot(stopShooter)); - } - - public static Command AutonomousShoot(Arm arm, Head head, ShootingPosition position){ - - return PrepareShoot(arm, head, position) - .andThen(Shoot(arm, head, false)); - } - - - - public static Command AutonomousShoot(Arm arm, Head head, Drivetrain drivetrain){ - - return AutonomousPrepareShoot(arm, head, () -> drivetrain.getDistanceToSpeaker()) - - .andThen(Shoot(arm, head, false)); - - } + } + + public static Command AutonomousShoot(Arm arm, Head head, ShootingPosition position) { + return PrepareShoot(arm, head, position) + .andThen(Shoot(arm, head, false)); + } + + public static Command AutonomousShoot(Arm arm, Head head, Drivetrain drivetrain) { + return AutonomousPrepareShoot(arm, head, () -> drivetrain.getDistanceToSpeaker()) + + .andThen(Shoot(arm, head, false)); + } /** * will finish after piece has been shot * * @param driverController - * for haptics + * for haptics * @param operatorController - * for haptics + * for haptics * @param arm - * subsystem + * subsystem * @param head - * subsystem + * subsystem * @param position - * position to shoot from + * position to shoot from * @return Command */ - //TODO: Shoot should not take in the position + // TODO: Shoot should not take in the position public static Command Shoot(XboxController driverController, XboxController operatorController, Arm arm, Head head) { return Shoot(arm, head) .andThen(new ScheduleCommand(HapticCommands.HapticTap(driverController, RumbleType.kBothRumble, 0.3, 0.3))) .andThen(new ScheduleCommand(HapticCommands.HapticTap(operatorController, RumbleType.kBothRumble, 0.3, 0.3))); } - - - - } diff --git a/src/main/java/frc/robot/commands/drivetrain/LockWheelsState.java b/src/main/java/frc/robot/commands/drivetrain/LockWheelsState.java index f4418d7..81cc311 100644 --- a/src/main/java/frc/robot/commands/drivetrain/LockWheelsState.java +++ b/src/main/java/frc/robot/commands/drivetrain/LockWheelsState.java @@ -6,34 +6,35 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drivetrain; -/**A state that locks the wheels of the swerve drive to force it to remain stationary */ -public class LockWheelsState extends Command { - /** Creates a new LockWheelsState. */ - private final Drivetrain swerveDrive; - public LockWheelsState(Drivetrain swerveDrive) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(swerveDrive); - this.swerveDrive = swerveDrive; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - swerveDrive.lock(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } +/** A state that locks the wheels of the swerve drive to force it to remain stationary */ +public class LockWheelsState extends Command { + /** Creates a new LockWheelsState. */ + private final Drivetrain swerveDrive; + + public LockWheelsState(Drivetrain swerveDrive) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(swerveDrive); + this.swerveDrive = swerveDrive; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + swerveDrive.lock(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/commands/drivetrain/TurnToTag.java b/src/main/java/frc/robot/commands/drivetrain/TurnToTag.java index 3801469..b13a74c 100644 --- a/src/main/java/frc/robot/commands/drivetrain/TurnToTag.java +++ b/src/main/java/frc/robot/commands/drivetrain/TurnToTag.java @@ -27,7 +27,6 @@ public class TurnToTag extends Command { private boolean invertFacing = false; private final Supplier xMovement; private final Supplier yMovement; - public TurnToTag(Drivetrain drivetrain, int tagID) { // Use addRequirements() here to declare subsystem dependencies. @@ -46,11 +45,12 @@ public TurnToTag(Drivetrain drivetrain, int tagID) { xMovement = () -> 0.0; yMovement = () -> 0.0; } - public TurnToTag(Drivetrain drivetrain, int tagID, boolean invertFacing){ + + public TurnToTag(Drivetrain drivetrain, int tagID, boolean invertFacing) { this(drivetrain, tagID); this.invertFacing = invertFacing; } - + public TurnToTag(Drivetrain drivetrain, int tagID, Supplier yMovement, Supplier xMovement) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(drivetrain); @@ -68,8 +68,8 @@ public TurnToTag(Drivetrain drivetrain, int tagID, Supplier yMovement, S this.xMovement = xMovement; this.yMovement = yMovement; } - - public TurnToTag(Drivetrain drivetrain, int tagID, boolean invertFacing, Supplier yMovement, Supplier xMovement){ + + public TurnToTag(Drivetrain drivetrain, int tagID, boolean invertFacing, Supplier yMovement, Supplier xMovement) { this(drivetrain, tagID, yMovement, xMovement); this.invertFacing = invertFacing; } @@ -81,14 +81,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(invertFacing){ - drivetrain.drive(drivetrain.getTargetSpeeds(xMovement.get(), yMovement.get(), tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle().plus(Rotation2d.fromDegrees(180)))); + if (invertFacing) { + drivetrain.drive(drivetrain.getTargetSpeeds(xMovement.get(), yMovement.get(), tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle().plus(Rotation2d.fromDegrees(180)))); + } else { + drivetrain.drive(drivetrain.getTargetSpeeds(xMovement.get(), yMovement.get(), tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle())); } - else{ - drivetrain.drive(drivetrain.getTargetSpeeds(xMovement.get(), yMovement.get(), tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle())); - - } - } // Called once the command ends or is interrupted. @@ -96,17 +93,15 @@ public void execute() { public void end(boolean interrupted) { drivetrain.drive(new ChassisSpeeds()); drivetrain.resetLastAngeScalar(); - } // Returns true when the command should end. @Override public boolean isFinished() { - if (invertFacing){ + if (invertFacing) { return Math.abs(drivetrain.getHeading().minus(tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle().plus(Rotation2d.fromDegrees(180))).getDegrees()) <= SwerveConstants.TURN_TO_TAG_RANGE_FOR_END; - } - else{ - return Math.abs(drivetrain.getHeading().minus(tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle()).getDegrees()) <= SwerveConstants.TURN_TO_TAG_RANGE_FOR_END; + } else { + return Math.abs(drivetrain.getHeading().minus(tagPose.getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle()).getDegrees()) <= SwerveConstants.TURN_TO_TAG_RANGE_FOR_END; } } } diff --git a/src/main/java/frc/robot/shuffleboard/ClimberTab.java b/src/main/java/frc/robot/shuffleboard/ClimberTab.java index 8893055..0ffcceb 100644 --- a/src/main/java/frc/robot/shuffleboard/ClimberTab.java +++ b/src/main/java/frc/robot/shuffleboard/ClimberTab.java @@ -1,6 +1,5 @@ package frc.robot.shuffleboard; - import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -11,29 +10,27 @@ public class ClimberTab extends ShuffleboardTabBase { private final DoublePublisher leftMotorPosition; private final DoublePublisher leftMotorSpeed; private final DoublePublisher rightMotorPosition; - private final DoublePublisher rightMotorSpeed; + private final DoublePublisher rightMotorSpeed; private final MotorTab motorTab = new MotorTab(2, "climber"); - + public ClimberTab(Climber climber) { this.climber = climber; - + NetworkTableInstance inst = NetworkTableInstance.getDefault(); - + NetworkTable networkTable = inst.getTable("logging/climber"); - - leftMotorPosition = networkTable.getDoubleTopic("Left Motor Position").publish(); - - leftMotorSpeed = networkTable.getDoubleTopic("Left Motor Speed").publish(); - - rightMotorPosition = networkTable.getDoubleTopic("Right Motor Position").publish(); - + + leftMotorPosition = networkTable.getDoubleTopic("Left Motor Position").publish(); + + leftMotorSpeed = networkTable.getDoubleTopic("Left Motor Speed").publish(); + + rightMotorPosition = networkTable.getDoubleTopic("Right Motor Position").publish(); + rightMotorSpeed = networkTable.getDoubleTopic("Right Motor Speed").publish(); - + motorTab.addMotor(climber.getMotors()); - - } - + @Override public void update() { leftMotorPosition.set(climber.getPositionLeftMotor()); @@ -42,14 +39,12 @@ public void update() { rightMotorSpeed.set(climber.getSpeedRight()); motorTab.update(); } - + @Override - public void activateShuffleboard() { - } - + public void activateShuffleboard() {} + @Override public String getNetworkTable() { return "climber"; } - } diff --git a/src/main/java/frc/robot/shuffleboard/HeadTab.java b/src/main/java/frc/robot/shuffleboard/HeadTab.java index 6af4b5e..d82c90e 100644 --- a/src/main/java/frc/robot/shuffleboard/HeadTab.java +++ b/src/main/java/frc/robot/shuffleboard/HeadTab.java @@ -87,7 +87,7 @@ public void activateShuffleboard() { shooterLayout.add("Spin Up (speaker)", head.SpinUpShooter(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(2, 0); shooterLayout.add("Spin Down", head.SpinDownShooter()).withPosition(2, 1); - //shooterLayout.add("Shoot (speaker)", head.Shoot(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(2, 2); + // shooterLayout.add("Shoot (speaker)", head.Shoot(ShootingConstants.ShootingPosition.SUBWOOFER)).withPosition(2, 2); } @Override diff --git a/src/main/java/frc/robot/shuffleboard/LEDTab.java b/src/main/java/frc/robot/shuffleboard/LEDTab.java index d3f5020..ec959ed 100644 --- a/src/main/java/frc/robot/shuffleboard/LEDTab.java +++ b/src/main/java/frc/robot/shuffleboard/LEDTab.java @@ -10,20 +10,20 @@ public class LEDTab extends ShuffleboardTabBase { private final LED led; - + public LEDTab(LED led) { this.led = led; - + NetworkTableInstance inst = NetworkTableInstance.getDefault(); - + NetworkTable networkTable = inst.getTable("logging/LED"); } - + @Override public void update() { // these functions have not been defined } - + @Override public void activateShuffleboard() { ShuffleboardTab tab = Shuffleboard.getTab("LED"); @@ -34,10 +34,9 @@ public void activateShuffleboard() { tab.add("Teleop Disabled Animation", new InstantCommand(() -> led.setTeleopDisabledAnimation()).ignoringDisable(true)); tab.add("Teleop Enabled Animation", new InstantCommand(() -> led.setTeleopEnabledAnimation()).ignoringDisable(true)); } - + @Override public String getNetworkTable() { return "LED"; } - } diff --git a/src/main/java/frc/robot/shuffleboard/MotorTab.java b/src/main/java/frc/robot/shuffleboard/MotorTab.java index 808a42e..c5e42b1 100644 --- a/src/main/java/frc/robot/shuffleboard/MotorTab.java +++ b/src/main/java/frc/robot/shuffleboard/MotorTab.java @@ -14,7 +14,7 @@ /** * This class is used to create a tab on the shuffleboard that displays information about the motor power and current */ -public class MotorTab{ +public class MotorTab { private final DoublePublisher[] busVoltagePublishers; private final DoublePublisher[] optionCurrentPublishers; private final DoublePublisher[] stickyFaultPublisher; @@ -23,73 +23,80 @@ public class MotorTab{ private final CANSparkMax[] motors = new CANSparkMax[Constants.NUMBER_OF_MOTORS]; private int numberOfMotors = 0; private final int totalNumberOfMotors; - + private final Alert tooManyMotors = new Alert("Too many motors", AlertType.ERROR); - + private final String tabName; private final ShuffleboardTab tab; private final int rowOffset; + /** * This constructor is used to create a MotorTab, periodic() must be called in the subsystem's periodic method - * @param totalNumberOfMotors the total number of motors that will be added to the MotorTab - * @param tabName the name of the tab - * @param rowOffset the number of rows that all of the motor displays will be lowered by + * + * @param totalNumberOfMotors + * the total number of motors that will be added to the MotorTab + * @param tabName + * the name of the tab + * @param rowOffset + * the number of rows that all of the motor displays will be lowered by */ - public MotorTab(int totalNumberOfMotors, String tabName, int rowOffset){ + public MotorTab(int totalNumberOfMotors, String tabName, int rowOffset) { busVoltagePublishers = new DoublePublisher[totalNumberOfMotors]; optionCurrentPublishers = new DoublePublisher[totalNumberOfMotors]; stickyFaultPublisher = new DoublePublisher[totalNumberOfMotors]; motorTemperaturePublishers = new DoublePublisher[totalNumberOfMotors]; motorEncoderPublishers = new DoublePublisher[totalNumberOfMotors]; - + this.tabName = tabName; this.totalNumberOfMotors = totalNumberOfMotors; tab = Shuffleboard.getTab(tabName); this.rowOffset = rowOffset; } - + /** * This constructor is used to create a MotorTab, periodic() must be called in the subsystem's periodic method - * @param totalNumberOfMotors the total number of motors that will be added to the MotorTab - * @param tabName the name of the tab + * + * @param totalNumberOfMotors + * the total number of motors that will be added to the MotorTab + * @param tabName + * the name of the tab */ - public MotorTab(int totalNumberOfMotors, String tabName){ + public MotorTab(int totalNumberOfMotors, String tabName) { this(totalNumberOfMotors, tabName, 0); } - - + /** * This method is used to add a motor to the MotorTab - * @param newMotors an array of the new motors to be added + * + * @param newMotors + * an array of the new motors to be added */ - public void addMotor(CANSparkMax[] newMotors){ - if(newMotors.length + numberOfMotors > totalNumberOfMotors){ + public void addMotor(CANSparkMax[] newMotors) { + if (newMotors.length + numberOfMotors > totalNumberOfMotors) { tooManyMotors.setText("Too many motors. Increase Constants.NUMBER_OF_MOTORS to: " + (newMotors.length + numberOfMotors)); tooManyMotors.set(true); return; } - + NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTable networkTable = inst.getTable("logging/" + tabName); - for(int i = 0; i < newMotors.length; i++){ + for (int i = 0; i < newMotors.length; i++) { motors[i + numberOfMotors] = newMotors[i]; busVoltagePublishers[i + numberOfMotors] = networkTable.getDoubleTopic("Motor: " + (newMotors[i].getDeviceId()) + " Bus Voltage").publish(); optionCurrentPublishers[i + numberOfMotors] = networkTable.getDoubleTopic("Motor: " + (newMotors[i].getDeviceId()) + " Total Current").publish(); stickyFaultPublisher[i + numberOfMotors] = networkTable.getDoubleTopic("Motor: " + (newMotors[i].getDeviceId()) + " Sticky Faults").publish(); motorTemperaturePublishers[i + numberOfMotors] = networkTable.getDoubleTopic("Motor: " + (newMotors[i].getDeviceId()) + " Motor Temperature").publish(); motorEncoderPublishers[i + numberOfMotors] = networkTable.getDoubleTopic("Motor: " + (newMotors[i].getDeviceId()) + " Encoder Position").publish(); - - // create shuffleboard entries for each of these with with a position + // create shuffleboard entries for each of these with with a position } numberOfMotors += newMotors.length; - } - + /** * this method must be called in activateShuffleboard() to add the motors to the shuffleboard in the correct position */ - public void activateShuffleboard(){ + public void activateShuffleboard() { for (int i = 0; i < motors.length; i++) { tab.add("Motor: " + (motors[i].getDeviceId()) + " Bus Voltage", busVoltagePublishers[i]).withPosition(0, i + rowOffset); tab.add("Motor: " + (motors[i].getDeviceId()) + " Total Current", optionCurrentPublishers[i]).withPosition(1, i + rowOffset); @@ -98,6 +105,7 @@ public void activateShuffleboard(){ tab.add("Motor: " + (motors[i].getDeviceId()) + " Encoder Position", motorEncoderPublishers[i]).withPosition(4, i + rowOffset); } } + /** this MUST be called in periodic() */ public void update() { for (int i = 0; i < numberOfMotors; i++) { @@ -108,7 +116,4 @@ public void update() { motorEncoderPublishers[i].set(motors[i].getEncoder().getPosition()); } } - - - } diff --git a/src/main/java/frc/robot/shuffleboard/ShuffleboardInfo.java b/src/main/java/frc/robot/shuffleboard/ShuffleboardInfo.java index a8f7394..f5355fa 100644 --- a/src/main/java/frc/robot/shuffleboard/ShuffleboardInfo.java +++ b/src/main/java/frc/robot/shuffleboard/ShuffleboardInfo.java @@ -17,31 +17,29 @@ public class ShuffleboardInfo extends SubsystemBase { ArrayList tabs; - + private static ShuffleboardInfo instance; private String[] copyTables; private boolean isActivated = false; private Alert enabledAlert = new Alert("Enabled batter voltage is below " + Constants.ENABLED_BATTERY_WARNING_VOLTAGE, AlertType.ERROR); private Alert disabledAlert = new Alert("Disabled batter voltage is below " + Constants.DISABLED_BATTERY_WARNING_VOLTAGE, AlertType.ERROR); - + public static ShuffleboardInfo getInstance() { if (instance == null) { instance = new ShuffleboardInfo(); } return instance; } - - private ShuffleboardInfo() { - - } - + + private ShuffleboardInfo() {} + public void addTabs(ArrayList tabs) { this.tabs = tabs; - + tabs.get(0).activateShuffleboard(); copyTables = new String[tabs.size() - 1]; // subtract one because driverstation doesn't need to be copied } - + public void activateTabs() { if (isActivated == false) { isActivated = true; @@ -53,7 +51,7 @@ public void activateTabs() { } } } - + @Override public void periodic() { // if robot is not connected to the field system, enable shuffleboard @@ -67,25 +65,23 @@ public void periodic() { tabs.get(i).update(); } } - + // copy over values from the logging network table to shuffleboard // NetworkTableInstance inst = NetworkTableInstance.getDefault(); // for (String tab : copyTables) { - // if (tab != null) { - // for (String key : inst.getTable("logging/" + tab).getKeys()) { - // NetworkTableEntry sourceEntry = inst.getTable("logging/" + tab).getEntry(key); - // NetworkTableEntry destinationEntry = inst.getTable("Shuffleboard/" + tab).getEntry(key); - // destinationEntry.setValue(sourceEntry.getValue()); - - // } - // } + // if (tab != null) { + // for (String key : inst.getTable("logging/" + tab).getKeys()) { + // NetworkTableEntry sourceEntry = inst.getTable("logging/" + tab).getEntry(key); + // NetworkTableEntry destinationEntry = inst.getTable("Shuffleboard/" + tab).getEntry(key); + // destinationEntry.setValue(sourceEntry.getValue()); + // } - - + // } + // } + // create an Alert if the battery voltage is below BATTERY_WARNING_VOLTAGE enabledAlert.set(RobotController.getBatteryVoltage() < Constants.ENABLED_BATTERY_WARNING_VOLTAGE && DriverStation.isEnabled()); disabledAlert.set(RobotController.getBatteryVoltage() < Constants.DISABLED_BATTERY_WARNING_VOLTAGE && !DriverStation.isEnabled()); - //batter voltage is displayed on the driver station tab - + // batter voltage is displayed on the driver station tab } } diff --git a/src/main/java/frc/robot/shuffleboard/ShuffleboardTabBase.java b/src/main/java/frc/robot/shuffleboard/ShuffleboardTabBase.java index e02ae5f..c926b19 100644 --- a/src/main/java/frc/robot/shuffleboard/ShuffleboardTabBase.java +++ b/src/main/java/frc/robot/shuffleboard/ShuffleboardTabBase.java @@ -2,22 +2,20 @@ /** * The base for shuffleboard tabs. Extend this class to create your tab. - * * ALWAYS push your output to */ public abstract class ShuffleboardTabBase { - /** * This will be called every 20ms. */ public abstract void update(); - + /** * This will be called when the tab is added to shuffleboard. Prior to this data * can be written to network tables but a shuffleboard tab can never be created. */ public abstract void activateShuffleboard(); - + /** * return the name of the network table being pushed to (not including the * logging/) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a510470..e7133c2 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -80,7 +80,7 @@ public class Arm extends SubsystemBase { private Timer time = new Timer(); private final MotorTab motorTab = new MotorTab(4, "arm", 2); - + /** Creates a new Arm. */ public Arm() { // setup arm motors @@ -119,18 +119,18 @@ public Arm() { armAbsoluteEncoder.setZeroOffset(ArmConstants.ARM_OFFSET); armTarget = armAbsoluteEncoder.getPosition(); - + armAngleBasedOnDistanceExtended.put(1.27, ShootingPosition.SUBWOOFER.arm_angle()); armAngleBasedOnDistanceExtended.put(2.9, 33.5); armAngleBasedOnDistanceExtended.put(3.1, 36.0); - - //TODO: Add Treemap values + + // TODO: Add Treemap values armAngleBasedOnDistanceRetracted.put(1.96, 18.6); armAngleBasedOnDistanceRetracted.put(2.47, 27.0); armAngleBasedOnDistanceRetracted.put(2.92, 31.8); armAngleBasedOnDistanceRetracted.put(2.96, 30.9); armAngleBasedOnDistanceRetracted.put(3.51, 35.1); - armAngleBasedOnDistanceRetracted.put(3.61,32.7); + armAngleBasedOnDistanceRetracted.put(3.61, 32.7); armAngleBasedOnDistanceRetracted.put(4.11, 37.3); armAngleBasedOnDistanceRetracted.put(4.25, 38.3); armAngleBasedOnDistanceRetracted.put(4.29, 38.3); @@ -196,7 +196,7 @@ private ArmFeedforward getArmFeedforward() { * safely set the target angle for the arm * * @param targetDegrees - * the target angle for the arm in degrees + * the target angle for the arm in degrees */ public void setArmTarget(double targetDegrees) { // make sure the move can be done safely @@ -204,21 +204,20 @@ public void setArmTarget(double targetDegrees) { armTarget = targetDegrees; } - + /** * sets the arm target based on the distance to the speaker and the interpolation table * * @param distance - * the distance to the speaker in meters + * the distance to the speaker in meters */ public void setArmTargetByDistanceExtended(double distance) { armTarget = MathUtil.clamp(armAngleBasedOnDistanceExtended.get(distance), ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE); } - + public void setArmTargetByDistanceRetracted(double distance) { armTarget = MathUtil.clamp(armAngleBasedOnDistanceRetracted.get(distance), ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE); } - public Command RaiseElevator() { return this.runOnce(() -> setElevatorTarget(ElevatorConstants.MAX_HEIGHT)); @@ -240,12 +239,11 @@ public Command Stow() { }); } - /** * sets the velocity for the arm by moving a position setpoint * * @param velocityDegreesPerSec - * the velocity for the arm in degrees per second + * the velocity for the arm in degrees per second */ public void setArmVelocity(double velocityDegreesPerSec) { armTarget = armTarget + velocityDegreesPerSec * dt; @@ -256,7 +254,7 @@ public void setArmVelocity(double velocityDegreesPerSec) { * safely set the target height for the elevator * * @param target - * the target height for the elevator in inches + * the target height for the elevator in inches */ public void setElevatorTarget(double target) { // make sure the move can be done safely @@ -270,27 +268,27 @@ public void setElevatorTarget(double target) { } elevatorTarget = target; } - + public Command SetTargets(ShootingPosition position) { return Commands.runOnce(() -> { - setArmTarget(position.arm_angle()); - setElevatorTarget(position.elevator_target()); + setArmTarget(position.arm_angle()); + setElevatorTarget(position.elevator_target()); }); } - - public Command SetTargets(Supplier distance){ + + public Command SetTargets(Supplier distance) { return Commands.runOnce(() -> { - //TODO: Remove me! - System.out.println("Distance is " + distance.get()); - setArmTargetByDistanceRetracted(distance.get()); - setElevatorTarget(ElevatorConstants.MIN_HEIGHT); + // TODO: Remove me! + System.out.println("Distance is " + distance.get()); + setArmTargetByDistanceRetracted(distance.get()); + setElevatorTarget(ElevatorConstants.MIN_HEIGHT); }); } - + public Command SetTargetsAuto(Supplier distance) { return Commands.runOnce(() -> { - setArmTargetByDistanceExtended(distance.get()); - setElevatorTarget(ElevatorConstants.MAX_HEIGHT); + setArmTargetByDistanceExtended(distance.get()); + setElevatorTarget(ElevatorConstants.MAX_HEIGHT); }); } @@ -298,7 +296,7 @@ public Command SetTargetsAuto(Supplier distance) { * sets the velocity for the elevator by moving a position setpoint * * @param velocityDegreesPerSec - * the velocity for the elevator in degrees per second + * the velocity for the elevator in degrees per second */ public void setElevatorVelocity(double velocityDegreesPerSec) { elevatorTarget = elevatorTarget + velocityDegreesPerSec * dt; @@ -406,9 +404,8 @@ public double getElevatorAbsoluteEncoderPosition() { public MotorTab getMotorTab() { return motorTab; } - - public boolean areArmAndElevatorAtTarget(){ - return (Math.abs(armTarget - armAbsoluteEncoder.getPosition()) < ArmConstants.ARM_AT_TARGET_DEADBAND) - && (Math.abs(elevatorTarget - elevatorEncoder.getPosition()) < ElevatorConstants.ELEVATOR_AT_TARGET_DEADBAND || ElevatorConstants.KILL_IT_ALL); + + public boolean areArmAndElevatorAtTarget() { + return (Math.abs(armTarget - armAbsoluteEncoder.getPosition()) < ArmConstants.ARM_AT_TARGET_DEADBAND) && (Math.abs(elevatorTarget - elevatorEncoder.getPosition()) < ElevatorConstants.ELEVATOR_AT_TARGET_DEADBAND || ElevatorConstants.KILL_IT_ALL); } } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 8cd6dbb..4330419 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -38,17 +38,16 @@ public Climber() { rightClimberEncoder.setPosition(0.0); // balanceController.setSetpoint(0.0); - + burnFlash(); } - - private void burnFlash(){ + + private void burnFlash() { Timer.delay(0.005); leftClimber.burnFlash(); Timer.delay(0.005); rightClimber.burnFlash(); Timer.delay(0.005); - } public void setSpeed(double leftSpeed, double rightSpeed) { @@ -74,8 +73,8 @@ public void setSpeedRight(double rightSpeed) { } // public void resetClimberEncoders() { - // leftClimberEncoder.setPosition(0.0); - // rightClimberEncoder.setPosition(0.0); + // leftClimberEncoder.setPosition(0.0); + // rightClimberEncoder.setPosition(0.0); // } public double getPositionRightMotor() { @@ -87,10 +86,9 @@ public double getPositionLeftMotor() { } @Override - public void periodic() { - } - - public CANSparkMax[] getMotors(){ - return new CANSparkMax[] {leftClimber, rightClimber}; + public void periodic() {} + + public CANSparkMax[] getMotors() { + return new CANSparkMax[] { leftClimber, rightClimber }; } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index b365769..995f27f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,7 +4,6 @@ package frc.robot.subsystems; - import frc.robot.util.LimelightHelpers; import com.pathplanner.lib.auto.AutoBuilder; @@ -59,18 +58,18 @@ public class Drivetrain extends SubsystemBase { private AprilTagFieldLayout fieldLayout; private boolean doVisionUpdates = false; - + private Timer timer = new Timer(); - + private LimelightHelpers.PoseEstimate poseData; - + private Vector kalmanStdDevs = VecBuilder.fill(.7, .7, Integer.MAX_VALUE); - + /** * Initialize {@link SwerveDrive} with the directory provided. * * @param directory - * Directory of swerve drive config files. + * Directory of swerve drive config files. */ public Drivetrain() { // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary @@ -90,7 +89,7 @@ public Drivetrain() { } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot - // via angle. + // via angle. setupPathPlanner(); timer.start(); @@ -105,7 +104,7 @@ public void setupPathPlanner() { this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your - // Constants class + // Constants class new PIDConstants(Constants.AutoConstants.LINEAR_AUTO_KP, Constants.AutoConstants.LINEAR_AUTO_KI, Constants.AutoConstants.LINEAR_AUTO_KD), // Translation PID constants new PIDConstants(swerveDrive.swerveController.config.headingPIDF.p, swerveDrive.swerveController.config.headingPIDF.i, swerveDrive.swerveController.config.headingPIDF.d), @@ -131,9 +130,9 @@ public void setupPathPlanner() { * Get the path follower with events. * * @param pathName - * PathPlanner path name. + * PathPlanner path name. * @param setOdomToStart - * Set the odometry position to the start of the path. + * Set the odometry position to the start of the path. * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. */ public Command getAutonomousCommand(String pathName, boolean setOdomToStart) { @@ -148,25 +147,23 @@ public Command getAutonomousCommand(String pathName, boolean setOdomToStart) { // event markers. return AutoBuilder.followPath(path); } - - public Command driveToPose(Pose2d pose){ - PathConstraints constraints = new PathConstraints( - swerveDrive.getMaximumVelocity(), 4.0, swerveDrive.getMaximumAngularVelocity(), Units.degreesToRadians(720)); + + public Command driveToPose(Pose2d pose) { + PathConstraints constraints = new PathConstraints(swerveDrive.getMaximumVelocity(), 4.0, swerveDrive.getMaximumAngularVelocity(), Units.degreesToRadians(720)); return AutoBuilder.pathfindToPose(pose, constraints, 0.0, 0.0); - } /** * Command to drive the robot using translative values and heading as a setpoint. * * @param translationX - * Translation in the X direction. Cubed for smoother controls. + * Translation in the X direction. Cubed for smoother controls. * @param translationY - * Translation in the Y direction. Cubed for smoother controls. + * Translation in the Y direction. Cubed for smoother controls. * @param headingX - * Heading X to calculate angle of the joystick. + * Heading X to calculate angle of the joystick. * @param headingY - * Heading Y to calculate angle of the joystick. + * Heading Y to calculate angle of the joystick. * @return Drive command. */ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY) { @@ -182,11 +179,11 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat * Command to drive the robot using translative values and heading as a setpoint. * * @param translationX - * Translation in the X direction. + * Translation in the X direction. * @param translationY - * Translation in the Y direction. + * Translation in the Y direction. * @param rotation - * Rotation as a value between [-1, 1] converted to radians. + * Rotation as a value between [-1, 1] converted to radians. * @return Drive command. */ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { @@ -200,11 +197,11 @@ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier trans * Command to drive the robot using translative values and heading as angular velocity. * * @param translationX - * Translation in the X direction. Cubed for smoother controls. + * Translation in the X direction. Cubed for smoother controls. * @param translationY - * Translation in the Y direction. Cubed for smoother controls. + * Translation in the Y direction. Cubed for smoother controls. * @param angularRotationX - * Angular velocity of the robot to set. Cubed for smoother controls. + * Angular velocity of the robot to set. Cubed for smoother controls. * @return Drive command. */ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { @@ -229,16 +226,16 @@ public Command turnToAngleCommand(Rotation2d angle) { * the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. * * @param translation - * {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall - * (field North) and positive y is torwards the left wall when looking through the driver station - * glass (field West). + * {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall + * (field North) and positive y is torwards the left wall when looking through the driver station + * glass (field West). * @param rotation - * Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. + * Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. * @param fieldRelative - * Drive mode. True for field-relative, false for robot-relative. + * Drive mode. True for field-relative, false for robot-relative. */ public void drive(Translation2d translation, double rotation, boolean fieldRelative) { swerveDrive.drive(translation, rotation, fieldRelative, false); // Open loop is disabled since it shouldn't be used most of the time. @@ -248,7 +245,7 @@ public void drive(Translation2d translation, double rotation, boolean fieldRelat * Drive the robot given a chassis field oriented velocity. * * @param velocity - * Velocity according to the field. + * Velocity according to the field. */ public void driveFieldOriented(ChassisSpeeds velocity) { swerveDrive.driveFieldOriented(velocity); @@ -258,7 +255,7 @@ public void driveFieldOriented(ChassisSpeeds velocity) { * Drive according to the chassis robot oriented velocity. * * @param velocity - * Robot oriented {@link ChassisSpeeds} + * Robot oriented {@link ChassisSpeeds} */ public void drive(ChassisSpeeds velocity) { swerveDrive.drive(velocity); @@ -267,10 +264,9 @@ public void drive(ChassisSpeeds velocity) { @Override public void periodic() { if (doVisionUpdates) { - try{ - processVision(); - } - catch(Exception e){} + try { + processVision(); + } catch (Exception e) {} } motorTab.update(); } @@ -279,23 +275,23 @@ public void periodic() { public void simulationPeriodic() {} private void processVision() { - //if(checkAllianceColors(Alliance.Blue)){ - LimelightHelpers.SetRobotOrientation("", getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); - //} - /*else{ - LimelightHelpers.SetRobotOrientation("", getPose().getRotation().plus(Rotation2d.fromDegrees(180)).getDegrees(), 0, 0, 0, 0, 0); - }*/ - - poseData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(""); - if (poseData.tagCount > 0 ) { - if( fieldLayout.getTagPose((int)LimelightHelpers.getFiducialID("")).orElseThrow().toPose2d().getTranslation(). - getDistance(getPose().getTranslation()) < VisionConstants.MAX_DETECTION_RANGE){ - swerveDrive.addVisionMeasurement(poseData.pose,poseData.timestampSeconds, kalmanStdDevs); - - } + // if(checkAllianceColors(Alliance.Blue)){ + LimelightHelpers.SetRobotOrientation("", getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); + // } + /* + * else{ + * LimelightHelpers.SetRobotOrientation("", getPose().getRotation().plus(Rotation2d.fromDegrees(180)).getDegrees(), 0, 0, 0, 0, 0); + * } + */ + + poseData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(""); + if (poseData.tagCount > 0) { + if (fieldLayout.getTagPose((int) LimelightHelpers.getFiducialID("")).orElseThrow().toPose2d().getTranslation().getDistance(getPose().getTranslation()) < VisionConstants.MAX_DETECTION_RANGE) { + swerveDrive.addVisionMeasurement(poseData.pose, poseData.timestampSeconds, kalmanStdDevs); } + } } - + private boolean checkAllianceColors(Alliance checkAgainst) { if (DriverStation.getAlliance().isPresent()) { return DriverStation.getAlliance().get() == checkAgainst; @@ -320,11 +316,10 @@ public SwerveDriveKinematics getKinematics() { * keep working. * * @param initialHolonomicPose - * The pose to set the odometry to + * The pose to set the odometry to */ public void resetOdometry(Pose2d initialHolonomicPose) { - - swerveDrive.setGyro(new Rotation3d(0,0,initialHolonomicPose.getRotation().getRadians())); + swerveDrive.setGyro(new Rotation3d(0, 0, initialHolonomicPose.getRotation().getRadians())); swerveDrive.resetOdometry(initialHolonomicPose); } @@ -342,7 +337,7 @@ public Pose2d getPose() { * Set chassis speeds with closed-loop velocity control. * * @param chassisSpeeds - * Chassis Speeds to set. + * Chassis Speeds to set. */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { swerveDrive.setChassisSpeeds(chassisSpeeds); @@ -352,7 +347,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { * Post the trajectory to the field. * * @param trajectory - * The trajectory to post. + * The trajectory to post. */ public void postTrajectory(Trajectory trajectory) { swerveDrive.postTrajectory(trajectory); @@ -362,7 +357,7 @@ public void postTrajectory(Trajectory trajectory) { * Sets the drive motors to brake/coast mode. * * @param brake - * True to set motors to brake mode, false for coast. + * True to set motors to brake mode, false for coast. */ public void setMotorBrake(boolean brake) { swerveDrive.setMotorIdleMode(brake); @@ -385,13 +380,13 @@ public void doVisionUpdates(boolean doVisionUpdates) { * the angle of the robot. * * @param xInput - * X joystick input for the robot to move in the X direction. + * X joystick input for the robot to move in the X direction. * @param yInput - * Y joystick input for the robot to move in the Y direction. + * Y joystick input for the robot to move in the Y direction. * @param headingX - * X joystick which controls the angle of the robot. + * X joystick which controls the angle of the robot. * @param headingY - * Y joystick which controls the angle of the robot. + * Y joystick which controls the angle of the robot. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) { @@ -407,8 +402,8 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin public Rotation2d getHeading() { return swerveDrive.getOdometryHeading(); } - - public void setHeadingCorrection(boolean doHeadingCorrection){ + + public void setHeadingCorrection(boolean doHeadingCorrection) { swerveDrive.setHeadingCorrection(doHeadingCorrection); } @@ -425,11 +420,11 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double thetaI * 90deg. * * @param xInput - * X joystick input for the robot to move in the X direction. + * X joystick input for the robot to move in the X direction. * @param yInput - * Y joystick input for the robot to move in the Y direction. + * Y joystick input for the robot to move in the Y direction. * @param angle - * The angle in as a {@link Rotation2d}. + * The angle in as a {@link Rotation2d}. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { @@ -491,8 +486,8 @@ public void lock() { public Rotation2d getPitch() { return swerveDrive.getPitch(); } - - public void resetLastAngeScalar(){ + + public void resetLastAngeScalar() { swerveDrive.swerveController.lastAngleScalar = getHeading().getRadians(); } diff --git a/src/main/java/frc/robot/subsystems/Head.java b/src/main/java/frc/robot/subsystems/Head.java index a5dcdd7..d323b77 100644 --- a/src/main/java/frc/robot/subsystems/Head.java +++ b/src/main/java/frc/robot/subsystems/Head.java @@ -139,11 +139,11 @@ private void setShooterSpeed(double rpm) { shooterControllerBottom.setReference(shooterSetPoint, ControlType.kVelocity); shooterControllerTop.setReference(shooterSetPoint, ControlType.kVelocity); } - - public void stopShooter(){ - shooterSetPoint = 0.0; - shooterMotorBottom.setVoltage(0); - shooterMotorTop.setVoltage(0); + + public void stopShooter() { + shooterSetPoint = 0.0; + shooterMotorBottom.setVoltage(0); + shooterMotorTop.setVoltage(0); } public double getShooterBottomSpeed() { diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index 4b2ddce..0e5ac6b 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -11,15 +11,15 @@ public class LED extends SubsystemBase { private final SerialPort serial; private final Head head; - + public LED(SerialPort.Port port, Head head) { serial = new SerialPort(9600, port); - + serial.setWriteBufferMode(SerialPort.WriteBufferMode.kFlushOnAccess); - + this.head = head; } - + @Override public void periodic() { // Set LED animations @@ -39,7 +39,7 @@ public void periodic() { setTeleopEnabledAnimation(); } } - + public void setDisconnectedAnimation() { serial.writeString("dp\n"); } @@ -51,23 +51,23 @@ public void setEStopAnimation() { public void setAutoDisabledAnimation() { serial.writeString("ad\n"); } - + public void setAutoEnabledAnimation() { serial.writeString("ae\n"); } - + public void setTeleopDisabledAnimation() { serial.writeString("td\n"); } - + public void setTeleopEnabledAnimation() { serial.writeString("te\n"); } - + public void setHoldingNoteAnimation() { serial.writeString("hn\n"); } - + public void setReadyToShootAnimation() { serial.writeString("rs\n"); } diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java index eaa5a47..ae72e3b 100644 --- a/src/main/java/frc/robot/util/Alert.java +++ b/src/main/java/frc/robot/util/Alert.java @@ -7,8 +7,6 @@ // license that can be found in the LICENSE file at // the root directory of this project. - - import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; @@ -23,127 +21,135 @@ /** Class for managing persistent alerts to be sent over NetworkTables. */ public class Alert { - private static Map groups = new HashMap(); - - private final AlertType type; - private boolean active = false; - private double activeStartTime = 0.0; - private String text; - - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, - * the appropriate entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) { - this("Alerts", text, type); - } - - /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate - * entries will be added to NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String group, String text, AlertType type) { - if (!groups.containsKey(group)) { - groups.put(group, new SendableAlerts()); - SmartDashboard.putData(group, groups.get(group)); - } - - this.text = text; - this.type = type; - groups.get(group).alerts.add(this); - } - - /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. - */ - public void set(boolean active) { - if (active && !this.active) { - activeStartTime = Timer.getFPGATimestamp(); - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case INFO: - System.out.println(text); - break; - } - } - this.active = active; - } - - /** Updates current alert text. */ - public void setText(String text) { - if (active && !text.equals(this.text)) { - switch (type) { - case ERROR: - DriverStation.reportError(text, false); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case INFO: - System.out.println(text); - break; - } - } - this.text = text; - } - - private static class SendableAlerts implements Sendable { - public final List alerts = new ArrayList<>(); - - public String[] getStrings(AlertType type) { - Predicate activeFilter = (Alert x) -> x.type == type && x.active; - Comparator timeSorter = - (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); - return alerts.stream() - .filter(activeFilter) - .sorted(timeSorter) - .map((Alert a) -> a.text) - .toArray(String[]::new); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } - } - - /** Represents an alert's level of urgency. */ - public static enum AlertType { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - */ - ERROR, - - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - */ - WARNING, - - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type - * for problems which are unlikely to affect the robot's functionality, or any other alerts - * which do not fall under "ERROR" or "WARNING". - */ - INFO - } -} \ No newline at end of file + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text + * Text to be displayed when the alert is active. + * @param type + * Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group + * Group identifier, also used as NetworkTables title + * @param text + * Text to be displayed when the alert is active. + * @param type + * Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + if (active && !this.active) { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + + case WARNING: + DriverStation.reportWarning(text, false); + break; + + case INFO: + System.out.println(text); + break; + } + } + this.active = active; + } + + /** Updates current alert text. */ + public void setText(String text) { + if (active && !text.equals(this.text)) { + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + + case WARNING: + DriverStation.reportWarning(text, false); + break; + + case INFO: + System.out.println(text); + break; + } + } + this.text = text; + } + + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); + + public String[] getStrings(AlertType type) { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } + + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, + + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } +} diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java index 01c16c1..8a82690 100644 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) +// LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot.util; @@ -27,966 +27,930 @@ import com.fasterxml.jackson.databind.ObjectMapper; public class LimelightHelpers { - - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Barcode { - - } - - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } - } - - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Detector() { - } - } - - public static class Results { - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - - } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } - - - } - - public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; - - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - public RawFiducial[] rawFiducials; - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - } - } - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - private static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - private static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - private static double extractBotPoseEntry(double[] inData, int position){ - if(inData.length < position+1) - { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); - var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray,6); - int tagCount = (int)extractBotPoseEntry(poseArray,7); - double tagSpan = extractBotPoseEntry(poseArray,8); - double tagDist = extractBotPoseEntry(poseArray,9); - double tagArea = extractBotPoseEntry(poseArray,10); - //getlastchange() in microseconds, ll latency in milliseconds - var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); - - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial*tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } - else{ - for(int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); - } - - private static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - - } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - - - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } - - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Parses Limelight's JSON results dump into a LimelightResults Object - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} \ No newline at end of file + public static class LimelightTarget_Retro { + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Fiducial { + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + public static class LimelightTarget_Detector { + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() {} + } + + public static class Results { + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; + } + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea, RawFiducial[] rawFiducials) { + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d(new Translation3d(inData[0], inData[1], inData[2]), new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + private static double extractBotPoseEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray, 6); + int tagCount = (int) extractBotPoseEntry(poseArray, 7); + double tagSpan = extractBotPoseEntry(poseArray, 8); + double tagDist = extractBotPoseEntry(poseArray, 9); + double tagArea = extractBotPoseEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) { + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} diff --git a/src/test/java/ConstantsTest.java b/src/test/java/ConstantsTest.java index 8b798fe..547bbe5 100644 --- a/src/test/java/ConstantsTest.java +++ b/src/test/java/ConstantsTest.java @@ -13,12 +13,12 @@ public void testArmAnglesAreWithinBounds() { assertTrue(ArmConstants.MAX_ANGLE > ArmConstants.FLOOR_PICKUP); assertTrue(ArmConstants.MAX_ANGLE > ArmConstants.STOW_ANGLE); assertTrue(ArmConstants.MAX_ANGLE > ArmConstants.MIN_ABOVE_PASS_ANGLE); - + assertTrue(ArmConstants.MIN_ANGLE < ArmConstants.SOURCE_ANGLE); assertTrue(ArmConstants.MIN_ANGLE < ArmConstants.FLOOR_PICKUP); assertTrue(ArmConstants.MIN_ANGLE < ArmConstants.STOW_ANGLE); assertTrue(ArmConstants.MIN_ANGLE < ArmConstants.MIN_ABOVE_PASS_ANGLE); - + // make sure source, amp, and stow are above the min above pass angle // assertTrue(ArmConstants.MIN_ABOVE_PASS_ANGLE < ArmConstants.SOURCE_ANGLE); // assertTrue(ArmConstants.MIN_ABOVE_PASS_ANGLE < ArmConstants.AMP_ANGLE); @@ -26,7 +26,5 @@ public void testArmAnglesAreWithinBounds() { // assertTrue(ArmConstants.MIN_ABOVE_PASS_ANGLE < ArmConstants.SPEAKER_SUBWOOFER_ANGLE); // assertTrue(ArmConstants.MIN_ABOVE_PASS_ANGLE < ArmConstants.SPEAKER_PODIUM_ANGLE); // assertTrue(ArmConstants.MIN_ABOVE_PASS_ANGLE < ArmConstants.MIN_ABOVE_PASS_ANGLE); - - } }