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

autos better + drive for time + rotate until target

Browse Source
This commit is contained in:
aarav18
2022-03-21 22:49:54 -06:00
parent 4c5106d14f
commit e83c1e5cf8
11 changed files with 184 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+33 -10
View File
@@ -46,6 +46,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Filesystem;
@@ -59,6 +61,7 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws;
@@ -71,7 +74,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ClimberCommands.RunClaw;
import frc4388.robot.commands.ClimberCommands.RunClimberPath;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
import frc4388.robot.commands.ShooterCommands.Seek;
@@ -97,6 +101,7 @@ import frc4388.utility.PathPlannerUtil;
import frc4388.utility.Vector2D;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedRawXboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -128,8 +133,8 @@ public class RobotContainer {
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
// Autonomous
@@ -371,8 +376,11 @@ public class RobotContainer {
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))));
PathPlannerState initState = traj.getInitialState();
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
@@ -383,12 +391,13 @@ public class RobotContainer {
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
@@ -421,6 +430,13 @@ public class RobotContainer {
// }).withName("No Autonomous Path");
// }
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
loadPath("Move Forward");
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
@@ -429,8 +445,16 @@ public class RobotContainer {
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
// // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
// "Diamond"));
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
}
public static XboxController getDriverController() {
@@ -563,7 +587,7 @@ public class RobotContainer {
}
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
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
@@ -584,8 +608,7 @@ public class RobotContainer {
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
// * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
m_robotSwerveDrive.getChassisSpeeds()[1]);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,