From c594f7d69044355839f20d5a6803cf7320f882d4 Mon Sep 17 00:00:00 2001 From: aborunda <90010674+aborunda@users.noreply.github.com> Date: Thu, 4 Nov 2021 17:59:45 -0600 Subject: [PATCH] Add more mode constants - Control modes not working right now --- .../deploy/Robot Data - Distances Casual.csv | 16 ++++ src/main/java/frc4388/robot/Constants.java | 17 +++- .../java/frc4388/robot/RobotContainer.java | 92 +++++++++++-------- .../frc4388/robot/subsystems/Shooter.java | 42 +++++---- 4 files changed, 108 insertions(+), 59 deletions(-) create mode 100644 src/main/deploy/Robot Data - Distances Casual.csv diff --git a/src/main/deploy/Robot Data - Distances Casual.csv b/src/main/deploy/Robot Data - Distances Casual.csv new file mode 100644 index 0000000..ac35296 --- /dev/null +++ b/src/main/deploy/Robot Data - Distances Casual.csv @@ -0,0 +1,16 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Center Displacement (deg) +0 ,16 ,6000 ,1 +65.9 ,16 ,6000 ,1 +103 ,19 ,6000 ,1 +126.6 ,20.28 ,6000 ,1.5 +156.6 ,28 ,6000 ,1.5 +174 ,28 ,6000 ,1.5 +178 ,28 ,6000 ,1.3 +180 ,28.5 ,6000 ,1.3 +185.85 ,28.5 ,6000 ,1.3 +187 ,28.5 ,6000 ,1.3 +200 ,28.4 ,6000 ,1.3 +231 ,28.4 ,6000 ,1.8 +245 ,28.8 ,6000 ,1.8 +262 ,28.8 ,6000 ,1.8 +999 ,28.8 ,6000 ,1.8 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 90b52bd..0a69c97 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,10 @@ package frc4388.robot; +import java.util.ArrayList; +import java.util.Vector; +import java.util.function.Consumer; + import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; @@ -26,13 +30,20 @@ public final class Constants { public enum Mode { COMPETITIVE, CASUAL; private static Mode mode; + private static Vector> changeHandlers = new Vector<>(); + public static void register(Consumer changeHandler) { + changeHandlers.add(changeHandler); + } public static Mode get() { return mode; } public static void set(Mode mode) { Mode.mode = mode; int i = mode.ordinal(); + // changeHandlers.forEach(c -> c.accept(mode)); 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; @@ -184,7 +195,8 @@ public final class Constants { public static final class IntakeConstants { public static final double EXTENDER_SPEED = 0.3; - public static final double INTAKE_SPEED = 1.0; + 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; @@ -194,7 +206,8 @@ public final class Constants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 0.75; + 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 */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index eccdd89..c44e451 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -158,54 +158,66 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - /* 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); - - configureButtonBindings(); - - /* 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)); - + setControls(Mode.COMPETITIVE); + Constants.Mode.register(this::setControls); } + public void setControls(Mode mode) { + 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("CAS CONTROLS"); + break; + } + } + /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() { + private void configureButtonBindingsCompetitive() { /* Test Buttons */ // A driver test button diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 0256ed0..ba350ba 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -23,6 +23,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; import frc4388.utility.Trims; @@ -36,7 +37,11 @@ public class Shooter extends SubsystemBase { public Double distance, hoodExt, drumVelocity, centerDisplacement; } - private ShooterTableEntry[] m_shooterTable; + private ShooterTableEntry[][] m_shooterTables; + + public ShooterTableEntry[] getActiveShooterTable() { + return m_shooterTables[Constants.Mode.get().ordinal()]; + } public boolean m_isDrumReady = false; public double m_fireVel; @@ -84,20 +89,23 @@ public class Shooter extends SubsystemBase { m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterTable = new ShooterTableEntry[] {new ShooterTableEntry()}; - // try { - // m_shooterTable = new CSV<>(ShooterTableEntry::new) { - // private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + try { + CSV csv = new CSV<>(ShooterTableEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); - // @Override - // protected String headerSanitizer(final String header) { - // return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); - // } - // }.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); - // new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable))).start(); - // } catch (final IOException e) { - // throw new RuntimeException(e); - // } + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + }; + m_shooterTables = new ShooterTableEntry[][] { + 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(); + } catch (final IOException e) { + throw new RuntimeException(e); + } } @Override @@ -159,15 +167,15 @@ public class Shooter extends SubsystemBase { } public Double getCenterDisplacement(final Double distance) { - return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.centerDisplacement).doubleValue(); + return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.centerDisplacement).doubleValue(); } public Double getVelocity(final Double distance) { - return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); + return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } public Double getHood(final Double distance) { - return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); + return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) {