mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
aarav18
213 lines
8.5 KiB
Java
213 lines
8.5 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc4388.robot;
|
|
|
|
|
|
import java.lang.reflect.Array;
|
|
import java.nio.file.FileSystem;
|
|
import java.util.Arrays;
|
|
import java.util.HashMap;
|
|
import java.util.List;
|
|
|
|
import javax.swing.plaf.nimbus.State;
|
|
|
|
import com.pathplanner.lib.PathPlanner;
|
|
import com.pathplanner.lib.PathPlannerTrajectory;
|
|
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
|
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
|
|
|
import edu.wpi.first.math.controller.HolonomicDriveController;
|
|
import edu.wpi.first.math.controller.PIDController;
|
|
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.TrajectoryConfig;
|
|
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
|
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
|
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
|
|
import edu.wpi.first.wpilibj.Filesystem;
|
|
import edu.wpi.first.wpilibj.XboxController;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
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.LED;
|
|
import frc4388.robot.subsystems.SwerveDrive;
|
|
import frc4388.utility.LEDPatterns;
|
|
import frc4388.utility.controller.DeadbandedXboxController;
|
|
|
|
/**
|
|
* This class is where the bulk of the robot should be declared. Since
|
|
* Command-based is a "declarative" paradigm, very little robot logic should
|
|
* actually be handled in the {@link Robot} periodic methods (other than the
|
|
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
|
* commands, and button mappings) should be declared here.
|
|
*/
|
|
public class RobotContainer {
|
|
/* RobotMap */
|
|
private final RobotMap m_robotMap = new RobotMap();
|
|
|
|
/* Subsystems */
|
|
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
|
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
|
|
|
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
|
|
|
/* Controllers */
|
|
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
|
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
|
|
|
/**
|
|
* The container for the robot. Contains subsystems, OI devices, and commands.
|
|
*/
|
|
public RobotContainer() {
|
|
configureButtonBindings();
|
|
/* Default Commands */
|
|
// drives the swerve drive with a two-axis input from the driver controller
|
|
m_robotSwerveDrive.setDefaultCommand(
|
|
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
|
getDriverController().getLeftX(),
|
|
getDriverController().getLeftY(),
|
|
getDriverController().getRightX(),
|
|
getDriverController().getRightY(),
|
|
true),
|
|
m_robotSwerveDrive));
|
|
|
|
// continually sends updates to the Blinkin LED controller to keep the lights on
|
|
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
|
}
|
|
|
|
/**
|
|
* 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() {
|
|
/* Driver Buttons */
|
|
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
|
|
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
|
|
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
|
|
|
|
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
|
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
|
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
|
|
|
|
|
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
|
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
|
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
|
|
|
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
|
|
|
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
|
//.whenPressed(this::resetOdometry);
|
|
|
|
/* Operator Buttons */
|
|
// activates "Lit Mode"
|
|
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
|
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
|
|
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
|
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
|
}
|
|
|
|
public SequentialCommandGroup runAuto(String path, double maxVel, double maxAccel) {
|
|
PathPlannerTrajectory traj = PathPlanner.loadPath(path, maxVel, maxAccel);
|
|
|
|
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
|
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
|
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
|
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
|
//thetaController.reset(new TrapezoidProfile.State(m_robotSwerveDrive.getOdometry()));
|
|
|
|
PPSwerveControllerCommand ppSCC = new PPSwerveControllerCommand(
|
|
traj,
|
|
m_robotSwerveDrive::getOdometry,
|
|
m_robotSwerveDrive.m_kinematics,
|
|
xController,
|
|
yController,
|
|
thetaController,
|
|
m_robotSwerveDrive::setModuleStates,
|
|
m_robotSwerveDrive);
|
|
|
|
PathPlannerState state = (PathPlannerState) traj.sample(0);
|
|
|
|
return new SequentialCommandGroup(
|
|
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
|
|
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation))),
|
|
//new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(traj.getInitialPose())),
|
|
ppSCC,
|
|
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
|
*
|
|
* @return the command to run in autonomous
|
|
*/
|
|
public Command getAutonomousCommand() {
|
|
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wik
|
|
|
|
// PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0);
|
|
// PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
|
|
// PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
|
|
|
|
// PathPlannerTrajectory ppCurrent = PathPlanner.loadPath("First Test Path", 1.0, 1.0); // change this to change auto
|
|
|
|
// PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
|
// PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
|
// ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
|
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
|
|
|
// PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
|
|
// ppCurrent,
|
|
// m_robotSwerveDrive::getOdometry,
|
|
// m_robotSwerveDrive.m_kinematics,
|
|
// xController,
|
|
// yController,
|
|
// thetaController,
|
|
// m_robotSwerveDrive::setModuleStates,
|
|
// m_robotSwerveDrive
|
|
// );
|
|
|
|
// return new SequentialCommandGroup(
|
|
// new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
|
|
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrent.getInitialPose())),
|
|
// ppSwerveControllerCommand,
|
|
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
|
|
// );
|
|
// return runAuto("Move Forward", 1.0, 1.0);
|
|
return runAuto("Move Down", 1.0, 1.0);
|
|
}
|
|
|
|
/**
|
|
* Add your docs here.
|
|
*/
|
|
public XboxController getDriverController() {
|
|
return m_driverXbox;
|
|
}
|
|
|
|
public Pose2d getOdometry() {
|
|
return m_robotSwerveDrive.getOdometry();
|
|
}
|
|
|
|
public void resetOdometry(Pose2d pose) {
|
|
m_robotSwerveDrive.resetOdometry(pose);
|
|
}
|
|
/**
|
|
* Add your docs here.
|
|
*/
|
|
public XboxController getOperatorController() {
|
|
return m_operatorXbox;
|
|
}
|
|
}
|