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

Updated Auto recording

Browse Source
This commit is contained in:
Abhi
2023-02-14 19:00:09 -07:00
parent 9e9652faae
commit 603be6538b
4 changed files with 61 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+15 -56
View File
@@ -7,9 +7,6 @@
package frc4388.robot; package frc4388.robot;
import javax.print.attribute.standard.OrientationRequested;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -20,7 +17,6 @@ import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxController;
/** /**
@@ -35,14 +31,13 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap(); public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
// private final LED m_robotLED = new LED(m_robotMap.LEDController); m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
/* Controllers */ /* Controllers */
// private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
// private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -52,13 +47,13 @@ public class RobotContainer {
public RobotContainer() { public RobotContainer() {
configureButtonBindings(); configureButtonBindings();
/* Default Commands */ // * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) getDeadbandedDriverController().getRight(),
, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); true);
}, m_robotSwerveDrive)
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); .withName("SwerveDrive DefaultCommand"));
} }
@@ -69,14 +64,12 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/ */
private void configureButtonBindings() { private void configureButtonBindings() {
/* Driver Buttons */ // * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
@@ -86,18 +79,13 @@ public class RobotContainer {
() -> getDeadbandedDriverController().getLeftX(), () -> getDeadbandedDriverController().getLeftX(),
() -> getDeadbandedDriverController().getLeftY(), () -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(), () -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY() () -> getDeadbandedDriverController().getRightY()))
))
.onFalse(new InstantCommand()); .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive)); .onTrue(new JoystickPlayback(m_robotSwerveDrive));
// /* Operator Buttons */ // * Operator Buttons
// // interrupt button
// new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand());
} }
/** /**
@@ -106,17 +94,9 @@ public class RobotContainer {
* @return the command to run in autonomous * @return the command to run in autonomous
*/ */
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
return new InstantCommand(); return new InstantCommand();
} }
/**
* Add your docs here.
*/
// public IHandController getDriverController() {
// return m_driverXbox;
// }
public DeadbandedXboxController getDeadbandedDriverController() { public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox; return this.m_driverXbox;
} }
@@ -124,25 +104,4 @@ public class RobotContainer {
public DeadbandedXboxController getDeadbandedOperatorController() { public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox; return this.m_operatorXbox;
} }
/**
* Add your docs here.
*/
// public IHandController getOperatorController() {
// return m_operatorXbox;
// }
/**
* Add your docs here.
*/
// public Joystick getOperatorJoystick() {
// return m_operatorXbox.getJoyStick();
// }
/**
* Add your docs here.
*/
// public Joystick getDriverJoystick() {
// return m_driverXbox.getJoyStick();
// }
} }
src/main/java/frc4388/robot/commands/JoystickPlayback.java
+2 -22
View File
@@ -6,34 +6,17 @@ package frc4388.robot.commands;
import java.io.File; import java.io.File;
import java.io.FileNotFoundException; import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.PrintWriter;
import java.nio.file.Files;
import java.nio.file.Path;
import java.sql.Time;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Scanner; import java.util.Scanner;
import java.util.function.Supplier;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase { public class JoystickPlayback extends CommandBase {
private static class TimedOutput {
public double leftX = 0.0;
public double leftY = 0.0;
public double rightX = 0.0;
public double rightY = 0.0;
public long timedOffset = 0;
}
private final SwerveDrive swerve; private final SwerveDrive swerve;
private Scanner input; private Scanner input;
private final ArrayList<TimedOutput> outputs; private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0; private int counter = 0;
private long startTime = 0; private long startTime = 0;
private long playbackTime = 0; private long playbackTime = 0;
@@ -42,10 +25,7 @@ public class JoystickPlayback extends CommandBase {
/** Creates a new JoystickPlayback. */ /** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve) { public JoystickPlayback(SwerveDrive swerve) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve; this.swerve = swerve;
outputs = new ArrayList<>();
addRequirements(this.swerve); addRequirements(this.swerve);
} }
src/main/java/frc4388/robot/commands/JoystickRecorder.java
+32 -33
View File
@@ -13,31 +13,28 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase { public class JoystickRecorder extends CommandBase {
public final SwerveDrive swerve;
SwerveDrive swerve;
Supplier<Double> leftXSupplier; public final Supplier<Double> leftX;
Supplier<Double> leftYSupplier; public final Supplier<Double> leftY;
Supplier<Double> rightXSupplier; public final Supplier<Double> rightX;
Supplier<Double> rightYSupplier; public final Supplier<Double> rightY;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
ArrayList<Object[]> outputs; private long startTime = -1;
private long startTime;
/** Creates a new JoystickRecorder. */ /** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftXSupplier, Supplier<Double> leftYSupplier, Supplier<Double> rightXSupplier, Supplier<Double> rightYSupplier) { public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
// Use addRequirements() here to declare subsystem dependencies. Supplier<Double> rightX, Supplier<Double> rightY)
{
this.swerve = swerve; this.swerve = swerve;
this.leftXSupplier = leftXSupplier; this.leftX = leftX;
this.leftYSupplier = leftYSupplier; this.leftY = leftY;
this.rightXSupplier = rightXSupplier; this.rightX = rightX;
this.rightYSupplier = rightYSupplier; this.rightY = rightY;
this.outputs = new ArrayList<Object[]>();
addRequirements(this.swerve); addRequirements(this.swerve);
} }
@@ -47,21 +44,24 @@ public class JoystickRecorder extends CommandBase {
public void initialize() { public void initialize() {
this.startTime = System.currentTimeMillis(); this.startTime = System.currentTimeMillis();
// timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); outputs.add(new TimedOutput());
outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0});
System.out.println("STARTING RECORDING");
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
Object[] inputs = new Object[] {(double) leftXSupplier.get(), (double) leftYSupplier.get(), (double) rightXSupplier.get(), (double) rightYSupplier.get(), (long) (System.currentTimeMillis() - startTime)}; var inputs = new TimedOutput();
inputs.leftX = leftX.get();
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs); outputs.add(inputs);
swerve.driveWithInput(new Translation2d((double) inputs[0], (double) inputs[1]), new Translation2d((double) inputs[2], (double) inputs[3]), true); swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
System.out.println("RECORDING"); true);
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@@ -69,18 +69,17 @@ public class JoystickRecorder extends CommandBase {
public void end(boolean interrupted) { public void end(boolean interrupted) {
File output = new File("/home/lvuser/JoystickInputs.txt"); File output = new File("/home/lvuser/JoystickInputs.txt");
try(PrintWriter writer = new PrintWriter(output)) { try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
for(Object[] input : outputs) { writer.println( input.leftX + "," + input.leftY + "," +
writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3] + "," + input[4]); input.rightX + "," + input.rightY + "," +
input.timedOffset);
} }
writer.close(); writer.close();
} catch(IOException e) { } catch (IOException e) {
e.printStackTrace(); e.printStackTrace();
} }
System.out.println("STOPPED RECORDING");
} }
// Returns true when the command should end. // Returns true when the command should end.
src/main/java/frc4388/utility/UtilityStructs.java
+12
View File
@@ -0,0 +1,12 @@
package frc4388.utility;
public class UtilityStructs {
public static class TimedOutput {
public double leftX = 0.0;
public double leftY = 0.0;
public double rightX = 0.0;
public double rightY = 0.0;
public long timedOffset = 0;
}
}