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;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -20,7 +17,6 @@ import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
/**
@@ -35,14 +31,13 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
/* 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_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -52,13 +47,13 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() ->
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true)
, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
}
@@ -69,14 +64,12 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Driver Buttons */
// * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
@@ -86,18 +79,13 @@ public class RobotContainer {
() -> getDeadbandedDriverController().getLeftX(),
() -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY()
))
() -> getDeadbandedDriverController().getRightY()))
.onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive));
// /* Operator Buttons */
// // interrupt button
// new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand());
// * Operator Buttons
}
/**
@@ -106,17 +94,9 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return new InstantCommand();
}
/**
* Add your docs here.
*/
// public IHandController getDriverController() {
// return m_driverXbox;
// }
public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox;
}
@@ -124,25 +104,4 @@ public class RobotContainer {
public DeadbandedXboxController getDeadbandedOperatorController() {
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.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.Scanner;
import java.util.function.Supplier;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.UtilityStructs.TimedOutput;
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 Scanner input;
private final ArrayList<TimedOutput> outputs;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
@@ -42,10 +25,7 @@ public class JoystickPlayback extends CommandBase {
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
outputs = new ArrayList<>();
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.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase {
SwerveDrive swerve;
public final SwerveDrive swerve;
Supplier<Double> leftXSupplier;
Supplier<Double> leftYSupplier;
Supplier<Double> rightXSupplier;
Supplier<Double> rightYSupplier;
ArrayList<Object[]> outputs;
private long startTime;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftXSupplier, Supplier<Double> leftYSupplier, Supplier<Double> rightXSupplier, Supplier<Double> rightYSupplier) {
// Use addRequirements() here to declare subsystem dependencies.
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY)
{
this.swerve = swerve;
this.leftXSupplier = leftXSupplier;
this.leftYSupplier = leftYSupplier;
this.rightXSupplier = rightXSupplier;
this.rightYSupplier = rightYSupplier;
this.outputs = new ArrayList<Object[]>();
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
addRequirements(this.swerve);
}
@@ -47,21 +44,24 @@ public class JoystickRecorder extends CommandBase {
public void initialize() {
this.startTime = System.currentTimeMillis();
// timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0});
outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0});
System.out.println("STARTING RECORDING");
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
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);
swerve.driveWithInput(new Translation2d((double) inputs[0], (double) inputs[1]), new Translation2d((double) inputs[2], (double) inputs[3]), true);
System.out.println("RECORDING");
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
}
// Called once the command ends or is interrupted.
@@ -69,18 +69,17 @@ public class JoystickRecorder extends CommandBase {
public void end(boolean interrupted) {
File output = new File("/home/lvuser/JoystickInputs.txt");
try(PrintWriter writer = new PrintWriter(output)) {
for(Object[] input : outputs) {
writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3] + "," + input[4]);
try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," +
input.rightX + "," + input.rightY + "," +
input.timedOffset);
}
writer.close();
} catch(IOException e) {
} catch (IOException e) {
e.printStackTrace();
}
System.out.println("STOPPED RECORDING");
}
// Returns true when the command should end.