102 lines
3.7 KiB
Java
102 lines
3.7 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 frc.robot;
|
|
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
|
import com.pathplanner.lib.path.PathPlannerPath;
|
|
import com.pathplanner.lib.path.PathPlannerTrajectory;
|
|
import com.pathplanner.lib.util.PathPlannerLogging;
|
|
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
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.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import frc.robot.constants.OIConstants;
|
|
import frc.robot.subsystems.Drivetrain;
|
|
import frc.robot.utilities.PoseSendable;
|
|
|
|
/*
|
|
* The objective for this part of testing is to bring autonomous back into the mix, by this point, we should be able
|
|
* to manually set a pose to the estimator from part 2 and have field oriented be accurate. We need to be able to set the robot
|
|
* up for a simple auto, run a practice match, have the robot do the simple auto, and when we hit teleoperated have the robot
|
|
* behavior normally, field oriented style.
|
|
*
|
|
* We should try to solve in this branch:
|
|
* - Does Field Oriented Drive work post autonomous?
|
|
*
|
|
* If this works, then this is how the code in the master branch needs to be structured in order for everything to work. We'll need
|
|
* to test more from there to make sure the other autos still work as intended.
|
|
*/
|
|
public class RobotContainer {
|
|
private Drivetrain drivetrain;
|
|
|
|
private CommandXboxController driver;
|
|
|
|
private PoseSendable poseSendable;
|
|
|
|
private SendableChooser<Command> autoChooser;
|
|
|
|
public RobotContainer() {
|
|
drivetrain = new Drivetrain(new Pose2d());
|
|
|
|
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
|
|
|
|
poseSendable = new PoseSendable();
|
|
|
|
autoChooser = AutoBuilder.buildAutoChooser();
|
|
|
|
configureBindings();
|
|
shuffleboardConfig();
|
|
}
|
|
|
|
private void configureBindings() {
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.teleopCommand(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
() -> { return -driver.getRightX(); },
|
|
OIConstants.kTeleopDriveDeadband
|
|
)
|
|
);
|
|
|
|
driver.a().onTrue(drivetrain.zeroHeadingCommand());
|
|
|
|
driver.b().onTrue(
|
|
new InstantCommand(() -> {
|
|
System.out.println("X: " + poseSendable.getX());
|
|
System.out.println("Y: " + poseSendable.getY());
|
|
System.out.println("Rotation: " + poseSendable.getRotation());
|
|
drivetrain.resetOdometry(poseSendable.getPose());
|
|
}
|
|
));
|
|
}
|
|
|
|
private void shuffleboardConfig() {
|
|
ShuffleboardTab tab = Shuffleboard.getTab("Testing");
|
|
tab.add("Drivetrain Pose", poseSendable)
|
|
.withPosition(0, 0)
|
|
.withSize(2, 3);
|
|
|
|
tab.add("Auto Selector", autoChooser)
|
|
.withPosition(2, 0)
|
|
.withSize(2, 1);
|
|
|
|
tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
|
|
.withPosition(2, 1)
|
|
.withSize(2, 1)
|
|
.withWidget(BuiltInWidgets.kTextView);
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
|
|
|
|
}
|
|
}
|