diff --git a/sample.auto b/sample.auto new file mode 100644 index 0000000..6b4c98b Binary files /dev/null and b/sample.auto differ diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index d664129..dca3e82 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -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", diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 313fcd4..4d81ee5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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; + } } diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 49ffcaf..f3d636d 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -75,6 +75,7 @@ public class PlaybackChooser { } } + for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt new file mode 100644 index 0000000..a65aea9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt @@ -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] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 3752550..4a91f7a 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -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); diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java new file mode 100644 index 0000000..206a2b5 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -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 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java new file mode 100644 index 0000000..0ad4100 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -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 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(); + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8cb65d9..7554671 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -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. diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/DataUtils.java new file mode 100644 index 0000000..3d998b6 --- /dev/null +++ b/src/main/java/frc4388/utility/DataUtils.java @@ -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(); + } +} diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index ac414e2..935dbbe 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -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; + } } diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c0384db --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -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); + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java new file mode 100644 index 0000000..34c0290 --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -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); + } +} diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java new file mode 100644 index 0000000..12e98cf --- /dev/null +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -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?"); + } +} \ No newline at end of file