diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 122243f..c041969 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,12 +14,16 @@ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Manipulator; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class RobotContainer { @@ -40,6 +44,8 @@ public class RobotContainer { private CommandXboxController driver; private CommandXboxController operator; + private SendableChooser autoChooser; + public RobotContainer() { arm = new Arm(); @@ -58,8 +64,12 @@ public class RobotContainer { driver = new CommandXboxController(OIConstants.kDriverControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort); + autoChooser = AutoBuilder.buildAutoChooser(); + configureButtonBindings(); + configureNamedCommands(); + configureShuffleboard(); } @@ -148,10 +158,22 @@ public class RobotContainer { ); } + private void configureNamedCommands() { + NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); + } + //creates tabs and transforms them on the shuffleboard private void configureShuffleboard() { + ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab); ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab); + Shuffleboard.selectTab(OIConstants.kAutoTab); + + autoTab.add("Auto Selection", autoChooser) + .withSize(2, 1) + .withPosition(0, 0) + .withWidget(BuiltInWidgets.kComboBoxChooser); + sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition) .withSize(2, 1) .withPosition(0, 0) @@ -164,7 +186,7 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return new PrintCommand("NO AUTO DEFINED"); + return autoChooser.getSelected(); } //teleop routines diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 9a99510..0bfbc31 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -1,5 +1,13 @@ package frc.robot.constants; +import java.io.IOException; + +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { @@ -15,4 +23,21 @@ public class AutoConstants { // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + + // TODO This is a constant being managed like a static rewriteable variable + public static RobotConfig kRobotConfig; + + public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController( + new PIDConstants(kPXController, 0, 0), + new PIDConstants(kPYController, 0, 0) + ); + + static { + try { + kRobotConfig = RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?"); + e.printStackTrace(); + } + } } diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index e1d484b..7e22167 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -6,5 +6,6 @@ public class OIConstants { public static final double kDriveDeadband = 0.05; - public static final String kSensorsTab = "SensorsTab"; + public static final String kAutoTab = "Auto Tab"; + public static final String kSensorsTab = "Sensors Tab"; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6a6264a..0e4689e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,9 +4,11 @@ package frc.robot.subsystems; +import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import com.pathplanner.lib.auto.AutoBuilder; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; @@ -18,10 +20,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; @@ -75,6 +77,23 @@ public class Drivetrain extends SubsystemBase { m_rearLeft.getPosition(), m_rearRight.getPosition() }); + + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getCurrentChassisSpeeds, + this::driveWithChassisSpeeds, + AutoConstants.kPPDriveController, + AutoConstants.kRobotConfig, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + ); } @Override @@ -90,6 +109,23 @@ public class Drivetrain extends SubsystemBase { }); } + public ChassisSpeeds getCurrentChassisSpeeds() { + return DrivetrainConstants.kDriveKinematics.toChassisSpeeds( + m_frontLeft.getState(), + m_frontRight.getState(), + m_rearLeft.getState(), + m_rearRight.getState() + ); + } + + public void driveWithChassisSpeeds(ChassisSpeeds speeds) { + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2); + SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); + + setModuleStates(newStates); + } + /** * Returns the currently-estimated pose of the robot. *