diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index dd8b63d..3b45ce5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -8,8 +8,6 @@ package frc4388.robot; import java.util.ArrayList; -import java.util.ConcurrentModificationException; -import java.util.Vector; import java.util.function.Consumer; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; @@ -29,260 +27,261 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public enum Mode { - COMPETITIVE, CASUAL; + public enum Mode { + COMPETITIVE, CASUAL; - private static Mode mode = Mode.COMPETITIVE; - private static Vector> changeHandlers = new Vector<>(); + private static Mode mode = Mode.COMPETITIVE; + private static ArrayList> changeHandlers = new ArrayList<>(); - public static void register(Consumer changeHandler) { - changeHandlers.add(changeHandler); - } - - public static Mode get() { - return mode; - } - - public static void set(Mode mode) { - System.out.println(mode); - int i = mode.ordinal(); - Mode.mode = mode; - CommandScheduler.getInstance().disable(); - changeHandlers.forEach(c -> c.accept(mode)); - CommandScheduler.getInstance().enable(); - DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR = DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR_MODES[i]; - IntakeConstants.INTAKE_SPEED = IntakeConstants.INTAKE_SPEED_MODES[i]; - StorageConstants.STORAGE_SPEED = StorageConstants.STORAGE_SPEED_MODES[i]; - } - public static void toggle() { - int i = mode.ordinal() + 1; - Mode[] values = values(); - i = i >= values.length ? 0 : i; - set(values[i]); - } + public static void register(Consumer changeHandler) { + changeHandlers.add(changeHandler); } - public static final int SELECTED_AUTO = 0; - - public static final class DriveConstants { - /* Drive Train IDs */ - public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; - public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; - public static final int DRIVE_LEFT_BACK_CAN_ID = 3; - public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; - public static final int PIGEON_ID = 6; - - /* Drive Inversions */ - public static final boolean isRightMotorInverted = true; - public static final boolean isLeftMotorInverted = false; - public static final boolean isRightArcadeInverted = false; - public static final boolean isAuxPIDInverted = false; - - /* Drive Configuration */ - public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Seconds from 0 to full power on motor - public static final double NEUTRAL_DEADBAND = 0.04; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); - public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops - public static final double COS_MULTIPLIER_LOW = 1.0; - public static final double COS_MULTIPLIER_HIGH = 0.8; - - /* Drive Train Characteristics */ - public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 7.29; - public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; - public static final double WHEEL_DIAMETER_INCHES = 6; - public static final double TICKS_PER_GYRO_REV = 8192; - public static final double TICKS_PER_MOTOR_REV = 2048; - - /* PID Constants Drive*/ - public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 30000; - public static final int DRIVE_ACCELERATION = 23000; - - private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.8 }; - public static double DRIVE_WITH_JOYSTICK_FACTOR; - - public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; - public static final int DRIVE_ACCELERATION_HIGH = 7000; - - public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); - - /* Trajectory Constants */ - public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; - public static final double TRACK_WIDTH_METERS = 0.648; - public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); - - /* Remote Sensors */ - public static final int REMOTE_0 = 0; - public static final int REMOTE_1 = 1; - - /* PID Indexes */ - public static final int PID_PRIMARY = 0; - public static final int PID_TURN = 1; - - /* PID SLOTS */ - public static final int SLOT_DISTANCE = 0; - public static final int SLOT_VELOCITY = 1; - public static final int SLOT_TURNING = 2; - public static final int SLOT_MOTION_MAGIC = 3; - - /* Ratio Calculation */ - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - public static final double TICK_TIME_TO_SECONDS = 0.1; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - public static final double INCHES_PER_METER = 39.370; - public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; - - public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1 / MOTOR_ROT_PER_WHEEL_ROT_HIGH; - public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH; - public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK_HIGH = 1 / TICKS_PER_INCH_HIGH; - - public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1 / MOTOR_ROT_PER_WHEEL_ROT_LOW; - public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; - public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK_LOW = 1 / TICKS_PER_INCH_LOW; + public static Mode get() { + return mode; } - public static final class ShooterConstants { - /* Motor IDs */ - public static final int SHOOTER_FALCON_BALLER_ID = 8; - public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15; - public static final int SHOOTER_ANGLE_ADJUST_ID = 10; - public static final int SHOOTER_ROTATE_ID = 9; - - /* PID Constants Shooter */ - public static final int SHOOTER_SLOT_IDX = 0; - public static final int SHOOTER_PID_LOOP_IDX = 1; - public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); // Ff was 0.055 - // public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); - public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double ENCODER_TICKS_PER_REV = 2048; - public static final double NEO_UNITS_PER_REV = 42; - public static final double DEGREES_PER_ROT = 360; - - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); - - public static final int TURRET_RIGHT_SOFT_LIMIT = -2; - public static final int TURRET_LEFT_SOFT_LIMIT = -55; - public static final double TURRET_SPEED_MULTIPLIER = 0.3; - public static final double TURRET_CALIBRATE_SPEED = 0.075; - public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; // 89.56696; - public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166; - - public static final int HOOD_UP_SOFT_LIMIT = 33; - public static final int HOOD_DOWN_SOFT_LIMIT = 3; - public static final double HOOD_CONVERT_SLOPE = 0.47; - public static final double HOOD_CONVERT_B = 40.5; - public static final double HOOD_CALIBRATE_SPEED = 0.2; - public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find - public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - - public static final double DRUM_RAMP_LIMIT = 1000; - public static final double DRUM_VELOCITY_BOUND = 300; + public static void set(Mode mode) { + System.out.println(mode); + int i = mode.ordinal(); + Mode.mode = mode; + CommandScheduler.getInstance().disable(); + changeHandlers.forEach(c -> c.accept(mode)); + CommandScheduler.getInstance().enable(); + DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR = DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR_MODES[i]; + IntakeConstants.INTAKE_SPEED = IntakeConstants.INTAKE_SPEED_MODES[i]; + StorageConstants.STORAGE_SPEED = StorageConstants.STORAGE_SPEED_MODES[i]; } - public static final class ClimberConstants { - public static final int CLIMBER_SPARK_ID = 14; + public static void toggle() { + int i = mode.ordinal() + 1; + Mode[] values = values(); + i = i >= values.length ? 0 : i; + set(values[i]); } + } - public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 30; - } + public static final int SELECTED_AUTO = 0; - public static final class IntakeConstants { - public static final double EXTENDER_SPEED = 0.3; - private static final double[] INTAKE_SPEED_MODES = { 1.0, 0.5 }; - public static double INTAKE_SPEED; + public static final class DriveConstants { + /* Drive Train IDs */ + public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; + public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; + public static final int DRIVE_LEFT_BACK_CAN_ID = 3; + public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; + public static final int PIGEON_ID = 6; - public static final int INTAKE_SPARK_ID = 12; - public static final int EXTENDER_SPARK_ID = 13; - } + /* Drive Inversions */ + public static final boolean isRightMotorInverted = true; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; - public static final class StorageConstants { - public static final int STORAGE_CAN_ID = 11; - public static final double STORAGE_PARTIAL_BALL = 2; - public static final double STORAGE_FULL_BALL = 7; - private static final double[] STORAGE_SPEED_MODES = { 0.75, 0.50 }; - public static double STORAGE_SPEED; - public static final double STORAGE_TIMEOUT = 3000; + /* Drive Configuration */ + public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Seconds from 0 to full power on motor + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); + public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + public static final double COS_MULTIPLIER_LOW = 1.0; + public static final double COS_MULTIPLIER_HIGH = 0.8; - /* Storage Characteristics */ - public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; // For the first storage belt - public static final double INCHES_PER_STORAGE_ROT = 1; // Circumference of the first storage belt + /* Drive Train Characteristics */ + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 7.29; + public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; + public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; + public static final double TICKS_PER_MOTOR_REV = 2048; - /* Ball Indexes */ - public static final int BEAM_SENSOR_SHOOTER = 11; - public static final int BEAM_SENSOR_USELESS = 12; - public static final int BEAM_SENSOR_STORAGE = 13; - public static final int BEAM_SENSOR_INTAKE = 14; + /* PID Constants Drive*/ + public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 30000; + public static final int DRIVE_ACCELERATION = 23000; - /* PID Gains */ - public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.8 }; + public static double DRIVE_WITH_JOYSTICK_FACTOR; - /* PID Values */ - public static final int SLOT_DISTANCE = 0; + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; + public static final int DRIVE_ACCELERATION_HIGH = 7000; - /* PID Indexes */ - public static final int PID_PRIMARY = 0; - } + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); - public static final class PneumaticsConstants { - public static final int PCM_MODULE_ID = 7; + /* Trajectory Constants */ + public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; + public static final double TRACK_WIDTH_METERS = 0.648; + public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); - public static final int SPEED_SHIFT_FORWARD_ID = 0; - public static final int SPEED_SHIFT_REVERSE_ID = 1; + /* Remote Sensors */ + public static final int REMOTE_0 = 0; + public static final int REMOTE_1 = 1; - public static final int COOL_FALCON_FORWARD_ID = 3; - public static final int COOL_FALCON_REVERSE_ID = 2; - } + /* PID Indexes */ + public static final int PID_PRIMARY = 0; + public static final int PID_TURN = 1; - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } + /* PID SLOTS */ + public static final int SLOT_DISTANCE = 0; + public static final int SLOT_VELOCITY = 1; + public static final int SLOT_TURNING = 2; + public static final int SLOT_MOTION_MAGIC = 3; - public static final class VisionConstants { - public static final double FOV = 29.8; // Field of view of limelight - public static final double TARGET_HEIGHT = 67.5; - public static final double LIME_ANGLE = 24.7; - public static final double TURN_P_VALUE = 0.8; - public static final double X_ANGLE_ERROR = 0.5; - public static final double MOTOR_DEAD_ZONE = 0.2; - public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; - public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; - public static final double GRAV = 385.83; + /* Ratio Calculation */ + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + public static final double TICK_TIME_TO_SECONDS = 0.1; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + public static final double INCHES_PER_METER = 39.370; + public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; - // Galactic Search - public static final double searchError = 2; - /* - public static final double bothCloseVisibleY = -17.69; - public static final double closeLeftVisibleY = -12.57; - public static final double closeRightVisibleY = -11.35; - public static final double farLeftVisibleX = 3.58; - public static final double farRightVisibleX = 7.04; - */ + public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1 / MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_HIGH = 1 / TICKS_PER_INCH_HIGH; - public static final double[] aRed = { 1.6, -11.7 }; - public static final double[] bRed = { 2.5, -5.5 }; - public static final double[] aBlue = { 9.9, 9.0 }; - public static final double[] bBlue = { 5.5, 13.3 }; - } + public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1 / MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_LOW = 1 / TICKS_PER_INCH_LOW; + } - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - public static final int BUTTON_FOX_ID = 2; - } + public static final class ShooterConstants { + /* Motor IDs */ + public static final int SHOOTER_FALCON_BALLER_ID = 8; + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15; + public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final int SHOOTER_ROTATE_ID = 9; + + /* PID Constants Shooter */ + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); // Ff was 0.055 + // public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final double SHOOTER_TURRET_MIN = -1.0; + public static final double ENCODER_TICKS_PER_REV = 2048; + public static final double NEO_UNITS_PER_REV = 42; + public static final double DEGREES_PER_ROT = 360; + + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + + public static final int TURRET_RIGHT_SOFT_LIMIT = -2; + public static final int TURRET_LEFT_SOFT_LIMIT = -55; + public static final double TURRET_SPEED_MULTIPLIER = 0.3; + public static final double TURRET_CALIBRATE_SPEED = 0.075; + public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; // 89.56696; + public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166; + + public static final int HOOD_UP_SOFT_LIMIT = 33; + public static final int HOOD_DOWN_SOFT_LIMIT = 3; + public static final double HOOD_CONVERT_SLOPE = 0.47; + public static final double HOOD_CONVERT_B = 40.5; + public static final double HOOD_CALIBRATE_SPEED = 0.2; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find + + public static final double DRUM_RAMP_LIMIT = 1000; + public static final double DRUM_VELOCITY_BOUND = 300; + } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 14; + } + + public static final class LevelerConstants { + public static final int LEVELER_CAN_ID = 30; + } + + public static final class IntakeConstants { + public static final double EXTENDER_SPEED = 0.3; + private static final double[] INTAKE_SPEED_MODES = { 1.0, 0.5 }; + public static double INTAKE_SPEED; + + public static final int INTAKE_SPARK_ID = 12; + public static final int EXTENDER_SPARK_ID = 13; + } + + public static final class StorageConstants { + public static final int STORAGE_CAN_ID = 11; + public static final double STORAGE_PARTIAL_BALL = 2; + public static final double STORAGE_FULL_BALL = 7; + private static final double[] STORAGE_SPEED_MODES = { 0.75, 0.50 }; + public static double STORAGE_SPEED; + public static final double STORAGE_TIMEOUT = 3000; + + /* Storage Characteristics */ + public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; // For the first storage belt + public static final double INCHES_PER_STORAGE_ROT = 1; // Circumference of the first storage belt + + /* Ball Indexes */ + public static final int BEAM_SENSOR_SHOOTER = 11; + public static final int BEAM_SENSOR_USELESS = 12; + public static final int BEAM_SENSOR_STORAGE = 13; + public static final int BEAM_SENSOR_INTAKE = 14; + + /* PID Gains */ + public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + + /* PID Values */ + public static final int SLOT_DISTANCE = 0; + + /* PID Indexes */ + public static final int PID_PRIMARY = 0; + } + + public static final class PneumaticsConstants { + public static final int PCM_MODULE_ID = 7; + + public static final int SPEED_SHIFT_FORWARD_ID = 0; + public static final int SPEED_SHIFT_REVERSE_ID = 1; + + public static final int COOL_FALCON_FORWARD_ID = 3; + public static final int COOL_FALCON_REVERSE_ID = 2; + } + + 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 VisionConstants { + public static final double FOV = 29.8; // Field of view of limelight + public static final double TARGET_HEIGHT = 67.5; + public static final double LIME_ANGLE = 24.7; + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double MOTOR_DEAD_ZONE = 0.2; + public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; + public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; + public static final double GRAV = 385.83; + + // Galactic Search + public static final double searchError = 2; + /* + public static final double bothCloseVisibleY = -17.69; + public static final double closeLeftVisibleY = -12.57; + public static final double closeRightVisibleY = -11.35; + public static final double farLeftVisibleX = 3.58; + public static final double farRightVisibleX = 7.04; + */ + + public static final double[] aRed = { 1.6, -11.7 }; + public static final double[] bRed = { 2.5, -5.5 }; + public static final double[] aBlue = { 9.9, 9.0 }; + public static final double[] bBlue = { 5.5, 13.3 }; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTON_FOX_ID = 2; + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index cd1f779..2822027 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,14 +7,10 @@ package frc4388.robot; -import java.util.function.Supplier; - import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -46,11 +42,11 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - SmartDashboard.putString("Is Auto Start?", "NAH"); Mode.set(Mode.COMPETITIVE); m_modeChooser.setDefaultOption(Mode.COMPETITIVE.name(), Mode.COMPETITIVE); m_modeChooser.addOption(Mode.CASUAL.name(), Mode.CASUAL); SmartDashboard.putData("Mode", m_modeChooser); + SmartDashboard.putString("Is Auto Start?", "NAH"); } /** @@ -141,7 +137,7 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); - m_robotContainer.shiftClimberRachet(false); + m_robotContainer.shiftClimberRatchet(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when @@ -161,15 +157,4 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { } - - @Override - public void testInit() { - } - - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7ee872b..f4c85d7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,7 +8,6 @@ package frc4388.robot; import java.nio.file.Path; -import java.util.function.Supplier; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -31,7 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.Mode; import frc4388.robot.Constants.OIConstants; -import frc4388.robot.commands.InterruptSubystem; +import frc4388.robot.commands.InterruptSubsystem; import frc4388.robot.commands.auto.Barrel; import frc4388.robot.commands.auto.BarrelMany; import frc4388.robot.commands.auto.BarrelStart; @@ -41,34 +40,24 @@ import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; import frc4388.robot.commands.auto.FiveBallAutoMiddle; import frc4388.robot.commands.auto.GalacticSearch; -import frc4388.robot.commands.auto.IdentifyPath; import frc4388.robot.commands.auto.SequentialTest; import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.Slalom; import frc4388.robot.commands.auto.TenBallAutoMiddle; -import frc4388.robot.commands.auto.Wait; -import frc4388.robot.commands.climber.DisengageRachet; +import frc4388.robot.commands.climber.DisengageRatchet; import frc4388.robot.commands.climber.RunClimberWithTriggers; import frc4388.robot.commands.climber.RunLevelerWithJoystick; -import frc4388.robot.commands.drive.DriveStraightAtVelocityPID; import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.PlaySongDrive; -import frc4388.robot.commands.drive.SkipSong; -import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.TrackTarget; -import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.shooter.RunHoodWithJoystick; import frc4388.robot.commands.shooter.ShootPrepGroup; -import frc4388.robot.commands.shooter.ShooterGoalPosition; import frc4388.robot.commands.shooter.ShooterManual; import frc4388.robot.commands.shooter.ShooterTrenchPosition; -import frc4388.robot.commands.shooter.ShooterVelocityControlPID; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.storage.ManageStorage; -import frc4388.robot.commands.storage.StoragePrep; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -88,688 +77,438 @@ import frc4388.utility.controller.JoystickManualButton; 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. + * 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 { - /* Subsystems */ - private final Drive m_robotDrive = new Drive(); - private final Pneumatics m_robotPneumatics = new Pneumatics(); - private final LED m_robotLED = new LED(); - private final Intake m_robotIntake = new Intake(); - private final Shooter m_robotShooter = new Shooter(); - private final ShooterAim m_robotShooterAim = new ShooterAim(); - private final ShooterHood m_robotShooterHood = new ShooterHood(); - private final Climber m_robotClimber = new Climber(); - private final Leveler m_robotLeveler = new Leveler(); - private final Storage m_robotStorage = new Storage(); + /* Subsystems */ + private final Drive m_robotDrive = new Drive(); + private final Pneumatics m_robotPneumatics = new Pneumatics(); + private final LED m_robotLED = new LED(); + private final Intake m_robotIntake = new Intake(); + private final Shooter m_robotShooter = new Shooter(); + private final ShooterAim m_robotShooterAim = new ShooterAim(); + private final ShooterHood m_robotShooterHood = new ShooterHood(); + private final Climber m_robotClimber = new Climber(); + private final Leveler m_robotLeveler = new Leveler(); + private final Storage m_robotStorage = new Storage(); - /* Cameras */ - private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); - private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); - public final LimeLight m_robotLime = new LimeLight(); + /* Cameras */ + private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); + private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); + public final LimeLight m_robotLime = new LimeLight(); - /* Controllers */ + /* Controllers */ + public boolean isGS = false; + private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); + private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private static XboxController m_buttonFox = new XboxController(OIConstants.BUTTON_FOX_ID); + private static XboxController m_manualXbox = new XboxController(3); - public boolean isGS = false; - private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - private static XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID); - private static XboxController m_manualXbox = new XboxController(3); + /* Autos */ + double m_totalTimeAuto; + SixBallAutoMiddle m_sixBallAutoMiddle; + SixBallAutoMiddle m_sixBallAutoMiddle0; + SixBallAutoMiddle m_sixBallAutoMiddle1; + EightBallAutoMiddle m_eightBallAutoMiddle; + DriveOffLineForward m_driveOffLineForward; + DriveOffLineBackward m_driveOffLineBackward; + FiveBallAutoMiddle m_fiveBallAutoMiddle; + TenBallAutoMiddle m_tenBallAutoMiddle; + Slalom m_slalom; + Barrel m_barrel; + BarrelStart m_barrelStart; + BarrelMany m_barrelMany; + Bounce m_bounce; + SequentialTest m_sequentialTest; + GalacticSearch m_galacticSearch; - /* Autos */ - double m_totalTimeAuto; + public static boolean m_isShooterManual = false; - SixBallAutoMiddle m_sixBallAutoMiddle; + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + Constants.Mode.register(this::setControls); + } - SixBallAutoMiddle m_sixBallAutoMiddle0; + public void setControls(Mode mode) { + CommandScheduler.getInstance().clearButtons(); + /* Passing Drive and Pneumatics Subsystems */ + m_robotPneumatics.passRequiredSubsystem(m_robotDrive); + m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); + m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); + m_robotShooterHood.passRequiredSubsystem(m_robotShooter); + m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); + m_robotLeveler.passRequiredSubsystem(m_robotClimber); - SixBallAutoMiddle m_sixBallAutoMiddle1; + final double drumShooterTargetPIDVelocity; - EightBallAutoMiddle m_eightBallAutoMiddle; - - DriveOffLineForward m_driveOffLineForward; - - DriveOffLineBackward m_driveOffLineBackward; - - FiveBallAutoMiddle m_fiveBallAutoMiddle; - - TenBallAutoMiddle m_tenBallAutoMiddle; - - Slalom m_slalom; - - Barrel m_barrel; - - BarrelStart m_barrelStart; - - BarrelMany m_barrelMany; - - Bounce m_bounce; - - SequentialTest m_sequentialTest; - - GalacticSearch m_galacticSearch; - - public static boolean m_isShooterManual = false; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - setControls(Mode.COMPETITIVE); - Constants.Mode.register(this::setControls); - } - - public void setControls(Mode mode) { - CommandScheduler.getInstance().clearButtons(); - switch (mode) { - case COMPETITIVE: - System.out.println("COMP CONTROLS"); - /* Passing Drive and Pneumatics Subsystems */ - m_robotPneumatics.passRequiredSubsystem(m_robotDrive); - m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); - - m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); - m_robotShooterHood.passRequiredSubsystem(m_robotShooter); - m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); - - m_robotLeveler.passRequiredSubsystem(m_robotClimber); - - configureButtonBindingsCompetitive(); - - /* Builds Autos */ - //buildAutos(); - - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); - //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); - - // drives intake with input from triggers on the opperator controller - m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // runs the turret with joystick - m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); - // runs the hood with joystick - m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); - // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(12000), m_robotShooter)); - // drives climber with input from triggers on the opperator controller - m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); - // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // runs the storage not - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); - m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); - break; - case CASUAL: - System.out.println("CASUAL CONTROLS"); - /* Passing Casual Mode Subsystems */ - m_robotPneumatics.passRequiredSubsystem(m_robotDrive); - m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); - - m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); - m_robotShooterHood.passRequiredSubsystem(m_robotShooter); - m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); - - m_robotLeveler.passRequiredSubsystem(m_robotClimber); - - configureButtonBindingsCasual(); - - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); - //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); - - // drives intake with input from triggers on the opperator controller - m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // runs the turret with joystick - m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); - // runs the hood with joystick - m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); - // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0), m_robotShooter)); - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // runs the storage not - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); - m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); - break; - } + if (mode == Mode.COMPETITIVE) { + drumShooterTargetPIDVelocity = 12000; + configureButtonBindingsCompetitive(); + /* Builds Autos */ + // buildAutos(); + /* Default Commands */ + // drives climber with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); + // drives the leveler with an axis input from the driver controller + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); + } else { + drumShooterTargetPIDVelocity = 0; + configureButtonBindingsCasual(); } - /** - * 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 configureButtonBindingsCompetitive() { - /* Test Buttons */ + /* Default Commands */ + // drives the robot with a two-axis input from the driver controller + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); + // m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); + // drives intake with input from triggers on the opperator controller + m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // runs the turret with joystick + m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); + // runs the hood with joystick + m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); + // moves the drum not + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(drumShooterTargetPIDVelocity), m_robotShooter)); + // continually sends updates to the Blinkin LED controller to keep the lights on + m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + // runs the storage not + m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); + m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime)); + } - // A driver test button - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) - .whenReleased(new InterruptSubystem(m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotShooterHood)); + /** + * 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 configureButtonBindingsCompetitive() { + /* Test Buttons */ + // A driver test button + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)); + // B driver test button + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); + // Y driver test button + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON).whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive)); + // X driver test button + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand()); + /* Driver Buttons */ + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + // Disengages the ratchet to allow for a climb + new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber)); + /* Operator Buttons */ + // shoots until released + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, + // m_robotStorage), false); + // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // shoots one ball + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, + // m_robotStorage), false); + // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // extends or retracts the extender + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); + // safety for climber and leveler + new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber)); + // starts tracking target + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff)); + // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); + // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); + // .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + // .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); + // Trims shooter + new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter)); + // Calibrates turret and hood + new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + // Run drum + new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false).whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) + .whenReleased(new InstantCommand(m_robotLime::limeOff)); + // Run drum manual + new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true).whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000))).whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + /* Button Box */ + // Storage Manual + new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); + // Meg + new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // Shooter Manual + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false)); + // Goal Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // Trench Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim)); + // .whenPressed(new SkipSong(m_robotDrive, 1)); + } - // B driver test button - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); + private void configureButtonBindingsCasual() { + /* Driver Buttons */ + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + /* Operator Buttons */ + // shoots until released + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, + // m_robotStorage), false); + // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // shoots one ball + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, + // m_robotStorage), false); + // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // extends or retracts the extender + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); + // safety for climber and leveler + new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber)); + // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); + // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); + // .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + // .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); + // Trims shooter + new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter)); + // Calibrates turret and hood + new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + // Run drum manual + new JoystickManualButton(getOperatorJoystick(), XboxController.A_BUTTON, true).whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000))).whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + /* Button Box */ + // Storage Manual + new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); + // Meg + new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // Shooter Manual + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false)); + // Goal Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // Trench Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim)); + // .whenPressed(new SkipSong(m_robotDrive, 1)); + } - // Y driver test button - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive)); + public void buildAutos() { + // resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); + String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" }; + m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); - // X driver test button - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new InstantCommand()); + String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" }; + m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths)); - /* Driver Buttons */ - // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); + String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" }; + m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths)); - // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + String[] slalom = new String[] { "Slalom" }; + m_slalom = new Slalom(m_robotDrive, buildPaths(slalom)); - // Disengages the rachet to allow for a climb - new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) - .whileHeld(new DisengageRachet(m_robotClimber)); + String[] barrel = new String[] { "BarrelStart" }; + m_barrel = new Barrel(m_robotDrive, buildPaths(barrel)); - /* Operator Buttons */ - // shoots until released - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); - //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) - .whenReleased(new InterruptSubystem(m_robotStorage)); + String[] barrelStart = new String[] { "Barrel" }; + m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart)); - // shoots one ball - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); - //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) - .whenReleased(new InterruptSubystem(m_robotStorage)); + String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" }; + m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce)); - // extends or retracts the extender - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); + String[] barrelMany = new String[] { "BarrelManyWaypoints" }; + m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany)); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); + String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" }; + m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); - // safety for climber and leveler - new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); + String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" }; + m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); - // starts tracking target - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooterAim)) - .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) - .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); + String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" }; + m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); - //.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); - //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); - //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) - //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); + String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" }; + m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); - //Trims shooter - new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) - .whenPressed(new TrimShooter(m_robotShooter)); + String[] tenBallAutoMiddlePaths = new String[] { "SixBallMidComplete", "TenBallMidComplete" }; + m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); - //Calibrates turret and hood - new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + String[] galacticSearchPaths = new String[] { "GSC_ARED", "GSC_ABLUE", "GSC_BRED", "GSC_BBLUE" }; + m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths)); + } + public void idenPath() { + m_robotLime.identifyPath(); + } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Create custom trajectories + // TrajectoryConfig config = getTrajectoryConfig(); + // Trajectory trajectory = getTrajectory(config); + // RamseteCommand ramseteCommand = getRamseteCommand(trajectory); + // Run path following command, then stop at the end. + try { + SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); + // return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); + return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + } catch (Exception e) { + System.err.println("ERROR"); + } + return new InstantCommand(); + } - //Run drum - new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) - //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) - .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); + TrajectoryConfig getTrajectoryConfig() { + return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics); + } - //Run drum manual - new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000))) - .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + public RamseteCommand getRamseteCommand(Trajectory trajectory) { + return new RamseteCommand(trajectory, m_robotDrive::getPose, new RamseteController(), DriveConstants.kDriveKinematics, m_robotDrive::tankDriveVelocity, m_robotDrive); + } + public RamseteCommand[] buildPaths(String[] paths) { + RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; + double[] times = new double[paths.length]; + Trajectory initialTrajectory; + m_totalTimeAuto = 0; + try { + if (true) { + String path = paths[0]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); - /* Button Box */ - // Storage Manual - new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH) - .whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))) - .whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); + SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString()); - // Meg - new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) - .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + initialTrajectory = trajectory; + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[0] = ramseteCommand; + times[0] = initialTrajectory.getTotalTimeSeconds(); + } - // Shooter Manual - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH) - .whileHeld(new ShooterManual(true)) - .whenReleased(new ShooterManual(false)); - - // Goal Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotDrive)); - - // Trench Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) - .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) - .whenReleased(new InterruptSubystem(m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotShooterHood)) - .whenReleased(new InterruptSubystem(m_robotShooterAim)); - //.whenPressed(new SkipSong(m_robotDrive, 1)); + for (int i = 1; i < paths.length; i++) { + String path = paths[i]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[i] = ramseteCommand; + times[i] = trajectory.getTotalTimeSeconds(); + } + } catch (Exception e) { + DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); } - private void configureButtonBindingsCasual() { - /* Test Buttons */ - - // A driver test button - - - /* Driver Buttons */ - // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); - - // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); - - - - - /* Operator Buttons */ - // shoots until released - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); - //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) - .whenReleased(new InterruptSubystem(m_robotStorage)); - - // shoots one ball - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); - //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) - .whenReleased(new InterruptSubystem(m_robotStorage)); - - // extends or retracts the extender - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); - - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); - - // safety for climber and leveler - new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); - - - - - //.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); - //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); - //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) - //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); - - //Trims shooter - new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) - .whenPressed(new TrimShooter(m_robotShooter)); - - //Calibrates turret and hood - new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); - - - - - - //Run drum manual - new JoystickManualButton(getOperatorJoystick(), XboxController.A_BUTTON, true) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000))) - .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); - - - - /* Button Box */ - // Storage Manual - new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH) - .whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))) - .whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); - - // Meg - new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) - .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotDrive)); - - // Shooter Manual - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH) - .whileHeld(new ShooterManual(true)) - .whenReleased(new ShooterManual(false)); - - // Goal Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotDrive)); - - // Trench Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) - .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) - .whenReleased(new InterruptSubystem(m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotShooterHood)) - .whenReleased(new InterruptSubystem(m_robotShooterAim)); - //.whenPressed(new SkipSong(m_robotDrive, 1)); + for (int i = 0; i < times.length; i++) { + m_totalTimeAuto += times[i]; } + return ramseteCommands; + } - public void buildAutos() { - //resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); + /** + * Sets Motors to a NeutralMode. + * @param mode NeutralMode to set motors to + */ + public void setDriveNeutralMode(NeutralMode mode) { + m_robotDrive.setDriveTrainNeutralMode(mode); + } - String[] sixBallAutoMiddlePaths = new String[]{ - "SixBallMidComplete" - }; + /** + * Sets the gear of the drivetrain + * @param state the gearing of the gearbox (true is high, false is low) + */ + public void setDriveGearState(boolean state) { + m_robotPneumatics.setShiftState(state); + } - m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + public void shiftClimberRatchet(boolean state) { + m_robotClimber.shiftServo(state); + } - String[] sixBallAutoMiddle0Paths = new String[]{ - "SixBallMid0" - }; + public void resetOdometry(Pose2d pose) { + m_robotDrive.setOdometry(pose); + } - m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths)); + public void resetGyroYawRobotContainer(double angle) { + m_robotDrive.resetGyroYaw(angle); + } - String[] sixBallAutoMiddle1Paths = new String[]{ - "SixBallMid1" - }; + /** + * Used for analog inputs like triggers and axises. + * @return IHandController interface for the Driver Controller. + */ + public IHandController getDriverController() { + return m_driverXbox; + } - m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths)); + /** + * Used for analog inputs like triggers and axises. + * @return The IHandController interface for the Operator Controller. + */ + public IHandController getOperatorController() { + return m_operatorXbox; + } - String[] slalom = new String[]{ - "Slalom" - }; + public IHandController getButtonFoxObject() { + return m_buttonFox; + } - m_slalom = new Slalom(m_robotDrive, buildPaths(slalom)); + /** + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator + * Xbox Controller. Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Operator Controller. + */ + public Joystick getOperatorJoystick() { + return m_operatorXbox.getJoyStick(); + } - String[] barrel = new String[]{ - "BarrelStart" - }; + /** + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox + * Controller. Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Driver Controller. + */ + public Joystick getDriverJoystick() { + return m_driverXbox.getJoyStick(); + } - m_barrel = new Barrel(m_robotDrive, buildPaths(barrel)); - - String[] barrelStart = new String[]{ - "Barrel" - }; - - m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart)); - - String[] bounce = new String[]{ - "Bounce1", - "Bounce2", - "Bounce3", - "Bounce4" - }; - - m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce)); - - String[] barrelMany = new String[]{ - "BarrelManyWaypoints" - }; - - m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany)); - - String[] eightBallAutoMiddlePaths = new String[]{ - "EightBallMidComplete" - }; - - m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); - - String[] driveOffLineForwardPaths = new String[]{ - "DriveOffLineForward" - }; - - m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); - - String[] driveOffLineBackwardPaths = new String[]{ - "DriveOffLineBackward" - }; - - m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); - - String[] fiveBallAutoMiddlePaths = new String[]{ - "FiveBallMidComplete" - }; - - m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); - - String[] tenBallAutoMiddlePaths = new String[]{ - "SixBallMidComplete", - "TenBallMidComplete" - }; - m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, - m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths)); - - String[] galacticSearchPaths = new String[]{ - "GSC_ARED", - "GSC_ABLUE", - "GSC_BRED", - "GSC_BBLUE" - }; - - m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths)); - - } - - public void idenPath() - { - m_robotLime.identifyPath(); - } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // Create custom trajectories - //TrajectoryConfig config = getTrajectoryConfig(); - //Trajectory trajectory = getTrajectory(config); - //RamseteCommand ramseteCommand = getRamseteCommand(trajectory); - // Run path following command, then stop at the end. - try { - SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); - return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); - - - } catch (Exception e) { - System.err.println("ERROR"); - } - - return new InstantCommand(); - } - - TrajectoryConfig getTrajectoryConfig() { - return new TrajectoryConfig( - DriveConstants.MAX_SPEED_METERS_PER_SECOND, - DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics); - } - - public RamseteCommand getRamseteCommand(Trajectory trajectory) { - RamseteCommand ramseteCommand = new RamseteCommand( - trajectory, - m_robotDrive::getPose, - new RamseteController(), - DriveConstants.kDriveKinematics, - m_robotDrive::tankDriveVelocity, - m_robotDrive); - - return ramseteCommand; - } - - public RamseteCommand[] buildPaths(String[] paths) { - RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; - double[] times = new double[paths.length]; - Trajectory initialTrajectory; - m_totalTimeAuto = 0; - - try { - if (true) { - String path = paths[0]; - String trajectoryJSON = "paths/" + path + ".wpilib.json"; - Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); - - SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString()); - - Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); - - initialTrajectory = trajectory; - RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); - ramseteCommands[0] = ramseteCommand; - times[0] = initialTrajectory.getTotalTimeSeconds(); - } - - for(int i = 1; i < paths.length; i++) { - String path = paths[i]; - String trajectoryJSON = "paths/" + path + ".wpilib.json"; - Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); - Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); - RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); - ramseteCommands[i] = ramseteCommand; - times[i] = trajectory.getTotalTimeSeconds(); - } - } catch (Exception e) { - DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); - } - - for (int i = 0; i < times.length; i++) { - m_totalTimeAuto += times[i]; - } - - return ramseteCommands; - } - - /** - * Sets Motors to a NeutralMode. - * @param mode NeutralMode to set motors to - */ - public void setDriveNeutralMode(NeutralMode mode) { - m_robotDrive.setDriveTrainNeutralMode(mode); - } - - /** - * Sets the gear of the drivetrain - * @param state the gearing of the gearbox (true is high, false is low) - */ - public void setDriveGearState(boolean state) { - m_robotPneumatics.setShiftState(state); - } - - /** - * - */ - public void shiftClimberRachet(boolean state) { - m_robotClimber.shiftServo(state); - } - - /** - * - */ - public void resetOdometry(Pose2d pose) { - m_robotDrive.setOdometry(pose); - } - - public void resetGyroYawRobotContainer(double angle) { - m_robotDrive.resetGyroYaw(angle); - } - - /** - * Used for analog inputs like triggers and axises. - * @return IHandController interface for the Driver Controller. - */ - public IHandController getDriverController() { - return m_driverXbox; - } - - - /** - * Used for analog inputs like triggers and axises. - * @return The IHandController interface for the Operator Controller. - */ - public IHandController getOperatorController() - { - return m_operatorXbox; - } - - public IHandController getButtonFoxObject() - { - return m_buttonFox; - } - - - /** - * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller. - * Generic HIDs/Joysticks can be used to set up JoystickButtons. - * @return The IHandController interface for the Operator Controller. - */ - public Joystick getOperatorJoystick() - { - return m_operatorXbox.getJoyStick(); - } - - /** - * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Driver Xbox Controller. - * Generic HIDs/Joysticks can be used to set up JoystickButtons. - * @return The IHandController interface for the Driver Controller. - */ - public Joystick getDriverJoystick() - { - return m_driverXbox.getJoyStick(); - } - - /** - * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Button Fox. - * Generic HIDs/Joysticks can be used to set up JoystickButtons. - * @return The IHandController interface for the Button Fox. - */ - public Joystick getButtonFox() - { - return m_buttonFox.getJoyStick(); - } + /** + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Button Fox. + * Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Button Fox. + */ + public Joystick getButtonFox() { + return m_buttonFox.getJoyStick(); + } } diff --git a/src/main/java/frc4388/robot/commands/InterruptSubystem.java b/src/main/java/frc4388/robot/commands/InterruptSubsystem.java similarity index 88% rename from src/main/java/frc4388/robot/commands/InterruptSubystem.java rename to src/main/java/frc4388/robot/commands/InterruptSubsystem.java index 5eae65f..11722f3 100644 --- a/src/main/java/frc4388/robot/commands/InterruptSubystem.java +++ b/src/main/java/frc4388/robot/commands/InterruptSubsystem.java @@ -10,11 +10,11 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class InterruptSubystem extends CommandBase { +public class InterruptSubsystem extends CommandBase { /** - * Creates a new InterruptSubystem. + * Creates a new InterruptSubsystem. */ - public InterruptSubystem(SubsystemBase subsystem) { + public InterruptSubsystem(SubsystemBase subsystem) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(subsystem); } diff --git a/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java b/src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java similarity index 90% rename from src/main/java/frc4388/robot/commands/climber/DisengageRachet.java rename to src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java index 46888ba..89296b4 100644 --- a/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java +++ b/src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java @@ -10,13 +10,13 @@ package frc4388.robot.commands.climber; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Climber; -public class DisengageRachet extends CommandBase { +public class DisengageRatchet extends CommandBase { Climber m_climber; /** - * Creates a new DisengageRachet command. + * Creates a new DisengageRatchet command. */ - public DisengageRachet(Climber subsystem) { + public DisengageRatchet(Climber subsystem) { // Use addRequirements() here to declare subsystem dependencies. m_climber = subsystem; } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 61409b0..687ede2 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -27,7 +27,7 @@ public class Climber extends SubsystemBase { //Spark m_spark = new Spark(4); public boolean m_climberSafety = false; - public boolean m_isRachetEngaged = true; + public boolean m_isRatchetEngaged = true; /** * Creates a new Climber. @@ -50,7 +50,7 @@ public class Climber extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Climber Safety", m_climberSafety); - SmartDashboard.putBoolean("Rachet", m_isRachetEngaged); + SmartDashboard.putBoolean("Rachet", m_isRatchetEngaged); } /** @@ -80,7 +80,7 @@ public class Climber extends SubsystemBase { } /** - * @param shift true to enage rachet, false to disengage + * @param shift true to engage ratchet, false to disengage */ public void shiftServo(boolean shift) { if (shift) { @@ -88,6 +88,6 @@ public class Climber extends SubsystemBase { } else { m_servo.setPosition(0.56); } - m_isRachetEngaged = shift; + m_isRatchetEngaged = shift; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ba350ba..cc2d7b0 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,9 +21,11 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; +import frc4388.robot.Robot; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; import frc4388.utility.Trims; @@ -102,7 +104,7 @@ public class Shooter extends SubsystemBase { csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()), csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances Casual.csv").toPath()) }; - new Thread(() -> System.out.println(CSV.ReflectionTable.create(getActiveShooterTable()))).start(); + new Thread(() -> System.out.println(CSV.ReflectionTable.create(getActiveShooterTable(), RobotBase.isSimulation()))).start(); } catch (final IOException e) { throw new RuntimeException(e); } diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java index aeac13c..24a83b1 100644 --- a/src/main/java/frc4388/utility/CSV.java +++ b/src/main/java/frc4388/utility/CSV.java @@ -26,6 +26,7 @@ import java.util.List; import java.util.Map; import java.util.Objects; import java.util.function.BiConsumer; +import java.util.function.BiFunction; import java.util.function.Function; import java.util.function.IntFunction; import java.util.function.Predicate; @@ -36,204 +37,206 @@ import java.util.stream.IntStream; import java.util.stream.Stream; public class CSV { - private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); - private final Supplier generator; - private final IntFunction arrayGenerator; - private final Map> setters; + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; - /** - * A binary string operator to be applied to the entire header of the CSV. - */ - protected String headerSanitizer(final String header) { - return SANITIZER.matcher(header).replaceAll(""); + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by + * the given generator. Private fields and fields of primitive types are not supported. + * @param generator a parameterless supplier which produces a new object with any number of fields + * corresponding to header names from a CSV file. The first character of the names + * from the header in the CSV file will be made lowercase and invalid characters + * will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array filled with populated + * objects created with the previously given generator. Cells are parsed using their corresponding + * field's {@code valueOf(String)} function. + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); + final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + if (colored) + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); } - /** - * A binary string operator to be applied to each name in the header of the CSV. - */ - protected String nameProcessor(final String name) { - return Character.toLowerCase(name.charAt(0)) + name.substring(1); + private static final Color GRADIENT_MIN = new Color(0x00, 0x33, 0x00); + private static final Color GRADIENT_MAX = new Color(0x00, 0xFF, 0x00); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } } - /** - * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by - * the given generator. Private fields and fields of primitive types are not supported. - * @param generator a parameterless supplier which produces a new object with any number of fields - * corresponding to header names from a CSV file. The first character of the names - * from the header in the CSV file will be made lowercase and invalid characters - * will be removed to match Java naming conventions. - * @see #read(Path) - */ - @SuppressWarnings("unchecked") - public CSV(final Supplier generator) { - final Class clazz = generator.get().getClass(); - final Map, Function> fieldParsers = new HashMap<>(); - this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); - this.generator = generator; - this.setters = new HashMap<>(); - for (final Field field : clazz.getFields()) { - final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); - if (parser != null) - this.setters.put(field.getName(), (final R obj, final String rawValue) -> { - try { - field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); - } catch (final IllegalAccessException e) { - throw new RuntimeException(e); - } - }); - } + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; } - /** - * Reads and parses the contents of the given CSV file, and returns an array filled with populated - * objects created with the previously given generator. Cells are parsed using their corresponding - * field's {@code valueOf(String)} function. - * @param path the path to a CSV file - * @return the parsed data from the CSV file - * @throws IOException if an I/O error occurs opening the file - */ - @SuppressWarnings("unchecked") - public R[] read(final Path path) throws IOException { - try (final BufferedReader reader = Files.newBufferedReader(path)) { - final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); - final Stream lines = reader.lines(); - return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); - } + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); } - @SuppressWarnings("unchecked") - private static Function getTypeParser(final Class type) { - try { - return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); - } catch (final NoSuchMethodException | IllegalAccessException e) { - return null; - } + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } } - private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { - final int recordStringLength = recordString.length(); - int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; - while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { - final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); - String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); - if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { - final int fieldLength = field.length(); - if (countTrailing(field, '"') % 2 == 0) { - tryFieldEndFromIndex = tryFieldEndIndex + 1; - continue; - } else - field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); - } - final BiConsumer setter = fieldParseSetters[i]; - if (setter != null) - setter.accept(object, field); - tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; - i++; - } - return object; + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; } - private static int countTrailing(final String str, final char c) { - final int l = str.length(); - int count = 0; - while (str.charAt(l - count - 1) == c && count < l) - count++; - return count; + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); } - public static class ReflectionTable { - public static String create(final T[] objects) { - final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); - final List> rows = new ArrayList<>(); - rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); - rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); - final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); - IntStream.range(0, fields.length).forEach(i -> { - final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); - rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); - }); - return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(MessageFormat.format("{0} %{1}{2}s {3}", row.get(i).escape, row.get(i).padRight ? "-" : "", columnWidths[i], RESET_STYLE), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); - } - - private static final Color GRADIENT_MIN = new Color(0x00, 0x33, 0x00); - private static final Color GRADIENT_MAX = new Color(0x00, 0xFF, 0x00); - private static final String CONTROL = "\033"; - private static final String CSI = "["; - private static final String LF = "\n"; - private static final String RESET = "0"; - private static final String BOLD = "1"; - private static final String ITALIC = "3"; - private static final String UNDERLINE = "4"; - private static final String BACKGROUND_RED = "41"; - private static final String FOREGROUND = "38"; - private static final String BACKGROUND = "48"; - private static final String TRUECOLOR = "2"; - private static final String SEPARATOR = ";"; - private static final String SGR = "m"; - private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; - private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; - private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; - private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; - private Object value; - private String string; - private boolean padRight; - private String escape; - - private ReflectionTable(final Object obj, final Field field) { - try { - value = field.get(obj); - string = Objects.toString(value); - padRight = !Number.class.isAssignableFrom(field.getType()); - escape = Objects.isNull(value) ? NULL_STYLE : ""; - } catch (final IllegalAccessException | IllegalArgumentException e) { - value = null; - string = e.getClass().getSimpleName(); - padRight = false; - escape = ERROR_STYLE; - } - } - - private ReflectionTable(final Field field) { - value = null; - string = field.getName(); - padRight = true; - escape = HEADER_STYLE; - } - - private Number getValue() { - return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); - } - - private void colorByValue(final Number min, final Number max) { - if (Objects.nonNull(value)) { - final double range = max.doubleValue() - min.doubleValue(); - final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; - final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); - escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); - } - } - - private static String colorTo24BitSGR(final Color color, final boolean background) { - return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; - } - - private static int range(final double normal, final int min, final int max) { - return (int) (normal * (max - min) + min); - } - - /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ - private static float contrastRatio(final Color lighter, final Color darker) { - return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); - } - - /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ - private static float relativeLuminance(final Color color) { - final float[] components = color.getRGBComponents(null); - final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); - final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); - final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); - return 0.2126f * r + 0.7152f * g + 0.0722f * b; - } + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } }