From 6244a33a7936f295de8bbfa7844ff090958c615a Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 8 Jan 2024 20:37:17 -0800 Subject: [PATCH] chore: set all indents to 4 spaces and removed all file headers --- build.gradle | 4 +- .../org/team1540/robot2024/Constants.java | 71 ++-- .../java/org/team1540/robot2024/Main.java | 31 +- .../java/org/team1540/robot2024/Robot.java | 286 ++++++++------- .../team1540/robot2024/RobotContainer.java | 217 ++++++----- .../robot2024/commands/DriveCommands.java | 15 +- .../commands/FeedForwardCharacterization.java | 148 ++++---- .../robot2024/subsystems/drive/Drive.java | 13 - .../robot2024/subsystems/drive/GyroIO.java | 30 +- .../subsystems/drive/GyroIOPigeon2.java | 49 +-- .../robot2024/subsystems/drive/Module.java | 337 +++++++++--------- .../robot2024/subsystems/drive/ModuleIO.java | 13 - .../subsystems/drive/ModuleIOSim.java | 13 - .../subsystems/drive/ModuleIOTalonFX.java | 13 - .../robot2024/util/LocalADStarAK.java | 228 ++++++------ .../robot2024/util/PolynomialRegression.java | 336 ++++++++--------- 16 files changed, 855 insertions(+), 949 deletions(-) diff --git a/build.gradle b/build.gradle index 3483186f..1bff1c2f 100755 --- a/build.gradle +++ b/build.gradle @@ -136,7 +136,7 @@ gversion { classPackage = "org.team1540.robot2024" className = "BuildConstants" dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" - indent = " " + timeZone = "America/Los_Angeles" + indent = " " } diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index d129bb24..0794e260 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024; import edu.wpi.first.math.util.Units; @@ -24,30 +11,36 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.SIM; - - public static enum Mode { - /** Running on a real robot. */ - REAL, - - /** Running a physics simulator. */ - SIM, - - /** Replaying from a log file. */ - REPLAY - } - - public static class Drivetrain { - public static final String CAN_BUS = ""; - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; - public static final boolean IS_TURN_MOTOR_INVERTED = true; - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - } + public static final Mode currentMode = Mode.SIM; + + public static enum Mode { + /** + * Running on a real robot. + */ + REAL, + + /** + * Running a physics simulator. + */ + SIM, + + /** + * Replaying from a log file. + */ + REPLAY + } + + public static class Drivetrain { + public static final String CAN_BUS = ""; + public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + public static final boolean IS_TURN_MOTOR_INVERTED = true; + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + } } diff --git a/src/main/java/org/team1540/robot2024/Main.java b/src/main/java/org/team1540/robot2024/Main.java index 991ccea3..6607b92e 100644 --- a/src/main/java/org/team1540/robot2024/Main.java +++ b/src/main/java/org/team1540/robot2024/Main.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024; import edu.wpi.first.wpilibj.RobotBase; @@ -21,14 +8,14 @@ * call. */ public final class Main { - private Main() {} + private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index 1944f44b..52709436 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/src/main/java/org/team1540/robot2024/Robot.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024; import edu.wpi.first.wpilibj2.command.Command; @@ -29,132 +16,161 @@ * project. */ public class Robot extends LoggedRobot { - private Command autonomousCommand; - private RobotContainer robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; + private Command autonomousCommand; + private RobotContainer robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger + Logger.start(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); + } + + /** + * This function is called periodically during all modes. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once when the robot is disabled. + */ + @Override + public void disabledInit() { + } + + /** + * This function is called periodically when disabled. + */ + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + autonomousCommand = robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } } - // Set up data receivers & replay source - switch (Constants.currentMode) { - case REAL: - // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case SIM: - // Running a physics simulator, log to NT - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); - break; + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { } - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - - // Start AdvantageKit logger - Logger.start(); - - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. - robotContainer = new RobotContainer(); - } - - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled commands, running already-scheduled commands, removing - // finished or interrupted commands, and running subsystem periodic() methods. - // This must be called from the robot's periodic block in order for anything in - // the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() {} - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - autonomousCommand = robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (autonomousCommand != null) { - autonomousCommand.schedule(); + /** + * This function is called once when teleop is enabled. + */ + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (autonomousCommand != null) { - autonomousCommand.cancel(); + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + /** + * This function is called once when test mode is enabled. + */ + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } + + /** + * This function is called once when the robot is first started up. + */ + @Override + public void simulationInit() { + } + + /** + * This function is called periodically whilst in simulation. + */ + @Override + public void simulationPeriodic() { } - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6e41e215..a891791d 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024; import com.pathplanner.lib.auto.AutoBuilder; @@ -35,106 +22,114 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Subsystems - private final Drive drive; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - private final CommandXboxController controllerCopilot = new CommandXboxController(1); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - drive = - new Drive( - new GyroIONavx(), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.FRONT_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.FRONT_RIGHT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.BACK_LEFT)), - new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.BACK_RIGHT))); - // drive = new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - // flywheel = new Flywheel(new FlywheelIOTalonFX()); - break; - - case SIM: - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - break; - - default: - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - break; + // Subsystems + private final Drive drive; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController controllerCopilot = new CommandXboxController(1); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + drive = + new Drive( + new GyroIONavx(), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(1, SwerveFactory.SwerveCorner.FRONT_LEFT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(7, SwerveFactory.SwerveCorner.FRONT_RIGHT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(4, SwerveFactory.SwerveCorner.BACK_LEFT)), + new ModuleIOTalonFX(SwerveFactory.getModuleMotors(3, SwerveFactory.SwerveCorner.BACK_RIGHT))); + // drive = new Drive( + // new GyroIOPigeon2(), + // new ModuleIOTalonFX(0), + // new ModuleIOTalonFX(1), + // new ModuleIOTalonFX(2), + // new ModuleIOTalonFX(3)); + // flywheel = new Flywheel(new FlywheelIOTalonFX()); + break; + + case SIM: + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + break; + + default: + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); + break; + } + + + // Set up auto routines + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + + // Set up FF characterization routines + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity)); + + // Configure the button bindings + configureButtonBindings(); } + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); - // Set up auto routines - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - - // Set up FF characterization routines - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity)); - - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - - } + } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/org/team1540/robot2024/commands/DriveCommands.java b/src/main/java/org/team1540/robot2024/commands/DriveCommands.java index d89ea257..f6bab7e3 100644 --- a/src/main/java/org/team1540/robot2024/commands/DriveCommands.java +++ b/src/main/java/org/team1540/robot2024/commands/DriveCommands.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.commands; import edu.wpi.first.math.MathUtil; @@ -45,7 +32,7 @@ public static Command joystickDrive(Drive drive, DoubleSupplier xSupplier, Doubl linearMagnitude = linearMagnitude * linearMagnitude; omega = Math.copySign(omega * omega, omega); - // Calcaulate new linear velocity + // Calculate new linear velocity Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection).transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); // Convert to field relative speeds & send command diff --git a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java index df35e2b8..1b5903eb 100644 --- a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java +++ b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java @@ -1,106 +1,96 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.commands; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import org.team1540.robot2024.util.PolynomialRegression; + import java.util.LinkedList; import java.util.List; import java.util.function.Consumer; import java.util.function.Supplier; public class FeedForwardCharacterization extends Command { - private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_VOLTS_PER_SEC = 0.1; + private static final double START_DELAY_SECS = 2.0; + private static final double RAMP_VOLTS_PER_SEC = 0.1; - private FeedForwardCharacterizationData data; - private final Consumer voltageConsumer; - private final Supplier velocitySupplier; + private FeedForwardCharacterizationData data; + private final Consumer voltageConsumer; + private final Supplier velocitySupplier; - private final Timer timer = new Timer(); + private final Timer timer = new Timer(); - /** Creates a new FeedForwardCharacterization command. */ - public FeedForwardCharacterization( - Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { - addRequirements(subsystem); - this.voltageConsumer = voltageConsumer; - this.velocitySupplier = velocitySupplier; - } + /** + * Creates a new FeedForwardCharacterization command. + */ + public FeedForwardCharacterization( + Subsystem subsystem, Consumer voltageConsumer, Supplier velocitySupplier) { + addRequirements(subsystem); + this.voltageConsumer = voltageConsumer; + this.velocitySupplier = velocitySupplier; + } - // Called when the command is initially scheduled. - @Override - public void initialize() { - data = new FeedForwardCharacterizationData(); - timer.reset(); - timer.start(); - } + // Called when the command is initially scheduled. + @Override + public void initialize() { + data = new FeedForwardCharacterizationData(); + timer.reset(); + timer.start(); + } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (timer.get() < START_DELAY_SECS) { - voltageConsumer.accept(0.0); - } else { - double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; - voltageConsumer.accept(voltage); - data.add(velocitySupplier.get(), voltage); + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (timer.get() < START_DELAY_SECS) { + voltageConsumer.accept(0.0); + } else { + double voltage = (timer.get() - START_DELAY_SECS) * RAMP_VOLTS_PER_SEC; + voltageConsumer.accept(voltage); + data.add(velocitySupplier.get(), voltage); + } } - } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - voltageConsumer.accept(0.0); - timer.stop(); - data.print(); - } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + voltageConsumer.accept(0.0); + timer.stop(); + data.print(); + } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } - public static class FeedForwardCharacterizationData { - private final List velocityData = new LinkedList<>(); - private final List voltageData = new LinkedList<>(); + public static class FeedForwardCharacterizationData { + private final List velocityData = new LinkedList<>(); + private final List voltageData = new LinkedList<>(); - public void add(double velocity, double voltage) { - if (Math.abs(velocity) > 1E-4) { - velocityData.add(Math.abs(velocity)); - voltageData.add(Math.abs(voltage)); - } - } + public void add(double velocity, double voltage) { + if (Math.abs(velocity) > 1E-4) { + velocityData.add(Math.abs(velocity)); + voltageData.add(Math.abs(voltage)); + } + } - public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { - return; - } + public void print() { + if (velocityData.size() == 0 || voltageData.size() == 0) { + return; + } - PolynomialRegression regression = - new PolynomialRegression( - velocityData.stream().mapToDouble(Double::doubleValue).toArray(), - voltageData.stream().mapToDouble(Double::doubleValue).toArray(), - 1); + PolynomialRegression regression = + new PolynomialRegression( + velocityData.stream().mapToDouble(Double::doubleValue).toArray(), + voltageData.stream().mapToDouble(Double::doubleValue).toArray(), + 1); - System.out.println("FF Characterization Results:"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + System.out.println("FF Characterization Results:"); + System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); + System.out.println(String.format("\tR2=%.5f", regression.R2())); + System.out.println(String.format("\tkS=%.5f", regression.beta(0))); + System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + } } - } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java index 04827c14..2850f663 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drive.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import com.pathplanner.lib.auto.AutoBuilder; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIO.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIO.java index 3212fa90..eded43af 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIO.java @@ -1,29 +1,17 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double yawVelocityRadPerSec = 0.0; - public double time = 0.0; - } + @AutoLog + class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + public double time = 0.0; + } - public default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputs inputs) { + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java index a867fe65..4de64a98 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; @@ -21,24 +8,26 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -/** IO implementation for Pigeon2 */ +/** + * IO implementation for Pigeon2 + */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal yaw = pigeon.getYaw(); - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZDevice(); + private final Pigeon2 pigeon = new Pigeon2(20); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZDevice(); - public GyroIOPigeon2() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(100.0); - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - } + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - } + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java index b9599180..151e8839 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import edu.wpi.first.math.controller.PIDController; @@ -23,164 +10,186 @@ public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private boolean forceTurn = false; + private double lastPositionMeters = 0.0; // Used for delta calculation + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + driveFeedback = new PIDController(0.05, 0.0, 0.0); + turnFeedback = new PIDController(7.0, 0.0, 0.0); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + driveFeedback = new PIDController(0.1, 0.0, 0.0); + turnFeedback = new PIDController(10.0, 0.0, 0.0); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / Constants.Drivetrain.WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); + } + } + } + + public SwerveModuleState runSetpoint(SwerveModuleState state) { + return runSetpoint(state, false); + } + + /** + * Runs the module with the specified setpoint state. Returns the optimized state. + */ + public SwerveModuleState runSetpoint(SwerveModuleState state, boolean forceTurn) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = ((Math.abs(optimizedState.speedMetersPerSecond) <= (Constants.Drivetrain.MAX_LINEAR_SPEED * 0.01)) && !forceTurn) ? angleSetpoint : optimizedState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering. + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** + * Runs the module with the specified voltage while controlling to zero degrees. + */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private boolean forceTurn = false; - private double lastPositionMeters = 0.0; // Used for delta calculation - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - turnFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; } - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } + /** + * Disables all outputs to motors. + */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + /** + * Sets whether brake mode is enabled. + */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); } - // Run closed loop turn control - if (angleSetpoint != null) { - - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / Constants.Drivetrain.WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } + /** + * Returns the current turn angle of the module. + */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } } - } - - public SwerveModuleState runSetpoint(SwerveModuleState state) { - return runSetpoint(state, false); - } - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state, boolean forceTurn) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = ((Math.abs(optimizedState.speedMetersPerSecond) <= (Constants.Drivetrain.MAX_LINEAR_SPEED * 0.01)) && !forceTurn) ? angleSetpoint : optimizedState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering. - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); + + /** + * Returns the current drive position of the module in meters. + */ + public double getPositionMeters() { + return inputs.drivePositionRad * Constants.Drivetrain.WHEEL_RADIUS; + } + + /** + * Returns the current drive velocity of the module in meters per second. + */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * Constants.Drivetrain.WHEEL_RADIUS; + } + + /** + * Returns the module position (turn angle and drive position). + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** + * Returns the module position delta since the last call to this method. + */ + public SwerveModulePosition getPositionDelta() { + var delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle()); + lastPositionMeters = getPositionMeters(); + return delta; + } + + /** + * Returns the module state (turn angle and drive velocity). + */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** + * Returns the drive velocity in radians/sec. + */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * Constants.Drivetrain.WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * Constants.Drivetrain.WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module position delta since the last call to this method. */ - public SwerveModulePosition getPositionDelta() { - var delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle()); - lastPositionMeters = getPositionMeters(); - return delta; - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java index fbf3068d..269aee93 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java index 9923a119..320c9cc7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import edu.wpi.first.math.MathUtil; diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java index a3fbf8af..1b6d2c5d 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java @@ -1,16 +1,3 @@ -// Copyright 2021-2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - package org.team1540.robot2024.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; diff --git a/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java b/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java index 217e344b..4a9e30f9 100644 --- a/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java +++ b/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java @@ -8,9 +8,11 @@ import com.pathplanner.lib.pathfinding.Pathfinder; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; + import java.util.ArrayList; import java.util.Collections; import java.util.List; + import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; @@ -19,133 +21,133 @@ // https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } + private final ADStarIO io = new ADStarIO(); - Logger.processInputs("LocalADStarAK", io); + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } - if (io.currentPathPoints.isEmpty()) { - return null; - } + Logger.processInputs("LocalADStarAK", io); - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); + return io.isNewPathAvailable; } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - - private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); + Logger.processInputs("LocalADStarAK", io); - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + if (io.currentPathPoints.isEmpty()) { + return null; + } - List pathPoints = new ArrayList<>(); - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add( - new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } - currentPathPoints = pathPoints; + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } } - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } } - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } } - } } diff --git a/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java b/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java index 8b7ef39a..1d7a2479 100644 --- a/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java +++ b/src/main/java/org/team1540/robot2024/util/PolynomialRegression.java @@ -23,183 +23,185 @@ * @author Kevin Wayne */ public class PolynomialRegression implements Comparable { - private final String variableName; // name of the predictor variable - private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error - private double sst; // total sum of squares - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name - * of the predictor variable. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree) { - this(x, y, degree, "n"); - } - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @param variableName the name of the predictor variable - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { - this.degree = degree; - this.variableName = variableName; - - int n = x.length; - QRDecomposition qr = null; - Matrix matrixX = null; - - // in case Vandermonde matrix does not have full rank, reduce degree until it - // does - while (true) { - - // build Vandermonde matrix - double[][] vandermonde = new double[n][this.degree + 1]; - for (int i = 0; i < n; i++) { - for (int j = 0; j <= this.degree; j++) { - vandermonde[i][j] = Math.pow(x[i], j); + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; + + // decrease degree and try again + this.degree--; } - } - matrixX = new Matrix(vandermonde); - // find least squares solution - qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; - // decrease degree and try again - this.degree--; + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); } - // create matrix from vector - Matrix matrixY = new Matrix(y, n); + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } - // linear regression coefficients - beta = qr.solve(matrixY); + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } - // mean of y[] values - double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; - double mean = sum / n; + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } - // total variation to be accounted for - for (int i = 0; i < n; i++) { - double dev = y[i] - mean; - sst += dev * dev; + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; } - // variation not accounted for - Matrix residuals = matrixX.times(beta).minus(matrixY); - sse = residuals.norm2() * residuals.norm2(); - } - - /** - * Returns the {@code j}th regression coefficient. - * - * @param j the index - * @return the {@code j}th regression coefficient - */ - public double beta(int j) { - // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; - return beta.get(j, 0); - } - - /** - * Returns the degree of the polynomial to fit. - * - * @return the degree of the polynomial to fit - */ - public int degree() { - return degree; - } - - /** - * Returns the coefficient of determination R2. - * - * @return the coefficient of determination R2, which is a real number between - * 0 and 1 - */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function - return 1.0 - sse / sst; - } - - /** - * Returns the expected response {@code y} given the value of the predictor variable {@code x}. - * - * @param x the value of the predictor variable - * @return the expected response {@code y} given the value of the predictor variable {@code x} - */ - public double predict(double x) { - // horner's method - double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); - return y; - } - - /** - * Returns a string representation of the polynomial regression model. - * - * @return a string representation of the polynomial regression model, including the best-fit - * polynomial and the coefficient of determination R2 - */ - public String toString() { - StringBuilder s = new StringBuilder(); - int j = degree; - - // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; - - // create remaining terms - while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); - else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); - j--; + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.10f ", beta(j))); + else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); - - // replace "+ -2n" with "- 2n" - return s.toString().replace("+ -", "- "); - } - - /** Compare lexicographically. */ - public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; - int maxDegree = Math.max(this.degree(), that.degree()); - for (int j = maxDegree; j >= 0; j--) { - double term1 = 0.0; - double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; + + /** + * Compare lexicographically. + */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; + } + return 0; + } + + /** + * Unit tests the {@code PolynomialRegression} data type. + * + * @param args the command-line arguments + */ + public static void main(String[] args) { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + System.out.println(regression); } - return 0; - } - - /** - * Unit tests the {@code PolynomialRegression} data type. - * - * @param args the command-line arguments - */ - public static void main(String[] args) { - double[] x = {10, 20, 40, 80, 160, 200}; - double[] y = {100, 350, 1500, 6700, 20160, 40000}; - PolynomialRegression regression = new PolynomialRegression(x, y, 3); - - System.out.println(regression); - } }