Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

autoChooser + playback filename

Browse Source
This commit is contained in:
aarav18
2023-02-25 12:51:23 -07:00
parent 22b930d621
commit f6b8c6e3f0
2 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+29 -4
View File
@@ -23,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -62,6 +63,20 @@ public class RobotContainer {
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
private SendableChooser<Command> chooser = new SendableChooser<>();
private Command noAuto = new InstantCommand();
private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt");
private Command blue1PathWithBalance = blue1Path.andThen(balance);
private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt", -1);
private Command red1PathWithBalance = red1Path.andThen(balance);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@@ -75,6 +90,17 @@ public class RobotContainer {
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
// * Auto Commands
chooser.setDefaultOption("NoAuto", noAuto);
chooser.addOption("Blue1Path", blue1Path);
chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
chooser.addOption("Red1Path", red1Path);
chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
SmartDashboard.putData(chooser);
}
@@ -104,8 +130,8 @@ public class RobotContainer {
() -> getDeadbandedDriverController().getRightY()))
.onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive, 1));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1));
// * Operator Buttons
}
@@ -117,8 +143,7 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
return new JoystickPlayback(m_robotSwerveDrive, 1)
.andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
return chooser.getSelected();
// return new InstantCommand();
}
src/main/java/frc4388/robot/commands/JoystickPlayback.java
+13 -5
View File
@@ -15,6 +15,8 @@ import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
@@ -22,15 +24,21 @@ public class JoystickPlayback extends CommandBase {
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
private int m_mult = 1;
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, int mult) {
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
m_mult = mult;
this.filename = filename;
this.mult = mult;
addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
@@ -38,7 +46,7 @@ public class JoystickPlayback extends CommandBase {
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/BlueNearDriveToChargeStation.txt"));
input = new Scanner(new File("/home/lvuser/" + filename));
String line = "";
while (input.hasNextLine()) {
@@ -52,7 +60,7 @@ public class JoystickPlayback extends CommandBase {
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * m_mult;
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);