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

gAtEkEePiNg

Browse Source
This commit is contained in:
aarav18
2022-03-18 14:09:25 -06:00
parent 556bbb32b4
commit 1d9940f81b
3 changed files with 114 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2
View File
@@ -174,6 +174,8 @@ public final class Constants {
public static final int ELBOW_ID = 31; public static final int ELBOW_ID = 31;
public static final int GYRO_ID = 14; public static final int GYRO_ID = 14;
public static final double INPUT_MULTIPLIER = 0.7;
// TODO Update this stuff too // TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
public static final double LOWER_ARM_LENGTH = 27; public static final double LOWER_ARM_LENGTH = 27;
src/main/java/frc4388/robot/RobotContainer.java
+106 -105
View File
@@ -22,6 +22,7 @@ import java.util.Comparator;
import java.util.List; import java.util.List;
import java.util.Objects; import java.util.Objects;
import java.util.Optional; import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.Function; import java.util.function.Function;
import java.util.logging.Level; import java.util.logging.Level;
import java.util.logging.Logger; import java.util.logging.Logger;
@@ -64,9 +65,6 @@ import frc4388.robot.subsystems.Claws;
import frc4388.robot.commands.RunClaw; import frc4388.robot.commands.RunClaw;
import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.ClimberRewrite;
import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.subsystems.Claws.ClawType;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.StorageConstants;
@@ -113,7 +111,7 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap(); public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
@@ -130,8 +128,8 @@ public class RobotContainer {
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); // private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); // private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
//public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
// Controllers // Controllers
@@ -156,19 +154,15 @@ public class RobotContainer {
public static boolean softLimits = true; public static boolean softLimits = true;
public static enum CurrentMode {TURRET, CLIMBER};
public CurrentMode currentMode = CurrentMode.TURRET;
/** /**
* The container for the robot. Contains subsystems, OI devices, and commands. * The container for the robot. Contains subsystems, OI devices, and commands.
*/ */
public RobotContainer() { public RobotContainer() {
configureButtonBindings(); configureButtonBindings();
/* Default Commands */ /* Default Commands */
// moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.7, getOperatorController().getRightY() * 0.7),
m_robotClimber));
// IK command // IK command
// m_robotClimber.setDefaultCommand( // m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
@@ -196,21 +190,28 @@ public class RobotContainer {
// m_robotBoomBoom, // m_robotBoomBoom,
// m_robotTurret).withName("Storage ManageStorage defaultCommand")); // m_robotTurret).withName("Storage ManageStorage defaultCommand"));
// m_robotClimber.setDefaultCommand( // Storage Management
// new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) // m_robotStorage.setDefaultCommand(
// ); // new RunCommand(() -> m_robotStorage.manageStorage(),
// Storage Management // m_robotStorage).withName("Storage manageStorage defaultCommand"));
/*m_robotStorage.setDefaultCommand(
new RunCommand(() -> m_robotStorage.manageStorage(),
m_robotStorage).withName("Storage manageStorage defaultCommand"));*/
// Serializer Management // Serializer Management
m_robotSerializer.setDefaultCommand( m_robotSerializer.setDefaultCommand(
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Turret Manual
// Turret Manual
m_robotTurret.setDefaultCommand( m_robotTurret.setDefaultCommand(
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)
m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); .until(() -> this.currentMode.equals(CurrentMode.CLIMBER)));
// .withName("Turret runTurretWithInput defaultCommand");
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(getDriverController().getLeftY(), getDriverController().getRightY()), m_robotClimber)
.until(() -> this.currentMode.equals(CurrentMode.TURRET)));
// EnableTurret()
m_robotHood.setDefaultCommand( m_robotHood.setDefaultCommand(
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
@@ -247,10 +248,6 @@ public class RobotContainer {
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
/* Operator Buttons */ /* Operator Buttons */
new JoystickButton(getOperatorController(), XboxController.Button.kY.value) new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
@@ -262,9 +259,6 @@ public class RobotContainer {
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
@@ -321,15 +315,16 @@ public class RobotContainer {
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
// .whileHeld(new TurretManual(m_robotTurret)); .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET));
// .whenReleased(EnableClimber()));
// control turret manual mode // control turret manual mode
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
// .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); // .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
@@ -339,6 +334,12 @@ public class RobotContainer {
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
} }
//EnableTurret()
//set turret default to joysticks, set climber default to null
//EnableClimber()
//set climber default to joysticks set turret defaults to null
/** /**
* Use this to pass the autonomous command to the main {@link Robot} class. * Use this to pass the autonomous command to the main {@link Robot} class.
* *
@@ -389,18 +390,18 @@ public class RobotContainer {
* *
* @return Odometry * @return Odometry
*/ */
// public Pose2d getOdometry() { public Pose2d getOdometry() {
// return m_robotSwerveDrive.getOdometry(); return m_robotSwerveDrive.getOdometry();
// } }
/** /**
* Set odometry to given pose. * Set odometry to given pose.
* *
* @param pose Pose to set odometry to. * @param pose Pose to set odometry to.
*/ */
// public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
// m_robotSwerveDrive.resetOdometry(pose); m_robotSwerveDrive.resetOdometry(pose);
// } }
/** /**
* Creates a WatchKey for the path planner directory and registers it with the * Creates a WatchKey for the path planner directory and registers it with the
@@ -409,26 +410,26 @@ public class RobotContainer {
* latest path files. * latest path files.
* Finally, adds the existing path files to the auto chooser * Finally, adds the existing path files to the auto chooser
*/ */
// private void autoInit() { private void autoInit() {
// try { // try {
// WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
// StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
// StandardWatchEventKinds.ENTRY_DELETE); // StandardWatchEventKinds.ENTRY_DELETE);
// // TODO: Store this and other commands as fields so they can be rescheduled. // // TODO: Store this and other commands as fields so they can be rescheduled.
// new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { // new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
// @Override // @Override
// public boolean runsWhenDisabled() { // public boolean runsWhenDisabled() {
// return true; // return true;
// } // }
// }.withName("Path Watcher").schedule(); // }.withName("Path Watcher").schedule();
// } catch (IOException exception) { // } catch (IOException exception) {
// LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); // LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
// } // }
// Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) // Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles())
// .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
// .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
// SmartDashboard.putData("Auto Chooser", autoChooser); // SmartDashboard.putData("Auto Chooser", autoChooser);
// } }
/** /**
* Creates a button on the SmartDashboard that will record the path of the * Creates a button on the SmartDashboard that will record the path of the
@@ -458,35 +459,35 @@ public class RobotContainer {
* @param watchKey The WatchKey that is being observed. * @param watchKey The WatchKey that is being observed.
*/ */
private void updateAutoChooser(WatchKey watchKey) { private void updateAutoChooser(WatchKey watchKey) {
List<WatchEvent<?>> watchEvents = watchKey.pollEvents(); // List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
if (!watchEvents.isEmpty()) { // if (!watchEvents.isEmpty()) {
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream() // List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
.filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); // .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
for (WatchEvent<?> pathWatchEvent : pathWatchEvents) { // for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
Path watchEventPath = (Path) pathWatchEvent.context(); // Path watchEventPath = (Path) pathWatchEvent.context();
File watchEventFile = watchEventPath.toFile(); // File watchEventFile = watchEventPath.toFile();
String watchEventFileName = watchEventFile.getName(); // String watchEventFileName = watchEventFile.getName();
if (watchEventFileName.endsWith(".path")) { // if (watchEventFileName.endsWith(".path")) {
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { // if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", // LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
watchEventFileName); // watchEventFileName);
autoChooser.addOption(watchEventFile.getName(), watchEventFile); // autoChooser.addOption(watchEventFile.getName(), watchEventFile);
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); // LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
if (watchEventFileName.equals(autoChooser.getSelected().getName())) { // if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); // LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
loadPath(watchEventFileName); // loadPath(watchEventFileName);
} // }
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
LOGGER.log(Level.SEVERE, // LOGGER.log(Level.SEVERE,
"PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", // "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
watchEventFileName); // watchEventFileName);
} // }
} // }
} // }
} // }
if (!watchKey.reset()) // if (!watchKey.reset())
LOGGER.severe("File watch key invalid."); // LOGGER.severe("File watch key invalid.");
} }
private void loadPath(String pathName) { private void loadPath(String pathName) {
@@ -498,22 +499,22 @@ public class RobotContainer {
} }
private void saveRecording() { private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos. // // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = PATHPLANNER_DIRECTORY // File outputFile = PATHPLANNER_DIRECTORY
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); // .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); // LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { // if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// TODO: Change to use measured maximum velocity and acceleration. // // TODO: Change to use measured maximum velocity and acceleration.
var path = createPath(null, null, false); // var path = createPath(null, null, false);
if (RobotBase.isReal()) // if (RobotBase.isReal())
path.write(outputFile); // path.write(outputFile);
StringWriter writer = new StringWriter(); // StringWriter writer = new StringWriter();
path.write(writer); // path.write(writer);
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); // recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
autoChooser.setDefaultOption(outputFile.getName(), outputFile); // autoChooser.setDefaultOption(outputFile.getName(), outputFile);
LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); // LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
} else // } else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); // LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
} }
public void recordPeriodic() { public void recordPeriodic() {
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+6 -2
View File
@@ -128,8 +128,8 @@ public class ClimberRewrite extends SubsystemBase {
} }
public void setMotors(double shoulderOutput, double elbowOutput) { public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput); m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER);
m_elbow.set(elbowOutput); m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);
} }
public double[] getJointAngles() { public double[] getJointAngles() {
@@ -282,4 +282,8 @@ public class ClimberRewrite extends SubsystemBase {
public static Point getClimberPosition(double[] jointAngles) { public static Point getClimberPosition(double[] jointAngles) {
return getClimberPosition(jointAngles[0], jointAngles[1]); return getClimberPosition(jointAngles[0], jointAngles[1]);
} }
public double getCurrent() {
return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent());
}
} }