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

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
66945
2022-03-20 19:49:31 -06:00
parent 446e671ac6 afc2015412
commit 5fbfc1426f
1 changed files with 58 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+58 -1
View File
@@ -390,6 +390,53 @@ public class RobotContainer {
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
/**
* Generate autonomous
* @param maxVel max velocity for the path (null to override default value of 5.0)
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
* @param inputs strings (paths) or commands you want to run (in order)
* @return array of commands
*/
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// parse input
for (int i=0; i<inputs.length; i++) {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
@@ -416,7 +463,17 @@ public class RobotContainer {
// return new RunCommand(() -> {
// }).withName("No Autonomous Path");
// }
return null;
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
// ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths.
// * return new ParallelCommandGroup(buildAuto(null,
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
return null;
}
public static XboxController getDriverController() {