mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
Merge branch 'virtualController' into shooter-test
This commit is contained in:
Abhishrek05
@@ -83,7 +83,7 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
|
||||
@@ -13,17 +13,17 @@ import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.commands.Autos.PlaybackChooser;
|
||||
import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.JoystickRecorder;
|
||||
//import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
//import frc4388.robot.commands.Swerve.JoystickRecorder;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
import frc4388.robot.commands.Intake.ArmIntakeIn;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
@@ -31,6 +31,7 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
/**
|
||||
@@ -61,7 +62,13 @@ public class RobotContainer {
|
||||
|
||||
/* Controllers */
|
||||
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);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
|
||||
|
||||
|
||||
/* Virtual Controllers */
|
||||
private final VirtualController m_virtualDriver = new VirtualController(0);
|
||||
private final VirtualController m_virtualOperator = new VirtualController(1);
|
||||
|
||||
private Limelight limelight = new Limelight();
|
||||
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
|
||||
@@ -86,9 +93,9 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
/* Autos */
|
||||
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
private Command startLeftMoveRight = new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
|
||||
private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
|
||||
private Command taxi = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
|
||||
private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
|
||||
|
||||
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
|
||||
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
|
||||
@@ -153,7 +160,9 @@ public class RobotContainer {
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
configureButtonBindings();
|
||||
configureVirtualButtonBindings();
|
||||
|
||||
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
CameraServer.startAutomaticCapture();
|
||||
@@ -182,8 +191,6 @@ public class RobotContainer {
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
|
||||
|
||||
|
||||
/* Auto Recording */
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
|
||||
@@ -196,10 +203,22 @@ public class RobotContainer {
|
||||
// "TwoNotePrt1.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
|
||||
.onFalse(new InstantCommand());
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
"2note.auto"))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
"2note.auto",
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
// /* Speed */
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
@@ -247,17 +266,77 @@ public class RobotContainer {
|
||||
.onTrue(i)
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
|
||||
|
||||
//spins up shooter, no wind down
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
|
||||
|
||||
}
|
||||
|
||||
private void configureVirtualButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
|
||||
/* Speed */
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
|
||||
|
||||
/* Operator Buttons */
|
||||
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
|
||||
|
||||
// Override Intake Position encoder: out
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
|
||||
|
||||
// Override Intake Position encoder: in
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
|
||||
|
||||
//Spin Shooter Motors
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
|
||||
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
|
||||
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.onTrue(ejectToShoot)
|
||||
.onFalse(turnOffShoot);
|
||||
|
||||
|
||||
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(i)
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.pidIn()));
|
||||
|
||||
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
//no auto
|
||||
return playbackChooser.getCommand();
|
||||
//no auto
|
||||
return new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
"2note.auto",
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false);
|
||||
//return playbackChooser.getCommand();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -270,4 +349,12 @@ public class RobotContainer {
|
||||
public DeadbandedXboxController getDeadbandedOperatorController() {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualDriverController() {
|
||||
return m_virtualDriver;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualOperatorController() {
|
||||
return m_virtualOperator;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -75,6 +75,7 @@ public class PlaybackChooser {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (var cmd_name : m_commandPool.keySet()) {
|
||||
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
}
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
AUTO file format
|
||||
|
||||
HEADER static size 0x5
|
||||
0x00 BYTE NUM AXES: defualts to 6
|
||||
0x01 BYTE NUM POV: defualts to 1
|
||||
0x02 BYTE NUM CONTROLLERS: defualts to 2
|
||||
0x03 SHORT FRAMES: any value greator or equal than one.
|
||||
|
||||
FRAME PER CONTROLLER: defualt size 0x34
|
||||
0x00 DOUBLE AXES[NUM AXES]
|
||||
0x30 SHORT BUTTONS
|
||||
0x32 SHORT POVs[NUM POV]
|
||||
|
||||
FRAME: size varrys
|
||||
FRAME PER CONTROLLER[NUM CONTROLLERS]
|
||||
INT UNIXTIMESTAMP
|
||||
|
||||
FILE:
|
||||
HEADER
|
||||
FRAME[FRAMES]
|
||||
@@ -102,7 +102,7 @@ public class JoystickPlayback extends Command {
|
||||
return;
|
||||
}
|
||||
lastIndex = i;
|
||||
}
|
||||
} // Why is this done rather than using the variable counter
|
||||
|
||||
TimedOutput lastOut = outputs.get(lastIndex - 1);
|
||||
TimedOutput out = outputs.get(lastIndex);
|
||||
|
||||
@@ -0,0 +1,145 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
|
||||
public class neoJoystickPlayback extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final String filename;
|
||||
private final VirtualController[] controllers;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
private int frame_index = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
private byte m_numAxes = 0;
|
||||
private byte m_numPOVs = 0;
|
||||
private byte m_numControllers = 0;
|
||||
private short m_numFrames = -1;
|
||||
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this.swerve = swerve;
|
||||
this.filename = filename;
|
||||
this.controllers = controllers;
|
||||
this.m_shouldfree = shouldfree;
|
||||
|
||||
if (instantload) loadAuto();
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
|
||||
this(swerve, filename, controllers, true, false);
|
||||
}
|
||||
|
||||
public boolean loadAuto() {
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
||||
return true;
|
||||
}
|
||||
|
||||
m_numAxes = stream.readNBytes(1)[0];
|
||||
m_numPOVs = stream.readNBytes(1)[0];
|
||||
m_numControllers = stream.readNBytes(1)[0];
|
||||
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
|
||||
if (m_numControllers > controllers.length) {
|
||||
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
|
||||
+ " virtual controllers but only " + controllers.length + " were given");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_numFrames; i++) {
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
for (int j = 0; j < m_numControllers; j++) {
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = new double[m_numAxes];
|
||||
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
|
||||
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
}
|
||||
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
short[] POV = new short[m_numPOVs];
|
||||
for (int k = 0; k < m_numPOVs; k++) {
|
||||
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
}
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[j] = controllerFrame;
|
||||
}
|
||||
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
|
||||
frames.add(frame);
|
||||
}
|
||||
|
||||
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
|
||||
return true;
|
||||
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
public void unloadAuto() {
|
||||
System.out.println("AUTOPLAYBACK: Auto unloaded");
|
||||
frames.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
frame_index = 0;
|
||||
|
||||
m_finished = !loadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if (frame_index >= m_numFrames) m_finished = true;
|
||||
if (m_finished) return;
|
||||
|
||||
// if (frame_index == 0) {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
// } else {
|
||||
// playbackTime = System.currentTimeMillis() - startTime;
|
||||
// }
|
||||
|
||||
AutoRecordingFrame frame = frames.get(frame_index);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
|
||||
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
|
||||
if (i == 0) {
|
||||
this.swerve.driveWithInput(
|
||||
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
|
||||
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
|
||||
true);
|
||||
}
|
||||
}
|
||||
frame_index++;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (VirtualController controller : controllers) controller.zeroControls();
|
||||
swerve.stopModules();
|
||||
if (m_shouldfree) frames.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
public class neoJoystickRecorder extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final XboxController[] controllers;
|
||||
private final String filename;
|
||||
private long startTime = -1;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
|
||||
this.swerve = swerve;
|
||||
this.controllers = controllers;
|
||||
this.filename = filename;
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
frames.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
|
||||
frames.add(frame);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
System.out.println("AUTORECORD: RECORDING");
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
XboxController controller = controllers[i];
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = {controller.getLeftX(), controller.getLeftY(),
|
||||
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
|
||||
controller.getRightX(), controller.getRightY()};
|
||||
short button = 0;
|
||||
for (int j = 0; j < 10; j++)
|
||||
if (controller.getRawButton(j+1))
|
||||
button |= 1 << j;
|
||||
short[] POV = {(short)(controller.getPOV())};
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[i] = controllerFrame;
|
||||
}
|
||||
|
||||
frames.add(frame);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
|
||||
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
|
||||
true); // Really jank way of doing this.
|
||||
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
||||
// header: size of 0x5
|
||||
// byte Number of axes per controller
|
||||
// byte Number of POVs per controller
|
||||
// byte Number of controllers
|
||||
// short Number of frames
|
||||
stream.write(new byte[]{6, 1, (byte) controllers.length});
|
||||
stream.write(DataUtils.shortToByteArray((short) frames.size()));
|
||||
|
||||
// frame
|
||||
// controller frame * number of controllers
|
||||
// int unix time stamp.
|
||||
for (AutoRecordingFrame frame : frames) {
|
||||
// controller frame
|
||||
// double axis * Number of axes per controller
|
||||
// short button states
|
||||
// short POV * Number of POVs per controller
|
||||
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
|
||||
for (double axis: controllerFrame.axes) {
|
||||
stream.write(DataUtils.doubleToByteArray(axis));
|
||||
}
|
||||
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
|
||||
for (short POV: controllerFrame.POV) {
|
||||
stream.write(DataUtils.shortToByteArray(POV));
|
||||
}
|
||||
}
|
||||
stream.write(DataUtils.intToByteArray(frame.timeStamp));
|
||||
}
|
||||
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
@@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
@@ -55,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1));
|
||||
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
public class DataUtils {
|
||||
public static byte[] doubleToByteArray(double value) {
|
||||
byte[] bytes = new byte[8];
|
||||
ByteBuffer.wrap(bytes).putDouble(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static double byteArrayToDouble(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getDouble();
|
||||
}
|
||||
|
||||
public static byte[] intToByteArray(int value) {
|
||||
byte[] bytes = new byte[4];
|
||||
ByteBuffer.wrap(bytes).putInt(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static int byteArrayToInt(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getInt();
|
||||
}
|
||||
|
||||
public static byte[] shortToByteArray(short value) {
|
||||
byte[] bytes = new byte[2];
|
||||
ByteBuffer.wrap(bytes).putShort(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static short byteArrayToShort(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getShort();
|
||||
}
|
||||
}
|
||||
@@ -13,4 +13,14 @@ public class UtilityStructs {
|
||||
|
||||
public long timedOffset = 0;
|
||||
}
|
||||
public static class AutoRecordingControllerFrame {
|
||||
public double[] axes = new double[6];
|
||||
public short button = 0;
|
||||
public short[] POV = new short[1];
|
||||
|
||||
}
|
||||
public static class AutoRecordingFrame {
|
||||
public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2];
|
||||
public int timeStamp;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.utility.configurable;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class ConfigurableDouble {
|
||||
private double defualtValue;
|
||||
private String name;
|
||||
|
||||
/**
|
||||
* Creates an new ConfigurableDouble through Smart Dashboard.
|
||||
* @param name the name of the Smart Dashboard key.
|
||||
* @param defualtValue the initilization value
|
||||
*/
|
||||
public ConfigurableDouble(String name, double defualtValue) {
|
||||
this.name = name;
|
||||
this.defualtValue = defualtValue;
|
||||
SmartDashboard.putNumber(name, defualtValue);
|
||||
}
|
||||
|
||||
public double get() {
|
||||
return SmartDashboard.getNumber(name, defualtValue);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.utility.configurable;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class ConfigurableString {
|
||||
private String defualtValue;
|
||||
private String name;
|
||||
|
||||
/**
|
||||
* Creates an new ConfigurableString through Smart Dashboard.
|
||||
* @param name the name of the Smart Dashboard key.
|
||||
* @param defualtValue the initilization value
|
||||
*/
|
||||
public ConfigurableString(String name, String defualtValue) {
|
||||
this.name = name;
|
||||
this.defualtValue = defualtValue;
|
||||
SmartDashboard.putString(name, defualtValue);
|
||||
}
|
||||
|
||||
public String get() {
|
||||
return SmartDashboard.getString(name, defualtValue);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
public class VirtualController extends GenericHID {
|
||||
private short m_buttonStates = 0;
|
||||
private short m_buttonStatesLastFrame = 0;
|
||||
private double[] m_axes = new double[6];
|
||||
private short[] m_pov = new short[1];
|
||||
|
||||
public VirtualController(int port) {
|
||||
super(port);
|
||||
}
|
||||
|
||||
public void setFrame(double[] axes, short buttonFlags, short[] pov) {
|
||||
m_axes = axes;
|
||||
setOutputs(buttonFlags);
|
||||
m_pov = pov;
|
||||
}
|
||||
|
||||
public void zeroControls() {
|
||||
m_axes = new double[6];
|
||||
m_buttonStates = 0;
|
||||
m_buttonStatesLastFrame = 0;
|
||||
m_pov = new short[1];
|
||||
}
|
||||
|
||||
public static boolean getFlag(int value, int index) {
|
||||
return ((value & 1 << index) != 0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButton(int button) { // man why are buttons indexed at 1.
|
||||
return getFlag(m_buttonStates, button - 1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonPressed(int button) {
|
||||
return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button));
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonReleased(int button) {
|
||||
return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button));
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRawAxis(int axis) {
|
||||
return m_axes[axis];
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPOV(int pov) {
|
||||
return m_pov[pov];
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getAxisCount() {
|
||||
return m_axes.length;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPOVCount() {
|
||||
return m_pov.length;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getButtonCount() {
|
||||
return 10;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isConnected() {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public HIDType getType() {
|
||||
return HIDType.kXInputGamepad;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Virtual Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getAxisType(int axis) {
|
||||
return 1; /* ! Warning, does not return accurate data.
|
||||
Hopefully this isn't a problem */
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setOutput(int outputNumber, boolean value) {
|
||||
// do not use
|
||||
//m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1];
|
||||
//m_buttonStates[outputNumber - 1] = value;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setOutputs(int value) {
|
||||
m_buttonStatesLastFrame = m_buttonStates;
|
||||
m_buttonStates = (short) value;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setRumble(RumbleType type, double value) {
|
||||
System.out.println("Why are you Setting rumble on an virtual controller?");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user