diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2b40696..ffd6713 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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 */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 818a6f0..507ca03 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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]); diff --git a/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java deleted file mode 100644 index 409c385..0000000 --- a/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java +++ /dev/null @@ -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 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 Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { - final Map.Entry 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 Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { - final Optional> 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; - } -} diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java deleted file mode 100644 index eb35638..0000000 --- a/src/main/java/frc4388/robot/commands/RecordDriveInput.java +++ /dev/null @@ -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 timedInput; - - private SwerveDrive swerve; - - private Supplier leftAxisXSupplier; - private Supplier leftAxisYSupplier; - private Supplier rightAxisXSupplier; - - private String autoName; - - private long startTime; - private long count; - - private int nthRecord; - - /** Creates a new RecordDriveInput. */ - public RecordDriveInput( - SwerveDrive swerve, - Supplier leftAxisXSupplier, - Supplier leftAxisYSupplier, - Supplier 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 leftAxisXSupplier, - Supplier leftAxisYSupplier, - Supplier 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; - } -} diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java deleted file mode 100644 index 838c42f..0000000 --- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java +++ /dev/null @@ -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)); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java b/src/main/java/frc4388/robot/commands/TimedWaitUntilCommand.java similarity index 96% rename from src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java rename to src/main/java/frc4388/robot/commands/TimedWaitUntilCommand.java index fc69bcf..0e4422d 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TimedWaitUntilCommand.java +++ b/src/main/java/frc4388/robot/commands/TimedWaitUntilCommand.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.shooter; +package frc4388.robot.commands; import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; diff --git a/src/main/java/frc4388/robot/commands/AutonomousBuilder.java b/src/main/java/frc4388/robot/commands/autonomous/AutonomousBuilder.java similarity index 98% rename from src/main/java/frc4388/robot/commands/AutonomousBuilder.java rename to src/main/java/frc4388/robot/commands/autonomous/AutonomousBuilder.java index 6687c28..5572118 100644 --- a/src/main/java/frc4388/robot/commands/AutonomousBuilder.java +++ b/src/main/java/frc4388/robot/commands/autonomous/AutonomousBuilder.java @@ -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; diff --git a/src/main/java/frc4388/robot/commands/PathPlannerCommand.java b/src/main/java/frc4388/robot/commands/autonomous/PathPlannerCommand.java similarity index 93% rename from src/main/java/frc4388/robot/commands/PathPlannerCommand.java rename to src/main/java/frc4388/robot/commands/autonomous/PathPlannerCommand.java index 5e214b9..f6d3f30 100644 --- a/src/main/java/frc4388/robot/commands/PathPlannerCommand.java +++ b/src/main/java/frc4388/robot/commands/autonomous/PathPlannerCommand.java @@ -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 { diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/PathRecorder.java b/src/main/java/frc4388/robot/commands/shuffleboard/PathRecorder.java index e9d9209..7f938c9 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/PathRecorder.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/PathRecorder.java @@ -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); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4f63806..93047c5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -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; } }