Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -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/deploy/Robot Data - Distances Casual.csv
+16
View File
@@ -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
1 Distance (in) Hood Ext. (u) Drum Velocity (u/ds) Center Displacement (deg)
2 0 16 6000 1
3 65.9 16 6000 1
4 103 19 6000 1
5 126.6 20.28 6000 1.5
6 156.6 28 6000 1.5
7 174 28 6000 1.5
8 178 28 6000 1.3
9 180 28.5 6000 1.3
10 185.85 28.5 6000 1.3
11 187 28.5 6000 1.3
12 200 28.4 6000 1.3
13 231 28.4 6000 1.8
14 245 28.8 6000 1.8
15 262 28.8 6000 1.8
16 999 28.8 6000 1.8
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) {