From c16471688f71d22e8fb6ffeb1919e158380e4f44 Mon Sep 17 00:00:00 2001 From: Connor Peach Date: Thu, 26 Oct 2023 17:33:22 -0600 Subject: [PATCH] formatting --- src/main/deploy/example.txt | 4 +- src/main/java/frc4388/robot/Constants.java | 356 ++++---- src/main/java/frc4388/robot/Main.java | 58 +- src/main/java/frc4388/robot/Robot.java | 268 +++--- .../java/frc4388/robot/RobotContainer.java | 700 +++++++-------- src/main/java/frc4388/robot/RobotMap.java | 558 ++++++------ .../robot/commands/Arm/PivotCommand.java | 66 +- .../robot/commands/Arm/TeleCommand.java | 78 +- .../robot/commands/Autos/AutoBalance.java | 84 +- .../robot/commands/Autos/Blue1Path.txt | 298 +++---- .../robot/commands/Autos/PlaybackChooser.java | 182 ++-- .../frc4388/robot/commands/Autos/Taxi.txt | 448 +++++----- .../robot/commands/BooleanCommand.java | 134 +-- .../commands/PelvicInflammatoryDisease.java | 120 +-- .../commands/Placement/AprilRotAlign.java | 84 +- .../Placement/DriveToLimeDistance.java | 88 +- .../robot/commands/Placement/LimeAlign.java | 96 +- .../commands/Swerve/JoystickPlayback.java | 282 +++--- .../commands/Swerve/JoystickRecorder.java | 196 ++-- .../robot/commands/Swerve/RotateToAngle.java | 70 +- .../frc4388/robot/subsystems/Apriltags.java | 72 +- .../java/frc4388/robot/subsystems/Arm.java | 318 +++---- .../java/frc4388/robot/subsystems/Claw.java | 122 +-- .../java/frc4388/robot/subsystems/LED.java | 126 +-- .../frc4388/robot/subsystems/Limelight.java | 330 +++---- .../frc4388/robot/subsystems/Location.java | 54 +- .../frc4388/robot/subsystems/SwerveDrive.java | 390 ++++---- .../robot/subsystems/SwerveModule.java | 322 +++---- .../java/frc4388/robot/subsystems/Vision.java | 76 +- .../java/frc4388/utility/AbhiIsADumbass.java | 86 +- src/main/java/frc4388/utility/AprilTag.java | 26 +- .../java/frc4388/utility/DeferredBlock.java | 46 +- src/main/java/frc4388/utility/Gains.java | 164 ++-- .../java/frc4388/utility/LEDPatterns.java | 90 +- src/main/java/frc4388/utility/RobotGyro.java | 400 ++++----- src/main/java/frc4388/utility/RobotTime.java | 158 ++-- src/main/java/frc4388/utility/RobotUnits.java | 54 +- .../java/frc4388/utility/UtilityStructs.java | 29 +- .../controller/DeadbandedXboxController.java | 54 +- .../utility/controller/IHandController.java | 42 +- .../utility/controller/XboxController.java | 436 ++++----- .../utility/controller/XboxTriggerButton.java | 136 +-- .../java/frc4388/mocks/MockPigeonIMU.java | 108 +-- .../robot/subsystems/LEDSubsystemTest.old | 122 +-- src/test/java/frc4388/utility/Delay.java | 8 + .../frc4388/utility/RobotGyroUtilityTest.old | 370 ++++---- .../frc4388/utility/RobotTimeUtilityTest.old | 210 ++--- todo.txt | 16 +- vendordeps/Phoenix.json | 844 +++++++++--------- vendordeps/REVLib.json | 144 +-- vendordeps/WPILibNewCommands.json | 74 +- vendordeps/navx_frc.json | 68 +- vendordeps/photonlib.json | 80 +- 53 files changed, 4880 insertions(+), 4865 deletions(-) create mode 100644 src/test/java/frc4388/utility/Delay.java diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt index 70c79b6..f022c09 100644 --- a/src/main/deploy/example.txt +++ b/src/main/deploy/example.txt @@ -1,3 +1,3 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index af39c6a..e894a0f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -1,178 +1,178 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import frc4388.utility.Gains; -import frc4388.utility.LEDPatterns; -import frc4388.utility.RobotUnits; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be - * declared globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final class SwerveDriveConstants { - - public static final double MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 0.8; - public static double ROTATION_SPEED = MAX_ROT_SPEED; - public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; - - public static final double CORRECTION_MIN = 10; - public static final double CORRECTION_MAX = 50; - - public static final double SLOW_SPEED = 0.8; - public static final double FAST_SPEED = 1.0; - public static final double TURBO_SPEED = 4.0; - - public static final class IDs { - public static final int LEFT_FRONT_WHEEL_ID = 2; - public static final int LEFT_FRONT_STEER_ID = 3; - public static final int LEFT_FRONT_ENCODER_ID = 10; - - public static final int RIGHT_FRONT_WHEEL_ID = 4; - public static final int RIGHT_FRONT_STEER_ID = 5; - public static final int RIGHT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; - - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; - } - - public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); - } - - public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - } - - public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; - - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - } - - public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value - public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value - } - - public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value - - // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class GyroConstants { - public static final int ID = 14; - } - - public static final class ArmConstants { - public static final double MIN_ARM_LEN = 1; - public static final double MAX_ARM_LEN = 2; - public static final double ARM_HEIGHT = 1; - public static final double CURVE_POWER = 2; - - public static final double TELE_TICKS_PER_SECOND = (-5); - - public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value - public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value - - public static final double TELE_FORWARD_SOFT_LIMIT = 100; - public static final double TELE_REVERSE_SOFT_LIMIT = 0; - - public static final double kP = 0; - public static final double kI = 0; - public static final double kD = 0; - - public static final double OFFSET = 0; - } - - public static final class LEDConstants { - // public static final int LED_SPARK_ID = 0; - - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } - - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - public static final double LEFT_AXIS_DEADBAND = 0.1; - public static final double RIGHT_AXIS_DEADBAND = 0.6; - public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing - } - - public static final class VisionConstants { - public static final String NAME = "photonCamera"; - - public static final int LIME_HIXELS = 640; - public static final int LIME_VIXELS = 480; - - public static final double H_FOV = 59.6; - public static final double V_FOV = 45.7; - - public static final double LIME_HEIGHT = 6.0; - public static final double LIME_ANGLE = 55.0; - - // public static final double HIGH_TARGET_HEIGHT = 46.0; - public static final double HIGH_TAPE_HEIGHT = 44.0; - - // public static final double MID_TARGET_HEIGHT = 34.0; - public static final double MID_TAPE_HEIGHT = 24.0; - - public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value - - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc4388.utility.Gains; +import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotUnits; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class SwerveDriveConstants { + + public static final double MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 0.8; + public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final double CORRECTION_MIN = 10; + public static final double CORRECTION_MAX = 50; + + public static final double SLOW_SPEED = 0.8; + public static final double FAST_SPEED = 1.0; + public static final double TURBO_SPEED = 4.0; + + public static final class IDs { + public static final int LEFT_FRONT_WHEEL_ID = 2; + public static final int LEFT_FRONT_STEER_ID = 3; + public static final int LEFT_FRONT_ENCODER_ID = 10; + + public static final int RIGHT_FRONT_WHEEL_ID = 4; + public static final int RIGHT_FRONT_STEER_ID = 5; + public static final int RIGHT_FRONT_ENCODER_ID = 11; + + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; + + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; + } + + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + } + + public static final class AutoConstants { + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune + + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; + + public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double WHEEL_DIAMETER_INCHES = 3.9; + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + + public static final double TICK_TIME_TO_SECONDS = 10; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + } + + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + } + + public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value + + // dimensions + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + + public static final class GyroConstants { + public static final int ID = 14; + } + + public static final class ArmConstants { + public static final double MIN_ARM_LEN = 1; + public static final double MAX_ARM_LEN = 2; + public static final double ARM_HEIGHT = 1; + public static final double CURVE_POWER = 2; + + public static final double TELE_TICKS_PER_SECOND = (-5); + + public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value + public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value + + public static final double TELE_FORWARD_SOFT_LIMIT = 100; + public static final double TELE_REVERSE_SOFT_LIMIT = 0; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + + public static final double OFFSET = 0; + } + + public static final class LEDConstants { + // public static final int LED_SPARK_ID = 0; + + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + public static final double LEFT_AXIS_DEADBAND = 0.1; + public static final double RIGHT_AXIS_DEADBAND = 0.6; + public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing + } + + public static final class VisionConstants { + public static final String NAME = "photonCamera"; + + public static final int LIME_HIXELS = 640; + public static final int LIME_VIXELS = 480; + + public static final double H_FOV = 59.6; + public static final double V_FOV = 45.7; + + public static final double LIME_HEIGHT = 6.0; + public static final double LIME_ANGLE = 55.0; + + // public static final double HIGH_TARGET_HEIGHT = 46.0; + public static final double HIGH_TAPE_HEIGHT = 44.0; + + // public static final double MID_TARGET_HEIGHT = 34.0; + public static final double MID_TAPE_HEIGHT = 24.0; + + public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + } +} diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index ad2d494..9cfbc11 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -1,29 +1,29 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -/** - * Do NOT add any static variables to this class, or any initialization at all. - * Unless you know what you are doing, do not modify this file except to - * change the parameter class to the startRobot call. - */ -public final class Main { - private Main() { - } - - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a63c659..e7b8afd 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -1,134 +1,134 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc4388.utility.DeferredBlock; -import frc4388.utility.RobotTime; - -/** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends TimedRobot { - Command m_autonomousCommand; - - private RobotTime m_robotTime = RobotTime.getInstance(); - private RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override - public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - - SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); - } - - /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - m_robotTime.updateTimes(); - - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - - CommandScheduler.getInstance().run(); - } - - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ - @Override - public void disabledInit() { - m_robotTime.endMatchTime(); - } - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() { - DeferredBlock.execute(); - super.disabledExit(); - } - - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ - @Override - public void autonomousInit() { - m_robotContainer.m_robotSwerveDrive.resetGyro(); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - m_robotTime.startMatchTime(); - } - - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { - } - - @Override - public void teleopInit() { - m_robotContainer.m_robotSwerveDrive.resetGyro(); - - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - m_robotContainer.m_robotSwerveDrive.resetGyro(); - m_robotTime.startMatchTime(); - - m_robotContainer.m_robotMap.restart_motor_tests(); - } - - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() {} - - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - m_robotContainer.m_robotMap.run_periodic_tests(); - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.DeferredBlock; +import frc4388.utility.RobotTime; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + Command m_autonomousCommand; + + private RobotTime m_robotTime = RobotTime.getInstance(); + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + + SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); + } + + /** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + m_robotTime.updateTimes(); + + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + m_robotTime.endMatchTime(); + } + + @Override + public void disabledPeriodic() {} + + @Override + public void disabledExit() { + DeferredBlock.execute(); + super.disabledExit(); + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_robotContainer.m_robotSwerveDrive.resetGyro(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + m_robotTime.startMatchTime(); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + m_robotContainer.m_robotSwerveDrive.resetGyro(); + + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + m_robotContainer.m_robotSwerveDrive.resetGyro(); + m_robotTime.startMatchTime(); + + m_robotContainer.m_robotMap.restart_motor_tests(); + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() {} + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + m_robotContainer.m_robotMap.run_periodic_tests(); + } +} diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cd728ce..9a0b737 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -1,350 +1,350 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import java.util.function.Consumer; - -import edu.wpi.first.math.geometry.Translation2d; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.CANSparkMax.FaultID; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import frc4388.robot.Constants.*; -import frc4388.robot.subsystems.Arm; -import frc4388.robot.subsystems.Claw; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.commands.BooleanCommand; -import frc4388.robot.commands.Arm.PivotCommand; -import frc4388.robot.commands.Arm.TeleCommand; -import frc4388.robot.commands.Autos.AutoBalance; -import frc4388.robot.commands.Autos.PlaybackChooser; -import frc4388.robot.commands.Placement.AprilRotAlign; -import frc4388.robot.commands.Placement.LimeAlign; -import frc4388.robot.commands.Swerve.RotateToAngle; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.XboxController; - -/** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic should - * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, - * commands, and button mappings) should be declared here. - */ -public class RobotContainer { - /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(); - - /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - m_robotMap.gyro); - - public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); - - public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin); - - public final Limelight m_robotLimeLight = new Limelight(); - - /* Controllers */ - private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - - /* Autos */ - public SendableChooser chooser = new SendableChooser<>(); - - // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); - - // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); - // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); - // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - - private PlaybackChooser playbackChooser; - - /* Commands */ - private Command emptyCommand = new InstantCommand(); - private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight); - - private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); - - private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); - - private boolean readyForPlacement = false; - private Boolean isPole = null; - - private SequentialCommandGroup alignToPole = - new SequentialCommandGroup( - new RotateToAngle(m_robotSwerveDrive, 0.0), - new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), - new RotateToAngle(m_robotSwerveDrive, 0.0), - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); - - // private SequentialCommandGroup alignToShelf = - // new SequentialCommandGroup( - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), - // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); - - private SequentialCommandGroup alignToShelf = - new SequentialCommandGroup( - new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), - new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); - - public SequentialCommandGroup place = null; - - private Consumer queuePlacement = (scg) -> { - place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null)); - }; - - // TODO: find actual values - private SequentialCommandGroup placeCubeHigh = - new SequentialCommandGroup( - // new InstantCommand(() -> System.out.println("Placing cone high")), - new PivotCommand(m_robotArm, 64 + 135), - new InstantCommand(() -> m_robotArm.setRotVel(0)), - new TeleCommand(m_robotArm, 95642), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 70 + 135), - new TeleCommand(m_robotArm, 32866), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeLow = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - configureButtonBindings(); - - // * Default Commands - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - - m_robotArm.setDefaultCommand(new RunCommand(() -> { - m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); - m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); - }, m_robotArm) - .withName("Arm DefaultCommand")); - - // * Auto Commands - chooser.setDefaultOption("NoAuto", emptyCommand); - chooser.addOption("alignToPole", alignToPole); - chooser.addOption("alignToShelf", alignToShelf); - - chooser.addOption("placeConeHigh", placeConeHigh); - chooser.addOption("placeConeMid", placeConeMid); - chooser.addOption("placeCubeHigh", placeCubeHigh); - chooser.addOption("placeCubeMid", placeCubeMid); - chooser.addOption("placeLow", placeLow); - - // chooser.addOption("Blue1Path", blue1Path); - // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); - - // chooser.addOption("Red1Path", red1Path); - // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - - playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) - .buildDisplay(); - } - - // here be dragons - enter if you dare - private static class TapState { - public boolean gearTapped = true; - public long gearTime = 0; - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // * Driver Buttons - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - - // because closure reasons - ask Daniel Thomas - // final TapState tap = new TapState(); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // .onTrue(new InstantCommand(() -> { - // tap.gearTapped = true; - // tap.gearTime = System.currentTimeMillis(); - // })) - // .whileTrue(new RunCommand(() -> { - // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { - // m_robotSwerveDrive.setToTurbo(); - // tap.gearTapped = false; - // } - // })) - // .onFalse(new InstantCommand(() -> { - // if (tap.gearTapped) - // m_robotSwerveDrive.setToFast(); - // else - // m_robotSwerveDrive.setToSlow(); - // })); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(interruptCommand.asProxy()); // final - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Blue1Path.txt")) - // .onFalse(new InstantCommand()); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); - - // * Operator Buttons - - // align (pole) - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final - .onTrue(alignToPole) - .onFalse(interruptCommand.asProxy()); - - // align (shelf) - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final - .onTrue(alignToShelf) - .onFalse(interruptCommand.asProxy()); - - // toggle claw - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .onTrue(toggleClaw.asProxy()); - - // kill soft limits - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - - // SmartDashboard.putBoolean("isn't SparkMAX", ); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin())) - .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); - - //Arm to Home - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(armToHome.asProxy()); - - //Interupt Button - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(interruptCommand.asProxy()); - - - // place high - new POVButton(getDeadbandedOperatorController(), 0) - .onTrue(new ConditionalCommand( - new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), - emptyCommand.asProxy(), - () -> readyForPlacement == true) - ); - - // place mid - new POVButton(getDeadbandedOperatorController(), 270) - .onTrue(new ConditionalCommand( - new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), - emptyCommand.asProxy(), - () -> readyForPlacement == true) - ); - - // place low - new POVButton(getDeadbandedOperatorController(), 180) - .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true)); - - // confirm - new POVButton(getDeadbandedOperatorController(), 90) - .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null)); - - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // return chooser.getSelected(); - return playbackChooser.getCommand(); - } - - public DeadbandedXboxController getDeadbandedDriverController() { - return this.m_driverXbox; - } - - public DeadbandedXboxController getDeadbandedOperatorController() { - return this.m_operatorXbox; - } - -} +x/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import java.util.function.Consumer; + +import edu.wpi.first.math.geometry.Translation2d; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.CANSparkMax.FaultID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; +import frc4388.robot.Constants.*; +import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.Claw; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.commands.BooleanCommand; +import frc4388.robot.commands.Arm.PivotCommand; +import frc4388.robot.commands.Arm.TeleCommand; +import frc4388.robot.commands.Autos.AutoBalance; +import frc4388.robot.commands.Autos.PlaybackChooser; +import frc4388.robot.commands.Placement.AprilRotAlign; +import frc4388.robot.commands.Placement.LimeAlign; +import frc4388.robot.commands.Swerve.RotateToAngle; +import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.controller.XboxController; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +public class RobotContainer { + /* RobotMap */ + public final RobotMap m_robotMap = new RobotMap(); + + /* Subsystems */ + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.rightFront, + m_robotMap.leftBack, + m_robotMap.rightBack, + m_robotMap.gyro); + + public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); + + public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin); + + public final Limelight m_robotLimeLight = new Limelight(); + + /* Controllers */ + private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + + /* Autos */ + public SendableChooser chooser = new SendableChooser<>(); + + // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); + + // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); + // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); + // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + + private PlaybackChooser playbackChooser; + + /* Commands */ + private Command emptyCommand = new InstantCommand(); + private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight); + + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); + + private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); + + private boolean readyForPlacement = false; + private Boolean isPole = null; + + private SequentialCommandGroup alignToPole = + new SequentialCommandGroup( + new RotateToAngle(m_robotSwerveDrive, 0.0), + new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), + new RotateToAngle(m_robotSwerveDrive, 0.0), + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); + + // private SequentialCommandGroup alignToShelf = + // new SequentialCommandGroup( + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), + // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), + // new RotateToAngle(m_robotSwerveDrive, 0.0), + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); + + private SequentialCommandGroup alignToShelf = + new SequentialCommandGroup( + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), + new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), + new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), + new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) + ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); + + public SequentialCommandGroup place = null; + + private Consumer queuePlacement = (scg) -> { + place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null)); + }; + + // TODO: find actual values + private SequentialCommandGroup placeCubeHigh = + new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println("Placing cone high")), + new PivotCommand(m_robotArm, 64 + 135), + new InstantCommand(() -> m_robotArm.setRotVel(0)), + new TeleCommand(m_robotArm, 95642), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 70 + 135), + new TeleCommand(m_robotArm, 32866), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + private SequentialCommandGroup placeLow = new SequentialCommandGroup( + new PivotCommand(m_robotArm, 0), + new TeleCommand(m_robotArm, 0), + toggleClaw.asProxy(), + armToHome.asProxy() + ); + + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + configureButtonBindings(); + + // * Default Commands + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); + + m_robotArm.setDefaultCommand(new RunCommand(() -> { + m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); + m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); + }, m_robotArm) + .withName("Arm DefaultCommand")); + + // * Auto Commands + chooser.setDefaultOption("NoAuto", emptyCommand); + chooser.addOption("alignToPole", alignToPole); + chooser.addOption("alignToShelf", alignToShelf); + + chooser.addOption("placeConeHigh", placeConeHigh); + chooser.addOption("placeConeMid", placeConeMid); + chooser.addOption("placeCubeHigh", placeCubeHigh); + chooser.addOption("placeCubeMid", placeCubeMid); + chooser.addOption("placeLow", placeLow); + + // chooser.addOption("Blue1Path", blue1Path); + // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); + + // chooser.addOption("Red1Path", red1Path); + // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); + + playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) + .buildDisplay(); + } + + // here be dragons - enter if you dare + private static class TapState { + public boolean gearTapped = true; + public long gearTime = 0; + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // * Driver Buttons + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final + + // because closure reasons - ask Daniel Thomas + // final TapState tap = new TapState(); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // .onTrue(new InstantCommand(() -> { + // tap.gearTapped = true; + // tap.gearTime = System.currentTimeMillis(); + // })) + // .whileTrue(new RunCommand(() -> { + // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { + // m_robotSwerveDrive.setToTurbo(); + // tap.gearTapped = false; + // } + // })) + // .onFalse(new InstantCommand(() -> { + // if (tap.gearTapped) + // m_robotSwerveDrive.setToFast(); + // else + // m_robotSwerveDrive.setToSlow(); + // })); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(interruptCommand.asProxy()); // final + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Blue1Path.txt")) + // .onFalse(new InstantCommand()); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + // .onFalse(new InstantCommand()); + + // * Operator Buttons + + // align (pole) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final + .onTrue(alignToPole) + .onFalse(interruptCommand.asProxy()); + + // align (shelf) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final + .onTrue(alignToShelf) + .onFalse(interruptCommand.asProxy()); + + // toggle claw + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final + .onTrue(toggleClaw.asProxy()); + + // kill soft limits + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); + + // SmartDashboard.putBoolean("isn't SparkMAX", ); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin())) + .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); + + //Arm to Home + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(armToHome.asProxy()); + + //Interupt Button + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(interruptCommand.asProxy()); + + + // place high + new POVButton(getDeadbandedOperatorController(), 0) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), + emptyCommand.asProxy(), + () -> readyForPlacement == true) + ); + + // place mid + new POVButton(getDeadbandedOperatorController(), 270) + .onTrue(new ConditionalCommand( + new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), + emptyCommand.asProxy(), + () -> readyForPlacement == true) + ); + + // place low + new POVButton(getDeadbandedOperatorController(), 180) + .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true)); + + // confirm + new POVButton(getDeadbandedOperatorController(), 90) + .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null)); + + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // return chooser.getSelected(); + return playbackChooser.getCommand(); + } + + public DeadbandedXboxController getDeadbandedDriverController() { + return this.m_driverXbox; + } + + public DeadbandedXboxController getDeadbandedOperatorController() { + return this.m_operatorXbox; + } + +} diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 113cf05..d8a49ce 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -1,279 +1,279 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import java.lang.reflect.Field; -import java.lang.reflect.Member; -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotGyro; - -/** - * Defines and holds all I/O objects on the Roborio. This is useful for unit - * testing and modularization. - */ -public class RobotMap { - private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); - public RobotGyro gyro = new RobotGyro(m_pigeon2); - - - public SwerveModule leftFront; - public SwerveModule rightFront; - public SwerveModule leftBack; - public SwerveModule rightBack; - - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotors(); - configArmMotors(); - - try { - setup_motor_tests(); - } catch (IllegalArgumentException | IllegalAccessException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - - /* LED Subsystem */ - // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - - void configureLEDMotorControllers() { - - } - - // swerve drive subsystem - public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - - public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - - public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - - void configureDriveMotors() { - // config factory default - leftFrontWheel.configFactoryDefault(); - leftFrontSteer.configFactoryDefault(); - - rightFrontWheel.configFactoryDefault(); - rightFrontSteer.configFactoryDefault(); - - leftBackWheel.configFactoryDefault(); - leftBackSteer.configFactoryDefault(); - - rightBackWheel.configFactoryDefault(); - rightBackSteer.configFactoryDefault(); - - // config open loop ramp - leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // config closed loop ramp - leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // config neutral deadband - leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); - - // set neutral mode - leftFrontWheel.setNeutralMode(NeutralMode.Brake); - rightFrontWheel.setNeutralMode(NeutralMode.Brake); - leftBackWheel.setNeutralMode(NeutralMode.Brake); - rightBackWheel.setNeutralMode(NeutralMode.Brake); - - leftFrontSteer.setNeutralMode(NeutralMode.Brake); - rightFrontSteer.setNeutralMode(NeutralMode.Brake); - leftBackSteer.setNeutralMode(NeutralMode.Brake); - rightBackSteer.setNeutralMode(NeutralMode.Brake); - - // initialize SwerveModules - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); - } - - // arm stuff - public WPI_TalonFX pivot = new WPI_TalonFX(15); - public WPI_TalonFX tele = new WPI_TalonFX(16); - public CANCoder pivotEncoder = new CANCoder(17); - - public void configArmMotors() { - // config factory default - pivot.configFactoryDefault(); - tele.configFactoryDefault(); - - // config open loop ramp - pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // config closed loop ramp - pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); - - // config neutral mode to brake - pivot.setNeutralMode(NeutralMode.Brake); - tele.setNeutralMode(NeutralMode.Brake); - - // soft limits - pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); - pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT); - pivot.configForwardSoftLimitEnable(false); - pivot.configReverseSoftLimitEnable(false); - - tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT); - tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT); - tele.configForwardSoftLimitEnable(false); - tele.configReverseSoftLimitEnable(false); - } - - // claw stuff (WHAT IS A SOAP ENGINEER) - PWM leftClaw = new PWM(0); - PWM rightClaw = new PWM(1); - CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless); - - // ============================================ - // =============== AUTO TESTING =============== - // ============================================ - - static class MotorTest { - boolean result; - String name; - - enum MotorType {FALCON, NEO} - - // why no union :( - MotorType motor_type; - WPI_TalonFX as_falcon; - CANSparkMax as_neo; - } - - private static final long TEST_TIME = 500; - - public ArrayList motor_tests = new ArrayList<>(); - private int test_count = 0; - private long test_time = 0; - private double motor_position = 0; - - public void restart_motor_tests() { - for (MotorTest test : motor_tests) { - test.result = false; - } - - test_time = 0; - test_count = 0; - motor_position = 0; - } - - public boolean run_periodic_tests() { - if (test_count >= motor_tests.size()) return true; - - MotorTest current_test = motor_tests.get(test_count); - - if (test_time == 0) { - test_time = System.currentTimeMillis(); - - switch (current_test.motor_type) { - case FALCON: - motor_position = current_test.as_falcon.getSelectedSensorPosition(); - current_test.as_falcon.set(.1); - break; - - case NEO: - // TODO: destroy robot - break; - } - } - - if (System.currentTimeMillis() - test_time >= TEST_TIME) { - switch (current_test.motor_type) { - case FALCON: - { - double new_pos = current_test.as_falcon.getSelectedSensorPosition(); - if (Math.abs(new_pos - motor_position) > .1) { - current_test.result = true; - } else { - System.out.printf("[DISCONNECTED] %s\n", current_test.name); - } - - break; - } - - case NEO: break; - } - - test_time = 0; - test_count++; - } - - return false; - } - - private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException { - Class map_clazz = this.getClass(); - - for (Field field : map_clazz.getFields()) { - if (field.getType().equals(WPI_TalonFX.class)) { - MotorTest test = new MotorTest(); - test.result = false; - test.name = field.getName(); - test.motor_type = MotorTest.MotorType.FALCON; - test.as_falcon = (WPI_TalonFX) field.get(this); - } - } - } -} \ No newline at end of file +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot; + +import java.lang.reflect.Field; +import java.lang.reflect.Member; +import java.util.ArrayList; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; +import frc4388.utility.RobotGyro; + +/** + * Defines and holds all I/O objects on the Roborio. This is useful for unit + * testing and modularization. + */ +public class RobotMap { + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); + + + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; + + public RobotMap() { + configureLEDMotorControllers(); + configureDriveMotors(); + configArmMotors(); + + try { + setup_motor_tests(); + } catch (IllegalArgumentException | IllegalAccessException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + /* LED Subsystem */ + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + + void configureLEDMotorControllers() { + + } + + // swerve drive subsystem + public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + + public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + void configureDriveMotors() { + // config factory default + leftFrontWheel.configFactoryDefault(); + leftFrontSteer.configFactoryDefault(); + + rightFrontWheel.configFactoryDefault(); + rightFrontSteer.configFactoryDefault(); + + leftBackWheel.configFactoryDefault(); + leftBackSteer.configFactoryDefault(); + + rightBackWheel.configFactoryDefault(); + rightBackSteer.configFactoryDefault(); + + // config open loop ramp + leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral deadband + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // set neutral mode + leftFrontWheel.setNeutralMode(NeutralMode.Brake); + rightFrontWheel.setNeutralMode(NeutralMode.Brake); + leftBackWheel.setNeutralMode(NeutralMode.Brake); + rightBackWheel.setNeutralMode(NeutralMode.Brake); + + leftFrontSteer.setNeutralMode(NeutralMode.Brake); + rightFrontSteer.setNeutralMode(NeutralMode.Brake); + leftBackSteer.setNeutralMode(NeutralMode.Brake); + rightBackSteer.setNeutralMode(NeutralMode.Brake); + + // initialize SwerveModules + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); + } + + // arm stuff + public WPI_TalonFX pivot = new WPI_TalonFX(15); + public WPI_TalonFX tele = new WPI_TalonFX(16); + public CANCoder pivotEncoder = new CANCoder(17); + + public void configArmMotors() { + // config factory default + pivot.configFactoryDefault(); + tele.configFactoryDefault(); + + // config open loop ramp + pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral mode to brake + pivot.setNeutralMode(NeutralMode.Brake); + tele.setNeutralMode(NeutralMode.Brake); + + // soft limits + pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT); + pivot.configForwardSoftLimitEnable(false); + pivot.configReverseSoftLimitEnable(false); + + tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT); + tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT); + tele.configForwardSoftLimitEnable(false); + tele.configReverseSoftLimitEnable(false); + } + + // claw stuff (WHAT IS A SOAP ENGINEER) + PWM leftClaw = new PWM(0); + PWM rightClaw = new PWM(1); + CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless); + + // ============================================ + // =============== AUTO TESTING =============== + // ============================================ + + static class MotorTest { + boolean result; + String name; + + enum MotorType {FALCON, NEO} + + // why no union :( + MotorType motor_type; + WPI_TalonFX as_falcon; + CANSparkMax as_neo; + } + + private static final long TEST_TIME = 500; + + public ArrayList motor_tests = new ArrayList<>(); + private int test_count = 0; + private long test_time = 0; + private double motor_position = 0; + + public void restart_motor_tests() { + for (MotorTest test : motor_tests) { + test.result = false; + } + + test_time = 0; + test_count = 0; + motor_position = 0; + } + + public boolean run_periodic_tests() { + if (test_count >= motor_tests.size()) return true; + + MotorTest current_test = motor_tests.get(test_count); + + if (test_time == 0) { + test_time = System.currentTimeMillis(); + + switch (current_test.motor_type) { + case FALCON: + motor_position = current_test.as_falcon.getSelectedSensorPosition(); + current_test.as_falcon.set(.1); + break; + + case NEO: + // TODO: destroy robot + break; + } + } + + if (System.currentTimeMillis() - test_time >= TEST_TIME) { + switch (current_test.motor_type) { + case FALCON: + { + double new_pos = current_test.as_falcon.getSelectedSensorPosition(); + if (Math.abs(new_pos - motor_position) > .1) { + current_test.result = true; + } else { + System.out.printf("[DISCONNECTED] %s\n", current_test.name); + } + + break; + } + + case NEO: break; + } + + test_time = 0; + test_count++; + } + + return false; + } + + private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException { + Class map_clazz = this.getClass(); + + for (Field field : map_clazz.getFields()) { + if (field.getType().equals(WPI_TalonFX.class)) { + MotorTest test = new MotorTest(); + test.result = false; + test.name = field.getName(); + test.motor_type = MotorTest.MotorType.FALCON; + test.as_falcon = (WPI_TalonFX) field.get(this); + } + } + } +} diff --git a/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java index 279e1b0..70fed4d 100644 --- a/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java @@ -1,33 +1,33 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Arm; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.Arm; - -public class PivotCommand extends PelvicInflammatoryDisease { - private final Arm arm; - private final double target; - - /** Creates a new ArmCommand. */ - public PivotCommand(Arm arm, double target) { - super(6, 1.5, 0, 0, 0.015); - this.arm = arm; - this.target = target; - addRequirements(arm); - } - - @Override - public double getError() { - return (target - arm.getArmRotation()) / 360; - } - - @Override - public void runWithOutput(double output) { - SmartDashboard.putNumber("pivot output", output); - arm.setRotVel(output); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Arm; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Arm; + +public class PivotCommand extends PelvicInflammatoryDisease { + private final Arm arm; + private final double target; + + /** Creates a new ArmCommand. */ + public PivotCommand(Arm arm, double target) { + super(6, 1.5, 0, 0, 0.015); + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public double getError() { + return (target - arm.getArmRotation()) / 360; + } + + @Override + public void runWithOutput(double output) { + SmartDashboard.putNumber("pivot output", output); + arm.setRotVel(output); + } +} diff --git a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java index a5a6e9f..1bb8978 100644 --- a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java +++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java @@ -1,39 +1,39 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Arm; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.Arm; - -public class TeleCommand extends CommandBase { - private final Arm arm; - private final double target; - private boolean goIn; - - /** Creates a new ArmCommand. */ - public TeleCommand(Arm arm, double target) { - this.arm = arm; - this.target = target; - addRequirements(arm); - } - - @Override - public void initialize() { - this.goIn = target < arm.getArmLength(); - } - - @Override - public void execute() { - arm.setTeleVel(goIn ? 1 : -1); - } - - @Override - public boolean isFinished() { - if (goIn) return arm.getArmLength() < target; - else return arm.getArmLength() > target; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + + package frc4388.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Arm; + +public class TeleCommand extends CommandBase { + private final Arm arm; + private final double target; + private boolean goIn; + + /** Creates a new ArmCommand. */ + public TeleCommand(Arm arm, double target) { + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public void initialize() { + this.goIn = target < arm.getArmLength(); + } + + @Override + public void execute() { + arm.setTeleVel(goIn ? 1 : -1); + } + + @Override + public boolean isFinished() { + if (goIn) return arm.getArmLength() < target; + else return arm.getArmLength() > target; + } +} diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java index 4d18c99..cdb278f 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java @@ -1,42 +1,42 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Autos; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.RobotGyro; - -public class AutoBalance extends PelvicInflammatoryDisease { - RobotGyro gyro; - SwerveDrive drive; - - /** Creates a new AutoBalance. */ - public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(0.6, 0, 0, 0, 0); - - this.gyro = gyro; - this.drive = drive; - - addRequirements(drive); - } - - @Override - public double getError() { - var pitch = gyro.getRoll(); - SmartDashboard.putNumber("pitch", pitch); - return pitch; - } - - @Override - public void runWithOutput(double output) { - double out2 = MathUtil.clamp(output / 40, -.5, .5); - - if (Math.abs(getError()) < 3) out2 = 0; - drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Autos; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.RobotGyro; + +public class AutoBalance extends PelvicInflammatoryDisease { + RobotGyro gyro; + SwerveDrive drive; + + /** Creates a new AutoBalance. */ + public AutoBalance(RobotGyro gyro, SwerveDrive drive) { + super(0.6, 0, 0, 0, 0); + + this.gyro = gyro; + this.drive = drive; + + addRequirements(drive); + } + + @Override + public double getError() { + var pitch = gyro.getRoll(); + SmartDashboard.putNumber("pitch", pitch); + return pitch; + } + + @Override + public void runWithOutput(double output) { + double out2 = MathUtil.clamp(output / 40, -.5, .5); + + if (Math.abs(getError()) < 3) out2 = 0; + drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); + } +} diff --git a/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt index 70d9ee7..4f49b0e 100644 --- a/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt +++ b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt @@ -1,150 +1,150 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,2 -0.0,0.0,0.0,0.0,18 -0.0,0.0,0.0,0.0,31 -0.0,0.0,0.0,0.0,61 -0.0,0.0,0.0,0.0,72 -0.0,0.0,0.0,0.0,85 -0.0,0.0,0.0,0.0,102 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,163 -0.0,0.0,0.0,0.0,175 -0.0,0.0,0.0,0.0,188 -0.0,0.0,0.0,0.0,205 -0.0,0.0,0.0,0.0,217 -0.0,0.0,0.0,0.0,229 -0.0,0.0,0.0,0.0,241 -0.0,0.0,0.0,0.0,261 -0.0,0.0,0.0,0.0,281 -0.0,0.0,0.0,0.0,302 -0.0,0.0,0.0,0.0,322 -0.0,0.0,0.0,0.0,341 -0.0,0.0,0.0,0.0,361 -0.0,-0.1328125,0.0,0.0,397 -0.0,-0.1328125,0.0,0.0,412 -0.0,-0.1484375,0.0,0.0,425 -0.0,-0.1796875,0.0,0.0,441 -0.0,-0.1875,0.0,0.0,461 -0.0,-0.1953125,0.0,0.0,481 -0.0,-0.234375,0.0,0.0,502 -0.0,-0.2890625,0.0,0.0,521 -0.0,-0.3125,0.0,0.0,541 -0.0,-0.328125,0.0,0.0,561 -0.0,-0.3671875,0.0,0.0,582 -0.0,-0.390625,0.0,0.0,601 -0.0,-0.4609375,0.0,0.0,642 -0.0,-0.4765625,0.0,0.0,653 -0.0,-0.4765625,0.0,0.0,666 -0.0,-0.4765625,0.0,0.0,681 -0.0,-0.4765625,0.0,0.0,702 -0.0,-0.4765625,0.0,0.0,721 -0.0,-0.4765625,0.0,0.0,741 -0.0,-0.4765625,0.0,0.0,762 -0.0,-0.4765625,0.0,0.0,782 -0.0,-0.484375,0.0,0.0,803 -0.0,-0.484375,0.0,0.0,821 -0.0,-0.4921875,0.0,0.0,842 -0.0,-0.5078125,0.0,0.0,878 -0.0,-0.5234375,0.0,0.0,893 -0.0,-0.5234375,0.0,0.0,906 -0.0,-0.53125,0.0,0.0,922 -0.0,-0.53125,0.0,0.0,942 -0.0,-0.53125,0.0,0.0,962 -0.0,-0.53125,0.0,0.0,982 -0.0,-0.5390625,0.0,0.0,1001 -0.0,-0.5390625,0.0,0.0,1022 -0.0,-0.546875,0.0,0.0,1042 -0.0,-0.546875,0.0,0.0,1061 -0.0,-0.546875,0.0,0.0,1082 -0.0,-0.546875,0.0,0.0,1164 -0.0,-0.546875,0.0,0.0,1176 -0.0,-0.546875,0.0,0.0,1190 -0.0,-0.546875,0.0,0.0,1202 -0.0,-0.546875,0.0,0.0,1219 -0.0,-0.546875,0.0,0.0,1230 -0.0,-0.546875,0.0,0.0,1244 -0.0,-0.546875,0.0,0.0,1258 -0.0,-0.546875,0.0,0.0,1268 -0.0,-0.546875,0.0,0.0,1281 -0.0,-0.546875,0.0,0.0,1301 -0.0,-0.546875,0.0,0.0,1321 -0.0,-0.546875,0.0,0.0,1363 -0.0,-0.546875,0.0,0.0,1376 -0.0,-0.546875,0.0,0.0,1388 -0.0,-0.546875,0.0,0.0,1402 -0.0,-0.546875,0.0,0.0,1422 -0.0,-0.546875,0.0,0.0,1441 -0.0,-0.546875,0.0,0.0,1461 -0.0,-0.546875,0.0,0.0,1482 -0.0,-0.546875,0.0,0.0,1502 -0.0,-0.546875,0.0,0.0,1521 -0.0,-0.546875,0.0,0.0,1542 -0.0,-0.546875,0.0,0.0,1562 -0.0,-0.546875,0.0,0.0,1598 -0.0,-0.546875,0.0,0.0,1609 -0.0,-0.546875,0.0,0.0,1621 -0.0,-0.546875,0.0,0.0,1642 -0.0,-0.546875,0.0,0.0,1661 -0.0,-0.546875,0.0,0.0,1682 -0.0,-0.546875,0.0,0.0,1702 -0.0,-0.546875,0.0,0.0,1722 -0.0,-0.546875,0.0,0.0,1742 -0.0,-0.546875,0.0,0.0,1762 -0.0,-0.546875,0.0,0.0,1781 -0.0,-0.5390625,0.0,0.0,1801 -0.0,-0.5390625,0.0,0.0,1835 -0.0,-0.5390625,0.0,0.0,1847 -0.0,-0.5390625,0.0,0.0,1861 -0.0,-0.5390625,0.0,0.0,1882 -0.0,-0.5390625,0.0,0.0,1901 -0.0,-0.5390625,0.0,0.0,1921 -0.0,-0.5390625,0.0,0.0,1941 -0.0,-0.5390625,0.0,0.0,1962 -0.0,-0.5390625,0.0,0.0,1983 -0.0,-0.5390625,0.0,0.0,2001 -0.0,-0.5390625,0.0,0.0,2022 -0.0,-0.5390625,0.0,0.0,2042 -0.0,-0.5390625,0.0,0.0,2085 -0.0,-0.5390625,0.0,0.0,2097 -0.0,-0.5390625,0.0,0.0,2113 -0.0,-0.5390625,0.0,0.0,2124 -0.0,-0.5390625,0.0,0.0,2141 -0.0,-0.5390625,0.0,0.0,2161 -0.0,-0.5390625,0.0,0.0,2181 -0.0,-0.5390625,0.0,0.0,2201 -0.0,-0.5390625,0.0,0.0,2221 -0.0,-0.5390625,0.0,0.0,2241 -0.0,-0.5390625,0.0,0.0,2262 -0.0,-0.5390625,0.0,0.0,2283 -0.0,-0.2265625,0.0,0.0,2377 -0.0,-0.2265625,0.0,0.0,2390 -0.0,0.0,0.0,0.0,2402 -0.0,0.0,0.0,0.0,2417 -0.0,0.0,0.0,0.0,2428 -0.0,0.0,0.0,0.0,2440 -0.0,0.0,0.0,0.0,2451 -0.0,0.0,0.0,0.0,2463 -0.0,0.0,0.0,0.0,2475 -0.0,0.0,0.0,0.0,2493 -0.0,0.0,0.0,0.0,2505 -0.0,0.0,0.0,0.0,2521 -0.0,0.0,0.0,0.0,2558 -0.0,0.0,0.0,0.0,2576 -0.0,0.0,0.0,0.0,2592 -0.0,0.0,0.0,0.0,2603 -0.0,0.0,0.0,0.0,2621 -0.0,0.0,0.0,0.0,2641 -0.0,0.0,0.0,0.0,2661 -0.0,0.0,0.0,0.0,2681 -0.0,0.0,0.0,0.0,2702 -0.0,0.0,0.0,0.0,2721 -0.0,0.0,0.0,0.0,2742 -0.0,0.0,0.0,0.0,2762 -0.0,0.0,0.0,0.0,2805 -0.0,0.0,0.0,0.0,2814 -0.0,0.0,0.0,0.0,2825 -0.0,0.0,0.0,0.0,2841 -0.0,0.0,0.0,0.0,2861 -0.0,0.0,0.0,0.0,2882 -0.0,0.0,0.0,0.0,2901 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,2 +0.0,0.0,0.0,0.0,18 +0.0,0.0,0.0,0.0,31 +0.0,0.0,0.0,0.0,61 +0.0,0.0,0.0,0.0,72 +0.0,0.0,0.0,0.0,85 +0.0,0.0,0.0,0.0,102 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,163 +0.0,0.0,0.0,0.0,175 +0.0,0.0,0.0,0.0,188 +0.0,0.0,0.0,0.0,205 +0.0,0.0,0.0,0.0,217 +0.0,0.0,0.0,0.0,229 +0.0,0.0,0.0,0.0,241 +0.0,0.0,0.0,0.0,261 +0.0,0.0,0.0,0.0,281 +0.0,0.0,0.0,0.0,302 +0.0,0.0,0.0,0.0,322 +0.0,0.0,0.0,0.0,341 +0.0,0.0,0.0,0.0,361 +0.0,-0.1328125,0.0,0.0,397 +0.0,-0.1328125,0.0,0.0,412 +0.0,-0.1484375,0.0,0.0,425 +0.0,-0.1796875,0.0,0.0,441 +0.0,-0.1875,0.0,0.0,461 +0.0,-0.1953125,0.0,0.0,481 +0.0,-0.234375,0.0,0.0,502 +0.0,-0.2890625,0.0,0.0,521 +0.0,-0.3125,0.0,0.0,541 +0.0,-0.328125,0.0,0.0,561 +0.0,-0.3671875,0.0,0.0,582 +0.0,-0.390625,0.0,0.0,601 +0.0,-0.4609375,0.0,0.0,642 +0.0,-0.4765625,0.0,0.0,653 +0.0,-0.4765625,0.0,0.0,666 +0.0,-0.4765625,0.0,0.0,681 +0.0,-0.4765625,0.0,0.0,702 +0.0,-0.4765625,0.0,0.0,721 +0.0,-0.4765625,0.0,0.0,741 +0.0,-0.4765625,0.0,0.0,762 +0.0,-0.4765625,0.0,0.0,782 +0.0,-0.484375,0.0,0.0,803 +0.0,-0.484375,0.0,0.0,821 +0.0,-0.4921875,0.0,0.0,842 +0.0,-0.5078125,0.0,0.0,878 +0.0,-0.5234375,0.0,0.0,893 +0.0,-0.5234375,0.0,0.0,906 +0.0,-0.53125,0.0,0.0,922 +0.0,-0.53125,0.0,0.0,942 +0.0,-0.53125,0.0,0.0,962 +0.0,-0.53125,0.0,0.0,982 +0.0,-0.5390625,0.0,0.0,1001 +0.0,-0.5390625,0.0,0.0,1022 +0.0,-0.546875,0.0,0.0,1042 +0.0,-0.546875,0.0,0.0,1061 +0.0,-0.546875,0.0,0.0,1082 +0.0,-0.546875,0.0,0.0,1164 +0.0,-0.546875,0.0,0.0,1176 +0.0,-0.546875,0.0,0.0,1190 +0.0,-0.546875,0.0,0.0,1202 +0.0,-0.546875,0.0,0.0,1219 +0.0,-0.546875,0.0,0.0,1230 +0.0,-0.546875,0.0,0.0,1244 +0.0,-0.546875,0.0,0.0,1258 +0.0,-0.546875,0.0,0.0,1268 +0.0,-0.546875,0.0,0.0,1281 +0.0,-0.546875,0.0,0.0,1301 +0.0,-0.546875,0.0,0.0,1321 +0.0,-0.546875,0.0,0.0,1363 +0.0,-0.546875,0.0,0.0,1376 +0.0,-0.546875,0.0,0.0,1388 +0.0,-0.546875,0.0,0.0,1402 +0.0,-0.546875,0.0,0.0,1422 +0.0,-0.546875,0.0,0.0,1441 +0.0,-0.546875,0.0,0.0,1461 +0.0,-0.546875,0.0,0.0,1482 +0.0,-0.546875,0.0,0.0,1502 +0.0,-0.546875,0.0,0.0,1521 +0.0,-0.546875,0.0,0.0,1542 +0.0,-0.546875,0.0,0.0,1562 +0.0,-0.546875,0.0,0.0,1598 +0.0,-0.546875,0.0,0.0,1609 +0.0,-0.546875,0.0,0.0,1621 +0.0,-0.546875,0.0,0.0,1642 +0.0,-0.546875,0.0,0.0,1661 +0.0,-0.546875,0.0,0.0,1682 +0.0,-0.546875,0.0,0.0,1702 +0.0,-0.546875,0.0,0.0,1722 +0.0,-0.546875,0.0,0.0,1742 +0.0,-0.546875,0.0,0.0,1762 +0.0,-0.546875,0.0,0.0,1781 +0.0,-0.5390625,0.0,0.0,1801 +0.0,-0.5390625,0.0,0.0,1835 +0.0,-0.5390625,0.0,0.0,1847 +0.0,-0.5390625,0.0,0.0,1861 +0.0,-0.5390625,0.0,0.0,1882 +0.0,-0.5390625,0.0,0.0,1901 +0.0,-0.5390625,0.0,0.0,1921 +0.0,-0.5390625,0.0,0.0,1941 +0.0,-0.5390625,0.0,0.0,1962 +0.0,-0.5390625,0.0,0.0,1983 +0.0,-0.5390625,0.0,0.0,2001 +0.0,-0.5390625,0.0,0.0,2022 +0.0,-0.5390625,0.0,0.0,2042 +0.0,-0.5390625,0.0,0.0,2085 +0.0,-0.5390625,0.0,0.0,2097 +0.0,-0.5390625,0.0,0.0,2113 +0.0,-0.5390625,0.0,0.0,2124 +0.0,-0.5390625,0.0,0.0,2141 +0.0,-0.5390625,0.0,0.0,2161 +0.0,-0.5390625,0.0,0.0,2181 +0.0,-0.5390625,0.0,0.0,2201 +0.0,-0.5390625,0.0,0.0,2221 +0.0,-0.5390625,0.0,0.0,2241 +0.0,-0.5390625,0.0,0.0,2262 +0.0,-0.5390625,0.0,0.0,2283 +0.0,-0.2265625,0.0,0.0,2377 +0.0,-0.2265625,0.0,0.0,2390 +0.0,0.0,0.0,0.0,2402 +0.0,0.0,0.0,0.0,2417 +0.0,0.0,0.0,0.0,2428 +0.0,0.0,0.0,0.0,2440 +0.0,0.0,0.0,0.0,2451 +0.0,0.0,0.0,0.0,2463 +0.0,0.0,0.0,0.0,2475 +0.0,0.0,0.0,0.0,2493 +0.0,0.0,0.0,0.0,2505 +0.0,0.0,0.0,0.0,2521 +0.0,0.0,0.0,0.0,2558 +0.0,0.0,0.0,0.0,2576 +0.0,0.0,0.0,0.0,2592 +0.0,0.0,0.0,0.0,2603 +0.0,0.0,0.0,0.0,2621 +0.0,0.0,0.0,0.0,2641 +0.0,0.0,0.0,0.0,2661 +0.0,0.0,0.0,0.0,2681 +0.0,0.0,0.0,0.0,2702 +0.0,0.0,0.0,0.0,2721 +0.0,0.0,0.0,0.0,2742 +0.0,0.0,0.0,0.0,2762 +0.0,0.0,0.0,0.0,2805 +0.0,0.0,0.0,0.0,2814 +0.0,0.0,0.0,0.0,2825 +0.0,0.0,0.0,0.0,2841 +0.0,0.0,0.0,0.0,2861 +0.0,0.0,0.0,0.0,2882 +0.0,0.0,0.0,0.0,2901 0.0,0.0,0.0,0.0,2922 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..45a25fd 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -1,91 +1,91 @@ -package frc4388.robot.commands.Autos; - -import java.io.File; -import java.util.ArrayList; -import java.util.HashMap; - -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc4388.robot.commands.Swerve.JoystickPlayback; -import frc4388.robot.subsystems.SwerveDrive; - -public class PlaybackChooser { - private final ArrayList> m_choosers = new ArrayList<>(); - private SendableChooser m_playback = null; - private final ArrayList m_widgets = new ArrayList<>(); - private final HashMap m_commandPool = new HashMap<>(); - - private final File m_dir = new File("/home/lvuser/autos/"); - private int m_cmdNum = 0; - private final SwerveDrive m_swerve; - - // commands - private Command m_noAuto = new InstantCommand(); - - public PlaybackChooser(SwerveDrive swerve, Object... pool) { - m_swerve = swerve; - } - - public PlaybackChooser addOption(String name, Command option) { - m_commandPool.put(name, option); - return this; - } - - public PlaybackChooser buildDisplay() { - for (int i = 0; i < 10; i++) { - appendCommand(); - } - m_playback = m_choosers.get(0); - nextChooser(); - - Shuffleboard.getTab("Auto Chooser") - .add("Add Sequence", new InstantCommand(() -> nextChooser())) - .withPosition(4, 0); - return this; - } - - // This will be bound to a button for the time being - public void appendCommand() { - var chooser = new SendableChooser(); - chooser.setDefaultOption("No Auto", m_noAuto); - - m_choosers.add(chooser); - ComplexWidget widget = Shuffleboard.getTab("Auto Chooser") - .add("Command: " + m_choosers.size(), chooser) - .withSize(4, 1) - .withPosition(0, m_choosers.size() - 1) - .withWidget(BuiltInWidgets.kSplitButtonChooser); - - m_widgets.add(widget); - } - - public void nextChooser() { - SendableChooser chooser = m_choosers.get(m_cmdNum++); - - for (String auto : m_dir.list()) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } - for (var cmd_name : m_commandPool.keySet()) { - chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); - } - } - - public Command getCommand() { - Command command = m_playback.getSelected(); - command = command == null ? m_noAuto : command.asProxy(); - - Command[] commands = new Command[m_cmdNum - 1]; - for (int i = 0; i < m_cmdNum - 1; i++) { - Command command2 = m_choosers.get(i + 1).getSelected(); - command2 = command2 == null ? m_noAuto : command2.asProxy(); - - commands[i] = command2.asProxy(); - } - - return command.andThen(commands); - } -} +package frc4388.robot.commands.Autos; + +import java.io.File; +import java.util.ArrayList; +import java.util.HashMap; + +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.commands.Swerve.JoystickPlayback; +import frc4388.robot.subsystems.SwerveDrive; + +public class PlaybackChooser { + private final ArrayList> m_choosers = new ArrayList<>(); + private SendableChooser m_playback = null; + private final ArrayList m_widgets = new ArrayList<>(); + private final HashMap m_commandPool = new HashMap<>(); + + private final File m_dir = new File("/home/lvuser/autos/"); + private int m_cmdNum = 0; + private final SwerveDrive m_swerve; + + // commands + private Command m_noAuto = new InstantCommand(); + + public PlaybackChooser(SwerveDrive swerve, Object... pool) { + m_swerve = swerve; + } + + public PlaybackChooser addOption(String name, Command option) { + m_commandPool.put(name, option); + return this; + } + + public PlaybackChooser buildDisplay() { + for (int i = 0; i < 10; i++) { + appendCommand(); + } + m_playback = m_choosers.get(0); + nextChooser(); + + Shuffleboard.getTab("Auto Chooser") + .add("Add Sequence", new InstantCommand(() -> nextChooser())) + .withPosition(4, 0); + return this; + } + + // This will be bound to a button for the time being + public void appendCommand() { + var chooser = new SendableChooser(); + chooser.setDefaultOption("No Auto", m_noAuto); + + m_choosers.add(chooser); + ComplexWidget widget = Shuffleboard.getTab("Auto Chooser") + .add("Command: " + m_choosers.size(), chooser) + .withSize(4, 1) + .withPosition(0, m_choosers.size() - 1) + .withWidget(BuiltInWidgets.kSplitButtonChooser); + + m_widgets.add(widget); + } + + public void nextChooser() { + SendableChooser chooser = m_choosers.get(m_cmdNum++); + + for (String auto : m_dir.list()) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } + for (var cmd_name : m_commandPool.keySet()) { + chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); + } + } + + public Command getCommand() { + Command command = m_playback.getSelected(); + command = command == null ? m_noAuto : command.asProxy(); + + Command[] commands = new Command[m_cmdNum - 1]; + for (int i = 0; i < m_cmdNum - 1; i++) { + Command command2 = m_choosers.get(i + 1).getSelected(); + command2 = command2 == null ? m_noAuto : command2.asProxy(); + + commands[i] = command2.asProxy(); + } + + return command.andThen(commands); + } +} diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt index c99ee2c..2e68168 100644 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt +++ b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt @@ -1,225 +1,225 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,12 -0.0,0.0,0.0,0.0,26 -0.0,0.0,0.0,0.0,37 -0.0,0.0,0.0,0.0,50 -0.0,0.0,0.0,0.0,62 -0.0,0.0,0.0,0.0,73 -0.0,0.0,0.0,0.0,88 -0.0,0.0,0.0,0.0,103 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,173 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,211 -0.0,0.0,0.0,0.0,223 -0.0,0.0,0.0,0.0,235 -0.0,0.0,0.0,0.0,247 -0.0,0.0,0.0,0.0,263 -0.0,0.0,0.0,0.0,283 -0.0,0.0,0.0,0.0,303 -0.0,-0.109375,0.0,0.0,323 -0.0,-0.1484375,0.0,0.0,343 -0.0,-0.2109375,0.0,0.0,363 -0.0,-0.3671875,0.0,0.0,398 -0.0,-0.4140625,0.0,0.0,411 -0.0,-0.4765625,0.0,0.0,425 -0.0,-0.5078125,0.0,0.0,443 -0.0,-0.5078125,0.0,0.0,463 -0.0,-0.53125,0.0,0.0,483 -0.0,-0.5546875,0.0,0.0,504 -0.0,-0.5625,0.0,0.0,523 -0.0,-0.5625,0.0,0.0,544 -0.0,-0.5703125,0.0,0.0,563 -0.0,-0.5859375,0.0,0.0,584 -0.0,-0.5859375,0.0,0.0,603 -0.0,-0.5859375,0.0,0.0,640 -0.0,-0.59375,0.0,0.0,657 -0.0,-0.6015625,0.0,0.0,672 -0.0,-0.6015625,0.0,0.0,685 -0.0,-0.6015625,0.0,0.0,703 -0.0,-0.6015625,0.0,0.0,723 -0.0,-0.6015625,0.0,0.0,743 -0.0,-0.6015625,0.0,0.0,763 -0.0,-0.6015625,0.0,0.0,783 -0.0,-0.6015625,0.0,0.0,803 -0.0,-0.6015625,0.0,0.0,823 -0.0,-0.6015625,0.0,0.0,844 -0.0,-0.6015625,0.0,0.0,878 -0.0,-0.6015625,0.0,0.0,893 -0.0,-0.6015625,0.0,0.0,907 -0.0,-0.6015625,0.0,0.0,924 -0.0,-0.609375,0.0,0.0,943 -0.0,-0.609375,0.0,0.0,963 -0.0,-0.609375,0.0,0.0,983 -0.0,-0.609375,0.0,0.0,1004 -0.0,-0.609375,0.0,0.0,1023 -0.0,-0.609375,0.0,0.0,1043 -0.0,-0.609375,0.0,0.0,1064 -0.0,-0.609375,0.0,0.0,1083 -0.0,-0.609375,0.0,0.0,1156 -0.0,-0.609375,0.0,0.0,1172 -0.0,-0.609375,0.0,0.0,1185 -0.0,-0.609375,0.0,0.0,1200 -0.0,-0.609375,0.0,0.0,1215 -0.0,-0.609375,0.0,0.0,1225 -0.0,-0.609375,0.0,0.0,1236 -0.0,-0.609375,0.0,0.0,1249 -0.0,-0.609375,0.0,0.0,1263 -0.0,-0.609375,0.0,0.0,1283 -0.0,-0.609375,0.0,0.0,1303 -0.0,-0.609375,0.0,0.0,1323 -0.0,-0.609375,0.0,0.0,1363 -0.0,-0.6015625,0.0,0.0,1376 -0.0,-0.6015625,0.0,0.0,1394 -0.0,-0.6015625,0.0,0.0,1405 -0.0,-0.6015625,0.0,0.0,1423 -0.0,-0.6015625,0.0,0.0,1443 -0.0,-0.6015625,0.0,0.0,1463 -0.0,-0.6015625,0.0,0.0,1483 -0.0,-0.6015625,0.0,0.0,1503 -0.0,-0.6015625,0.0,0.0,1523 -0.0,-0.6015625,0.0,0.0,1543 -0.0,-0.6015625,0.0,0.0,1563 -0.0,-0.6015625,0.0,0.0,1597 -0.0,-0.6015625,0.0,0.0,1608 -0.0,-0.6015625,0.0,0.0,1624 -0.0,-0.6015625,0.0,0.0,1643 -0.0,-0.6015625,0.0,0.0,1664 -0.0,-0.5859375,0.0,0.0,1683 -0.0,-0.5859375,0.0,0.0,1703 -0.0,-0.5625,0.0,0.0,1723 -0.0,-0.5625,0.0,0.0,1743 -0.0,-0.5625,0.0,0.0,1763 -0.0,-0.5625,0.0,0.0,1783 -0.0,-0.5625,0.0,0.0,1803 -0.0,-0.5625,0.0,0.0,1843 -0.0,-0.5625,0.0,0.0,1855 -0.0,-0.5625,0.0,0.0,1868 -0.0,-0.5625,0.0,0.0,1883 -0.0,-0.5625,0.0,0.0,1903 -0.0,-0.5625,0.0,0.0,1923 -0.0,-0.5625,0.0,0.0,1943 -0.0,-0.5625,0.0,0.0,1963 -0.0,-0.5625,0.0,0.0,1983 -0.0,-0.5625,0.0,0.0,2003 -0.0,-0.5625,0.0,0.0,2024 -0.0,-0.5625,0.0,0.0,2043 -0.0,-0.5625,0.0,0.0,2081 -0.0,-0.5625,0.0,0.0,2093 -0.0,-0.5625,0.0,0.0,2105 -0.0,-0.5625,0.0,0.0,2123 -0.0,-0.5625,0.0,0.0,2143 -0.0,-0.5625,0.0,0.0,2163 -0.0,-0.5625,0.0,0.0,2183 -0.0,-0.5625,0.0,0.0,2203 -0.0,-0.5625,0.0,0.0,2223 -0.0,-0.5625,0.0,0.0,2243 -0.0,-0.5625,0.0,0.0,2263 -0.0,-0.5625,0.0,0.0,2283 -0.0,-0.5625,0.0,0.0,2366 -0.0,-0.5625,0.0,0.0,2377 -0.0,-0.5625,0.0,0.0,2394 -0.0,-0.5703125,0.0,0.0,2405 -0.0,-0.5703125,0.0,0.0,2418 -0.0,-0.5703125,0.0,0.0,2431 -0.0,-0.5703125,0.0,0.0,2444 -0.0,-0.5703125,0.0,0.0,2458 -0.0,-0.5703125,0.0,0.0,2470 -0.0,-0.5703125,0.0,0.0,2485 -0.0,-0.5703125,0.0,0.0,2503 -0.0,-0.5703125,0.0,0.0,2523 -0.0,-0.5703125,0.0,0.0,2563 -0.0,-0.5703125,0.0,0.0,2577 -0.0,-0.5703125,0.0,0.0,2591 -0.0,-0.5703125,0.0,0.0,2608 -0.0,-0.5703125,0.0,0.0,2624 -0.0,-0.5703125,0.0,0.0,2643 -0.0,-0.5703125,0.0,0.0,2677 -0.0,-0.5703125,0.0,0.0,2698 -0.0,-0.5703125,0.0,0.0,2711 -0.0,-0.5703125,0.0,0.0,2725 -0.0,-0.5703125,0.0,0.0,2743 -0.0,-0.5703125,0.0,0.0,2764 -0.0,-0.5703125,0.0,0.0,2810 -0.0,-0.5703125,0.0,0.0,2820 -0.0,-0.5703125,0.0,0.0,2833 -0.0,-0.5703125,0.0,0.0,2845 -0.0,-0.5703125,0.0,0.0,2863 -0.0,-0.5703125,0.0,0.0,2883 -0.0,-0.5703125,0.0,0.0,2904 -0.0,-0.5703125,0.0,0.0,2924 -0.0,-0.5703125,0.0,0.0,2943 -0.0,-0.5703125,0.0,0.0,2963 -0.0,-0.5703125,0.0,0.0,2983 -0.0,-0.5703125,0.0,0.0,3003 -0.0,-0.5703125,0.0,0.0,3033 -0.0,-0.5703125,0.0,0.0,3050 -0.0,-0.5703125,0.0,0.0,3065 -0.0,-0.5703125,0.0,0.0,3083 -0.0,-0.5703125,0.0,0.0,3103 -0.0,-0.5703125,0.0,0.0,3123 -0.0,-0.5703125,0.0,0.0,3144 -0.0,-0.5703125,0.0,0.0,3164 -0.0,-0.5703125,0.0,0.0,3184 -0.0,-0.5703125,0.0,0.0,3203 -0.0,-0.5703125,0.0,0.0,3223 -0.0,-0.5703125,0.0,0.0,3243 -0.0,-0.5703125,0.0,0.0,3272 -0.0,-0.5703125,0.0,0.0,3289 -0.0,-0.5703125,0.0,0.0,3303 -0.0,-0.5703125,0.0,0.0,3323 -0.0,-0.5703125,0.0,0.0,3343 -0.0,-0.5703125,0.0,0.0,3363 -0.0,-0.5703125,0.0,0.0,3383 -0.0,-0.5703125,0.0,0.0,3403 -0.0,-0.5703125,0.0,0.0,3423 -0.0,-0.5703125,0.0,0.0,3443 -0.0,-0.5703125,0.0,0.0,3463 -0.0,-0.5703125,0.0,0.0,3483 -0.0,-0.5703125,0.0,0.0,3566 -0.0,-0.5703125,0.0,0.0,3578 -0.0,-0.5703125,0.0,0.0,3596 -0.0,-0.5703125,0.0,0.0,3610 -0.0,-0.5703125,0.0,0.0,3623 -0.0,-0.5703125,0.0,0.0,3640 -0.0,-0.5703125,0.0,0.0,3651 -0.0,-0.5703125,0.0,0.0,3663 -0.0,-0.5703125,0.0,0.0,3678 -0.0,-0.5703125,0.0,0.0,3691 -0.0,-0.5703125,0.0,0.0,3706 -0.0,-0.5703125,0.0,0.0,3723 -0.0,-0.5703125,0.0,0.0,3766 -0.0,-0.5703125,0.0,0.0,3778 -0.0,-0.5703125,0.0,0.0,3792 -0.0,-0.5703125,0.0,0.0,3807 -0.0,-0.5703125,0.0,0.0,3823 -0.0,-0.5703125,0.0,0.0,3843 -0.0,-0.53125,0.0,0.0,3863 -0.0,-0.53125,0.0,0.0,3884 -0.0,-0.421875,0.0,0.0,3904 -0.0,0.0,0.0,0.0,3924 -0.0,0.0,0.0,0.0,3944 -0.0,0.0,0.0,0.0,3963 -0.0,0.0,0.0,0.0,3999 -0.0,0.0,0.0,0.0,4010 -0.0,0.0,0.0,0.0,4025 -0.0,0.0,0.0,0.0,4043 -0.0,0.0,0.0,0.0,4063 -0.0,0.0,0.0,0.0,4083 -0.0,0.0,0.0,0.0,4103 -0.0,0.0,0.0,0.0,4123 -0.0,0.0,0.0,0.0,4143 -0.0,0.0,0.0,0.0,4163 -0.0,0.0,0.0,0.0,4183 -0.0,0.0,0.0,0.0,4203 -0.0,0.0,0.0,0.0,4236 -0.0,0.0,0.0,0.0,4247 -0.0,0.0,0.0,0.0,4264 -0.0,0.0,0.0,0.0,4284 -0.0,0.0,0.0,0.0,4304 -0.0,0.0,0.0,0.0,4324 -0.0,0.0,0.0,0.0,4343 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,12 +0.0,0.0,0.0,0.0,26 +0.0,0.0,0.0,0.0,37 +0.0,0.0,0.0,0.0,50 +0.0,0.0,0.0,0.0,62 +0.0,0.0,0.0,0.0,73 +0.0,0.0,0.0,0.0,88 +0.0,0.0,0.0,0.0,103 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,160 +0.0,0.0,0.0,0.0,173 +0.0,0.0,0.0,0.0,185 +0.0,0.0,0.0,0.0,200 +0.0,0.0,0.0,0.0,211 +0.0,0.0,0.0,0.0,223 +0.0,0.0,0.0,0.0,235 +0.0,0.0,0.0,0.0,247 +0.0,0.0,0.0,0.0,263 +0.0,0.0,0.0,0.0,283 +0.0,0.0,0.0,0.0,303 +0.0,-0.109375,0.0,0.0,323 +0.0,-0.1484375,0.0,0.0,343 +0.0,-0.2109375,0.0,0.0,363 +0.0,-0.3671875,0.0,0.0,398 +0.0,-0.4140625,0.0,0.0,411 +0.0,-0.4765625,0.0,0.0,425 +0.0,-0.5078125,0.0,0.0,443 +0.0,-0.5078125,0.0,0.0,463 +0.0,-0.53125,0.0,0.0,483 +0.0,-0.5546875,0.0,0.0,504 +0.0,-0.5625,0.0,0.0,523 +0.0,-0.5625,0.0,0.0,544 +0.0,-0.5703125,0.0,0.0,563 +0.0,-0.5859375,0.0,0.0,584 +0.0,-0.5859375,0.0,0.0,603 +0.0,-0.5859375,0.0,0.0,640 +0.0,-0.59375,0.0,0.0,657 +0.0,-0.6015625,0.0,0.0,672 +0.0,-0.6015625,0.0,0.0,685 +0.0,-0.6015625,0.0,0.0,703 +0.0,-0.6015625,0.0,0.0,723 +0.0,-0.6015625,0.0,0.0,743 +0.0,-0.6015625,0.0,0.0,763 +0.0,-0.6015625,0.0,0.0,783 +0.0,-0.6015625,0.0,0.0,803 +0.0,-0.6015625,0.0,0.0,823 +0.0,-0.6015625,0.0,0.0,844 +0.0,-0.6015625,0.0,0.0,878 +0.0,-0.6015625,0.0,0.0,893 +0.0,-0.6015625,0.0,0.0,907 +0.0,-0.6015625,0.0,0.0,924 +0.0,-0.609375,0.0,0.0,943 +0.0,-0.609375,0.0,0.0,963 +0.0,-0.609375,0.0,0.0,983 +0.0,-0.609375,0.0,0.0,1004 +0.0,-0.609375,0.0,0.0,1023 +0.0,-0.609375,0.0,0.0,1043 +0.0,-0.609375,0.0,0.0,1064 +0.0,-0.609375,0.0,0.0,1083 +0.0,-0.609375,0.0,0.0,1156 +0.0,-0.609375,0.0,0.0,1172 +0.0,-0.609375,0.0,0.0,1185 +0.0,-0.609375,0.0,0.0,1200 +0.0,-0.609375,0.0,0.0,1215 +0.0,-0.609375,0.0,0.0,1225 +0.0,-0.609375,0.0,0.0,1236 +0.0,-0.609375,0.0,0.0,1249 +0.0,-0.609375,0.0,0.0,1263 +0.0,-0.609375,0.0,0.0,1283 +0.0,-0.609375,0.0,0.0,1303 +0.0,-0.609375,0.0,0.0,1323 +0.0,-0.609375,0.0,0.0,1363 +0.0,-0.6015625,0.0,0.0,1376 +0.0,-0.6015625,0.0,0.0,1394 +0.0,-0.6015625,0.0,0.0,1405 +0.0,-0.6015625,0.0,0.0,1423 +0.0,-0.6015625,0.0,0.0,1443 +0.0,-0.6015625,0.0,0.0,1463 +0.0,-0.6015625,0.0,0.0,1483 +0.0,-0.6015625,0.0,0.0,1503 +0.0,-0.6015625,0.0,0.0,1523 +0.0,-0.6015625,0.0,0.0,1543 +0.0,-0.6015625,0.0,0.0,1563 +0.0,-0.6015625,0.0,0.0,1597 +0.0,-0.6015625,0.0,0.0,1608 +0.0,-0.6015625,0.0,0.0,1624 +0.0,-0.6015625,0.0,0.0,1643 +0.0,-0.6015625,0.0,0.0,1664 +0.0,-0.5859375,0.0,0.0,1683 +0.0,-0.5859375,0.0,0.0,1703 +0.0,-0.5625,0.0,0.0,1723 +0.0,-0.5625,0.0,0.0,1743 +0.0,-0.5625,0.0,0.0,1763 +0.0,-0.5625,0.0,0.0,1783 +0.0,-0.5625,0.0,0.0,1803 +0.0,-0.5625,0.0,0.0,1843 +0.0,-0.5625,0.0,0.0,1855 +0.0,-0.5625,0.0,0.0,1868 +0.0,-0.5625,0.0,0.0,1883 +0.0,-0.5625,0.0,0.0,1903 +0.0,-0.5625,0.0,0.0,1923 +0.0,-0.5625,0.0,0.0,1943 +0.0,-0.5625,0.0,0.0,1963 +0.0,-0.5625,0.0,0.0,1983 +0.0,-0.5625,0.0,0.0,2003 +0.0,-0.5625,0.0,0.0,2024 +0.0,-0.5625,0.0,0.0,2043 +0.0,-0.5625,0.0,0.0,2081 +0.0,-0.5625,0.0,0.0,2093 +0.0,-0.5625,0.0,0.0,2105 +0.0,-0.5625,0.0,0.0,2123 +0.0,-0.5625,0.0,0.0,2143 +0.0,-0.5625,0.0,0.0,2163 +0.0,-0.5625,0.0,0.0,2183 +0.0,-0.5625,0.0,0.0,2203 +0.0,-0.5625,0.0,0.0,2223 +0.0,-0.5625,0.0,0.0,2243 +0.0,-0.5625,0.0,0.0,2263 +0.0,-0.5625,0.0,0.0,2283 +0.0,-0.5625,0.0,0.0,2366 +0.0,-0.5625,0.0,0.0,2377 +0.0,-0.5625,0.0,0.0,2394 +0.0,-0.5703125,0.0,0.0,2405 +0.0,-0.5703125,0.0,0.0,2418 +0.0,-0.5703125,0.0,0.0,2431 +0.0,-0.5703125,0.0,0.0,2444 +0.0,-0.5703125,0.0,0.0,2458 +0.0,-0.5703125,0.0,0.0,2470 +0.0,-0.5703125,0.0,0.0,2485 +0.0,-0.5703125,0.0,0.0,2503 +0.0,-0.5703125,0.0,0.0,2523 +0.0,-0.5703125,0.0,0.0,2563 +0.0,-0.5703125,0.0,0.0,2577 +0.0,-0.5703125,0.0,0.0,2591 +0.0,-0.5703125,0.0,0.0,2608 +0.0,-0.5703125,0.0,0.0,2624 +0.0,-0.5703125,0.0,0.0,2643 +0.0,-0.5703125,0.0,0.0,2677 +0.0,-0.5703125,0.0,0.0,2698 +0.0,-0.5703125,0.0,0.0,2711 +0.0,-0.5703125,0.0,0.0,2725 +0.0,-0.5703125,0.0,0.0,2743 +0.0,-0.5703125,0.0,0.0,2764 +0.0,-0.5703125,0.0,0.0,2810 +0.0,-0.5703125,0.0,0.0,2820 +0.0,-0.5703125,0.0,0.0,2833 +0.0,-0.5703125,0.0,0.0,2845 +0.0,-0.5703125,0.0,0.0,2863 +0.0,-0.5703125,0.0,0.0,2883 +0.0,-0.5703125,0.0,0.0,2904 +0.0,-0.5703125,0.0,0.0,2924 +0.0,-0.5703125,0.0,0.0,2943 +0.0,-0.5703125,0.0,0.0,2963 +0.0,-0.5703125,0.0,0.0,2983 +0.0,-0.5703125,0.0,0.0,3003 +0.0,-0.5703125,0.0,0.0,3033 +0.0,-0.5703125,0.0,0.0,3050 +0.0,-0.5703125,0.0,0.0,3065 +0.0,-0.5703125,0.0,0.0,3083 +0.0,-0.5703125,0.0,0.0,3103 +0.0,-0.5703125,0.0,0.0,3123 +0.0,-0.5703125,0.0,0.0,3144 +0.0,-0.5703125,0.0,0.0,3164 +0.0,-0.5703125,0.0,0.0,3184 +0.0,-0.5703125,0.0,0.0,3203 +0.0,-0.5703125,0.0,0.0,3223 +0.0,-0.5703125,0.0,0.0,3243 +0.0,-0.5703125,0.0,0.0,3272 +0.0,-0.5703125,0.0,0.0,3289 +0.0,-0.5703125,0.0,0.0,3303 +0.0,-0.5703125,0.0,0.0,3323 +0.0,-0.5703125,0.0,0.0,3343 +0.0,-0.5703125,0.0,0.0,3363 +0.0,-0.5703125,0.0,0.0,3383 +0.0,-0.5703125,0.0,0.0,3403 +0.0,-0.5703125,0.0,0.0,3423 +0.0,-0.5703125,0.0,0.0,3443 +0.0,-0.5703125,0.0,0.0,3463 +0.0,-0.5703125,0.0,0.0,3483 +0.0,-0.5703125,0.0,0.0,3566 +0.0,-0.5703125,0.0,0.0,3578 +0.0,-0.5703125,0.0,0.0,3596 +0.0,-0.5703125,0.0,0.0,3610 +0.0,-0.5703125,0.0,0.0,3623 +0.0,-0.5703125,0.0,0.0,3640 +0.0,-0.5703125,0.0,0.0,3651 +0.0,-0.5703125,0.0,0.0,3663 +0.0,-0.5703125,0.0,0.0,3678 +0.0,-0.5703125,0.0,0.0,3691 +0.0,-0.5703125,0.0,0.0,3706 +0.0,-0.5703125,0.0,0.0,3723 +0.0,-0.5703125,0.0,0.0,3766 +0.0,-0.5703125,0.0,0.0,3778 +0.0,-0.5703125,0.0,0.0,3792 +0.0,-0.5703125,0.0,0.0,3807 +0.0,-0.5703125,0.0,0.0,3823 +0.0,-0.5703125,0.0,0.0,3843 +0.0,-0.53125,0.0,0.0,3863 +0.0,-0.53125,0.0,0.0,3884 +0.0,-0.421875,0.0,0.0,3904 +0.0,0.0,0.0,0.0,3924 +0.0,0.0,0.0,0.0,3944 +0.0,0.0,0.0,0.0,3963 +0.0,0.0,0.0,0.0,3999 +0.0,0.0,0.0,0.0,4010 +0.0,0.0,0.0,0.0,4025 +0.0,0.0,0.0,0.0,4043 +0.0,0.0,0.0,0.0,4063 +0.0,0.0,0.0,0.0,4083 +0.0,0.0,0.0,0.0,4103 +0.0,0.0,0.0,0.0,4123 +0.0,0.0,0.0,0.0,4143 +0.0,0.0,0.0,0.0,4163 +0.0,0.0,0.0,0.0,4183 +0.0,0.0,0.0,0.0,4203 +0.0,0.0,0.0,0.0,4236 +0.0,0.0,0.0,0.0,4247 +0.0,0.0,0.0,0.0,4264 +0.0,0.0,0.0,0.0,4284 +0.0,0.0,0.0,0.0,4304 +0.0,0.0,0.0,0.0,4324 +0.0,0.0,0.0,0.0,4343 0.0,0.0,0.0,0.0,4363 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/BooleanCommand.java b/src/main/java/frc4388/robot/commands/BooleanCommand.java index 91e49b3..23c32ff 100644 --- a/src/main/java/frc4388/robot/commands/BooleanCommand.java +++ b/src/main/java/frc4388/robot/commands/BooleanCommand.java @@ -1,67 +1,67 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class BooleanCommand extends CommandBase { - - private Supplier onTrue; - private Supplier onFalse; - - private Supplier condition; - - private Supplier selected; - - - /** Creates a new BooleanCommand. */ - public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier condition) { - this.onTrue = onTrue; - this.onFalse = onFalse; - this.condition = condition; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if (condition.get()) { - selected = onTrue; - } else { - selected = onFalse; - } - if (selected.get() != null) { - selected.get().initialize(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (selected.get() != null) { - selected.get().execute(); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - if (selected.get() != null) { - selected.get().end(interrupted); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (selected.get() != null) { - return selected.get().isFinished(); - } else { - return true; - } - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class BooleanCommand extends CommandBase { + + private Supplier onTrue; + private Supplier onFalse; + + private Supplier condition; + + private Supplier selected; + + + /** Creates a new BooleanCommand. */ + public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier condition) { + this.onTrue = onTrue; + this.onFalse = onFalse; + this.condition = condition; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (condition.get()) { + selected = onTrue; + } else { + selected = onFalse; + } + if (selected.get() != null) { + selected.get().initialize(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (selected.get() != null) { + selected.get().execute(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (selected.get() != null) { + selected.get().end(interrupted); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (selected.get() != null) { + return selected.get().isFinished(); + } else { + return true; + } + } +} diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index a3c9e6c..0b949fb 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -1,60 +1,60 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.utility.Gains; - -public abstract class PelvicInflammatoryDisease extends CommandBase { - protected Gains gains; - private double output = 0; - private double tolerance = 0; - - /** Creates a new PelvicInflammatoryDisease. */ - public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) { - gains = new Gains(kp, ki, kd, kf, 0); - this.tolerance = tolerance; - } - - public PelvicInflammatoryDisease(Gains gains, double tolerance) { - this.gains = gains; - this.tolerance = tolerance; - } - - /** produces the error from the setpoint */ - public abstract double getError(); - - /** figure it out bitch */ - public abstract void runWithOutput(double output); - - // Called when the command is initially scheduled. - @Override - public final void initialize() { - output = 0; - } - - private double prevError, cumError = 0; - - // Called every time the scheduler runs while the command is scheduled. - @Override - public final void execute() { - double error = getError(); - cumError += error * .02; // 20 ms - double delta = error - prevError; - - output = error * gains.kP; - output += cumError * gains.kI; - output += delta * gains.kD; - output += gains.kF; - - runWithOutput(output); - } - - // Returns true when the command should end. - @Override - public final boolean isFinished() { - return Math.abs(getError()) < tolerance; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.Gains; + +public abstract class PelvicInflammatoryDisease extends CommandBase { + protected Gains gains; + private double output = 0; + private double tolerance = 0; + + /** Creates a new PelvicInflammatoryDisease. */ + public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; + } + + public PelvicInflammatoryDisease(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + + /** figure it out bitch */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + @Override + public final void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return Math.abs(getError()) < tolerance; + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java index 212d979..666aea3 100644 --- a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java @@ -1,42 +1,42 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Placement; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; - -public class AprilRotAlign extends PelvicInflammatoryDisease { - - SwerveDrive drive; - Limelight lime; - - /** Creates a new AprilRotAlign. */ - public AprilRotAlign(SwerveDrive drive, Limelight lime) { - super(0.1, 0.2, 0.0, 0.0, 0.0); - - this.drive = drive; - this.lime = lime; - - addRequirements(drive, lime); - } - - @Override - public double getError() { - double err = 0.0; - - try { - err = lime.getAprilSkew(); - } catch (NullPointerException ex) {} - - return err; - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Placement; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class AprilRotAlign extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + /** Creates a new AprilRotAlign. */ + public AprilRotAlign(SwerveDrive drive, Limelight lime) { + super(0.1, 0.2, 0.0, 0.0, 0.0); + + this.drive = drive; + this.lime = lime; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + double err = 0.0; + + try { + err = lime.getAprilSkew(); + } catch (NullPointerException ex) {} + + return err; + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java index e79cffc..5a4fa73 100644 --- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java +++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java @@ -1,44 +1,44 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Placement; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; - -public class DriveToLimeDistance extends PelvicInflammatoryDisease { - - SwerveDrive drive; - Limelight lime; - - double targetDistance; - DoubleSupplier ds; - - /** Creates a new DriveToLimeDistance. */ - public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { - super(0.5, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.lime = lime; - this.targetDistance = targetDistance; - this.ds = ds; - - addRequirements(drive, lime); - } - - @Override - public double getError() { - return targetDistance - ds.getAsDouble(); - } - - @Override - public void runWithOutput(double output) { - System.out.println(output / Math.abs(getError())); - drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Placement; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveToLimeDistance extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + double targetDistance; + DoubleSupplier ds; + + /** Creates a new DriveToLimeDistance. */ + public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { + super(0.5, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.lime = lime; + this.targetDistance = targetDistance; + this.ds = ds; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + return targetDistance - ds.getAsDouble(); + } + + @Override + public void runWithOutput(double output) { + System.out.println(output / Math.abs(getError())); + drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java index 5b2e87f..2c0900c 100644 --- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java @@ -1,48 +1,48 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Placement; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; - -public class LimeAlign extends PelvicInflammatoryDisease { - - SwerveDrive drive; - Limelight lime; - - DoubleSupplier ds; - - public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) { - super(0.4, 0.4, 0.0, 0.0, tolerance); - - this.drive = drive; - this.lime = lime; - this.ds = ds; - - addRequirements(drive, lime); - } - - @Override - public double getError() { - double err = 0.0; - - try { - System.out.println(ds.getAsDouble()); - err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); - } catch (NullPointerException ex) {} - - return err; - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Placement; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; + +public class LimeAlign extends PelvicInflammatoryDisease { + + SwerveDrive drive; + Limelight lime; + + DoubleSupplier ds; + + public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) { + super(0.4, 0.4, 0.0, 0.0, tolerance); + + this.drive = drive; + this.lime = lime; + this.ds = ds; + + addRequirements(drive, lime); + } + + @Override + public double getError() { + double err = 0.0; + + try { + System.out.println(ds.getAsDouble()); + err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); + } catch (NullPointerException ex) {} + + return err; + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 56e8d43..a0f3af1 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -1,141 +1,141 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.ArrayList; -import java.util.Scanner; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickPlayback extends CommandBase { - private final SwerveDrive swerve; - private String filename; - private int mult = 1; - private Scanner input; - private final ArrayList outputs = new ArrayList<>(); - private int counter = 0; - private long startTime = 0; - private long playbackTime = 0; - private int lastIndex; - private boolean m_finished = false; // ! find a better way - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { - this.swerve = swerve; - this.filename = filename; - this.mult = mult; - - addRequirements(this.swerve); - } - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename) { - this(swerve, filename, 1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - m_finished = false; - - startTime = System.currentTimeMillis(); - playbackTime = 0; - lastIndex = 0; - try { - input = new Scanner(new File("/home/lvuser/autos/" + filename)); - - String line = ""; - while (input.hasNextLine()) { - line = input.nextLine(); - - if (line.isEmpty() || line.isBlank() || line.equals("\n")) { - continue; - } - - String[] values = line.split(","); - System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult; - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timedOffset = Long.parseLong(values[4]); - - outputs.add(out); - } - - input.close(); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (counter == 0) { - startTime = System.currentTimeMillis(); - playbackTime = 0; - } else { - playbackTime = System.currentTimeMillis() - startTime; - } - - // skip to reasonable time frame - // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing - { - int i = lastIndex == 0 ? 1 : lastIndex; - while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { - i++; - } - - if (i >= outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - lastIndex = i; - } - - TimedOutput lastOut = outputs.get(lastIndex - 1); - TimedOutput out = outputs.get(lastIndex); - - double deltaTime = out.timedOffset - lastOut.timedOffset; - double playbackDelta = playbackTime - lastOut.timedOffset; - - double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); - double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); - double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); - double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); - - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - // new Translation2d(out.rightX, out.rightY), - // true); - - this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), - new Translation2d(lerpRX, lerpRY), - true); - - counter++; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - input.close(); - swerve.stopModules(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_finished; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import java.io.File; +import java.io.FileNotFoundException; +import java.util.ArrayList; +import java.util.Scanner; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickPlayback extends CommandBase { + private final SwerveDrive swerve; + private String filename; + private int mult = 1; + private Scanner input; + private final ArrayList outputs = new ArrayList<>(); + private int counter = 0; + private long startTime = 0; + private long playbackTime = 0; + private int lastIndex; + private boolean m_finished = false; // ! find a better way + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { + this.swerve = swerve; + this.filename = filename; + this.mult = mult; + + addRequirements(this.swerve); + } + + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename) { + this(swerve, filename, 1); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + m_finished = false; + + startTime = System.currentTimeMillis(); + playbackTime = 0; + lastIndex = 0; + try { + input = new Scanner(new File("/home/lvuser/autos/" + filename)); + + String line = ""; + while (input.hasNextLine()) { + line = input.nextLine(); + + if (line.isEmpty() || line.isBlank() || line.equals("\n")) { + continue; + } + + String[] values = line.split(","); + System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); + + var out = new TimedOutput(); + out.leftX = Double.parseDouble(values[0]) * mult; + out.leftY = Double.parseDouble(values[1]); + out.rightX = Double.parseDouble(values[2]); + out.rightY = Double.parseDouble(values[3]); + + out.timedOffset = Long.parseLong(values[4]); + + outputs.add(out); + } + + input.close(); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (counter == 0) { + startTime = System.currentTimeMillis(); + playbackTime = 0; + } else { + playbackTime = System.currentTimeMillis() - startTime; + } + + // skip to reasonable time frame + // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing + { + int i = lastIndex == 0 ? 1 : lastIndex; + while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { + i++; + } + + if (i >= outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + lastIndex = i; + } + + TimedOutput lastOut = outputs.get(lastIndex - 1); + TimedOutput out = outputs.get(lastIndex); + + double deltaTime = out.timedOffset - lastOut.timedOffset; + double playbackDelta = playbackTime - lastOut.timedOffset; + + double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); + double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); + double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); + double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); + + // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + // new Translation2d(out.rightX, out.rightY), + // true); + + this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + new Translation2d(lerpRX, lerpRY), + true); + + counter++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + input.close(); + swerve.stopModules(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 84608cc..d52b53e 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -1,97 +1,99 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import java.io.File; -import java.io.IOException; -import java.io.PrintWriter; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickRecorder extends CommandBase { - public final SwerveDrive swerve; - - public final Supplier leftX; - public final Supplier leftY; - public final Supplier rightX; - public final Supplier rightY; - private String filename; - public final ArrayList outputs = new ArrayList<>(); - private long startTime = -1; - - - /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY, - String filename) - { - this.swerve = swerve; - this.leftX = leftX; - this.leftY = leftY; - this.rightX = rightX; - this.rightY = rightY; - this.filename = filename; - - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - - this.startTime = System.currentTimeMillis(); - - outputs.add(new TimedOutput()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - var inputs = new TimedOutput(); - inputs.leftX = leftX.get(); - inputs.leftY = leftY.get(); - inputs.rightX = rightX.get(); - inputs.rightY = rightY.get(); - inputs.timedOffset = System.currentTimeMillis() - startTime; - - outputs.add(inputs); - - swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), - new Translation2d(inputs.rightX, inputs.rightY), - true); - - System.out.println("RECORDING"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - File output = new File("/home/lvuser/autos/" + filename); - - try (PrintWriter writer = new PrintWriter(output)) { - for (var input : outputs) { - writer.println( input.leftX + "," + input.leftY + "," + - input.rightX + "," + input.rightY + "," + - input.timedOffset); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import java.io.File; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; +import frc4388.utility.controller.XboxController; + +public class JoystickRecorder extends CommandBase { + public final SwerveDrive swerve; + public final XboxController driveXbox; + public final XboxController operatorXbox; + + private String filename; + public final ArrayList outputs = new ArrayList<>(); + private long startTime = -1; + + /** Creates a new JoystickRecorder. */ + public JoystickRecorder(SwerveDrive swerve, XboxController driveXbox, + XboxController operatorXbox, String filename) { + this.swerve = swerve; + this.driveXbox = driveXbox; + this.operatorXbox = operatorXbox; + this.filename = filename; + + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + + this.startTime = System.currentTimeMillis(); + + outputs.add(new TimedOutput()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + var inputs = new TimedOutput(); + inputs.driverLeftX = driveXbox.getLeftXAxis(); + inputs.driverLeftY = driveXbox.getLeftYAxis(); + inputs.driverRightX = driveXbox.getRightXAxis(); + inputs.driverRightY = driveXbox.getRightYAxis(); + + inputs.operatorLeftX = driveXbox.getLeftXAxis(); + inputs.operatorLeftY = driveXbox.getLeftYAxis(); + inputs.operatorRightX = driveXbox.getRightXAxis(); + inputs.operatorRightY = driveXbox.getRightYAxis(); + + inputs.timedOffset = System.currentTimeMillis() - startTime; + + outputs.add(inputs); + + swerve.driveWithInput(new Translation2d(inputs.driverLeftX, inputs.driverLeftX), + new Translation2d(inputs.driverRightX, inputs.driverRightY), + true); + + System.out.println("RECORDING"); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + File output = new File("/home/lvuser/autos/" + filename); + + try (PrintWriter writer = new PrintWriter(output)) { + for (var input : outputs) { + writer.println( input.driverLeftX + "," + input.driverLeftX + "," + + input.driverRightX + "," + input.driverRightY + "," + + input.operatorLeftX + "," + input.operatorLeftX + "," + + input.operatorRightX + "," + input.operatorRightY + "," + + input.timedOffset); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java index 8cf630f..b47084a 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -1,35 +1,35 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PelvicInflammatoryDisease; -import frc4388.robot.subsystems.SwerveDrive; - -public class RotateToAngle extends PelvicInflammatoryDisease { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PelvicInflammatoryDisease; +import frc4388.robot.subsystems.SwerveDrive; + +public class RotateToAngle extends PelvicInflammatoryDisease { + + SwerveDrive drive; + double targetAngle; + + /** Creates a new RotateToAngle. */ + public RotateToAngle(SwerveDrive drive, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + this.targetAngle = targetAngle; + + addRequirements(drive); + } + + @Override + public double getError() { + return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java index c6062e8..35bc6c7 100644 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java @@ -1,36 +1,36 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} +package frc4388.robot.subsystems; + +//import edu.wpi.first.apriltag.AprilTag; +//import edu.wpi.first.math.geometry.Pose3d; +//import edu.wpi.first.math.geometry.Rotation3d; +//import edu.wpi.first.networktables.NetworkTable; +//import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Apriltags { + public static class Tag { + public boolean visible = true; + public double x, y, z = 0; + public double ry, rp, rr = 0; + } + + public Tag getTagPosRot() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + final Tag tag = new Tag(); + tag.visible = isAprilTag(); + tag.x = tagTable.getEntry("TagPosX").getDouble(0); + tag.y = tagTable.getEntry("TagPosY").getDouble(0); + tag.z = tagTable.getEntry("TagPosZ").getDouble(0); + tag.ry = tagTable.getEntry("TagRotY").getDouble(0); + tag.rp = tagTable.getEntry("TagRotP").getDouble(0); + tag.rr = tagTable.getEntry("TagRotR").getDouble(0); + + return tag; + } + + public boolean isAprilTag() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + return tagTable.getEntry("IsTag").getBoolean(false); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 94081df..e99e59c 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,159 +1,159 @@ -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderConfiguration; -import frc4388.robot.Constants.ArmConstants; -import frc4388.utility.DeferredBlock; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Arm extends SubsystemBase { - private WPI_TalonFX m_tele; - public WPI_TalonFX m_pivot; - private CANCoder m_pivotEncoder; - - // Moves arm to distance [dist] then returns new ang - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) { - m_tele = tele; - m_pivot = pivot; - m_pivotEncoder = encoder; - - m_tele.configFactoryDefault(); - m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_pivot.configFactoryDefault(); - - // * Example of deferred code - new DeferredBlock(() -> resetTeleSoftLimit()); - - CANCoderConfiguration config = new CANCoderConfiguration(); - config.magnetOffsetDegrees = ArmConstants.OFFSET; - m_pivotEncoder.configAllSettings(config); - } - - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { - this(pivot, tele, encoder, false); - } - - public void setRotVel(double vel) { - if (vel > 1) vel = 1; - - var degrees = Math.abs(getArmRotation()) - 135; - SmartDashboard.putNumber("arm degrees", degrees); - SmartDashboard.putNumber("arm rot vel", vel); - - if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { - m_pivot.set(ControlMode.PercentOutput, 0); - } else if (degrees > 90 && vel > 0) { - m_pivot.set(ControlMode.PercentOutput, .15 * vel); - } else if (degrees < 25 && vel < 0) { - m_pivot.set(ControlMode.PercentOutput, .15 * vel); - } else { - m_pivot.set(ControlMode.PercentOutput, .3 * vel); - } - } - - public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, -0.5 * vel); - } - - public void armSetRotation(double rot) { - if (rot > 1 || rot < 0) return; - // Move arm code - m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); - } - - public void armSetLength(double len) { - if (len > 1 || len < 0) return; - // Move arm code - m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + - ArmConstants.TELE_FORWARD_SOFT_LIMIT); - - if (m_tele.isRevLimitSwitchClosed() == 1) { - m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT); - } else if (m_tele.isFwdLimitSwitchClosed() == 1) { - m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT); - } - } - - public double getArmLength() { - return m_tele.getSelectedSensorPosition() - tele_soft; - } - - public double getArmRotation() { - return m_pivotEncoder.getAbsolutePosition(); - } - - public void runPivotTele(double pivot, double tele) { - double abs_pivot = Math.toRadians(getArmRotation() - 135); - double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) / - (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); - - if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { - setRotVel(pivot); - setTeleVel(tele); - } - } - - /** - * Checks that an input is within bounds - * @param _len length from 0 to 1 - * @param _theta radians from the zero (straight up) - * @return - */ - public static boolean checkLimits(double _len, double _theta) { - var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN; - var x = len * Math.cos(_theta); - var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); - - var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); - return y < minHeight; - } - - boolean tele_softLimit = false; - double tele_soft = 0; - public void resetTeleSoftLimit() { - if (!tele_softLimit) { - tele_soft = m_tele.getSelectedSensorPosition(); - m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); - m_tele.configReverseSoftLimitThreshold(tele_soft); - m_tele.configForwardSoftLimitEnable(true); - m_tele.configReverseSoftLimitEnable(true); - } else { - m_tele.configForwardSoftLimitEnable(false); - m_tele.configReverseSoftLimitEnable(false); - } - - tele_softLimit = !tele_softLimit; - } - - boolean resetable = true; - boolean tele_reset = true; - - @Override - public void periodic() { - - if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { - var tele_soft = m_tele.getSelectedSensorPosition(); - m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); - m_tele.configReverseSoftLimitThreshold(1000 - tele_soft); - m_tele.configForwardSoftLimitEnable(true); - m_tele.configReverseSoftLimitEnable(true); - tele_reset = false; - } else if (m_tele.isFwdLimitSwitchClosed() == 0) { - tele_reset = true; - } - - // double x = Math.cos(Math.toRadians(degrees)); - SmartDashboard.putNumber("arm length", getArmLength()); - } - - public void killSoftLimits() { - resetTeleSoftLimit(); - } -} \ No newline at end of file +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; +import frc4388.robot.Constants.ArmConstants; +import frc4388.utility.DeferredBlock; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + private WPI_TalonFX m_tele; + public WPI_TalonFX m_pivot; + private CANCoder m_pivotEncoder; + + // Moves arm to distance [dist] then returns new ang + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) { + m_tele = tele; + m_pivot = pivot; + m_pivotEncoder = encoder; + + m_tele.configFactoryDefault(); + m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_pivot.configFactoryDefault(); + + // * Example of deferred code + new DeferredBlock(() -> resetTeleSoftLimit()); + + CANCoderConfiguration config = new CANCoderConfiguration(); + config.magnetOffsetDegrees = ArmConstants.OFFSET; + m_pivotEncoder.configAllSettings(config); + } + + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { + this(pivot, tele, encoder, false); + } + + public void setRotVel(double vel) { + if (vel > 1) vel = 1; + + var degrees = Math.abs(getArmRotation()) - 135; + SmartDashboard.putNumber("arm degrees", degrees); + SmartDashboard.putNumber("arm rot vel", vel); + + if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { + m_pivot.set(ControlMode.PercentOutput, 0); + } else if (degrees > 90 && vel > 0) { + m_pivot.set(ControlMode.PercentOutput, .15 * vel); + } else if (degrees < 25 && vel < 0) { + m_pivot.set(ControlMode.PercentOutput, .15 * vel); + } else { + m_pivot.set(ControlMode.PercentOutput, .3 * vel); + } + } + + public void setTeleVel(double vel) { + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); + } + + public void armSetRotation(double rot) { + if (rot > 1 || rot < 0) return; + // Move arm code + m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + + ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + } + + public void armSetLength(double len) { + if (len > 1 || len < 0) return; + // Move arm code + m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + + ArmConstants.TELE_FORWARD_SOFT_LIMIT); + + if (m_tele.isRevLimitSwitchClosed() == 1) { + m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT); + } else if (m_tele.isFwdLimitSwitchClosed() == 1) { + m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } + } + + public double getArmLength() { + return m_tele.getSelectedSensorPosition() - tele_soft; + } + + public double getArmRotation() { + return m_pivotEncoder.getAbsolutePosition(); + } + + public void runPivotTele(double pivot, double tele) { + double abs_pivot = Math.toRadians(getArmRotation() - 135); + double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + + if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { + setRotVel(pivot); + setTeleVel(tele); + } + } + + /** + * Checks that an input is within bounds + * @param _len length from 0 to 1 + * @param _theta radians from the zero (straight up) + * @return + */ + public static boolean checkLimits(double _len, double _theta) { + var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN; + var x = len * Math.cos(_theta); + var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); + + var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); + return y < minHeight; + } + + boolean tele_softLimit = false; + double tele_soft = 0; + public void resetTeleSoftLimit() { + if (!tele_softLimit) { + tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configReverseSoftLimitThreshold(tele_soft); + m_tele.configForwardSoftLimitEnable(true); + m_tele.configReverseSoftLimitEnable(true); + } else { + m_tele.configForwardSoftLimitEnable(false); + m_tele.configReverseSoftLimitEnable(false); + } + + tele_softLimit = !tele_softLimit; + } + + boolean resetable = true; + boolean tele_reset = true; + + @Override + public void periodic() { + + if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { + var tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configReverseSoftLimitThreshold(1000 - tele_soft); + m_tele.configForwardSoftLimitEnable(true); + m_tele.configReverseSoftLimitEnable(true); + tele_reset = false; + } else if (m_tele.isFwdLimitSwitchClosed() == 0) { + tele_reset = true; + } + + // double x = Math.cos(Math.toRadians(degrees)); + SmartDashboard.putNumber("arm length", getArmLength()); + } + + public void killSoftLimits() { + resetTeleSoftLimit(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index 985f70a..f80e444 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,61 +1,61 @@ -package frc4388.robot.subsystems; - -import java.util.Timer; -import java.util.TimerTask; - -import com.revrobotics.CANSparkMax; - -import edu.wpi.first.wpilibj.PWM; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Claw extends SubsystemBase { - - private final PWM m_leftMotor; - private final PWM m_rightMotor; - private final CANSparkMax m_spinnyspin; - - private boolean m_open = false; - - // Opens claw - public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) { - m_leftMotor = leftMotor; - m_rightMotor = rightMotor; - m_spinnyspin = spinnyspin; - - setClaw(true); - } - - public void setClaw(boolean open) { - // Open claw - m_open = open; - m_leftMotor.setRaw(m_open ? 1000 : 2000); - m_rightMotor.setRaw(m_open ? 2000 : 1000); - - if (!m_open) { - m_spinnyspin.set(-0.2); - - new Timer().schedule(new TimerTask() { - @Override - public void run() { - nospinnyspin(); - } - }, 750); - } - } - - public void toggle() { - setClaw(!m_open); - } - - public boolean isClawOpen() { - return m_open; - } - - public void yesspinnyspin() { - m_spinnyspin.set(0.2); - } - - public void nospinnyspin() { - m_spinnyspin.set(0); - } -} +package frc4388.robot.subsystems; + +import java.util.Timer; +import java.util.TimerTask; + +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Claw extends SubsystemBase { + + private final PWM m_leftMotor; + private final PWM m_rightMotor; + private final CANSparkMax m_spinnyspin; + + private boolean m_open = false; + + // Opens claw + public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) { + m_leftMotor = leftMotor; + m_rightMotor = rightMotor; + m_spinnyspin = spinnyspin; + + setClaw(true); + } + + public void setClaw(boolean open) { + // Open claw + m_open = open; + m_leftMotor.setRaw(m_open ? 1000 : 2000); + m_rightMotor.setRaw(m_open ? 2000 : 1000); + + if (!m_open) { + m_spinnyspin.set(-0.2); + + new Timer().schedule(new TimerTask() { + @Override + public void run() { + nospinnyspin(); + } + }, 750); + } + } + + public void toggle() { + setClaw(!m_open); + } + + public boolean isClawOpen() { + return m_open; + } + + public void yesspinnyspin() { + m_spinnyspin.set(0.2); + } + + public void nospinnyspin() { + m_spinnyspin.set(0); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 0d4af80..8d828c8 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -1,63 +1,63 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.LEDConstants; -import frc4388.utility.LEDPatterns; - -/** - * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED - * Driver - */ -public class LED extends SubsystemBase { - - private LEDPatterns m_currentPattern; - private Spark m_LEDController; - - /** - * Add your docs here. - */ - public LED(Spark LEDController){ - m_LEDController = LEDController; - setPattern(LEDConstants.DEFAULT_PATTERN); - updateLED(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); - } - - @Override - public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); - } - - /** - * Add your docs here. - */ - public void updateLED(){ - m_LEDController.set(m_currentPattern.getValue()); - } - - /** - * Add your docs here. - */ - public void setPattern(LEDPatterns pattern){ - m_currentPattern = pattern; - m_LEDController.set(m_currentPattern.getValue()); - } - - /** - * Add your docs here. - * @return - */ - public LEDPatterns getPattern() { - return m_currentPattern; - } -} \ No newline at end of file +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.LEDConstants; +import frc4388.utility.LEDPatterns; + +/** + * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED + * Driver + */ +public class LED extends SubsystemBase { + + private LEDPatterns m_currentPattern; + private Spark m_LEDController; + + /** + * Add your docs here. + */ + public LED(Spark LEDController){ + m_LEDController = LEDController; + setPattern(LEDConstants.DEFAULT_PATTERN); + updateLED(); + System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); + } + + @Override + public void periodic(){ + SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + } + + /** + * Add your docs here. + */ + public void updateLED(){ + m_LEDController.set(m_currentPattern.getValue()); + } + + /** + * Add your docs here. + */ + public void setPattern(LEDPatterns pattern){ + m_currentPattern = pattern; + m_LEDController.set(m_currentPattern.getValue()); + } + + /** + * Add your docs here. + * @return + */ + public LEDPatterns getPattern() { + return m_currentPattern; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..399e6f0 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -1,165 +1,165 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; - -public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; - - private boolean lightOn; - - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } - - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } - - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } - - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } - - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } - - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } - - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget(); - } - - private List getAprilCorners() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget().getDetectedCorners(); - } - - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); - - if (bottomSide == null) return 0; - - TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); - TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - - return bottomLeft.y - bottomRight.y; - } - - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; - - ArrayList bottomSide = new ArrayList<>(); - - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); - - for (TargetCorner c : box) { - if (c.y > l1.y) l1 = c; - } - - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } - - bottomSide.add(l1); - bottomSide.add(l2); - - return bottomSide; - } - - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; - - double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + aprilPoint.getPitch(); - - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } - - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = (ArrayList) result.getTargets(); - - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } - - return lowest; - } - - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; - - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + tapePoint.getPitch(); - - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; - } - - @Override - public void periodic() {} -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; + +public class Limelight extends SubsystemBase { + + private PhotonCamera cam; + private PhotonPoseEstimator photonPoseEstimator; + + private boolean lightOn; + + /** Creates a new Limelight. */ + public Limelight() { + cam = new PhotonCamera(VisionConstants.NAME); + cam.setDriverMode(false); + } + + public void setLEDs(boolean on) { + lightOn = on; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void toggleLEDs() { + lightOn = !lightOn; + cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + public void setDriverMode(boolean driverMode) { + cam.setDriverMode(driverMode); + } + + public void setToLimePipeline() { + cam.setPipelineIndex(1); + setLEDs(true); + } + + public void setToAprilPipeline() { + cam.setPipelineIndex(0); + setLEDs(false); + } + + public PhotonTrackedTarget getAprilPoint() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget(); + } + + private List getAprilCorners() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + return result.getBestTarget().getDetectedCorners(); + } + + public double getAprilSkew() { + List corners = getAprilCorners(); + ArrayList bottomSide = getAprilBottomSide(corners); + + if (bottomSide == null) return 0; + + TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); + TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); + + return bottomLeft.y - bottomRight.y; + } + + private ArrayList getAprilBottomSide(List box) { + if (box == null) return null; + + ArrayList bottomSide = new ArrayList<>(); + + TargetCorner l1 = new TargetCorner(-1, -1); + TargetCorner l2 = new TargetCorner(-1, -1); + + for (TargetCorner c : box) { + if (c.y > l1.y) l1 = c; + } + + for (TargetCorner c : box) { + if (c.y == l1.y) continue; + if (c.y > l2.y) l2 = c; + } + + bottomSide.add(l1); + bottomSide.add(l2); + + return bottomSide; + } + + public double getDistanceToApril() { + PhotonTrackedTarget aprilPoint = getAprilPoint(); + if (aprilPoint == null) return -1; + + double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + aprilPoint.getPitch(); + + double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); + return distanceToApril; + } + + public PhotonTrackedTarget getLowestTape() { + if (!cam.isConnected()) return null; + + PhotonPipelineResult result = cam.getLatestResult(); + + if (!result.hasTargets()) return null; + + ArrayList points = (ArrayList) result.getTargets(); + + PhotonTrackedTarget lowest = points.get(0); + for (PhotonTrackedTarget point : points) { + if (point.getPitch() < lowest.getPitch()) { + lowest = point; + } + } + + return lowest; + } + + public double getDistanceToTape() { + PhotonTrackedTarget tapePoint = getLowestTape(); + if (tapePoint == null) return -1; + + double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; + double theta = 35.0 + tapePoint.getPitch(); + + double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); + return distanceToTape; + } + + @Override + public void periodic() {} +} diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java index 0bf5322..0501adf 100644 --- a/src/main/java/frc4388/robot/subsystems/Location.java +++ b/src/main/java/frc4388/robot/subsystems/Location.java @@ -1,27 +1,27 @@ -package frc4388.robot.subsystems; - -import frc4388.robot.subsystems.Apriltags.Tag; - -public class Location { - final Apriltags apriltag = new Apriltags(); - - private boolean isLimelight = false; - private boolean isApriltag = false; - - //Determines which source to get pos and rot from and also resets - private void reoderPrio(){ - isLimelight = false; //If limelight gets position and if within a certain range of poles - isApriltag = apriltag.isAprilTag(); - } - - public Tag getPosRot() { - reoderPrio(); - if(isApriltag){ - return apriltag.getTagPosRot(); - } else if (isLimelight) { - return null; - } - - return null; - } -} +package frc4388.robot.subsystems; + +import frc4388.robot.subsystems.Apriltags.Tag; + +public class Location { + final Apriltags apriltag = new Apriltags(); + + private boolean isLimelight = false; + private boolean isApriltag = false; + + //Determines which source to get pos and rot from and also resets + private void reoderPrio(){ + isLimelight = false; //If limelight gets position and if within a certain range of poles + isApriltag = apriltag.isAprilTag(); + } + + public Tag getPosRot() { + reoderPrio(); + if(isApriltag){ + return apriltag.getTagPosRot(); + } else if (isLimelight) { + return null; + } + + return null; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 055230b..974a817 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -1,195 +1,195 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.RobotGyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class SwerveDrive extends SubsystemBase { - - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; - - private SwerveModule[] modules; - - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - - private RobotGyro gyro; - - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - - public double rotTarget = 0.0; - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; - - this.gyro = gyro; - - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - } - - boolean stopped = false; - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - double rot = 0; - - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - SmartDashboard.putBoolean("drift correction", true); - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - } else { - // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - /** - * Set each module of the swerve drive to the corresponding desired state. - * @param desiredStates Array of module states to set. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } - - public boolean rotateToTarget(double angle) { - double currentAngle = getGyroAngle(); - double error = angle - currentAngle; - - driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } - - return false; - } - - public double getGyroAngle() { - return gyro.getAngle(); - } - - public void resetGyro() { - gyro.reset(); - rotTarget = 0.0; - } - - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - } - - public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } - } - - public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - } - - public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - } - - public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - } - - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { - - } - } - - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } - } - -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.RobotGyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveDrive extends SubsystemBase { + + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; + + private SwerveModule[] modules; + + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + + private RobotGyro gyro; + + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + + public double rotTarget = 0.0; + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + this.leftFront = leftFront; + this.rightFront = rightFront; + this.leftBack = leftBack; + this.rightBack = rightBack; + + this.gyro = gyro; + + this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; + } + + boolean stopped = false; + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { + + double rot = 0; + + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + SmartDashboard.putBoolean("drift correction", false); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } + + SmartDashboard.putBoolean("drift correction", true); + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + } + + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + } else { + // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + + /** + * Set each module of the swerve drive to the corresponding desired state. + * @param desiredStates Array of module states to set. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } + } + + public boolean rotateToTarget(double angle) { + double currentAngle = getGyroAngle(); + double error = angle - currentAngle; + + driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); + + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } + + return false; + } + + public double getGyroAngle() { + return gyro.getAngle(); + } + + public void resetGyro() { + gyro.reset(); + rotTarget = 0.0; + } + + public void stopModules() { + for (SwerveModule module : this.modules) { + module.stop(); + } + } + + public SwerveDriveKinematics getKinematics() { + return this.kinematics; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + } + + public void shiftDown() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + } else { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } + } + + public void setToSlow() { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + } + + public void setToFast() { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + } + + public void setToTurbo() { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + } + + public void shiftUp() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + } else { + + } + } + + public void toggleGear(double angle) { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..107b895 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -1,161 +1,161 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.Gains; - -public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; - private CANCoder encoder; - - public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - - /** Creates a new SwerveModule. */ - public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.encoder = encoder; - - TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - angleConfig.slot0.kP = swerveGains.kP; - angleConfig.slot0.kI = swerveGains.kI; - angleConfig.slot0.kD = swerveGains.kD; - - // use the CANcoder as the remote sensor for the primary TalonFX PID - angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - angleMotor.configAllSettings(angleConfig); - - encoder.configMagnetOffset(offset); - - driveMotor.setSelectedSensorPosition(0); - driveMotor.config_kP(0, 0.2); - } - - /** - * Get the drive motor of the SwerveModule - * @return the drive motor of the SwerveModule - */ - public WPI_TalonFX getDriveMotor() { - return this.driveMotor; - } - - /** - * Get the angle motor of the SwerveModule - * @return the angle motor of the SwerveModule - */ - public WPI_TalonFX getAngleMotor() { - return this.angleMotor; - } - - /** - * Get the CANcoder of the SwerveModule - * @return the CANcoder of the SwerveModule - */ - public CANCoder getEncoder() { - return this.encoder; - } - - /** - * Get the angle of a SwerveModule as a Rotation2d - * @return the angle of a SwerveModule as a Rotation2d - */ - public Rotation2d getAngle() { - // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - } - - public double getAngularVel() { - return this.angleMotor.getSelectedSensorVelocity(); - } - - public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - } - - public double getDriveVel() { - return this.driveMotor.getSelectedSensorVelocity(0); - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public void rotateToAngle(double angle) { - angleMotor.set(TalonFXControlMode.Position, angle); - } - - /** - * Get state of swerve module - * @return speed in m/s and angle in degrees - */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, - getAngle() - ); - } - - /** - * Returns the current position of the SwerveModule - * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - } - - /** - * Set the speed and rotation of the SwerveModule from a SwerveModuleState object - * @param desiredState a SwerveModuleState representing the desired new state of the module - */ - public void setDesiredState(SwerveModuleState desiredState) { - Rotation2d currentRotation = this.getAngle(); - - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - - // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); - - // calculate the new absolute position of the SwerveModule based on the difference in rotation - double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - - // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - - angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - } - - public void reset(double position) { - encoder.setPositionToAbsolute(); - } - - public double getCurrent() { - return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - } - - public double getVoltage() { - return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; + +public class SwerveModule extends SubsystemBase { + private WPI_TalonFX driveMotor; + private WPI_TalonFX angleMotor; + private CANCoder encoder; + + public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; + + /** Creates a new SwerveModule. */ + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.encoder = encoder; + + TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + angleConfig.slot0.kP = swerveGains.kP; + angleConfig.slot0.kI = swerveGains.kI; + angleConfig.slot0.kD = swerveGains.kD; + + // use the CANcoder as the remote sensor for the primary TalonFX PID + angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + angleMotor.configAllSettings(angleConfig); + + encoder.configMagnetOffset(offset); + + driveMotor.setSelectedSensorPosition(0); + driveMotor.config_kP(0, 0.2); + } + + /** + * Get the drive motor of the SwerveModule + * @return the drive motor of the SwerveModule + */ + public WPI_TalonFX getDriveMotor() { + return this.driveMotor; + } + + /** + * Get the angle motor of the SwerveModule + * @return the angle motor of the SwerveModule + */ + public WPI_TalonFX getAngleMotor() { + return this.angleMotor; + } + + /** + * Get the CANcoder of the SwerveModule + * @return the CANcoder of the SwerveModule + */ + public CANCoder getEncoder() { + return this.encoder; + } + + /** + * Get the angle of a SwerveModule as a Rotation2d + * @return the angle of a SwerveModule as a Rotation2d + */ + public Rotation2d getAngle() { + // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + } + + public double getAngularVel() { + return this.angleMotor.getSelectedSensorVelocity(); + } + + public double getDrivePos() { + return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + } + + public double getDriveVel() { + return this.driveMotor.getSelectedSensorVelocity(0); + } + + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public void rotateToAngle(double angle) { + angleMotor.set(TalonFXControlMode.Position, angle); + } + + /** + * Get state of swerve module + * @return speed in m/s and angle in degrees + */ + public SwerveModuleState getState() { + return new SwerveModuleState( + Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + getAngle() + ); + } + + /** + * Returns the current position of the SwerveModule + * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + } + + /** + * Set the speed and rotation of the SwerveModule from a SwerveModuleState object + * @param desiredState a SwerveModuleState representing the desired new state of the module + */ + public void setDesiredState(SwerveModuleState desiredState) { + Rotation2d currentRotation = this.getAngle(); + + SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + + // calculate the difference between our current rotational position and our new rotational position + Rotation2d rotationDelta = state.angle.minus(currentRotation); + + // calculate the new absolute position of the SwerveModule based on the difference in rotation + double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + + // convert the CANCoder from its position reading to ticks + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + + driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + } + + public void reset(double position) { + encoder.setPositionToAbsolute(); + } + + public double getCurrent() { + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } + + public double getVoltage() { + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 371f621..61d74dd 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -1,38 +1,38 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -} +package frc4388.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class Vision { + private final NetworkTableEntry m_isTags; + private final NetworkTableEntry m_xPoses; + private final NetworkTableEntry m_yPoses; + private final NetworkTableEntry m_zPoses; + + public Vision() { + final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); + + m_isTags = tagTable.getEntry("IsTag"); + m_xPoses = tagTable.getEntry("TagPosX"); + m_yPoses = tagTable.getEntry("TagPosY"); + m_zPoses = tagTable.getEntry("TagPosZ"); + } + + public AprilTag[] getAprilTags() { + if (!m_isTags.getBoolean(false)) return new AprilTag[0]; + + double xarr[] = m_xPoses.getDoubleArray(new double[] {}); + double yarr[] = m_yPoses.getDoubleArray(new double[] {}); + double zarr[] = m_zPoses.getDoubleArray(new double[] {}); + + AprilTag tags[] = new AprilTag[xarr.length]; + for (int i = 0; i < tags.length; i++) { + tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); + } + + return tags; + } +} diff --git a/src/main/java/frc4388/utility/AbhiIsADumbass.java b/src/main/java/frc4388/utility/AbhiIsADumbass.java index 14a9515..59d06df 100644 --- a/src/main/java/frc4388/utility/AbhiIsADumbass.java +++ b/src/main/java/frc4388/utility/AbhiIsADumbass.java @@ -1,43 +1,43 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -/** Exception that occurs if the limelight can't see enough points - * @author Abhi Sachi - * @author Aarav Shah - */ -public class AbhiIsADumbass extends Exception { - /** - * Creates new AbhiIsADumbass with error text 'null' - */ - public AbhiIsADumbass() { - super("Unable to see sufficient vision points (Abhi is a dumbass)"); - } - - /** Creates new AbhiIsADumbass with error text message - * - * @param message Error text message - */ - public AbhiIsADumbass(String message) { - super(message); - } - - /** Creates new AbhiIsADumbass with error text message and detailed stack trace - * - * @param message Error text message - * @param cause Root cause of error - */ - public AbhiIsADumbass(String message, Throwable cause) { - super(message, cause); - } - - /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace - * - * @param cause Root cause of error - */ - public AbhiIsADumbass(Throwable cause) { - super("Unable to see sufficient vision points (Abhi is a dumbass)", cause); - } -} +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +/** Exception that occurs if the limelight can't see enough points + * @author Abhi Sachi + * @author Aarav Shah + */ +public class AbhiIsADumbass extends Exception { + /** + * Creates new AbhiIsADumbass with error text 'null' + */ + public AbhiIsADumbass() { + super("Unable to see sufficient vision points (Abhi is a dumbass)"); + } + + /** Creates new AbhiIsADumbass with error text message + * + * @param message Error text message + */ + public AbhiIsADumbass(String message) { + super(message); + } + + /** Creates new AbhiIsADumbass with error text message and detailed stack trace + * + * @param message Error text message + * @param cause Root cause of error + */ + public AbhiIsADumbass(String message, Throwable cause) { + super(message, cause); + } + + /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace + * + * @param cause Root cause of error + */ + public AbhiIsADumbass(Throwable cause) { + super("Unable to see sufficient vision points (Abhi is a dumbass)", cause); + } +} diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java index c2ad269..8d269e3 100644 --- a/src/main/java/frc4388/utility/AprilTag.java +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -1,13 +1,13 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +} diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java index 20d3c57..1a8e397 100644 --- a/src/main/java/frc4388/utility/DeferredBlock.java +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -1,23 +1,23 @@ -package frc4388.utility; - -import java.util.ArrayList; - -public class DeferredBlock { - private static ArrayList m_blocks = new ArrayList<>(); - private static boolean m_hasRun = false; - - public DeferredBlock(Runnable block) { - m_blocks.add(block); - } - - public static void execute() { - if (m_hasRun) return; - - for (Runnable block : m_blocks) { - block.run(); - } - - m_blocks.clear(); // for garbage collection - m_hasRun = true; - } -} +package frc4388.utility; + +import java.util.ArrayList; + +public class DeferredBlock { + private static ArrayList m_blocks = new ArrayList<>(); + private static boolean m_hasRun = false; + + public DeferredBlock(Runnable block) { + m_blocks.add(block); + } + + public static void execute() { + if (m_hasRun) return; + + for (Runnable block : m_blocks) { + block.run(); + } + + m_blocks.clear(); // for garbage collection + m_hasRun = true; + } +} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 7a3a026..a1ef977 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -1,83 +1,83 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -/** Add your docs here. */ -public class Gains { - public double kP; - public double kI; - public double kD; - public double kF; - public int kIZone; - public double kPeakOutput; - public double kMaxOutput; - public double kMinOutput; - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kPeakOutput = kPeakOutput; - this.kMaxOutput = kPeakOutput; - this.kMinOutput = -kPeakOutput; - } - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kPeakOutput = 1.0; - this.kMaxOutput = 1.0; - this.kMinOutput = -1.0; - } - - public Gains(double kP, double kI, double kD) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - } - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. - * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kMaxOutput = kMaxOutput; - this.kMinOutput = kMinOutput; - this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); - } +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java index 6df032c..807de98 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/LEDPatterns.java @@ -1,45 +1,45 @@ -package frc4388.utility; - -/** - * Add your docs here. - */ -public enum LEDPatterns { - /* PALLETTE PATTERNS */ - RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), - RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), - PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), - PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), - RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f), - RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f), - RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f), - BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), - GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), - - /* COLOR 1 PATTERNS */ - C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), - C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), - - /* COLOR 2 PATTERNS */ - C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), - C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), - - /* COLOR 1 AND 2 PATTERNS */ - C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), - C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), - - /* SOLID COLORS */ - SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), - SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), - SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), - SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), - SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); - - /* GETTERS/SETTERS */ - private final float id; - LEDPatterns(float id) { - this.id = id; - } - public float getValue() { - return id; - } -} \ No newline at end of file +package frc4388.utility; + +/** + * Add your docs here. + */ +public enum LEDPatterns { + /* PALLETTE PATTERNS */ + RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), + RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), + PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), + PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), + RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f), + RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f), + RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f), + BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), + GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), + + /* COLOR 1 PATTERNS */ + C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), + C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), + + /* COLOR 2 PATTERNS */ + C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), + C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), + + /* COLOR 1 AND 2 PATTERNS */ + C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), + C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), + + /* SOLID COLORS */ + SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), + SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), + SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), + SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), + SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); + + /* GETTERS/SETTERS */ + private final float id; + LEDPatterns(float id) { + this.id = id; + } + public float getValue() { + return id; + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index a037914..4da3e13 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -1,200 +1,200 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.kauailabs.navx.frc.AHRS; - -// import edu.wpi.first.wpilibj.GyroBase; -import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.math.MathUtil; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro implements Gyro { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private WPI_Pigeon2 m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; //true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - private double pitchZero = 0; - private double rollZero = 0; - - /** - * Creates a Gyro based on a pigeon - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(WPI_Pigeon2 gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro){ - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Resets yaw, pitch, and roll. - */ - public void resetZeroValues() { - if (!m_isGyroAPigeon) return; - - pitchZero = m_pigeon.getPitch(); - rollZero = m_pigeon.getRoll(); - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

NavX: - *

Calibrate the gyro by running for a number of samples and computing the center value. Then use - * the center value as the Accumulator center value for subsequent measurements. It's important to - * make sure that the robot is not moving while the centering calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - * - *

Pigeon: - *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to - * make sure that the robot is not moving while the tempurature calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - */ - @Override - public void calibrate() { - if (m_isGyroAPigeon) { - m_pigeon.calibrate(); - } else { - m_navX.calibrate(); - } - } - - @Override - public void reset() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(0); - } else { - m_navX.reset(); - } - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - double[] ypr = new double[3]; - m_pigeon.getYawPitchRoll(ypr); - - return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; - } - - @Override - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; - } else { - return m_navX.getAngle(); - } - } - - public double getYaw() { - return this.getAngle(); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - @Override - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public WPI_Pigeon2 getPigeon(){ - return m_pigeon; - } - - public AHRS getNavX(){ - return m_navX; - } - - @Override - public void close() throws Exception { - - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.kauailabs.navx.frc.AHRS; + +// import edu.wpi.first.wpilibj.GyroBase; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.math.MathUtil; + +/** + * Gyro class that allows for interchangeable use between a pigeon and a navX + */ +public class RobotGyro implements Gyro { + private RobotTime m_robotTime = RobotTime.getInstance(); + + private WPI_Pigeon2 m_pigeon = null; + private AHRS m_navX = null; + public boolean m_isGyroAPigeon; //true if pigeon, false if navX + + private double m_lastPigeonAngle; + private double m_deltaPigeonAngle; + + private double pitchZero = 0; + private double rollZero = 0; + + /** + * Creates a Gyro based on a pigeon + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(WPI_Pigeon2 gyro) { + m_pigeon = gyro; + m_isGyroAPigeon = true; + } + + /** + * Creates a Gyro based on a navX + * @param gyro the gyroscope to use for Gyro + */ + public RobotGyro(AHRS gyro){ + m_navX = gyro; + m_isGyroAPigeon = false; + } + + /** + * Resets yaw, pitch, and roll. + */ + public void resetZeroValues() { + if (!m_isGyroAPigeon) return; + + pitchZero = m_pigeon.getPitch(); + rollZero = m_pigeon.getRoll(); + } + + /** + * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note + * that the getRate() method for a navX will likely be much more accurate than for a pigeon. + */ + public void updatePigeonDeltas() { + double currentPigeonAngle = getAngle(); + m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; + m_lastPigeonAngle = currentPigeonAngle; + } + + /** + *

NavX: + *

Calibrate the gyro by running for a number of samples and computing the center value. Then use + * the center value as the Accumulator center value for subsequent measurements. It's important to + * make sure that the robot is not moving while the centering calculations are in progress, this + * is typically done when the robot is first turned on while it's sitting at rest before the + * competition starts. + * + *

Pigeon: + *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot + * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon + * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to + * make sure that the robot is not moving while the tempurature calculations are in progress, this + * is typically done when the robot is first turned on while it's sitting at rest before the + * competition starts. + */ + @Override + public void calibrate() { + if (m_isGyroAPigeon) { + m_pigeon.calibrate(); + } else { + m_navX.calibrate(); + } + } + + @Override + public void reset() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(0); + } else { + m_navX.reset(); + } + } + + /** + * Get Yaw, Pitch, and Roll data. + * + * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. + * Yaw is within [-368,640, +368,640] degrees. + * Pitch is within [-90,+90] degrees. + * Roll is within [-90,+90] degrees. + */ + private double[] getPigeonAngles() { + double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); + + return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + } + + @Override + public double getAngle() { + if (m_isGyroAPigeon) { + return getPigeonAngles()[0]; + } else { + return m_navX.getAngle(); + } + } + + public double getYaw() { + return this.getAngle(); + } + + /** + * Gets an absolute heading of the robot + * @return heading from -180 to 180 degrees + */ + public double getHeading() { + return getHeading(getAngle()); + } + + /** + * Gets an absolute heading of the robot + * @return heading from -180 to 180 degrees + */ + public double getHeading(double angle) { + return Math.IEEEremainder(angle, 360); + } + + /** + * Returns the current pitch value (in degrees, from -90 to 90) + * reported by the sensor. Pitch is a measure of rotation around + * the Y Axis. + * @return The current pitch value in degrees (-90 to 90). + */ + public double getPitch() { + if (m_isGyroAPigeon) { + return MathUtil.clamp(getPigeonAngles()[1], -90, 90); + } else { + return MathUtil.clamp(m_navX.getPitch(), -90, 90); + } + } + + /** + * Returns the current roll value (in degrees, from -90 to 90) + * reported by the sensor. Roll is a measure of rotation around + * the X Axis. + * @return The current roll value in degrees (-90 to 90). + */ + public double getRoll() { + if (m_isGyroAPigeon) { + return MathUtil.clamp(getPigeonAngles()[2], -90, 90); + } else { + return MathUtil.clamp(m_navX.getRoll(), -90, 90); + } + } + + @Override + public double getRate() { + if (m_isGyroAPigeon) { + return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; + } else { + return m_navX.getRate(); + } + } + + public WPI_Pigeon2 getPigeon(){ + return m_pigeon; + } + + public AHRS getNavX(){ + return m_navX; + } + + @Override + public void close() throws Exception { + + } +} diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java index 694f850..c632bbb 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/RobotTime.java @@ -1,79 +1,79 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -/** - *

Keeps track of Robot times like time passed, delta time, etc - *

All times are in milliseconds - */ -public class RobotTime { - private long m_currTime = System.currentTimeMillis(); - public long m_deltaTime = 0; - - private long m_startRobotTime = m_currTime; - public long m_robotTime = 0; - public long m_lastRobotTime = 0; - - private long m_startMatchTime = 0; - public long m_matchTime = 0; - public long m_lastMatchTime = 0; - - public long m_frameNumber = 0; - - /** - * Private constructor prevents other classes from instantiating - */ - private RobotTime(){} - - private static RobotTime instance = null; - - /** - * Gets the instance of Robot Time. If there is no instance running one will be created. - * @return instance of Robot Time - */ - public static RobotTime getInstance() { - if (instance == null) { - instance = new RobotTime(); - } - return instance; - } - - /** - * Call this once per periodic loop. - */ - public void updateTimes() { - m_lastRobotTime = m_robotTime; - m_lastMatchTime = m_matchTime; - - m_currTime = System.currentTimeMillis(); - m_robotTime = m_currTime - m_startRobotTime; - m_deltaTime = m_robotTime - m_lastRobotTime; - m_frameNumber++; - - if (m_startMatchTime != 0) { - m_matchTime = m_currTime - m_startMatchTime; - } - } - - /** - * Call this in both the auto and periodic inits - */ - public void startMatchTime() { - if (m_startMatchTime == 0) { - m_startMatchTime = m_currTime; - } - } - - /** - * Call this in disabled init - */ - public void endMatchTime() { - m_startMatchTime = 0; - m_matchTime = 0; - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +/** + *

Keeps track of Robot times like time passed, delta time, etc + *

All times are in milliseconds + */ +public class RobotTime { + private long m_currTime = System.currentTimeMillis(); + public long m_deltaTime = 0; + + private long m_startRobotTime = m_currTime; + public long m_robotTime = 0; + public long m_lastRobotTime = 0; + + private long m_startMatchTime = 0; + public long m_matchTime = 0; + public long m_lastMatchTime = 0; + + public long m_frameNumber = 0; + + /** + * Private constructor prevents other classes from instantiating + */ + private RobotTime(){} + + private static RobotTime instance = null; + + /** + * Gets the instance of Robot Time. If there is no instance running one will be created. + * @return instance of Robot Time + */ + public static RobotTime getInstance() { + if (instance == null) { + instance = new RobotTime(); + } + return instance; + } + + /** + * Call this once per periodic loop. + */ + public void updateTimes() { + m_lastRobotTime = m_robotTime; + m_lastMatchTime = m_matchTime; + + m_currTime = System.currentTimeMillis(); + m_robotTime = m_currTime - m_startRobotTime; + m_deltaTime = m_robotTime - m_lastRobotTime; + m_frameNumber++; + + if (m_startMatchTime != 0) { + m_matchTime = m_currTime - m_startMatchTime; + } + } + + /** + * Call this in both the auto and periodic inits + */ + public void startMatchTime() { + if (m_startMatchTime == 0) { + m_startMatchTime = m_currTime; + } + } + + /** + * Call this in disabled init + */ + public void endMatchTime() { + m_startMatchTime = 0; + m_matchTime = 0; + } +} diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java index 9e07312..ab2eac5 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/RobotUnits.java @@ -1,27 +1,27 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -/** Aarav's good units class (better than WPILib) - * @author Aarav Shah */ - -public class RobotUnits { - // constants - - // angle conversions - public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} - - public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} - - // falcon conversions - public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} - - public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} - - // distance conversions - public static double metersToFeet(final double meters) {return meters * 3.28084;} - - public static double feetToMeters(final double feet) {return feet / 3.28084;} -} \ No newline at end of file +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +/** Aarav's good units class (better than WPILib) + * @author Aarav Shah */ + +public class RobotUnits { + // constants + + // angle conversions + public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} + + public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} + + // falcon conversions + public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} + + public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} + + // distance conversions + public static double metersToFeet(final double meters) {return meters * 3.28084;} + + public static double feetToMeters(final double feet) {return feet / 3.28084;} +} diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..40d54c3 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -1,12 +1,17 @@ -package frc4388.utility; - -public class UtilityStructs { - public static class TimedOutput { - public double leftX = 0.0; - public double leftY = 0.0; - public double rightX = 0.0; - public double rightY = 0.0; - - public long timedOffset = 0; - } -} +package frc4388.utility; + +public class UtilityStructs { + public static class TimedOutput { + public double driverLeftX = 0.0; + public double driverLeftY = 0.0; + public double driverRightX = 0.0; + public double driverRightY = 0.0; + + public double operatorLeftX = 0.0; + public double operatorLeftY = 0.0; + public double operatorRightX = 0.0; + public double operatorRightY = 0.0; + + public long timedOffset = 0; + } +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 4577a2e..a0a5017 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,27 +1,27 @@ -package frc4388.utility.controller; - -import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; - -public class DeadbandedXboxController extends XboxController { - public DeadbandedXboxController(int port) { super(port); } - - @Override public double getLeftX() { return getLeft().getX(); } - @Override public double getLeftY() { return getLeft().getY(); } - @Override public double getRightX() { return getRight().getX(); } - @Override public double getRightY() { return getRight().getY(); } - - public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } - public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } - - public static Translation2d skewToDeadzonedCircle(double x, double y) { - Translation2d translation2d = new Translation2d(x, y); - double magnitude = translation2d.getNorm(); - - if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); - - return translation2d; - } -} \ No newline at end of file +package frc4388.utility.controller; + +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; + +public class DeadbandedXboxController extends XboxController { + public DeadbandedXboxController(int port) { super(port); } + + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } + + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } + + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); + + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + + return translation2d; + } +} diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java index 13aa763..0a10c70 100644 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -1,21 +1,21 @@ -package frc4388.utility.controller; - -/** - * Add your docs here. - */ -public interface IHandController { - - public double getLeftXAxis(); - - public double getLeftYAxis(); - - public double getRightXAxis(); - - public double getRightYAxis(); - - public double getLeftTriggerAxis(); - - public double getRightTriggerAxis(); - - public int getDpadAngle(); -} +package frc4388.utility.controller; + +/** + * Add your docs here. + */ +public interface IHandController { + + public double getLeftXAxis(); + + public double getLeftYAxis(); + + public double getRightXAxis(); + + public double getRightYAxis(); + + public double getLeftTriggerAxis(); + + public double getRightTriggerAxis(); + + public int getDpadAngle(); +} diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 8b8f0f8..5af6017 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -1,218 +1,218 @@ -package frc4388.utility.controller; - -import edu.wpi.first.wpilibj.Joystick; - -/** - * This is a wrapper for the WPILib Joystick class that represents an XBox - * controller. - * @author frc1675 - */ -public class XboxController implements IHandController -{ - public static final int LEFT_X_AXIS = 0; - public static final int LEFT_Y_AXIS = 1; - public static final int LEFT_TRIGGER_AXIS = 2; - public static final int RIGHT_TRIGGER_AXIS = 3; - public static final int RIGHT_X_AXIS = 4; - public static final int RIGHT_Y_AXIS = 5; - public static final int LEFT_RIGHT_DPAD_AXIS = 6; - public static final int TOP_BOTTOM_DPAD_AXIS = 6; - - public static final int A_BUTTON = 1; - public static final int B_BUTTON = 2; - public static final int X_BUTTON = 3; - public static final int Y_BUTTON = 4; - public static final int LEFT_BUMPER_BUTTON = 5; - public static final int RIGHT_BUMPER_BUTTON = 6; - public static final int BACK_BUTTON = 7; - public static final int START_BUTTON = 8; - - public static final int LEFT_JOYSTICK_BUTTON = 9; - public static final int RIGHT_JOYSTICK_BUTTON = 10; - - private static final double LEFT_DPAD_TOLERANCE = -0.9; - private static final double RIGHT_DPAD_TOLERANCE = 0.9; - private static final double BOTTOM_DPAD_TOLERANCE = -0.9; - private static final double TOP_DPAD_TOLERANCE = 0.9; - - private static final double LEFT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; - - private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; - private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; - private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double DEADZONE = 0.1; - - private Joystick m_stick; - - /** - * Add your docs here. - */ - public XboxController(int portNumber){ - m_stick = new Joystick(portNumber); - } - - /** - * Add your docs here. - */ - public Joystick getJoyStick() { - return m_stick; - } - - /** - * Checks if the input falls within the deadzone. - * @param input from an axis on the controller - * @return true if input falls in deadzone, false if input falls outside deadzone - */ - private boolean inDeadZone(double input){ - return (Math.abs(input) < DEADZONE); - } - - /** - * Updates an input to have a deadzone around the 0 position - * @param input from an axis on the controller - * @return updated input - */ - private double getAxisWithDeadZoneCheck(double input){ - if(inDeadZone(input)){ - return 0.0; - } else { - return input; - } - } - - public boolean getAButton(){ - return m_stick.getRawButton(A_BUTTON); - } - - public boolean getXButton(){ - return m_stick.getRawButton(X_BUTTON); - } - - public boolean getBButton(){ - return m_stick.getRawButton(B_BUTTON); - } - - public boolean getYButton(){ - return m_stick.getRawButton(Y_BUTTON); - } - - public boolean getBackButton(){ - return m_stick.getRawButton(BACK_BUTTON); - } - - public boolean getStartButton(){ - return m_stick.getRawButton(START_BUTTON); - } - - public boolean getLeftBumperButton(){ - return m_stick.getRawButton(LEFT_BUMPER_BUTTON); - } - - public boolean getRightBumperButton(){ - return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); - } - - public boolean getLeftJoystickButton(){ - return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } - - public boolean getRightJoystickButton(){ - return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); - } - - public double getLeftXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); - } - - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); - } - - public double getRightXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); - } - - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); - } - - public double getLeftTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); - } - - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); - } - - /** - * Get the angle input from the dpad. - * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. - */ - public int getDpadAngle() { - return m_stick.getPOV(0); - } - - public boolean getDPadLeft(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } - - public boolean getDPadRight(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } - - public boolean getDPadTop(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } - - public boolean getDPadBottom(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } - - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } - - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } - - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } - - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } - - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } - - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } - - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } - - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } - - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } - - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } -} \ No newline at end of file +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.Joystick; + +/** + * This is a wrapper for the WPILib Joystick class that represents an XBox + * controller. + * @author frc1675 + */ +public class XboxController implements IHandController +{ + public static final int LEFT_X_AXIS = 0; + public static final int LEFT_Y_AXIS = 1; + public static final int LEFT_TRIGGER_AXIS = 2; + public static final int RIGHT_TRIGGER_AXIS = 3; + public static final int RIGHT_X_AXIS = 4; + public static final int RIGHT_Y_AXIS = 5; + public static final int LEFT_RIGHT_DPAD_AXIS = 6; + public static final int TOP_BOTTOM_DPAD_AXIS = 6; + + public static final int A_BUTTON = 1; + public static final int B_BUTTON = 2; + public static final int X_BUTTON = 3; + public static final int Y_BUTTON = 4; + public static final int LEFT_BUMPER_BUTTON = 5; + public static final int RIGHT_BUMPER_BUTTON = 6; + public static final int BACK_BUTTON = 7; + public static final int START_BUTTON = 8; + + public static final int LEFT_JOYSTICK_BUTTON = 9; + public static final int RIGHT_JOYSTICK_BUTTON = 10; + + private static final double LEFT_DPAD_TOLERANCE = -0.9; + private static final double RIGHT_DPAD_TOLERANCE = 0.9; + private static final double BOTTOM_DPAD_TOLERANCE = -0.9; + private static final double TOP_DPAD_TOLERANCE = 0.9; + + private static final double LEFT_TRIGGER_TOLERANCE = 0.5; + private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; + + private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; + private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; + private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double DEADZONE = 0.1; + + private Joystick m_stick; + + /** + * Add your docs here. + */ + public XboxController(int portNumber){ + m_stick = new Joystick(portNumber); + } + + /** + * Add your docs here. + */ + public Joystick getJoyStick() { + return m_stick; + } + + /** + * Checks if the input falls within the deadzone. + * @param input from an axis on the controller + * @return true if input falls in deadzone, false if input falls outside deadzone + */ + private boolean inDeadZone(double input){ + return (Math.abs(input) < DEADZONE); + } + + /** + * Updates an input to have a deadzone around the 0 position + * @param input from an axis on the controller + * @return updated input + */ + private double getAxisWithDeadZoneCheck(double input){ + if(inDeadZone(input)){ + return 0.0; + } else { + return input; + } + } + + public boolean getAButton(){ + return m_stick.getRawButton(A_BUTTON); + } + + public boolean getXButton(){ + return m_stick.getRawButton(X_BUTTON); + } + + public boolean getBButton(){ + return m_stick.getRawButton(B_BUTTON); + } + + public boolean getYButton(){ + return m_stick.getRawButton(Y_BUTTON); + } + + public boolean getBackButton(){ + return m_stick.getRawButton(BACK_BUTTON); + } + + public boolean getStartButton(){ + return m_stick.getRawButton(START_BUTTON); + } + + public boolean getLeftBumperButton(){ + return m_stick.getRawButton(LEFT_BUMPER_BUTTON); + } + + public boolean getRightBumperButton(){ + return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); + } + + public boolean getLeftJoystickButton(){ + return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); + } + + public boolean getRightJoystickButton(){ + return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); + } + + public double getLeftXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); + } + + public double getLeftYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); + } + + public double getRightXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); + } + + public double getRightYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); + } + + public double getLeftTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); + } + + public double getRightTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); + } + + /** + * Get the angle input from the dpad. + * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. + */ + public int getDpadAngle() { + return m_stick.getPOV(0); + } + + public boolean getDPadLeft(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); + } + + public boolean getDPadRight(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); + } + + public boolean getDPadTop(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); + } + + public boolean getDPadBottom(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); + } + + public boolean getLeftTrigger(){ + return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); + } + + public boolean getRightTrigger(){ + return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); + } + + public boolean getRightAxisUpTrigger(){ + return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); + } + + public boolean getRightAxisDownTrigger(){ + return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); + } + + public boolean getRightAxisLeftTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); + } + + public boolean getRightAxisRightTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); + } + + public boolean getLeftAxisUpTrigger(){ + return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); + } + + public boolean getLeftAxisDownTrigger(){ + return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); + } + + public boolean getLeftAxisLeftTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); + } + + public boolean getLeftAxisRightTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); + } +} diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 8c2fe88..e13408b 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -1,68 +1,68 @@ -package frc4388.utility.controller; - -import edu.wpi.first.wpilibj2.command.button.Button; - -/** - * Mapping for the Xbox controller triggers to allow triggers to be defined as - * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger - * exceeds a tolerance defined in {@link XboxController}. - */ -public class XboxTriggerButton extends Button { - public static final int RIGHT_TRIGGER = 0; - public static final int LEFT_TRIGGER = 1; - public static final int RIGHT_AXIS_UP_TRIGGER = 2; - public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; - public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; - public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; - public static final int LEFT_AXIS_UP_TRIGGER = 6; - public static final int LEFT_AXIS_DOWN_TRIGGER = 7; - public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; - public static final int LEFT_AXIS_LEFT_TRIGGER = 9; - - private XboxController m_controller; - private int m_trigger; - - /** - * Creates a Trigger-Button mapped to a specific Xbox controller and trigger - */ - public XboxTriggerButton(XboxController controller, int trigger) { - m_controller = controller; - m_trigger = trigger; - } - - /** {@inheritDoc} */ - // @Override - public boolean get() { - if (m_trigger == RIGHT_TRIGGER) { - return m_controller.getRightTrigger(); - } - else if (m_trigger == LEFT_TRIGGER) { - return m_controller.getLeftTrigger(); - } - else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { - return m_controller.getRightAxisUpTrigger(); - } - else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { - return m_controller.getRightAxisDownTrigger(); - } - else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { - return m_controller.getRightAxisRightTrigger(); - } - else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { - return m_controller.getRightAxisLeftTrigger(); - } - else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { - return m_controller.getLeftAxisUpTrigger(); - } - else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { - return m_controller.getLeftAxisDownTrigger(); - } - else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { - return m_controller.getLeftAxisRightTrigger(); - } - else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { - return m_controller.getLeftAxisLeftTrigger(); - } - return false; - } -} +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj2.command.button.Button; + +/** + * Mapping for the Xbox controller triggers to allow triggers to be defined as + * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger + * exceeds a tolerance defined in {@link XboxController}. + */ +public class XboxTriggerButton extends Button { + public static final int RIGHT_TRIGGER = 0; + public static final int LEFT_TRIGGER = 1; + public static final int RIGHT_AXIS_UP_TRIGGER = 2; + public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; + public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; + public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; + public static final int LEFT_AXIS_UP_TRIGGER = 6; + public static final int LEFT_AXIS_DOWN_TRIGGER = 7; + public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; + public static final int LEFT_AXIS_LEFT_TRIGGER = 9; + + private XboxController m_controller; + private int m_trigger; + + /** + * Creates a Trigger-Button mapped to a specific Xbox controller and trigger + */ + public XboxTriggerButton(XboxController controller, int trigger) { + m_controller = controller; + m_trigger = trigger; + } + + /** {@inheritDoc} */ + // @Override + public boolean get() { + if (m_trigger == RIGHT_TRIGGER) { + return m_controller.getRightTrigger(); + } + else if (m_trigger == LEFT_TRIGGER) { + return m_controller.getLeftTrigger(); + } + else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { + return m_controller.getRightAxisUpTrigger(); + } + else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { + return m_controller.getRightAxisDownTrigger(); + } + else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { + return m_controller.getRightAxisRightTrigger(); + } + else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { + return m_controller.getRightAxisLeftTrigger(); + } + else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { + return m_controller.getLeftAxisUpTrigger(); + } + else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { + return m_controller.getLeftAxisDownTrigger(); + } + else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { + return m_controller.getLeftAxisRightTrigger(); + } + else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { + return m_controller.getLeftAxisLeftTrigger(); + } + return false; + } +} diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java index ecddde7..ac7583e 100644 --- a/src/test/java/frc4388/mocks/MockPigeonIMU.java +++ b/src/test/java/frc4388/mocks/MockPigeonIMU.java @@ -1,54 +1,54 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.mocks; - -import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.sensors.PigeonIMU; - -/** - * Add your docs here. - */ -public class MockPigeonIMU extends PigeonIMU { - public int m_deviceNumber; - public double currentYaw; - public double currentPitch; - public double currentRoll; - - public MockPigeonIMU(int deviceNumber) { - super(deviceNumber); - m_deviceNumber = deviceNumber; - } - - @Override - public ErrorCode setYaw(double angleDeg) { - currentYaw = angleDeg; - return ErrorCode.OK; - } - - /** - * @param currentPitch the Pitch to set - */ - public void setCurrentPitch(double currentPitch) { - this.currentPitch = currentPitch; - } - - /** - * @param currentRoll the Roll to set - */ - public void setCurrentRoll(double currentRoll) { - this.currentRoll = currentRoll; - } - - @Override - public ErrorCode getYawPitchRoll(double[] ypr_deg) { - ypr_deg[0] = currentYaw; - ypr_deg[1] = currentPitch; - ypr_deg[2] = currentRoll; - return ErrorCode.OK; - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.mocks; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.sensors.PigeonIMU; + +/** + * Add your docs here. + */ +public class MockPigeonIMU extends PigeonIMU { + public int m_deviceNumber; + public double currentYaw; + public double currentPitch; + public double currentRoll; + + public MockPigeonIMU(int deviceNumber) { + super(deviceNumber); + m_deviceNumber = deviceNumber; + } + + @Override + public ErrorCode setYaw(double angleDeg) { + currentYaw = angleDeg; + return ErrorCode.OK; + } + + /** + * @param currentPitch the Pitch to set + */ + public void setCurrentPitch(double currentPitch) { + this.currentPitch = currentPitch; + } + + /** + * @param currentRoll the Roll to set + */ + public void setCurrentRoll(double currentRoll) { + this.currentRoll = currentRoll; + } + + @Override + public ErrorCode getYawPitchRoll(double[] ypr_deg) { + ypr_deg[0] = currentYaw; + ypr_deg[1] = currentPitch; + ypr_deg[2] = currentRoll; + return ErrorCode.OK; + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old index 9b2dc8e..4d97824 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old @@ -1,61 +1,61 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertEquals; - -// import static org.junit.Assert.assertEquals; -// import static org.mockito.Mockito.mock; -// import org.junit.Test; - -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import frc4388.robot.Constants.LEDConstants; -import frc4388.utility.LEDPatterns; - -/** - * Add your docs here. - */ -public class LEDSubsystemTest { - @Test - public void testConstructor() { - // Arrange - Spark ledController = new Spark(0); - - // Act - LED led = new LED(ledController); - - // Assert - assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); - } - - @Test - public void testPatterns() { - // Arrange - Spark ledController = new Spark(0); - LED led = new LED(ledController); - - // Act - led.setPattern(LEDPatterns.RAINBOW_RAINBOW); - - // Assert - assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); - - // Act - led.setPattern(LEDPatterns.BLUE_BREATH); - - // Assert - assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); - - // Act - led.setPattern(LEDPatterns.SOLID_BLACK); - - // Assert - assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.assertEquals; + +// import static org.junit.Assert.assertEquals; +// import static org.mockito.Mockito.mock; +// import org.junit.Test; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import frc4388.robot.Constants.LEDConstants; +import frc4388.utility.LEDPatterns; + +/** + * Add your docs here. + */ +public class LEDSubsystemTest { + @Test + public void testConstructor() { + // Arrange + Spark ledController = new Spark(0); + + // Act + LED led = new LED(ledController); + + // Assert + assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); + } + + @Test + public void testPatterns() { + // Arrange + Spark ledController = new Spark(0); + LED led = new LED(ledController); + + // Act + led.setPattern(LEDPatterns.RAINBOW_RAINBOW); + + // Assert + assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); + + // Act + led.setPattern(LEDPatterns.BLUE_BREATH); + + // Assert + assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); + + // Act + led.setPattern(LEDPatterns.SOLID_BLACK); + + // Assert + assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); + } +} diff --git a/src/test/java/frc4388/utility/Delay.java b/src/test/java/frc4388/utility/Delay.java new file mode 100644 index 0000000..492ab06 --- /dev/null +++ b/src/test/java/frc4388/utility/Delay.java @@ -0,0 +1,8 @@ +// package frc4388.utility; + +// public class Delay { +// void delaymillis(long millis) { +// delay(millis) + +// } +// } diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.old b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old index 1dca123..80978b2 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.old +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old @@ -1,185 +1,185 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertEquals; - -// import static org.junit.Assert.*; -// import static org.mockito.Mockito.*; - -import com.kauailabs.navx.frc.AHRS; - -import frc4388.mocks.MockPigeonIMU; -import frc4388.robot.Constants.DriveConstants; - -/** - * Add your docs here. - */ -public class RobotGyroUtilityTest { - // TODO UNTESTED: most functions for NavX - - private RobotGyro gyroPigeon; - private RobotGyro gyroNavX; - - @Test - public void testConstructor() { - // Arrange - MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); - AHRS navX = new AHRS(); - gyroPigeon = new RobotGyro(pigeon); - gyroNavX = new RobotGyro(navX); - - // Assert - assertEquals(true, gyroPigeon.m_isGyroAPigeon); - assertEquals(pigeon, gyroPigeon.getPigeon()); - assertEquals(null, gyroPigeon.getNavX()); - assertEquals(false, gyroNavX.m_isGyroAPigeon); - assertEquals(navX, gyroNavX.getNavX()); - assertEquals(null, gyroNavX.getPigeon()); - } - - @Test - public void testHeadingPigeon() { - // Arrange - MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); - gyroPigeon = new RobotGyro(pigeon); - - // Act & Assert - assertEquals(-90, gyroPigeon.getHeading(270), 0.0001); - assertEquals(-45, gyroPigeon.getHeading(315), 0.0001); - assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001); - assertEquals(30, gyroPigeon.getHeading(30), 0.0001); - assertEquals(0, gyroPigeon.getHeading(0), 0.0001); - assertEquals(180, gyroPigeon.getHeading(180), 0.0001); - assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001); - assertEquals(97, gyroPigeon.getHeading(1537), 0.0001); - assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001); - } - - @Test - public void testYawPitchRollPigeon() { - // Arrange - MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); - gyroPigeon = new RobotGyro(pigeon); - - // Assert - assertEquals(0, gyroPigeon.getAngle(), 0.0001); - - // Act - pigeon.setYaw(40); - - // Assert - assertEquals(40, gyroPigeon.getAngle(), 0.0001); - - // Act - gyroPigeon.reset(); - - // Assert - assertEquals(0, gyroPigeon.getAngle(), 0.0001); - - // Act - pigeon.setYaw(-1457); - pigeon.setCurrentPitch(100); - pigeon.setCurrentRoll(100); - - // Assert - assertEquals(-1457, gyroPigeon.getAngle(), 0.0001); - assertEquals(90, gyroPigeon.getPitch(), 0.0001); - assertEquals(90, gyroPigeon.getRoll(), 0.0001); - - // Act - pigeon.setCurrentPitch(45); - pigeon.setCurrentRoll(45); - - // Assert - assertEquals(45, gyroPigeon.getPitch(), 0.0001); - assertEquals(45, gyroPigeon.getRoll(), 0.0001); - - // Act - pigeon.setCurrentPitch(0); - pigeon.setCurrentRoll(0); - - // Assert - assertEquals(0, gyroPigeon.getPitch(), 0.0001); - assertEquals(0, gyroPigeon.getRoll(), 0.0001); - - // Act - pigeon.setCurrentPitch(-60); - pigeon.setCurrentRoll(-60); - - // Assert - assertEquals(-60, gyroPigeon.getPitch(), 0.0001); - assertEquals(-60, gyroPigeon.getRoll(), 0.0001); - - // Act - pigeon.setCurrentPitch(-90); - pigeon.setCurrentRoll(-90); - - // Assert - assertEquals(-90, gyroPigeon.getPitch(), 0.0001); - assertEquals(-90, gyroPigeon.getRoll(), 0.0001); - - // Act - pigeon.setCurrentPitch(-100); - pigeon.setCurrentRoll(-100); - - // Assert - assertEquals(-90, gyroPigeon.getPitch(), 0.0001); - assertEquals(-90, gyroPigeon.getRoll(), 0.0001); - } - - @Test - public void testRatesPigeon() { - // Arrange - MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); - gyroPigeon = new RobotGyro(pigeon); - RobotTime robotTime = RobotTime.getInstance(); - gyroPigeon.updatePigeonDeltas(); - - // Act - robotTime.m_deltaTime = 5; - pigeon.setYaw(0); - gyroPigeon.updatePigeonDeltas(); - - // Assert - assertEquals(0, gyroPigeon.getRate(), 1); - - // Act - robotTime.m_deltaTime = 5; - pigeon.setYaw(90); - gyroPigeon.updatePigeonDeltas(); - - // Assert - assertEquals(18000, gyroPigeon.getRate(), 0.001); - - // Act - robotTime.m_deltaTime = 5; - pigeon.setYaw(90); - gyroPigeon.updatePigeonDeltas(); - - // Assert - assertEquals(0, gyroPigeon.getRate(), 0.001); - - // Act - robotTime.m_deltaTime = 3; - pigeon.setYaw(-30); - gyroPigeon.updatePigeonDeltas(); - - // Assert - assertEquals(-40000, gyroPigeon.getRate(), 0.001); - - // Act - robotTime.m_deltaTime = 6; - pigeon.setYaw(690); - gyroPigeon.updatePigeonDeltas(); - - // Assert - assertEquals(120000, gyroPigeon.getRate(), 0.001); - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.assertEquals; + +// import static org.junit.Assert.*; +// import static org.mockito.Mockito.*; + +import com.kauailabs.navx.frc.AHRS; + +import frc4388.mocks.MockPigeonIMU; +import frc4388.robot.Constants.DriveConstants; + +/** + * Add your docs here. + */ +public class RobotGyroUtilityTest { + // TODO UNTESTED: most functions for NavX + + private RobotGyro gyroPigeon; + private RobotGyro gyroNavX; + + @Test + public void testConstructor() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + AHRS navX = new AHRS(); + gyroPigeon = new RobotGyro(pigeon); + gyroNavX = new RobotGyro(navX); + + // Assert + assertEquals(true, gyroPigeon.m_isGyroAPigeon); + assertEquals(pigeon, gyroPigeon.getPigeon()); + assertEquals(null, gyroPigeon.getNavX()); + assertEquals(false, gyroNavX.m_isGyroAPigeon); + assertEquals(navX, gyroNavX.getNavX()); + assertEquals(null, gyroNavX.getPigeon()); + } + + @Test + public void testHeadingPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + + // Act & Assert + assertEquals(-90, gyroPigeon.getHeading(270), 0.0001); + assertEquals(-45, gyroPigeon.getHeading(315), 0.0001); + assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001); + assertEquals(30, gyroPigeon.getHeading(30), 0.0001); + assertEquals(0, gyroPigeon.getHeading(0), 0.0001); + assertEquals(180, gyroPigeon.getHeading(180), 0.0001); + assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001); + assertEquals(97, gyroPigeon.getHeading(1537), 0.0001); + assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001); + } + + @Test + public void testYawPitchRollPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + + // Assert + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + // Act + pigeon.setYaw(40); + + // Assert + assertEquals(40, gyroPigeon.getAngle(), 0.0001); + + // Act + gyroPigeon.reset(); + + // Assert + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + // Act + pigeon.setYaw(-1457); + pigeon.setCurrentPitch(100); + pigeon.setCurrentRoll(100); + + // Assert + assertEquals(-1457, gyroPigeon.getAngle(), 0.0001); + assertEquals(90, gyroPigeon.getPitch(), 0.0001); + assertEquals(90, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(45); + pigeon.setCurrentRoll(45); + + // Assert + assertEquals(45, gyroPigeon.getPitch(), 0.0001); + assertEquals(45, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(0); + pigeon.setCurrentRoll(0); + + // Assert + assertEquals(0, gyroPigeon.getPitch(), 0.0001); + assertEquals(0, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-60); + pigeon.setCurrentRoll(-60); + + // Assert + assertEquals(-60, gyroPigeon.getPitch(), 0.0001); + assertEquals(-60, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-90); + pigeon.setCurrentRoll(-90); + + // Assert + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + + // Act + pigeon.setCurrentPitch(-100); + pigeon.setCurrentRoll(-100); + + // Assert + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + } + + @Test + public void testRatesPigeon() { + // Arrange + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + gyroPigeon = new RobotGyro(pigeon); + RobotTime robotTime = RobotTime.getInstance(); + gyroPigeon.updatePigeonDeltas(); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(0); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(0, gyroPigeon.getRate(), 1); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(90); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(18000, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 5; + pigeon.setYaw(90); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(0, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 3; + pigeon.setYaw(-30); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(-40000, gyroPigeon.getRate(), 0.001); + + // Act + robotTime.m_deltaTime = 6; + pigeon.setYaw(690); + gyroPigeon.updatePigeonDeltas(); + + // Assert + assertEquals(120000, gyroPigeon.getRate(), 0.001); + } +} diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.old b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old index c0d4551..ba5fb7a 100644 --- a/src/test/java/frc4388/utility/RobotTimeUtilityTest.old +++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old @@ -1,105 +1,105 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertEquals; - -// import static org.junit.Assert.*; - -/** - * Add your docs here. - */ -public class RobotTimeUtilityTest { - RobotTime robotTime = RobotTime.getInstance(); - - @Test - public void testUpdateTimes() { - // Arrange - long lastTime; - robotTime.m_deltaTime = 0; - robotTime.m_robotTime = 0; - robotTime.m_lastRobotTime = 0; - robotTime.m_frameNumber = 0; - robotTime.endMatchTime(); - robotTime.m_lastMatchTime = 0; - - // Assert - assertEquals(0, robotTime.m_deltaTime); - assertEquals(0, robotTime.m_robotTime); - assertEquals(0, robotTime.m_lastRobotTime); - assertEquals(0, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(1, robotTime.m_frameNumber); - lastTime = robotTime.m_robotTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_deltaTime > 0); - assertEquals(true, robotTime.m_robotTime > 0); - assertEquals(lastTime, robotTime.m_lastRobotTime); - assertEquals(2, robotTime.m_frameNumber); - } - - @Test - public void testMatchTime() { - // Arrange - long lastTime; - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(0, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - robotTime.startMatchTime(); - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(true, robotTime.m_matchTime > 0); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - wait(1); - robotTime.updateTimes(); - robotTime.endMatchTime(); - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - lastTime = robotTime.m_matchTime; - - // Act - wait(1); - robotTime.updateTimes(); - - // Assert - assertEquals(0, robotTime.m_matchTime); - assertEquals(lastTime, robotTime.m_lastMatchTime); - } - - private void wait(int millis) { - try { - Thread.sleep(millis); - } catch (Exception e) {} - } -} +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.assertEquals; + +// import static org.junit.Assert.*; + +/** + * Add your docs here. + */ +public class RobotTimeUtilityTest { + RobotTime robotTime = RobotTime.getInstance(); + + @Test + public void testUpdateTimes() { + // Arrange + long lastTime; + robotTime.m_deltaTime = 0; + robotTime.m_robotTime = 0; + robotTime.m_lastRobotTime = 0; + robotTime.m_frameNumber = 0; + robotTime.endMatchTime(); + robotTime.m_lastMatchTime = 0; + + // Assert + assertEquals(0, robotTime.m_deltaTime); + assertEquals(0, robotTime.m_robotTime); + assertEquals(0, robotTime.m_lastRobotTime); + assertEquals(0, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(1, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_deltaTime > 0); + assertEquals(true, robotTime.m_robotTime > 0); + assertEquals(lastTime, robotTime.m_lastRobotTime); + assertEquals(2, robotTime.m_frameNumber); + } + + @Test + public void testMatchTime() { + // Arrange + long lastTime; + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(0, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + robotTime.startMatchTime(); + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(true, robotTime.m_matchTime > 0); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + wait(1); + robotTime.updateTimes(); + robotTime.endMatchTime(); + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + // Act + wait(1); + robotTime.updateTimes(); + + // Assert + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + } + + private void wait(int millis) { + try { + Thread.sleep(millis); + } catch (Exception e) {} + } +} diff --git a/todo.txt b/todo.txt index 2224b57..c645264 100644 --- a/todo.txt +++ b/todo.txt @@ -1,8 +1,8 @@ -Revised Autosystem: - - Autoplayback - - Fix Arm Respool - - Via Commands - - Arm - - Sequential Command System - - Manual Auto Creation - - Shuffleboard IO \ No newline at end of file +Revised Autosystem: + - Autoplayback + \ Fix Arm Respool + x Via Commands + - Arm + - Sequential Command System + - Manual Auto Creation + - Shuffleboard IO diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 4bd4424..76b56f5 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,423 +1,423 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.2", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.2" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.2" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.1", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.30.2", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] } \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index f2d0b7d..41d55ec 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,73 +1,73 @@ -{ - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2023.1.3", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2023.1.3" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2023.1.3", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2023.1.3", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2023.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2023.1.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2023.1.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 65dcc03..da4bc52 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,37 +1,37 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index 1718692..d628017 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,35 +1,35 @@ -{ - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "4.0.447", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "4.0.447" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "4.0.447", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "windowsx86-64" - ] - } - ] +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "4.0.447", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "4.0.447" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "4.0.447", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index dad3105..f4d4ae6 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,41 +1,41 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2023.4.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2023.4.2", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2023.4.2" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2023.4.2" - } - ] +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] } \ No newline at end of file