Team4388/Robot-Essentials
1
0
Fork 0
mirror of https://github.com/Team4388/Robot-Essentials.git synced 2026-06-08 16:28:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'formalise' of https://github.com/Team4388/Robot-Essentials into formalise

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-04 15:45:10 -07:00
parent ad34a8254b 9bc2bbfd98
commit fcf7a3e98c
6 changed files with 27 additions and 340 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitattributes
+2
View File
@@ -0,0 +1,2 @@
# Auto detect text files and perform LF normalization
* text=auto
.github/workflows/gradle.yml
+23
View File
@@ -0,0 +1,23 @@
name: Java CI
on:
push:
branches:
- master
pull_request:
branches:
- master
jobs:
build:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v1
- name: Set up JDK 17
uses: actions/setup-java@v1
with:
java-version: 17
- name: Change wrapper permissions
run: chmod +x ./gradlew
- name: Build with Gradle
run: ./gradlew build
README.md
+2
View File
@@ -0,0 +1,2 @@
# Robot-Essentials
Basic code for any Ridgebotics robot project
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
-98
View File
@@ -1,98 +0,0 @@
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.subsystems.SwerveDrive;
public class PlaybackChooser {
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;
private final SwerveDrive m_swerve;
// commands
private Command m_noAuto = new InstantCommand();
public PlaybackChooser(SwerveDrive swerve, Object... pool) {
m_swerve = swerve;
}
public PlaybackChooser addOption(String name, Command option) {
m_commandPool.put(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 appendCommand() {
var chooser = new SendableChooser<Command>();
chooser.setDefaultOption("No Auto", m_noAuto);
m_choosers.add(chooser);
ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
.add("Command: " + m_choosers.size(), chooser)
.withSize(4, 1)
.withPosition(0, m_choosers.size() - 1)
.withWidget(BuiltInWidgets.kSplitButtonChooser);
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 Command getCommand() {
Command command = m_playback.getSelected();
command = command == null ? m_noAuto : command.asProxy();
Command[] commands = new Command[m_cmdNum - 1];
for (int i = 0; i < m_cmdNum - 1; i++) {
Command command2 = m_choosers.get(i + 1).getSelected();
command2 = command2 == null ? m_noAuto : command2.asProxy();
commands[i] = command2.asProxy();
}
return command.andThen(commands);
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
-145
View File
@@ -1,145 +0,0 @@
// 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 java.io.File;
import java.io.FileNotFoundException;
import java.util.ArrayList;
import java.util.Scanner;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends Command {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
this.filename = filename;
this.mult = mult;
addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]);
outputs.add(out);
}
input.close();
} catch (FileNotFoundException e) {
e.printStackTrace();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
}
// skip to reasonable time frame
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY),
// true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
counter++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
-97
View File
@@ -1,97 +0,0 @@
// 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 java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
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.UtilityStructs.TimedOutput;
public class JoystickRecorder extends Command {
public final SwerveDrive swerve;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
String filename)
{
this.swerve = swerve;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename;
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
this.startTime = System.currentTimeMillis();
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
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.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
//System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File output = new File("/home/lvuser/autos/" + filename);
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) {
e.printStackTrace();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}