Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add more mode constants

Browse Source 
- Control modes not working right now
This commit is contained in:
aborunda
2021-11-04 17:59:45 -06:00
parent 1503c7f027
commit c594f7d690
4 changed files with 108 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+15 -2
View File
@@ -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<Consumer<Mode>> changeHandlers = new Vector<>();
public static void register(Consumer<Mode> 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 */
src/main/java/frc4388/robot/RobotContainer.java
+52 -40
View File
@@ -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
src/main/java/frc4388/robot/subsystems/Shooter.java
+25 -17
View File
@@ -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<ShooterTableEntry> 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 <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {