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

Initial commit

Browse Source
This commit is contained in:
C4llSqin
2025-01-04 16:09:10 -07:00
committed by GitHub
commit 248284ef69
59 changed files with 5409 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Autos/Taxi.txt
+225
View File
@@ -0,0 +1,225 @@
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,0
0.0,0.0,0.0,0.0,12
0.0,0.0,0.0,0.0,26
0.0,0.0,0.0,0.0,37
0.0,0.0,0.0,0.0,50
0.0,0.0,0.0,0.0,62
0.0,0.0,0.0,0.0,73
0.0,0.0,0.0,0.0,88
0.0,0.0,0.0,0.0,103
0.0,0.0,0.0,0.0,116
0.0,0.0,0.0,0.0,160
0.0,0.0,0.0,0.0,173
0.0,0.0,0.0,0.0,185
0.0,0.0,0.0,0.0,200
0.0,0.0,0.0,0.0,211
0.0,0.0,0.0,0.0,223
0.0,0.0,0.0,0.0,235
0.0,0.0,0.0,0.0,247
0.0,0.0,0.0,0.0,263
0.0,0.0,0.0,0.0,283
0.0,0.0,0.0,0.0,303
0.0,-0.109375,0.0,0.0,323
0.0,-0.1484375,0.0,0.0,343
0.0,-0.2109375,0.0,0.0,363
0.0,-0.3671875,0.0,0.0,398
0.0,-0.4140625,0.0,0.0,411
0.0,-0.4765625,0.0,0.0,425
0.0,-0.5078125,0.0,0.0,443
0.0,-0.5078125,0.0,0.0,463
0.0,-0.53125,0.0,0.0,483
0.0,-0.5546875,0.0,0.0,504
0.0,-0.5625,0.0,0.0,523
0.0,-0.5625,0.0,0.0,544
0.0,-0.5703125,0.0,0.0,563
0.0,-0.5859375,0.0,0.0,584
0.0,-0.5859375,0.0,0.0,603
0.0,-0.5859375,0.0,0.0,640
0.0,-0.59375,0.0,0.0,657
0.0,-0.6015625,0.0,0.0,672
0.0,-0.6015625,0.0,0.0,685
0.0,-0.6015625,0.0,0.0,703
0.0,-0.6015625,0.0,0.0,723
0.0,-0.6015625,0.0,0.0,743
0.0,-0.6015625,0.0,0.0,763
0.0,-0.6015625,0.0,0.0,783
0.0,-0.6015625,0.0,0.0,803
0.0,-0.6015625,0.0,0.0,823
0.0,-0.6015625,0.0,0.0,844
0.0,-0.6015625,0.0,0.0,878
0.0,-0.6015625,0.0,0.0,893
0.0,-0.6015625,0.0,0.0,907
0.0,-0.6015625,0.0,0.0,924
0.0,-0.609375,0.0,0.0,943
0.0,-0.609375,0.0,0.0,963
0.0,-0.609375,0.0,0.0,983
0.0,-0.609375,0.0,0.0,1004
0.0,-0.609375,0.0,0.0,1023
0.0,-0.609375,0.0,0.0,1043
0.0,-0.609375,0.0,0.0,1064
0.0,-0.609375,0.0,0.0,1083
0.0,-0.609375,0.0,0.0,1156
0.0,-0.609375,0.0,0.0,1172
0.0,-0.609375,0.0,0.0,1185
0.0,-0.609375,0.0,0.0,1200
0.0,-0.609375,0.0,0.0,1215
0.0,-0.609375,0.0,0.0,1225
0.0,-0.609375,0.0,0.0,1236
0.0,-0.609375,0.0,0.0,1249
0.0,-0.609375,0.0,0.0,1263
0.0,-0.609375,0.0,0.0,1283
0.0,-0.609375,0.0,0.0,1303
0.0,-0.609375,0.0,0.0,1323
0.0,-0.609375,0.0,0.0,1363
0.0,-0.6015625,0.0,0.0,1376
0.0,-0.6015625,0.0,0.0,1394
0.0,-0.6015625,0.0,0.0,1405
0.0,-0.6015625,0.0,0.0,1423
0.0,-0.6015625,0.0,0.0,1443
0.0,-0.6015625,0.0,0.0,1463
0.0,-0.6015625,0.0,0.0,1483
0.0,-0.6015625,0.0,0.0,1503
0.0,-0.6015625,0.0,0.0,1523
0.0,-0.6015625,0.0,0.0,1543
0.0,-0.6015625,0.0,0.0,1563
0.0,-0.6015625,0.0,0.0,1597
0.0,-0.6015625,0.0,0.0,1608
0.0,-0.6015625,0.0,0.0,1624
0.0,-0.6015625,0.0,0.0,1643
0.0,-0.6015625,0.0,0.0,1664
0.0,-0.5859375,0.0,0.0,1683
0.0,-0.5859375,0.0,0.0,1703
0.0,-0.5625,0.0,0.0,1723
0.0,-0.5625,0.0,0.0,1743
0.0,-0.5625,0.0,0.0,1763
0.0,-0.5625,0.0,0.0,1783
0.0,-0.5625,0.0,0.0,1803
0.0,-0.5625,0.0,0.0,1843
0.0,-0.5625,0.0,0.0,1855
0.0,-0.5625,0.0,0.0,1868
0.0,-0.5625,0.0,0.0,1883
0.0,-0.5625,0.0,0.0,1903
0.0,-0.5625,0.0,0.0,1923
0.0,-0.5625,0.0,0.0,1943
0.0,-0.5625,0.0,0.0,1963
0.0,-0.5625,0.0,0.0,1983
0.0,-0.5625,0.0,0.0,2003
0.0,-0.5625,0.0,0.0,2024
0.0,-0.5625,0.0,0.0,2043
0.0,-0.5625,0.0,0.0,2081
0.0,-0.5625,0.0,0.0,2093
0.0,-0.5625,0.0,0.0,2105
0.0,-0.5625,0.0,0.0,2123
0.0,-0.5625,0.0,0.0,2143
0.0,-0.5625,0.0,0.0,2163
0.0,-0.5625,0.0,0.0,2183
0.0,-0.5625,0.0,0.0,2203
0.0,-0.5625,0.0,0.0,2223
0.0,-0.5625,0.0,0.0,2243
0.0,-0.5625,0.0,0.0,2263
0.0,-0.5625,0.0,0.0,2283
0.0,-0.5625,0.0,0.0,2366
0.0,-0.5625,0.0,0.0,2377
0.0,-0.5625,0.0,0.0,2394
0.0,-0.5703125,0.0,0.0,2405
0.0,-0.5703125,0.0,0.0,2418
0.0,-0.5703125,0.0,0.0,2431
0.0,-0.5703125,0.0,0.0,2444
0.0,-0.5703125,0.0,0.0,2458
0.0,-0.5703125,0.0,0.0,2470
0.0,-0.5703125,0.0,0.0,2485
0.0,-0.5703125,0.0,0.0,2503
0.0,-0.5703125,0.0,0.0,2523
0.0,-0.5703125,0.0,0.0,2563
0.0,-0.5703125,0.0,0.0,2577
0.0,-0.5703125,0.0,0.0,2591
0.0,-0.5703125,0.0,0.0,2608
0.0,-0.5703125,0.0,0.0,2624
0.0,-0.5703125,0.0,0.0,2643
0.0,-0.5703125,0.0,0.0,2677
0.0,-0.5703125,0.0,0.0,2698
0.0,-0.5703125,0.0,0.0,2711
0.0,-0.5703125,0.0,0.0,2725
0.0,-0.5703125,0.0,0.0,2743
0.0,-0.5703125,0.0,0.0,2764
0.0,-0.5703125,0.0,0.0,2810
0.0,-0.5703125,0.0,0.0,2820
0.0,-0.5703125,0.0,0.0,2833
0.0,-0.5703125,0.0,0.0,2845
0.0,-0.5703125,0.0,0.0,2863
0.0,-0.5703125,0.0,0.0,2883
0.0,-0.5703125,0.0,0.0,2904
0.0,-0.5703125,0.0,0.0,2924
0.0,-0.5703125,0.0,0.0,2943
0.0,-0.5703125,0.0,0.0,2963
0.0,-0.5703125,0.0,0.0,2983
0.0,-0.5703125,0.0,0.0,3003
0.0,-0.5703125,0.0,0.0,3033
0.0,-0.5703125,0.0,0.0,3050
0.0,-0.5703125,0.0,0.0,3065
0.0,-0.5703125,0.0,0.0,3083
0.0,-0.5703125,0.0,0.0,3103
0.0,-0.5703125,0.0,0.0,3123
0.0,-0.5703125,0.0,0.0,3144
0.0,-0.5703125,0.0,0.0,3164
0.0,-0.5703125,0.0,0.0,3184
0.0,-0.5703125,0.0,0.0,3203
0.0,-0.5703125,0.0,0.0,3223
0.0,-0.5703125,0.0,0.0,3243
0.0,-0.5703125,0.0,0.0,3272
0.0,-0.5703125,0.0,0.0,3289
0.0,-0.5703125,0.0,0.0,3303
0.0,-0.5703125,0.0,0.0,3323
0.0,-0.5703125,0.0,0.0,3343
0.0,-0.5703125,0.0,0.0,3363
0.0,-0.5703125,0.0,0.0,3383
0.0,-0.5703125,0.0,0.0,3403
0.0,-0.5703125,0.0,0.0,3423
0.0,-0.5703125,0.0,0.0,3443
0.0,-0.5703125,0.0,0.0,3463
0.0,-0.5703125,0.0,0.0,3483
0.0,-0.5703125,0.0,0.0,3566
0.0,-0.5703125,0.0,0.0,3578
0.0,-0.5703125,0.0,0.0,3596
0.0,-0.5703125,0.0,0.0,3610
0.0,-0.5703125,0.0,0.0,3623
0.0,-0.5703125,0.0,0.0,3640
0.0,-0.5703125,0.0,0.0,3651
0.0,-0.5703125,0.0,0.0,3663
0.0,-0.5703125,0.0,0.0,3678
0.0,-0.5703125,0.0,0.0,3691
0.0,-0.5703125,0.0,0.0,3706
0.0,-0.5703125,0.0,0.0,3723
0.0,-0.5703125,0.0,0.0,3766
0.0,-0.5703125,0.0,0.0,3778
0.0,-0.5703125,0.0,0.0,3792
0.0,-0.5703125,0.0,0.0,3807
0.0,-0.5703125,0.0,0.0,3823
0.0,-0.5703125,0.0,0.0,3843
0.0,-0.53125,0.0,0.0,3863
0.0,-0.53125,0.0,0.0,3884
0.0,-0.421875,0.0,0.0,3904
0.0,0.0,0.0,0.0,3924
0.0,0.0,0.0,0.0,3944
0.0,0.0,0.0,0.0,3963
0.0,0.0,0.0,0.0,3999
0.0,0.0,0.0,0.0,4010
0.0,0.0,0.0,0.0,4025
0.0,0.0,0.0,0.0,4043
0.0,0.0,0.0,0.0,4063
0.0,0.0,0.0,0.0,4083
0.0,0.0,0.0,0.0,4103
0.0,0.0,0.0,0.0,4123
0.0,0.0,0.0,0.0,4143
0.0,0.0,0.0,0.0,4163
0.0,0.0,0.0,0.0,4183
0.0,0.0,0.0,0.0,4203
0.0,0.0,0.0,0.0,4236
0.0,0.0,0.0,0.0,4247
0.0,0.0,0.0,0.0,4264
0.0,0.0,0.0,0.0,4284
0.0,0.0,0.0,0.0,4304
0.0,0.0,0.0,0.0,4324
0.0,0.0,0.0,0.0,4343
0.0,0.0,0.0,0.0,4363
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt
+20
View File
@@ -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]
src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java
+107
View File
@@ -0,0 +1,107 @@
// package frc4388.robot.commands.Autos;
// import java.io.File;
// import java.util.ArrayList;
// import java.util.HashMap;
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
// import edu.wpi.first.wpilibj2.command.Command;
// import edu.wpi.first.wpilibj2.command.InstantCommand;
// import frc4388.robot.commands.Swerve.JoystickPlayback;
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
// import frc4388.robot.subsystems.SwerveDrive;
// import frc4388.utility.controller.VirtualController;
// public class neoPlaybackChooser {
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
// private final SwerveDrive m_swerve;
// private final VirtualController[] m_controllers;
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
// // private SendableChooser<Command> m_playback = null;
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
// // private final File m_dir = new File("/home/lvuser/autos/");
// // private int m_cmdNum = 0;
// // commands
// private Command m_noAuto = new InstantCommand();
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
// m_teamChosser.addOption("Red", "red");
// m_teamChosser.setDefaultOption("Blue", "blue");
// m_teamChosser.addOption("Nuetral", "nuetral");
// m_possitionChosser.addOption("AMP", "amp");
// m_possitionChosser.setDefaultOption("Center", "center");
// m_possitionChosser.addOption("Source", "source");
// m_swerve = swerve;
// m_controllers = controllers;
// }
// public neoPlaybackChooser addOption(String name, String option) {
// m_autoNameChosser.addOption(name, option);
// return this;
// }
// // public PlaybackChooser buildDisplay() {
// // for (int i = 0; i < 10; i++) {
// // appendCommand();
// // }
// // m_playback = m_choosers.get(0);
// // nextChooser();
// // // ! This does not work, why?
// // Shuffleboard.getTab("Auto Chooser")
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
// // .withPosition(4, 0);
// // return this;
// // }
// // This will be bound to a button for the time being
// public void render() {
// // var chooser = new SendableChooser<Command>();
// // // chooser.setDefaultOption("No Auto", m_noAuto);
// // m_choosers.add(chooser);
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
// .add("Command: " + m_choosers.size(), chooser)
// .withSize(4, 1)
// .withPosition(0, m_choosers.size() - 1)
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
// .withWidget(BuiltInWidgets.kComboBoxChooser);
// m_widgets.add(widget);
// }
// // public void nextChooser() {
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
// // String[] dirs = m_dir.list();
// // if(dirs != null){ // Fix funny error
// // for (String auto : dirs) {
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
// // }
// // }
// // for (var cmd_name : m_commandPool.keySet()) {
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
// // }
// // }
// public String autoName() {
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
// }
// public Command getCommand() {
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
// }
// }
src/main/java/frc4388/robot/commands/PID.java
+60
View File
@@ -0,0 +1,60 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains;
public abstract class PID extends Command {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
/** produces the error from the setpoint */
public abstract double getError();
/** todo: javadoc */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
@Override
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
runWithOutput(output);
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+35
View File
@@ -0,0 +1,35 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.SwerveDrive;
public class RotateToAngle extends PID {
SwerveDrive drive;
double targetAngle;
/** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
this.targetAngle = targetAngle;
addRequirements(drive);
}
@Override
public double getError() {
return targetAngle - drive.getGyroAngle();
}
@Override
public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+197
View File
@@ -0,0 +1,197 @@
package frc4388.robot.commands.Swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
import java.util.function.Supplier;
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;
/**
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
* @author Zachary Wilke
*/
public class neoJoystickPlayback extends Command {
private final SwerveDrive swerve;
private final VirtualController[] controllers;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
private final Supplier<String> filenameGetter;
private String filename;
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;
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param shouldfree Unloads the auto on compleation or intruption.
* @param instantload Load the auto on object instantiation
*/
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
this.swerve = swerve;
this.filenameGetter = filenameGetter;
this.controllers = controllers;
this.m_shouldfree = shouldfree;
if (instantload) loadAuto();
addRequirements(this.swerve);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filename a String containing the name of the auto file you wish to playback.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param shouldfree unloads the auto on compleation or intruption.
* @param instantload load the auto on object instantiation
*/
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
this(swerve, () -> filename, controllers, shouldfree, instantload);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
*/
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
this(swerve, filenameGetter, controllers, true, false);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param filename a String containing the name of the auto file you wish to playback.
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
*/
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
this(swerve, () -> filename, controllers, true, false);
}
/**
* Load the auto file from disk into memory
* @return Returns true if loading was successful, else wise; return false
* @implNote if the auto is already loaded, it will return true.
*/
public boolean loadAuto() {
filename = filenameGetter.get();
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;
}
}
/**
* Unloads the auto.
*/
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) unloadAuto();
}
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+129
View File
@@ -0,0 +1,129 @@
package frc4388.robot.commands.Swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
import java.util.function.Supplier;
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;
/**
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
* @author Zachary Wilke
*/
public class neoJoystickRecorder extends Command {
private final SwerveDrive swerve;
private final XboxController[] controllers;
private String filename;
private final Supplier<String> filenameGetter;
private long startTime = -1;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
*/
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
this.swerve = swerve;
this.controllers = controllers;
this.filenameGetter = filenameGetter;
this.filename = "";
addRequirements(this.swerve);
}
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
* @param filename a String containing the name of the auto file you wish to playback.
*/
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
this(swerve, controllers, () -> filename);
}
@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);
this.filename = this.filenameGetter.get();
}
@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();
}
}
}