mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-08 16:28:07 -06:00
Cleanup swerve drive
Remove remaining unused commands Move autonomous commands to subpackage Change speed modes on swerve drive to use a boolean
This commit is contained in:
nathanrsxtn
@@ -33,7 +33,7 @@ public final class Constants {
|
||||
public static final String DRIVE_CAN_NAME = "rio";
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 2.3;
|
||||
public static final double ROTATION_SPEED = 2.3 * 2;
|
||||
/** Distance between centers of right and left wheels on robot */
|
||||
public static final double TRACK_WIDTH = Units.inchesToMeters(23.75);
|
||||
/** Distance between centers of front and back wheels on robot */
|
||||
|
||||
@@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj2.command.button.POVButton;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.AutonomousBuilder;
|
||||
import frc4388.robot.commands.autonomous.AutonomousBuilder;
|
||||
import frc4388.robot.commands.extender.DeployExtender;
|
||||
import frc4388.robot.commands.extender.RetractExtender;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
@@ -169,12 +169,12 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
private void configureDriverButtonBindings() {
|
||||
/* Left Bumper > Shift Down */
|
||||
/* Left Bumper > Shift To Low */
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
/* Right Bumper > Shift Up */
|
||||
.whenPressed(() -> m_robotSwerveDrive.setSpeedMode(false));
|
||||
/* Right Bumper > Shift To High */
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
.whenPressed(() -> m_robotSwerveDrive.setSpeedMode(true));
|
||||
/* Left Stick > Unbound */
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftStick.value)
|
||||
.whenPressed(new PrintCommand("Unbound"));
|
||||
@@ -340,6 +340,7 @@ public class RobotContainer {
|
||||
builder.addBooleanProperty("Shooter Safety", this::isLockedOn, null);
|
||||
});
|
||||
putData("Field", m_robotSwerveDrive.m_field);
|
||||
// TODO: Make the PDP dashboard entry work.
|
||||
// putData("PDP", new PowerDistribution() {
|
||||
// @Override
|
||||
// public void initSendable(SendableBuilder builder) {
|
||||
@@ -359,7 +360,7 @@ public class RobotContainer {
|
||||
}
|
||||
});
|
||||
putData("Drivebase", m_robotSwerveDrive);
|
||||
putData("Gyro", m_robotSwerveDrive.m_gyro);
|
||||
putData("Gyro", m_robotMap.gyro);
|
||||
putData("Drive Speed", new NTSendable() {
|
||||
@Override
|
||||
public void initSendable(NTSendableBuilder builder) {
|
||||
@@ -367,8 +368,8 @@ public class RobotContainer {
|
||||
builder.getEntry(".instance").setDouble(0);
|
||||
builder.addStringProperty("default", () -> "Low", null);
|
||||
builder.addStringArrayProperty("options", () -> new String[] {"Low", "High"}, null);
|
||||
builder.addStringProperty("active", () -> m_robotSwerveDrive.speedAdjust == SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW ? "Low" : "High", null);
|
||||
builder.addStringProperty("selected", null, val -> m_robotSwerveDrive.speedAdjust = "Low".equals(val) ? SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW : SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST);
|
||||
builder.addStringProperty("active", () -> m_robotSwerveDrive.getSpeedMode() ? "High" : "Low", null);
|
||||
builder.addStringProperty("selected", null, val -> m_robotSwerveDrive.setSpeedMode("High".equals(val)));
|
||||
}
|
||||
});
|
||||
putData("Accelerometer", new NTSendable() {
|
||||
@@ -380,7 +381,7 @@ public class RobotContainer {
|
||||
NetworkTableEntry entryZ = builder.getEntry("Z");
|
||||
builder.setUpdateTable(() -> {
|
||||
short[] data = new short[3];
|
||||
m_robotSwerveDrive.m_gyro.getBiasedAccelerometer(data);
|
||||
m_robotMap.gyro.getBiasedAccelerometer(data);
|
||||
entryX.setDouble(data[0]);
|
||||
entryY.setDouble(data[1]);
|
||||
entryZ.setDouble(data[2]);
|
||||
|
||||
@@ -1,152 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.util.Comparator;
|
||||
import java.util.Map;
|
||||
import java.util.Optional;
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.IntStream;
|
||||
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.CSV;
|
||||
|
||||
public class PlaybackDriveInput extends CommandBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(PlaybackDriveInput.class.getSimpleName());
|
||||
|
||||
public static class DriveInputEntry {
|
||||
public double millis, leftAxisX, leftAxisY, rightAxisX;
|
||||
}
|
||||
|
||||
private DriveInputEntry[] driveInputEntries;
|
||||
|
||||
private SwerveDrive swerve;
|
||||
private long startTime;
|
||||
|
||||
/** Creates a new PlaybackDriveInput. */
|
||||
public PlaybackDriveInput(SwerveDrive swerve, String autoName) {
|
||||
this.swerve = swerve;
|
||||
|
||||
try {
|
||||
// This is a helper class that allows us to read a CSV file into a Java array.
|
||||
CSV<DriveInputEntry> csv = new CSV<>(DriveInputEntry::new) {
|
||||
// This is a regular expression that removes all parentheses from the header of the CSV file.
|
||||
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
|
||||
|
||||
/**
|
||||
* Removes the parentheses from the CSV header
|
||||
*
|
||||
* @param header The header to be sanitized.
|
||||
* @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
|
||||
* The parentheses.matcher(header) is matching the header with the parentheses regular
|
||||
* expression. The replaceAll method is replacing all of the parentheses with an empty
|
||||
* string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
|
||||
* the parent sanitizer.
|
||||
*/
|
||||
@Override
|
||||
protected String headerSanitizer(final String header) {
|
||||
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
|
||||
}
|
||||
};
|
||||
// This is reading the CSV file into a Java array.
|
||||
driveInputEntries = csv.read(new File(Filesystem.getDeployDirectory(), autoName + ".csv").toPath());
|
||||
// This is a running a helper method that is logging the contents of the table to the console on a new thread.
|
||||
// ! new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(driveInputEntries, RobotBase.isSimulation()))).start();
|
||||
} catch (final IOException exception) {
|
||||
DriveInputEntry dummyEntry = new DriveInputEntry();
|
||||
dummyEntry.millis = 0.0;
|
||||
dummyEntry.leftAxisX = 0.0;
|
||||
dummyEntry.leftAxisY = 0.0;
|
||||
dummyEntry.rightAxisX = 0.0;
|
||||
driveInputEntries = new DriveInputEntry[] { dummyEntry };
|
||||
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
|
||||
}
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
long currentTime = System.currentTimeMillis() - startTime;
|
||||
|
||||
double leftAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisX).doubleValue();
|
||||
double leftAxisY = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisY).doubleValue();
|
||||
double rightAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.rightAxisX).doubleValue();
|
||||
|
||||
swerve.driveWithInput(leftAxisX, leftAxisY, rightAxisX, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
|
||||
* given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the
|
||||
* interpolation (y) between the two values (y0 and y1) accquired by applying the target getter
|
||||
* function to the lower and upper bounds entries.
|
||||
*
|
||||
* @param table An array of entries to search through.
|
||||
* @param lookupValue The value to lookup in the table.
|
||||
* @param lookupGetter A function that takes an entry from the table and returns .
|
||||
* @param targetGetter A function that takes an E and returns a Number.
|
||||
* @return The interpolated value.
|
||||
*/
|
||||
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
|
||||
final E closestRecord = closestEntry.getValue();
|
||||
final int closestRecordIndex = closestEntry.getKey();
|
||||
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
|
||||
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
|
||||
}
|
||||
|
||||
/**
|
||||
* If the value is in the table, return the entry. Otherwise, return the entry with the closest
|
||||
* value
|
||||
*
|
||||
* @param table The array of values to search.
|
||||
* @param value The value to search for.
|
||||
* @param valueGetter A function that takes an element of the table and returns a Number to compare
|
||||
* with the given value.
|
||||
* @param exactMatch If true, the lookup will only return a match if the value is exactly equal to
|
||||
* the value of the entry. If false, the lookup will return a match with a value closest to
|
||||
* the given value.
|
||||
* @return The entry with the closest value to the given value.
|
||||
*/
|
||||
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
|
||||
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a
|
||||
* linear interpolation of the two values y0 and y1
|
||||
*
|
||||
* @param x The value to interpolate.
|
||||
* @param x0 the x coordinate of the lower bound to interpolate to
|
||||
* @param y0 The value at x0.
|
||||
* @param x1 the x-coordinate of the upper bound to interpolate to
|
||||
* @param y1 The value at x1.
|
||||
* @return The interpolation between y0 and y1 at x.
|
||||
*/
|
||||
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
|
||||
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
|
||||
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -1,103 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FileOutputStream;
|
||||
import java.io.PrintWriter;
|
||||
import java.util.HashMap;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class RecordDriveInput extends CommandBase {
|
||||
private HashMap<Long, double[]> timedInput;
|
||||
|
||||
private SwerveDrive swerve;
|
||||
|
||||
private Supplier<Double> leftAxisXSupplier;
|
||||
private Supplier<Double> leftAxisYSupplier;
|
||||
private Supplier<Double> rightAxisXSupplier;
|
||||
|
||||
private String autoName;
|
||||
|
||||
private long startTime;
|
||||
private long count;
|
||||
|
||||
private int nthRecord;
|
||||
|
||||
/** Creates a new RecordDriveInput. */
|
||||
public RecordDriveInput(
|
||||
SwerveDrive swerve,
|
||||
Supplier<Double> leftAxisXSupplier,
|
||||
Supplier<Double> leftAxisYSupplier,
|
||||
Supplier<Double> rightAxisXSupplier,
|
||||
String autoName,
|
||||
int nthRecord)
|
||||
{
|
||||
this.swerve = swerve;
|
||||
this.leftAxisXSupplier = leftAxisXSupplier;
|
||||
this.leftAxisYSupplier = leftAxisYSupplier;
|
||||
this.rightAxisXSupplier = rightAxisXSupplier;
|
||||
this.autoName = autoName;
|
||||
this.nthRecord = nthRecord;
|
||||
|
||||
timedInput = new HashMap<>();
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
public RecordDriveInput(
|
||||
SwerveDrive swerve,
|
||||
Supplier<Double> leftAxisXSupplier,
|
||||
Supplier<Double> leftAxisYSupplier,
|
||||
Supplier<Double> rightAxisXSupplier,
|
||||
String autoName)
|
||||
{
|
||||
this(swerve, leftAxisXSupplier, leftAxisYSupplier, rightAxisXSupplier, autoName, 1);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = System.currentTimeMillis();
|
||||
count = 0;
|
||||
|
||||
timedInput.put((long) 0, new double[] {0, 0, 0});
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
long timeFromStart = System.currentTimeMillis() - startTime;
|
||||
double[] input = {leftAxisXSupplier.get(), leftAxisYSupplier.get(), rightAxisXSupplier.get()};
|
||||
|
||||
if(count % (long) nthRecord == 0)
|
||||
timedInput.put(timeFromStart, input);
|
||||
|
||||
swerve.driveWithInput(input[0], input[1], input[2], true);
|
||||
count++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
File csvOutput = new File(autoName + ".csv");
|
||||
try(PrintWriter writer = new PrintWriter(csvOutput)) {
|
||||
writer.println("millis,leftx,lefty,rightx");
|
||||
|
||||
for(long millis : timedInput.keySet())
|
||||
writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
|
||||
|
||||
//// writer.println(( + 1) + ",0,0,0");
|
||||
writer.close();
|
||||
} catch(Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
|
||||
public class RunCommandForTime extends CommandBase {
|
||||
|
||||
// command
|
||||
Command command;
|
||||
|
||||
// timing
|
||||
long start;
|
||||
long elapsed;
|
||||
double duration;
|
||||
|
||||
// override
|
||||
boolean override;
|
||||
|
||||
/**
|
||||
* Runs given command for given time.
|
||||
* @param command Command to run.
|
||||
* @param duration Time to run for, in seconds.
|
||||
* @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended.
|
||||
*/
|
||||
public RunCommandForTime(Command command, double duration, boolean override) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
|
||||
this.command = command;
|
||||
this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
|
||||
this.override = override;
|
||||
|
||||
addRequirements(this.command.getRequirements().toArray(Subsystem[]::new));
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs given command for given time.
|
||||
* @param command Command to run.
|
||||
* @param duration Time to run for, in seconds.
|
||||
*/
|
||||
public RunCommandForTime(Command command, double duration) {
|
||||
this(command, duration, false);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
this.start = System.currentTimeMillis();
|
||||
this.command.initialize();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
this.elapsed = System.currentTimeMillis() - this.start;
|
||||
this.command.execute();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
this.command.end(interrupted);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (this.override) {
|
||||
return (this.elapsed >= this.duration);
|
||||
} else {
|
||||
return (this.command.isFinished() && (this.elapsed >= this.duration));
|
||||
}
|
||||
}
|
||||
}
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands.shooter;
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands;
|
||||
package frc4388.robot.commands.autonomous;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||
@@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import frc4388.robot.RobotContainer;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.commands.TimedWaitUntilCommand;
|
||||
import frc4388.robot.commands.extender.DeployExtender;
|
||||
import frc4388.robot.commands.shooter.TimedWaitUntilCommand;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Extender;
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
package frc4388.robot.commands;
|
||||
package frc4388.robot.commands.autonomous;
|
||||
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Level;
|
||||
@@ -49,7 +49,7 @@ public class PathPlannerCommand extends SequentialCommandGroup {
|
||||
|
||||
addCommands(
|
||||
new InstantCommand(() -> m_swerveDrive.resetOdometry(initialPosition), m_swerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_swerveDrive::getOdometry, SwerveDriveConstants.DRIVE_KINEMATICS, xController, yController, thetaController, m_swerveDrive::setModuleStates, m_swerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_swerveDrive::getPoseMeters, SwerveDriveConstants.DRIVE_KINEMATICS, xController, yController, thetaController, m_swerveDrive::setModuleStates, m_swerveDrive),
|
||||
new InstantCommand(m_swerveDrive::stopModules, m_swerveDrive)
|
||||
);
|
||||
} else {
|
||||
@@ -36,7 +36,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.NotifierCommand;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.PathPlannerCommand;
|
||||
import frc4388.robot.commands.autonomous.PathPlannerCommand;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
|
||||
@@ -99,8 +99,8 @@ public class PathRecorder extends CommandBase {
|
||||
*/
|
||||
@Override
|
||||
public void execute() {
|
||||
Translation2d position = m_swerveDrive.m_odometry.getPoseMeters().getTranslation();
|
||||
Rotation2d rotation = m_swerveDrive.m_gyro.getRotation2d();
|
||||
Translation2d position = m_swerveDrive.getPoseMeters().getTranslation();
|
||||
Rotation2d rotation = m_swerveDrive.getRotation2d();
|
||||
// FIXME: Chassis speeds are created from joystick inputs and may not reflect actual robot velocity.
|
||||
Translation2d velocity = new Translation2d(m_swerveDrive.getChassisSpeeds().vxMetersPerSecond, m_swerveDrive.getChassisSpeeds().vyMetersPerSecond);
|
||||
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
|
||||
|
||||
@@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
@@ -23,17 +22,14 @@ public class SwerveDrive extends SubsystemBase {
|
||||
private SwerveModule m_frontRight;
|
||||
private SwerveModule m_backLeft;
|
||||
private SwerveModule m_backRight;
|
||||
private WPI_Pigeon2 m_gyro;
|
||||
|
||||
public SwerveModule[] modules;
|
||||
public WPI_Pigeon2 m_gyro;
|
||||
|
||||
public SwerveDriveOdometry m_odometry;
|
||||
// public SwerveDriveOdometry m_odometry;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
public double rotTarget;
|
||||
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
private SwerveModule[] m_modules;
|
||||
private SwerveDriveOdometry m_odometry;
|
||||
private ChassisSpeeds m_chassisSpeeds;
|
||||
private boolean m_speedMode;
|
||||
private boolean m_ignoreAngles;
|
||||
private double m_targetHeading;
|
||||
|
||||
public final Field2d m_field = new Field2d();
|
||||
|
||||
@@ -44,25 +40,20 @@ public class SwerveDrive extends SubsystemBase {
|
||||
m_backRight = backRight;
|
||||
m_gyro = gyro;
|
||||
|
||||
modules = new SwerveModule[] { m_frontLeft, m_frontRight, m_backLeft, m_backRight };
|
||||
SmartDashboard.putNumber("OMEGA", 0.0);
|
||||
// m_poseEstimator = new SwerveDrivePoseEstimator(
|
||||
// getRegGyro(),//m_gyro.getRotation2d(),
|
||||
// new Pose2d(),
|
||||
// m_kinematics,
|
||||
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
|
||||
// VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
|
||||
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
|
||||
|
||||
m_modules = new SwerveModule[] { m_frontLeft, m_frontRight, m_backLeft, m_backRight };
|
||||
m_odometry = new SwerveDriveOdometry(SwerveDriveConstants.DRIVE_KINEMATICS, m_gyro.getRotation2d());
|
||||
m_chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
m_gyro.reset();
|
||||
SmartDashboard.putData("Field", m_field);
|
||||
}
|
||||
|
||||
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
|
||||
Translation2d speed = new Translation2d(speedX, speedY);
|
||||
driveWithInput(speed, rot, fieldRelative);
|
||||
@Override
|
||||
public void periodic() {
|
||||
updateOdometry();
|
||||
updateSmartDash();
|
||||
|
||||
m_field.setRobotPose(getPoseMeters());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -74,26 +65,45 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
|
||||
ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
|
||||
m_ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
|
||||
|
||||
double mag = speed.getNorm();
|
||||
speed = speed.times(mag * speedAdjust);
|
||||
double magnitude = speed.getNorm();
|
||||
speed = speed.times(magnitude * getMaxSpeed());
|
||||
|
||||
double xSpeedMetersPerSecond = speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
|
||||
SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
|
||||
setModuleStates(states);
|
||||
m_chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(Math.toRadians(m_gyro.getAngle() + 90))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED);
|
||||
setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(m_chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
if (rightStick.getNorm() > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = -Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
double rotDifference = rotTarget - m_gyro.getRotation2d().getRadians();
|
||||
leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rotDifference, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2)));
|
||||
} else chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
|
||||
setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds));
|
||||
// Use the right joystick to set heading.
|
||||
if (rightStick.getNorm() > OIConstants.RIGHT_AXIS_DEADBAND) {
|
||||
/*
|
||||
Map the right jostick coordinates to a ccw+ heading.
|
||||
|
||||
Right Stick Polar Radians Omega Radians
|
||||
┌──˗y───┐ ╭─˖π/2──╮ ╭──˖0───╮
|
||||
˗x ˖x -> ̠˖π ̠˖0 -> ˖π/2 ˗π/2
|
||||
└──˖y───┘ ╰─˗π/2──╯ ╰──˖π───╯
|
||||
(x,y) tan⁻¹(y/x) tan⁻¹(-y/-x)
|
||||
*/
|
||||
m_targetHeading = Math.atan2(-rightStick.getX(), -rightStick.getY());
|
||||
}
|
||||
// Calculate the error between the the target heading and the current heading.
|
||||
double headingError = m_targetHeading - Math.toRadians(m_gyro.getYaw());
|
||||
|
||||
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * getMaxSpeed());
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
m_chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speed.getX(), speed.getY(), headingError, new Rotation2d(Math.toRadians(m_gyro.getAngle() + 90)));
|
||||
} else {
|
||||
// Create robot-relative speeds.
|
||||
m_chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(m_chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -103,48 +113,24 @@ public class SwerveDrive extends SubsystemBase {
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
// int i = 2; {
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModule module = m_modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state, ignoreAngles);
|
||||
module.setDesiredState(state, m_ignoreAngles);
|
||||
}
|
||||
// modules[0].setDesiredState(desiredStates[0], false);
|
||||
}
|
||||
|
||||
public void setModuleRotationsToAngle(double angle) {
|
||||
for (int i = 0; i < modules.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
module.rotateToAngle(angle);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
updateOdometry();
|
||||
updateSmartDash();
|
||||
|
||||
// SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
|
||||
// SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
|
||||
// SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
// SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
|
||||
|
||||
m_field.setRobotPose(getOdometry());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
private void updateSmartDash() {
|
||||
// odometry
|
||||
SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
|
||||
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
|
||||
SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
|
||||
// Odometry
|
||||
SmartDashboard.putNumber("Odometry: X", getPoseMeters().getX());
|
||||
SmartDashboard.putNumber("Odometry: Y", getPoseMeters().getY());
|
||||
SmartDashboard.putNumber("Odometry: Theta", getPoseMeters().getRotation().getDegrees());
|
||||
|
||||
// chassis speeds
|
||||
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
|
||||
// SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
|
||||
// SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
|
||||
// SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
|
||||
// Chassis Speeds
|
||||
// TODO: Measure the actual max velocity in m/s of the robot to have accurate chassis speeds.
|
||||
// SmartDashboard.putNumber("Chassis Speed: 𝚡", chassisSpeeds.vxMetersPerSecond);
|
||||
// SmartDashboard.putNumber("Chassis Speed: 𝚢", chassisSpeeds.vyMetersPerSecond);
|
||||
// SmartDashboard.putNumber("Chassis Speed: ω", chassisSpeeds.omegaRadiansPerSecond);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -152,7 +138,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* @return Current chassis speeds (vx, vy, ω)
|
||||
*/
|
||||
public ChassisSpeeds getChassisSpeeds() {
|
||||
return chassisSpeeds;
|
||||
return m_chassisSpeeds;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -160,15 +146,12 @@ public class SwerveDrive extends SubsystemBase {
|
||||
*
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
// return m_odometry.getPoseMeters();
|
||||
public Pose2d getPoseMeters() {
|
||||
return m_odometry.getPoseMeters();
|
||||
// return m_poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
public Pose2d getAutoOdo() {
|
||||
Pose2d workingPose = getOdometry();
|
||||
return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
|
||||
public Rotation2d getRotation2d() {
|
||||
return m_gyro.getRotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -195,52 +178,42 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2)); // + (Math.PI/2));
|
||||
Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians());
|
||||
|
||||
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
|
||||
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
|
||||
|
||||
m_odometry.update(actual, // m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
|
||||
modules[0].getState(), modules[1].getState(), modules[2].getState(), modules[3].getState());
|
||||
Rotation2d gyroAngle = new Rotation2d(m_gyro.getRotation2d().getRadians());
|
||||
m_odometry.update(gyroAngle, m_modules[0].getState(), m_modules[1].getState(), m_modules[2].getState(), m_modules[3].getState());
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets pigeon.
|
||||
*/
|
||||
public void resetGyro() {
|
||||
m_targetHeading = 0;
|
||||
m_gyro.reset();
|
||||
rotTarget = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop all four swerve modules.
|
||||
*/
|
||||
public void stopModules() {
|
||||
modules[0].stop();
|
||||
modules[1].stop();
|
||||
modules[2].stop();
|
||||
modules[3].stop();
|
||||
m_modules[0].stop();
|
||||
m_modules[1].stop();
|
||||
m_modules[2].stop();
|
||||
m_modules[3].stop();
|
||||
}
|
||||
|
||||
/**
|
||||
* Switches speed modes.
|
||||
* Sets the speed mode.
|
||||
*
|
||||
* @param shift True if fast mode, false if slow mode.
|
||||
* @param mode True if fast mode, false if slow mode.
|
||||
*/
|
||||
public void highSpeed(boolean shift) {
|
||||
if (shift) {
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
} else {
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
}
|
||||
public void setSpeedMode(boolean mode) {
|
||||
m_speedMode = mode;
|
||||
}
|
||||
|
||||
public double getCurrent() {
|
||||
return m_frontLeft.getCurrent() + m_frontRight.getCurrent() + m_backRight.getCurrent() + m_backLeft.getCurrent();
|
||||
public boolean getSpeedMode() {
|
||||
return m_speedMode;
|
||||
}
|
||||
|
||||
public double getVoltage() {
|
||||
return m_frontLeft.getVoltage() + m_frontRight.getVoltage() + m_backRight.getVoltage() + m_backLeft.getVoltage();
|
||||
public double getMaxSpeed() {
|
||||
return m_speedMode ? SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user