diff --git a/build.gradle b/build.gradle index 598ccb9..a3ae5e3 100644 --- a/build.gradle +++ b/build.gradle @@ -155,4 +155,5 @@ dokkaHtml { description = 'Generates HTML documentation using Dokka' group = 'documentation' outputDirectory.set(file("${layout.buildDirectory.get().asFile.path}${File.separator}dokka")) + suppressInheritedMembers = true } diff --git a/src/main/java/frc/robot/Main.kt b/src/main/java/frc/robot/Main.java similarity index 50% rename from src/main/java/frc/robot/Main.kt rename to src/main/java/frc/robot/Main.java index c592be2..533c98d 100644 --- a/src/main/java/frc/robot/Main.kt +++ b/src/main/java/frc/robot/Main.java @@ -1,15 +1,14 @@ -package frc.robot +package frc.robot; -import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj.RobotBase; -object Main { +public final class Main { /** * Main initialization function. Do not perform any initialization here. * * If you change your main robot class, change the parameter type. */ - @JvmStatic - fun main(args: Array) { - RobotBase.startRobot { Robot() } + public static void main(String... args) { + RobotBase.startRobot(Robot::new); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..9b6b34a --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,87 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * 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() { + robotContainer = new RobotContainer(); + + Logger.recordMetadata("ProjectName", "SwerveBaseTemplate"); + + if (isReal()) { + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + new PowerDistribution( + 1, PowerDistribution.ModuleType.kRev); // Enables power distribution logging + } else { + setUseTiming(false); // Run as fast as possible + String logPath = + LogFileUtil + .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver( + new WPILOGWriter( + LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + } + + // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the + // "Understanding Data Flow" page + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may + // be added. + } + + /** + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + autonomousCommand = robotContainer.getAutonomousCommand(); + autonomousCommand.schedule(); + } + + /** This function is called once when teleop mode is initialized. */ + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + /** This function is called once when test mode is initialized. */ + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } +} diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt deleted file mode 100644 index 96dc955..0000000 --- a/src/main/java/frc/robot/Robot.kt +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -package frc.robot - -import edu.wpi.first.wpilibj.PowerDistribution -import edu.wpi.first.wpilibj.Timer -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.CommandScheduler -import frc.robot.utils.dash -import org.littletonrobotics.junction.LogFileUtil -import org.littletonrobotics.junction.LoggedRobot -import org.littletonrobotics.junction.Logger -import org.littletonrobotics.junction.networktables.NT4Publisher -import org.littletonrobotics.junction.wpilog.WPILOGReader -import org.littletonrobotics.junction.wpilog.WPILOGWriter - -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -class Robot : LoggedRobot() { - private var autonomousCommand: Command? = null - - private var robotContainer: RobotContainer? = null - - private val timer = Timer() - private val avgTimer = Timer() - private var timesRan = 0 - private val timesRanArr = ArrayList() - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - override fun robotInit() { - avgTimer.start() - timer.start() - robotContainer = RobotContainer() - - Logger.recordMetadata("ProjectName", "SwerveBaseTemplate") - - if (isReal()) { - Logger.addDataReceiver(WPILOGWriter()) - // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(NT4Publisher()) - // Publish data to NetworkTables - PowerDistribution(1, PowerDistribution.ModuleType.kRev) - // Enables power distribution logging - } else { - setUseTiming(false) - // Run as fast as possible - val logPath = LogFileUtil.findReplayLog() - // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(WPILOGReader(logPath)) - // Read replay log - Logger.addDataReceiver(WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))) - // Save outputs to a new log - } - - // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the - // "Understanding Data Flow" page - Logger - .start() // Start logging! No more data receivers, replay sources, or metadata values may be - // added. - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - * This runs after the mode specific periodic functions, but before LiveWindow and SmartDashboard - * integrated updating. - */ - override fun robotPeriodic() { - CommandScheduler.getInstance().run() - timesRan++ - - if (timer.advanceIfElapsed(1.0)) { - println("1 second has passed, times ran is $timesRan") - dash("Hurtz Ketchup" to timesRan) - timesRanArr.add(timesRan) - timesRan = 0 - } - - if (avgTimer.advanceIfElapsed(15.0)) { - val avgTimesRan = - timesRanArr - .stream() - .mapToInt { a: Int? -> a!! } - .average() - .orElse(404.0) // Something's wrong - println("15 seconds have passed, times ran is $avgTimesRan") - dash("Hurtz Ketchup Average" to avgTimesRan) - timesRanArr.clear() - } - } - - /** This function is called once each time the robot enters Disabled mode. */ - override fun disabledInit() {} - - override fun disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your [RobotContainer] class. */ - override fun autonomousInit() { - autonomousCommand = robotContainer!!.autonomousCommand.also { it.schedule() } - } - - /** This function is called periodically during autonomous. */ - override fun autonomousPeriodic() {} - - override fun teleopInit() { - if (autonomousCommand != null) { - autonomousCommand!!.cancel() - } - } - - /** This function is called periodically during operator control. */ - override fun teleopPeriodic() {} - - override fun testInit() { - CommandScheduler.getInstance().cancelAll() - } - - /** This function is called periodically during test mode. */ - override fun testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - override fun simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - override fun simulationPeriodic() {} -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..38ca63d --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +package frc.robot; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.*; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.PadDrive; +import frc.robot.subsystems.Photonvision; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.utils.LogitechGamingPad; +import frc.robot.utils.RobotParameters.SwerveParameters.*; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and trigger mappings) should be declared here. + */ +public class RobotContainer { + private final SwerveSubsystem swerveSubsystem; + private final Photonvision photonvision; + + private final JoystickButton padA; + private final JoystickButton padB; + private final JoystickButton padX; + private final JoystickButton padY; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + LogitechGamingPad pad = new LogitechGamingPad(0); + padA = new JoystickButton(pad, 1); + padB = new JoystickButton(pad, 2); + padX = new JoystickButton(pad, 3); + padY = new JoystickButton(pad, 4); + + photonvision = new Photonvision(); + swerveSubsystem = new SwerveSubsystem(photonvision); + swerveSubsystem.setDefaultCommand( + new PadDrive(swerveSubsystem, pad, Thresholds.IS_FIELD_ORIENTED)); + + configureBindings(); + } + + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger} or our {@link JoystickButton} constructor with an arbitrary predicate, or via + * the named factories in {@link CommandGenericHID}'s subclasses for {@link + * CommandXboxController}/{@link CommandPS4Controller} controllers or {@link CommandJoystick}. + */ + private void configureBindings() { + // padA.onTrue(new InstantCommand(swerveSubsystem::addRotorPositionsforModules)); + // padB.onTrue(new InstantCommand(swerveSubsystem::zeroHeading)); + // padY.onTrue(new InstantCommand(swerveSubsystem::configAAcornMode)); + // padX.onTrue(new InstantCommand(swerveSubsystem::configSlowMode)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new PathPlannerAuto("Straight Auto"); + } +} diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt deleted file mode 100644 index cee06cb..0000000 --- a/src/main/java/frc/robot/RobotContainer.kt +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -package frc.robot - -import com.pathplanner.lib.commands.PathPlannerAuto -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.button.JoystickButton -import frc.robot.commands.PadDrive -import frc.robot.subsystems.Photonvision -import frc.robot.subsystems.SwerveSubsystem -import frc.robot.utils.LogitechGamingPad -import frc.robot.utils.RobotParameters.SwerveParameters - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the [Robot] - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ -class RobotContainer { - private val swerveSubsystem: SwerveSubsystem - private val photonvision: Photonvision - - private val padA: JoystickButton - private val padB: JoystickButton - private val padX: JoystickButton - private val padY: JoystickButton - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - init { - val pad = LogitechGamingPad(0) - padA = JoystickButton(pad, 1) - padB = JoystickButton(pad, 2) - padX = JoystickButton(pad, 3) - padY = JoystickButton(pad, 4) - - photonvision = Photonvision() - swerveSubsystem = SwerveSubsystem(photonvision) - swerveSubsystem.defaultCommand = - PadDrive(swerveSubsystem, pad, SwerveParameters.Thresholds.IS_FIELD_ORIENTED) - - configureBindings() - } - - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * [Trigger.Trigger] or our [JoystickButton] constructor with an arbitrary predicate, or via the - * named factories in [ ]'s subclasses for [ ]/[ - * PS4][edu.wpi.first.wpilibj2.command.button.CommandPS4Controller] controllers or - * [Flight][edu.wpi.first.wpilibj2.command.button.CommandJoystick]. - */ - private fun configureBindings() { - // padA.onTrue(new InstantCommand(swerveSubsystem::addRotorPositionsforModules)); - // padB.onTrue(new InstantCommand(swerveSubsystem::zeroHeading)); - // padY.onTrue(new InstantCommand(swerveSubsystem::configAAcornMode)); - // padX.onTrue(new InstantCommand(swerveSubsystem::configSlowMode)); - } - - val autonomousCommand: Command - /** - * Use this to pass the autonomous command to the main [Robot] class. - * - * @return the command to run in autonomous - */ - get() = PathPlannerAuto("Straight Auto") -} diff --git a/src/main/java/frc/robot/commands/PadDrive.java b/src/main/java/frc/robot/commands/PadDrive.java new file mode 100644 index 0000000..a76c260 --- /dev/null +++ b/src/main/java/frc/robot/commands/PadDrive.java @@ -0,0 +1,85 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.utils.Dash; +import frc.robot.utils.LogitechGamingPad; +import frc.robot.utils.RobotParameters; +import frc.robot.utils.RobotParameters.SwerveParameters.Thresholds; + +/** Command to control the robot's swerve drive using a Logitech gaming pad. */ +public class PadDrive extends Command { + private final SwerveSubsystem swerveSubsystem; + private final LogitechGamingPad pad; + private final boolean isFieldOriented; + + /** + * Constructs a new PadDrive command. + * + * @param swerveSubsystem The swerve subsystem used by this command. + * @param pad The Logitech gaming pad used to control the robot. + * @param isFieldOriented Whether the drive is field-oriented. + */ + public PadDrive(SwerveSubsystem swerveSubsystem, LogitechGamingPad pad, boolean isFieldOriented) { + this.swerveSubsystem = swerveSubsystem; + this.pad = pad; + this.isFieldOriented = isFieldOriented; + addRequirements(this.swerveSubsystem); + } + + /** + * Called every time the scheduler runs while the command is scheduled. This method retrieves the + * current position from the gaming pad, calculates the rotation, logs the joystick values, and + * sets the drive speeds for the swerve subsystem. + */ + @Override + public void execute() { + Coordinate position = positionSet(pad); + + double rotation = + -pad.getRightAnalogXAxis() * RobotParameters.MotorParameters.MAX_ANGULAR_SPEED; + if (Math.abs(pad.getRightAnalogXAxis()) < 0.2) { + rotation = 0.0; + } + + Dash.dash( + Dash.pairOf("X Joystick", position.x()), + Dash.pairOf("Y Joystick", position.y()), + Dash.pairOf("Rotation", rotation)); + + swerveSubsystem.setDriveSpeeds(position.y(), position.x(), rotation * 0.8, isFieldOriented); + } + + /** + * Returns true when the command should end. + * + * @return Always returns false, as this command never ends on its own. + */ + @Override + public boolean isFinished() { + return false; + } + + /** Record representing a coordinate with x and y values. */ + public record Coordinate(double x, double y) {} + + /** + * Sets the position based on the input from the Logitech gaming pad. + * + * @param pad The Logitech gaming pad. + * @return The coordinate representing the position. + */ + public static Coordinate positionSet(LogitechGamingPad pad) { + double x = -pad.getLeftAnalogXAxis() * RobotParameters.MotorParameters.MAX_SPEED; + if (Math.abs(x) < Thresholds.X_DEADZONE) { + x = 0.0; + } + + double y = -pad.getLeftAnalogYAxis() * RobotParameters.MotorParameters.MAX_SPEED; + if (Math.abs(y) < Thresholds.Y_DEADZONE) { + y = 0.0; + } + + return new Coordinate(x, y); + } +} diff --git a/src/main/java/frc/robot/commands/PadDrive.kt b/src/main/java/frc/robot/commands/PadDrive.kt deleted file mode 100644 index dddc998..0000000 --- a/src/main/java/frc/robot/commands/PadDrive.kt +++ /dev/null @@ -1,73 +0,0 @@ -package frc.robot.commands - -import edu.wpi.first.wpilibj2.command.Command -import frc.robot.subsystems.SwerveSubsystem -import frc.robot.utils.LogitechGamingPad -import frc.robot.utils.RobotParameters.MotorParameters -import frc.robot.utils.RobotParameters.SwerveParameters.Thresholds.X_DEADZONE -import frc.robot.utils.RobotParameters.SwerveParameters.Thresholds.Y_DEADZONE -import frc.robot.utils.dash -import kotlin.math.abs - -/** Command to control the robot's swerve drive using a Logitech gaming pad. */ -class PadDrive( - private val swerveSubsystem: SwerveSubsystem, - private val pad: LogitechGamingPad, - private val isFieldOriented: Boolean, -) : Command() { - init { - addRequirements(this.swerveSubsystem) - } - - /** - * Called every time the scheduler runs while the command is scheduled. This method retrieves the - * current position from the gaming pad, calculates the rotation, logs the joystick values, and - * sets the drive speeds for the swerve subsystem. - */ - override fun execute() { - val position = positionSet(pad) - - var rotation = -pad.rightAnalogXAxis * MotorParameters.MAX_ANGULAR_SPEED - if (abs(pad.rightAnalogXAxis) < 0.2) { - rotation = 0.0 - } - - dash("X Joystick" to position.x, "Y Joystick" to position.y, "Rotation" to rotation) - - swerveSubsystem.setDriveSpeeds(position.y, position.x, rotation * 0.8, isFieldOriented) - } - - /** - * Returns true when the command should end. - * - * @return Always returns false, as this command never ends on its own. - */ - override fun isFinished(): Boolean { - return false - } - - /** Record representing a coordinate with x and y values. */ - @JvmRecord data class Coordinate(val x: Double, val y: Double) - - companion object { - /** - * Sets the position based on the input from the Logitech gaming pad. - * - * @param pad The Logitech gaming pad. - * @return The coordinate representing the position. - */ - fun positionSet(pad: LogitechGamingPad): Coordinate { - var x = -pad.leftAnalogXAxis * MotorParameters.MAX_SPEED - if (abs(x) < X_DEADZONE) { - x = 0.0 - } - - var y = -pad.leftAnalogYAxis * MotorParameters.MAX_SPEED - if (abs(y) < Y_DEADZONE) { - y = 0.0 - } - - return Coordinate(x, y) - } - } -} diff --git a/src/main/java/frc/robot/subsystems/Photonvision.java b/src/main/java/frc/robot/subsystems/Photonvision.java new file mode 100644 index 0000000..b866ac0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Photonvision.java @@ -0,0 +1,178 @@ +package frc.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.Dash; +import frc.robot.utils.Extensions; +import frc.robot.utils.RobotParameters.PhotonVisionConstants; +import java.util.List; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** The PhotonVision subsystem handles vision processing using PhotonVision cameras. */ +public class Photonvision extends SubsystemBase { + // PhotonVision cameras + private PhotonCamera camera = new PhotonCamera("Camera"); + + // Pose estimator for determining the robot's position on the field + private PhotonPoseEstimator photonPoseEstimator; + + private final Translation2d cameraTrans = new Translation2d(0.31, 0.0); + + // AprilTag field layout for the 2024 Crescendo field + private AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); + + // Transformation from the robot to the camera + private Transform3d cameraPos = + new Transform3d( + Extensions.dimensionIncrease(cameraTrans, PhotonVisionConstants.CAMERA_ONE_HEIGHT_METER), + new Rotation3d( + 0.0, + Math.toRadians(360 - PhotonVisionConstants.CAMERA_ONE_ANGLE_DEG), + Math.toRadians(180.0))); + + private PhotonTrackedTarget target = null; + private boolean targetVisible = false; + private double yaw = -15.0; + private double targetPoseAmbiguity = 7157.0; + private double range = 0.0; + private double rangeToTarget = 0.0; + private List result; + private PhotonPipelineResult currentResult = null; + private boolean camTag = false; + private Translation3d currentPose = null; + + /** Constructs a new PhotonVision subsystem. */ + public Photonvision() { + // Initialize result + result = camera.getAllUnreadResults(); + photonPoseEstimator = + new PhotonPoseEstimator( + aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraPos); + } + + /** + * This method is called periodically by the scheduler. It updates the tracked targets and + * displays relevant information on the SmartDashboard. + */ + @Override + public void periodic() { + result = camera.getAllUnreadResults(); + currentResult = result.isEmpty() ? null : result.get(0); + + if (currentResult == null) return; + + photonPoseEstimator.update(currentResult); + + target = currentResult.getBestTarget(); + targetPoseAmbiguity = target != null ? target.getPoseAmbiguity() : 7157.0; + + for (PhotonTrackedTarget tag : currentResult.getTargets()) { + if (tag.getFiducialId() == 7 || tag.getFiducialId() == 4) { + yaw = tag.getYaw(); + } + } + + Dash.dash( + Dash.pairOf("yaw to target", yaw), + Dash.pairOf("range target", rangeToTarget), + Dash.pairOf("april tag distance", getDistanceSubwoofer()), + Dash.pairOf("april tag yaw", getSubwooferYaw()), + Dash.pairOf("cam ambiguity", targetPoseAmbiguity), + Dash.pairOf("_targets", currentResult.hasTargets())); + } + + /** + * Checks if there is a tag. + * + * @return true if there is a tag, false otherwise. + */ + public boolean hasTag() { + return currentResult != null && currentResult.hasTargets(); + } + + /** + * Gets the estimated global pose of the robot. + * + * @param prevEstimatedRobotPose The previous estimated pose of the robot. + * @return The estimated robot pose, or null if no pose could be estimated. + */ + public EstimatedRobotPose getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return currentResult != null ? photonPoseEstimator.update(currentResult).orElse(null) : null; + } + + /** + * Gets the estimated global pose of the robot. + * + * @return The estimated global pose of the robot. + */ + public Transform3d getEstimatedGlobalPose() { + return currentResult != null && currentResult.getMultiTagResult().isPresent() + ? currentResult.getMultiTagResult().get().estimatedPose.best + : new Transform3d(0.0, 0.0, 0.0, new Rotation3d()); + } + + /** + * Gets the forward distance to the target. + * + * @return The forward distance to the target. + */ + public double getPivotPosition() { + // 10/14/2024 outside tuning + // jayden why are you so bad at tuning + // Desmos: https://www.desmos.com/calculator/naalukjxze + double r = getDistanceSubwoofer() + 0.6; + double f = -1.39223; // power 5 + double e = 20.9711; // power 4 + double d = -122.485; // power 3 + double c = 342.783; // power 2 + double b = -447.743; // power 1 + double a = 230.409; // constant + + return (f * Math.pow(r, 5.0)) + + (e * Math.pow(r, 4.0)) + + (d * Math.pow(r, 3.0)) + + (c * Math.pow(r, 2.0)) + + (b * r) + + a; + } + + /** + * Gets the distance to the subwoofer. + * + * @return The distance to the subwoofer. + */ + public double getDistanceSubwoofer() { + currentPose = getEstimatedGlobalPose().getTranslation(); + if (currentPose != null) { + if (DriverStation.getAlliance().isEmpty() + || DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + return Math.sqrt( + Math.pow(currentPose.getX() - 16.5, 2.0) + Math.pow(currentPose.getY() - 5.5, 2.0)); + } else { + return Math.sqrt( + Math.pow(currentPose.getX(), 2.0) + Math.pow(currentPose.getY() - 5.5, 2.0)); + } + } else { + return 687.0; + } + } + + /** + * Gets the yaw of the subwoofer. + * + * @return The yaw of the subwoofer. + */ + public double getSubwooferYaw() { + return 180 - Math.toDegrees(getEstimatedGlobalPose().getRotation().getAngle()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Photonvision.kt b/src/main/java/frc/robot/subsystems/Photonvision.kt deleted file mode 100644 index bdf1367..0000000 --- a/src/main/java/frc/robot/subsystems/Photonvision.kt +++ /dev/null @@ -1,166 +0,0 @@ -@file:Suppress("MemberVisibilityCanBePrivate", "unused") - -package frc.robot.subsystems - -import edu.wpi.first.apriltag.AprilTagFieldLayout -import edu.wpi.first.apriltag.AprilTagFields -import edu.wpi.first.math.geometry.* -import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.robot.utils.RobotParameters.PhotonVisionConstants -import frc.robot.utils.dash -import frc.robot.utils.to3D -import java.util.* -import kotlin.math.pow -import kotlin.math.sqrt -import org.photonvision.EstimatedRobotPose -import org.photonvision.PhotonCamera -import org.photonvision.PhotonPoseEstimator -import org.photonvision.PhotonPoseEstimator.PoseStrategy -import org.photonvision.targeting.PhotonPipelineResult -import org.photonvision.targeting.PhotonTrackedTarget - -/** The PhotonVision subsystem handles vision processing using PhotonVision cameras. */ -class Photonvision : SubsystemBase() { - // PhotonVision cameras - private var camera: PhotonCamera = PhotonCamera("Camera") - - // Pose estimator for determining the robot's position on the field - private var photonPoseEstimator: PhotonPoseEstimator - - private val cameraTrans = Translation2d(0.31, 0.0) - - // AprilTag field layout for the 2024 Crescendo field - private var aprilTagFieldLayout: AprilTagFieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo) - - // Transformation from the robot to the camera - private var cameraPos: Transform3d = - Transform3d( - cameraTrans.to3D(PhotonVisionConstants.CAMERA_ONE_HEIGHT_METER), - Rotation3d( - 0.0, - Math.toRadians(360 - PhotonVisionConstants.CAMERA_ONE_ANGLE_DEG), - Math.toRadians(180.0), - ), - ) - - var target: PhotonTrackedTarget? = null - var targetVisible: Boolean = false - /** - * Horizontal - * - * See - * [NetworkTables API](https://docs.photonvision.org/en/latest/docs/additional-resources/nt-api.html#getting-target-information) - */ - var yaw: Double = -15.0 - var targetPoseAmbiguity: Double = 7157.0 - var range: Double = 0.0 - - var rangeToTarget: Double = 0.0 - - var result: List - var currentResult: PhotonPipelineResult? = null - - var camTag: Boolean = false - - var currentPose: Translation3d? = null - - /** Constructs a new PhotonVision subsystem. */ - init { - // Intialize result - result = camera.allUnreadResults - photonPoseEstimator = - PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraPos) - } - - /** - * This method is called periodically by the scheduler. It updates the tracked targets and - * displays relevant information on the SmartDashboard. - */ - override fun periodic() { - result = camera.allUnreadResults - currentResult = result.firstOrNull() ?: return - - photonPoseEstimator.update(currentResult) - - target = currentResult?.bestTarget - targetPoseAmbiguity = target?.poseAmbiguity ?: 7157.0 - - currentResult?.targets?.forEach { tag -> - if (tag.fiducialId == 7 || tag.fiducialId == 4) { - yaw = tag.yaw - } - } - - dash( - "yaw to target" to yaw, - "range target" to rangeToTarget, - "april tag distance" to distanceSubwoofer, - "april tag yaw" to subwooferYaw, - "cam ambiguity" to targetPoseAmbiguity, - "_targets" to (currentResult?.hasTargets() ?: false), - ) - } - - fun hasTag(): Boolean = currentResult?.hasTargets() ?: false - - /** - * Gets the estimated global pose of the robot. - * - * @param prevEstimatedRobotPose The previous estimated pose of the robot. - * @return The estimated robot pose, or null if no pose could be estimated. - */ - fun getEstimatedGlobalPose(prevEstimatedRobotPose: Pose2d?): EstimatedRobotPose? { - photonPoseEstimator.setReferencePose(prevEstimatedRobotPose) - currentResult?.let { - return photonPoseEstimator.update(it).orElse(null) - } ?: return null - } - - val estimatedGlobalPose: Transform3d - get() = - currentResult?.multiTagResult?.get()?.estimatedPose?.best - ?: Transform3d(0.0, 0.0, 0.0, Rotation3d()) - - val pivotPosition: Double - /** - * Forward distance to target - * - * See - * [NetworkTables API](https://docs.photonvision.org/en/latest/docs/additional-resources/nt-api.html#getting-target-information) - */ - get() { - // 10/14/2024 outside tuning - // jayden why are you so bad at tuning - // Desmos: https://www.desmos.com/calculator/naalukjxze - val r = distanceSubwoofer + 0.6 - val f = -1.39223 // power 5 - val e = 20.9711 // power 4 - val d = -122.485 // power 3 - val c = 342.783 // power 2 - val b = -447.743 // power 1 - val a = 230.409 // constant - - return (f * r.pow(5.0)) + (e * r.pow(4.0)) + (d * r.pow(3.0)) + (c * r.pow(2.0)) + (b * r) + a - } - - val distanceSubwoofer: Double - get() { - currentPose = estimatedGlobalPose.translation - val localCurrentPose = currentPose - return if (localCurrentPose != null) { - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - sqrt((localCurrentPose.x - 16.5).pow(2.0) + (localCurrentPose.y - 5.5).pow(2.0)) - } else { - sqrt(localCurrentPose.x.pow(2.0) + (localCurrentPose.y - 5.5).pow(2.0)) - } - } else { - 687.0 - } - } - - val subwooferYaw: Double - get() = // currentPose = getEstimatedGlobalPose().getRotation().getAngle(); - 180 - Math.toDegrees(estimatedGlobalPose.rotation.angle) -} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..4cf58d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,226 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TorqueCurrentConfigs; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.utils.Dash; +import frc.robot.utils.PID; +import frc.robot.utils.RobotParameters.*; +import frc.robot.utils.RobotParameters.SwerveParameters.*; + +/** Represents a swerve module used in a swerve drive system. */ +public class SwerveModule { + private final TalonFX driveMotor; + private final CANcoder canCoder; + private final TalonFX steerMotor; + private final PositionVoltage positionSetter; + private final VelocityTorqueCurrentFOC velocitySetter; + private final SwerveModulePosition swerveModulePosition; + private SwerveModuleState state; + private double driveVelocity; + private double drivePosition; + private double steerPosition; + private double steerVelocity; + private final TalonFXConfiguration driveConfigs; + private final TalonFXConfiguration steerConfigs; + private final TorqueCurrentConfigs driveTorqueConfigs; + + /** + * Constructs a new SwerveModule. + * + * @param driveId The ID of the drive motor. + * @param steerId The ID of the steer motor. + * @param canCoderID The ID of the CANcoder. + * @param canCoderDriveStraightSteerSetPoint The set point for the CANcoder drive straight steer. + */ + public SwerveModule( + int driveId, int steerId, int canCoderID, double canCoderDriveStraightSteerSetPoint) { + driveMotor = new TalonFX(driveId); + canCoder = new CANcoder(canCoderID); + steerMotor = new TalonFX(steerId); + positionSetter = new PositionVoltage(0.0).withSlot(0); + velocitySetter = new VelocityTorqueCurrentFOC(0.0); + swerveModulePosition = new SwerveModulePosition(); + state = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)); + + positionSetter.EnableFOC = true; + + driveConfigs = new TalonFXConfiguration(); + driveConfigs.Slot0.kP = PIDParameters.DRIVE_PID_AUTO.getP(); + driveConfigs.Slot0.kI = PIDParameters.DRIVE_PID_AUTO.getI(); + driveConfigs.Slot0.kD = PIDParameters.DRIVE_PID_AUTO.getD(); + driveConfigs.Slot0.kV = PIDParameters.DRIVE_PID_V_AUTO; + driveConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfigs.MotorOutput.Inverted = SwerveParameters.Thresholds.DRIVE_MOTOR_INVERETED; + driveConfigs.CurrentLimits.SupplyCurrentLimit = MotorParameters.DRIVE_SUPPLY_LIMIT; + driveConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfigs.CurrentLimits.StatorCurrentLimit = MotorParameters.DRIVE_STATOR_LIMIT; + driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true; + + steerConfigs = new TalonFXConfiguration(); + steerConfigs.Slot0.kP = PIDParameters.STEER_PID_AUTO.getP(); + steerConfigs.Slot0.kI = PIDParameters.STEER_PID_AUTO.getI(); + steerConfigs.Slot0.kD = PIDParameters.STEER_PID_AUTO.getD(); + steerConfigs.Slot0.kV = 0.0; + steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + steerConfigs.MotorOutput.Inverted = SwerveParameters.Thresholds.STEER_MOTOR_INVERTED; + steerConfigs.Feedback.FeedbackRemoteSensorID = canCoderID; + steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + steerConfigs.Feedback.RotorToSensorRatio = MotorParameters.STEER_MOTOR_GEAR_RATIO; + steerConfigs.CurrentLimits.SupplyCurrentLimit = MotorParameters.STEER_SUPPLY_LIMIT; + steerConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; + + driveTorqueConfigs = new TorqueCurrentConfigs(); + + CANcoderConfiguration canCoderConfiguration = new CANcoderConfiguration(); + canCoderConfiguration.MagnetSensor.AbsoluteSensorRange = + AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + canCoderConfiguration.MagnetSensor.SensorDirection = + SensorDirectionValue.CounterClockwise_Positive; + canCoderConfiguration.MagnetSensor.MagnetOffset = + SwerveParameters.Thresholds.ENCODER_OFFSET + canCoderDriveStraightSteerSetPoint; + + driveMotor.getConfigurator().apply(driveConfigs); + steerMotor.getConfigurator().apply(steerConfigs); + canCoder.getConfigurator().apply(canCoderConfiguration); + + driveVelocity = driveMotor.getVelocity().getValueAsDouble(); + drivePosition = driveMotor.getPosition().getValueAsDouble(); + steerVelocity = steerMotor.getVelocity().getValueAsDouble(); + steerPosition = steerMotor.getPosition().getValueAsDouble(); + } + + /** + * Gets the current position of the swerve module. + * + * @return The current position of the swerve module. + */ + public SwerveModulePosition getPosition() { + driveVelocity = driveMotor.getVelocity().getValueAsDouble(); + drivePosition = driveMotor.getPosition().getValueAsDouble(); + steerVelocity = steerMotor.getVelocity().getValueAsDouble(); + steerPosition = steerMotor.getPosition().getValueAsDouble(); + + swerveModulePosition.angle = + Rotation2d.fromDegrees( + ((360 * canCoder.getAbsolutePosition().getValueAsDouble()) % 360 + 360) % 360); + swerveModulePosition.distanceMeters = + (drivePosition / MotorParameters.DRIVE_MOTOR_GEAR_RATIO * MotorParameters.METERS_PER_REV); + + return swerveModulePosition; + } + + /** + * Gets the current state of the swerve module. + * + * @return The current state of the swerve module, including the angle and speed. + */ + public SwerveModuleState getState() { + state.angle = Rotation2d.fromRotations(steerMotor.getPosition().getValueAsDouble()); + state.speedMetersPerSecond = + (driveMotor.getRotorVelocity().getValueAsDouble() + / MotorParameters.DRIVE_MOTOR_GEAR_RATIO + * MotorParameters.METERS_PER_REV); + return state; + } + + /** + * Sets the state of the swerve module. + * + * @param value The desired state of the swerve module. + */ + public void setState(SwerveModuleState value) { + // Get the current position of the swerve module + SwerveModulePosition newPosition = getPosition(); + + // Optimize the state based on the current position + state.optimize(newPosition.angle); + + // Set the angle for the steer motor + double angleToSet = state.angle.getRotations(); + steerMotor.setControl(positionSetter.withPosition(angleToSet)); + + // Set the velocity for the drive motor + double velocityToSet = + (state.speedMetersPerSecond + * (MotorParameters.DRIVE_MOTOR_GEAR_RATIO / MotorParameters.METERS_PER_REV)); + driveMotor.setControl(velocitySetter.withVelocity(velocityToSet)); + + // Log the actual and set values for debugging + Dash.dash( + Dash.pairOf( + "drive actual speed " + canCoder.getDeviceID(), + driveMotor.getVelocity().getValueAsDouble()), + Dash.pairOf("drive set speed " + canCoder.getDeviceID(), velocityToSet), + Dash.pairOf( + "steer actual angle " + canCoder.getDeviceID(), + steerMotor.getRotorPosition().getValueAsDouble()), + Dash.pairOf("steer set angle " + canCoder.getDeviceID(), angleToSet), + Dash.pairOf( + "desired state after optimize " + canCoder.getDeviceID(), state.angle.getRotations())); + + // Update the state + state = value; + } + + /** Stops the swerve module motors. */ + public void stop() { + steerMotor.stopMotor(); + driveMotor.stopMotor(); + } + + /** + * Sets the drive PID values. + * + * @param pid The PID object containing the PID values. + * @param velocity The velocity value. + */ + public void setDrivePID(PID pid, double velocity) { + driveConfigs.Slot0.kP = pid.getP(); + driveConfigs.Slot0.kI = pid.getI(); + driveConfigs.Slot0.kD = pid.getD(); + driveConfigs.Slot0.kV = velocity; + driveMotor.getConfigurator().apply(driveConfigs); + } + + /** + * Sets the steer PID values. + * + * @param pid The PID object containing the PID values. + * @param velocity The velocity value. + */ + public void setSteerPID(PID pid, double velocity) { + steerConfigs.Slot0.kP = pid.getP(); + steerConfigs.Slot0.kI = pid.getI(); + steerConfigs.Slot0.kD = pid.getD(); + steerConfigs.Slot0.kV = velocity; + steerMotor.getConfigurator().apply(steerConfigs); + } + + /** Sets the PID values for teleoperation mode. */ + public void setTELEPID() { + setDrivePID(PIDParameters.DRIVE_PID_TELE, PIDParameters.DRIVE_PID_V_TELE); + setSteerPID(PIDParameters.STEER_PID_TELE, 0.0); + } + + /** Sets the PID values for autonomous mode. */ + public void setAUTOPID() { + setDrivePID(PIDParameters.DRIVE_PID_AUTO, PIDParameters.DRIVE_PID_V_AUTO); + } + + /** Resets the drive motor position to zero. */ + public void resetDrivePosition() { + driveMotor.setPosition(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.kt b/src/main/java/frc/robot/subsystems/SwerveModule.kt deleted file mode 100644 index 0424f46..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveModule.kt +++ /dev/null @@ -1,221 +0,0 @@ -package frc.robot.subsystems - -import com.ctre.phoenix6.configs.CANcoderConfiguration -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.configs.TorqueCurrentConfigs -import com.ctre.phoenix6.controls.PositionVoltage -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC -import com.ctre.phoenix6.hardware.CANcoder -import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue -import com.ctre.phoenix6.signals.NeutralModeValue -import com.ctre.phoenix6.signals.SensorDirectionValue -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.kinematics.SwerveModulePosition -import edu.wpi.first.math.kinematics.SwerveModuleState -import frc.robot.utils.PID -import frc.robot.utils.RobotParameters.MotorParameters -import frc.robot.utils.RobotParameters.SwerveParameters -import frc.robot.utils.RobotParameters.SwerveParameters.PIDParameters -import frc.robot.utils.dash - -/** The [SwerveModule] class includes all the motors to control the swerve drive. */ -class SwerveModule( - driveId: Int, - steerId: Int, - canCoderID: Int, - canCoderDriveStraightSteerSetPoint: Double, -) { - private val driveMotor = TalonFX(driveId) - - private val canCoder = CANcoder(canCoderID) - private val steerMotor = TalonFX(steerId) - - private val positionSetter: PositionVoltage = PositionVoltage(0.0).withSlot(0) - private val velocitySetter = VelocityTorqueCurrentFOC(0.0) - - private val swerveModulePosition = SwerveModulePosition() - var state = SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)) - get() { - state.angle = Rotation2d.fromRotations(steerMotor.position.valueAsDouble) - state.speedMetersPerSecond = - (driveMotor.rotorVelocity.valueAsDouble / MotorParameters.DRIVE_MOTOR_GEAR_RATIO * - MotorParameters.METERS_PER_REV) - return field - } - set(value) { - val newPosition = position - state.optimize(newPosition.angle) - - val angleToSet = state.angle.rotations - steerMotor.setControl(positionSetter.withPosition(angleToSet)) - - val velocityToSet = - (state.speedMetersPerSecond * - (MotorParameters.DRIVE_MOTOR_GEAR_RATIO / MotorParameters.METERS_PER_REV)) - driveMotor.setControl(velocitySetter.withVelocity(velocityToSet)) - - dash( - "drive actual speed " + canCoder.deviceID to driveMotor.velocity.valueAsDouble, - "drive set speed " + canCoder.deviceID to velocityToSet, - "steer actual angle " + canCoder.deviceID to steerMotor.rotorPosition.valueAsDouble, - "steer set angle " + canCoder.deviceID to angleToSet, - "desired state after optimize " + canCoder.deviceID to state.angle.rotations, - ) - - field = value - } - - private var driveVelocity: Double - private var drivePosition: Double - private var steerPosition: Double - private var steerVelocity: Double - - private val driveConfigs: TalonFXConfiguration - private val steerConfigs: TalonFXConfiguration - - private val driveTorqueConfigs: TorqueCurrentConfigs - - init { - positionSetter.EnableFOC = true - - driveConfigs = - TalonFXConfiguration().apply { - Slot0.apply { - kP = PIDParameters.DRIVE_PID_AUTO.p - kI = PIDParameters.DRIVE_PID_AUTO.i - kD = PIDParameters.DRIVE_PID_AUTO.d - kV = PIDParameters.DRIVE_PID_V_AUTO - } - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - // DutyCycleNeutralDeadband = 0.02; - Inverted = SwerveParameters.Thresholds.DRIVE_MOTOR_INVERETED - } - CurrentLimits.apply { - SupplyCurrentLimit = MotorParameters.DRIVE_SUPPLY_LIMIT - SupplyCurrentLimitEnable = true - StatorCurrentLimit = MotorParameters.DRIVE_STATOR_LIMIT - StatorCurrentLimitEnable = true - } - } - steerConfigs = - TalonFXConfiguration().apply { - Slot0.apply { - kP = PIDParameters.STEER_PID_AUTO.p - kI = PIDParameters.STEER_PID_AUTO.i - kD = PIDParameters.STEER_PID_AUTO.d - kV = 0.0 - } - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - // DutyCycleNeutralDeadband = 0.001; - Inverted = SwerveParameters.Thresholds.STEER_MOTOR_INVERTED - } - Feedback.apply { - FeedbackRemoteSensorID = canCoderID - FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder - RotorToSensorRatio = MotorParameters.STEER_MOTOR_GEAR_RATIO - } - CurrentLimits.apply { - SupplyCurrentLimit = MotorParameters.STEER_SUPPLY_LIMIT - SupplyCurrentLimitEnable = true - } - } - - driveTorqueConfigs = TorqueCurrentConfigs() - // driveTorqueConfigs.PeakForwardTorqueCurrent = 45; - // driveTorqueConfigs.PeakReverseTorqueCurrent = 45; - // driveTorqueConfigs.TorqueNeutralDeadband = 0; - - val canCoderConfiguration = - CANcoderConfiguration().apply { - MagnetSensor.apply { - AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf - SensorDirection = SensorDirectionValue.CounterClockwise_Positive - MagnetOffset = - SwerveParameters.Thresholds.ENCODER_OFFSET + canCoderDriveStraightSteerSetPoint - } - } - - driveMotor.configurator.apply(driveConfigs) - steerMotor.configurator.apply(steerConfigs) - canCoder.configurator.apply(canCoderConfiguration) - - driveMotor.apply { - driveVelocity = velocity.valueAsDouble - drivePosition = position.valueAsDouble - } - steerMotor.apply { - steerVelocity = velocity.valueAsDouble - steerPosition = position.valueAsDouble - } - } - - val position: SwerveModulePosition - /** - * Gets the current position of the swerve module. - * - * @return The current position of the swerve module. - */ - get() { - driveMotor.apply { - driveVelocity = velocity.valueAsDouble - drivePosition = position.valueAsDouble - } - steerMotor.apply { - steerVelocity = velocity.valueAsDouble - steerPosition = position.valueAsDouble - } - - return swerveModulePosition.apply { - angle = - Rotation2d.fromDegrees( - ((360 * canCoder.absolutePosition.valueAsDouble) % 360 + 360) % 360 - ) - distanceMeters = - (drivePosition / MotorParameters.DRIVE_MOTOR_GEAR_RATIO * MotorParameters.METERS_PER_REV) - } - } - - fun stop() { - steerMotor.stopMotor() - driveMotor.stopMotor() - } - - fun setDrivePID(pid: PID, velocity: Double) { - driveConfigs.Slot0.apply { - kP = pid.p - kI = pid.i - kD = pid.d - kV = velocity - } - - driveMotor.configurator.apply(driveConfigs) - } - - fun setSteerPID(pid: PID, velocity: Double) { - steerConfigs.Slot0.apply { - kP = pid.p - kI = pid.i - kD = pid.d - kV = velocity - } - - steerMotor.configurator.apply(steerConfigs) - } - - fun setTELEPID() { - setDrivePID(PIDParameters.DRIVE_PID_TELE, PIDParameters.DRIVE_PID_V_TELE) - setSteerPID(PIDParameters.STEER_PID_TELE, 0.0) - } - - fun setAUTOPID() { - setDrivePID(PIDParameters.DRIVE_PID_AUTO, PIDParameters.DRIVE_PID_V_AUTO) - } - - fun resetDrivePosition() { - driveMotor.setPosition(0.0) - } -} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java new file mode 100644 index 0000000..d0f3112 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -0,0 +1,364 @@ +package frc.robot.subsystems; + +import static frc.robot.utils.RobotParameters.SwerveParameters.Thresholds.SHOULD_INVERT; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.Dash; +import frc.robot.utils.PID; +import frc.robot.utils.RobotParameters.*; +import frc.robot.utils.RobotParameters.SwerveParameters.*; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; + +/** + * The SwerveSubsystem class represents the swerve drive subsystem of the robot. It handles the + * control and state estimation of the swerve drive modules. + */ +public class SwerveSubsystem extends SubsystemBase { + private final SwerveDrivePoseEstimator poseEstimator; + private final Field2d field = new Field2d(); + private final Pigeon2 pidgey = new Pigeon2(MotorParameters.PIDGEY_ID); + private final SwerveModuleState[] states = new SwerveModuleState[4]; + private final SwerveModule[] modules; + private final Photonvision photonvision; + private final PID pid; + private double velocity = 0.0; + + /** + * Constructs a new SwerveSubsystem. + * + * @param photonvision The Photonvision instance used for vision processing. + */ + public SwerveSubsystem(Photonvision photonvision) { + this.photonvision = photonvision; + this.modules = initializeModules(); + this.pid = initializePID(); + this.pidgey.reset(); + this.poseEstimator = initializePoseEstimator(); + configureAutoBuilder(); + } + + /** + * Initializes the swerve modules. + * + * @return An array of initialized SwerveModule objects. + */ + private SwerveModule[] initializeModules() { + return new SwerveModule[] { + new SwerveModule( + MotorParameters.FRONT_LEFT_DRIVE_ID, + MotorParameters.FRONT_LEFT_STEER_ID, + MotorParameters.FRONT_LEFT_CAN_CODER_ID, + SwerveParameters.Thresholds.CANCODER_VAL9), + new SwerveModule( + MotorParameters.FRONT_RIGHT_DRIVE_ID, + MotorParameters.FRONT_RIGHT_STEER_ID, + MotorParameters.FRONT_RIGHT_CAN_CODER_ID, + SwerveParameters.Thresholds.CANCODER_VAL10), + new SwerveModule( + MotorParameters.BACK_LEFT_DRIVE_ID, + MotorParameters.BACK_LEFT_STEER_ID, + MotorParameters.BACK_LEFT_CAN_CODER_ID, + SwerveParameters.Thresholds.CANCODER_VAL11), + new SwerveModule( + MotorParameters.BACK_RIGHT_DRIVE_ID, + MotorParameters.BACK_RIGHT_STEER_ID, + MotorParameters.BACK_RIGHT_CAN_CODER_ID, + SwerveParameters.Thresholds.CANCODER_VAL12) + }; + } + + /** + * Initializes the PID controller. + * + * @return A new PID object with values from the SmartDashboard. + */ + private PID initializePID() { + return new PID( + SmartDashboard.getNumber("AUTO: P", PIDParameters.DRIVE_PID_AUTO.getP()), + SmartDashboard.getNumber("AUTO: I", PIDParameters.DRIVE_PID_AUTO.getI()), + SmartDashboard.getNumber("AUTO: D", PIDParameters.DRIVE_PID_AUTO.getD())); + } + + /** + * Initializes the SwerveDrivePoseEstimator. + * + * @return A new SwerveDrivePoseEstimator object. + */ + private SwerveDrivePoseEstimator initializePoseEstimator() { + return new SwerveDrivePoseEstimator( + SwerveParameters.PhysicalParameters.kinematics, + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5.0)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30.0))); + } + + /** Configures the AutoBuilder for autonomous driving. */ + private void configureAutoBuilder() { + AutoBuilder.configure( + this::getPose, + this::newPose, + this::getAutoSpeeds, + this::chassisSpeedsDrive, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PIDParameters.config, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) { + return false; + } + if (SHOULD_INVERT) { + return alliance.get() == Alliance.Red; + } else { + return alliance.get() != Alliance.Blue; + } + }, + this); + } + + /** + * This method is called periodically by the scheduler. It updates the pose estimator and + * dashboard values. + */ + @Override + public void periodic() { + if (DriverStation.isTeleop()) { + EstimatedRobotPose estimatedPose = + photonvision.getEstimatedGlobalPose(poseEstimator.getEstimatedPosition()); + if (estimatedPose != null) { + double timestamp = estimatedPose.timestampSeconds; + Pose2d visionMeasurement2d = estimatedPose.estimatedPose.toPose2d(); + poseEstimator.addVisionMeasurement(visionMeasurement2d, timestamp); + SwerveParameters.Thresholds.currentPose = poseEstimator.getEstimatedPosition(); + } + } + + poseEstimator.update(getPidgeyRotation(), getModulePositions()); + + field.setRobotPose(poseEstimator.getEstimatedPosition()); + + Dash.dash( + Dash.pairOf("Pitch", pidgey.getPitch().getValueAsDouble()), + Dash.pairOf("Heading", -pidgey.getYaw().getValueAsDouble()), + Dash.pairOf("Yaw", pidgey.getYaw().getValueAsDouble()), + Dash.pairOf("Roll", pidgey.getRoll().getValueAsDouble()), + Dash.pairOf("Robot Pose", field)); + } + + /** + * Sets the drive speeds for the swerve modules. + * + * @param forwardSpeed The forward speed. + * @param leftSpeed The left speed. + * @param turnSpeed The turn speed. + * @param isFieldOriented Whether the drive is field-oriented. + */ + public void setDriveSpeeds( + double forwardSpeed, double leftSpeed, double turnSpeed, boolean isFieldOriented) { + Dash.dash( + Dash.pairOf("Forward speed", forwardSpeed), + Dash.pairOf("Left speed", leftSpeed), + Dash.pairOf("Pidgey Heading", getHeading())); + + ChassisSpeeds speeds = new ChassisSpeeds(forwardSpeed, leftSpeed, turnSpeed); + if (!isFieldOriented) { + speeds.toRobotRelativeSpeeds(getPidgeyRotation()); + } + + SwerveModuleState[] states2 = + SwerveParameters.PhysicalParameters.kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states2, MotorParameters.MAX_SPEED); + setModuleStates(states2); + } + + /** + * Gets the rotation of the Pigeon2 IMU. + * + * @return The rotation of the Pigeon2 IMU. + */ + public Rotation2d getPidgeyRotation() { + return pidgey.getRotation2d(); + } + + /** + * Gets the heading of the robot. + * + * @return The heading of the robot. + */ + public double getHeading() { + return -pidgey.getYaw().getValueAsDouble(); + } + + /** + * Gets the yaw of the Pigeon2 IMU. + * + * @return The yaw of the Pigeon2 IMU. + */ + public double getPidgeyYaw() { + return pidgey.getYaw().getValueAsDouble(); + } + + /** Sets the initial heading of the robot based on the alliance color. */ + public void setInitialHeading() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty() || alliance.get() == Alliance.Red) { + pidgey.setYaw(27.4); + } else { + pidgey.setYaw(-27.4); + } + } + + /** Resets the Pigeon2 IMU. */ + public void resetPidgey() { + pidgey.reset(); + } + + /** + * Gets the current pose of the robot. + * + * @return The current pose of the robot. + */ + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Resets the pose of the robot to zero. */ + public void zeroPose() { + poseEstimator.resetPosition( + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))); + } + + /** + * Sets a new pose for the robot. + * + * @param pose The new pose. + */ + public void newPose(Pose2d pose) { + poseEstimator.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), pose); + } + + /** + * Gets the chassis speeds for autonomous driving. + * + * @return The chassis speeds for autonomous driving. + */ + public ChassisSpeeds getAutoSpeeds() { + SwerveDriveKinematics k = SwerveParameters.PhysicalParameters.kinematics; + return k.toChassisSpeeds(getModuleStates()); + } + + /** + * Gets the rotation of the Pigeon2 IMU for PID control. + * + * @return The rotation of the Pigeon2 IMU for PID control. + */ + public Rotation2d getRotationPidggy() { + return Rotation2d.fromDegrees(-pidgey.getRotation2d().getDegrees()); + } + + /** + * Drives the robot using chassis speeds. + * + * @param chassisSpeeds The chassis speeds. + */ + public void chassisSpeedsDrive(ChassisSpeeds chassisSpeeds) { + SwerveModuleState[] newStates = + SwerveParameters.PhysicalParameters.kinematics.toSwerveModuleStates(chassisSpeeds); + setModuleStates(newStates); + } + + /** + * Gets the states of the swerve modules. + * + * @return The states of the swerve modules. + */ + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; + for (int i = 0; i < modules.length; i++) { + moduleStates[i] = modules[i].getState(); + } + return moduleStates; + } + + /** + * Sets the states of the swerve modules. + * + * @param states The states of the swerve modules. + */ + public void setModuleStates(SwerveModuleState[] states) { + for (int i = 0; i < states.length; i++) { + modules[i].setState(states[i]); + } + } + + /** + * Gets the positions of the swerve modules. + * + * @return The positions of the swerve modules. + */ + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[states.length]; + for (int i = 0; i < positions.length; i++) { + positions[i] = modules[i].getPosition(); + } + return positions; + } + + /** Stops all swerve modules. */ + public void stop() { + for (SwerveModule module : modules) { + module.stop(); + } + } + + /** Sets the PID constants for autonomous driving. */ + public void setAutoPID() { + for (SwerveModule module : modules) { + module.setAUTOPID(); + } + } + + /** Sets the PID constants for teleoperated driving. */ + public void setTelePID() { + for (SwerveModule module : modules) { + module.setTELEPID(); + } + } + + /** Resets the drive positions of all swerve modules. */ + public void resetDrive() { + for (SwerveModule module : modules) { + module.resetDrivePosition(); + } + } + + /** Sets custom PID constants for the drive. */ + public void setCustomDrivePID() { + Dash.dashPID("Drive", pid, PIDParameters.DRIVE_PID_V_AUTO, v -> velocity = v); + for (SwerveModule module : modules) { + module.setDrivePID(pid, velocity); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt b/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt deleted file mode 100644 index 2566dd0..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt +++ /dev/null @@ -1,331 +0,0 @@ -@file:Suppress("MemberVisibilityCanBePrivate", "unused") - -package frc.robot.subsystems - -import com.ctre.phoenix6.hardware.Pigeon2 -import com.pathplanner.lib.auto.AutoBuilder -import com.pathplanner.lib.config.PIDConstants -import com.pathplanner.lib.controllers.PPHolonomicDriveController -import edu.wpi.first.math.VecBuilder -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.math.kinematics.SwerveDriveKinematics -import edu.wpi.first.math.kinematics.SwerveModulePosition -import edu.wpi.first.math.kinematics.SwerveModuleState -import edu.wpi.first.math.util.Units -import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj.DriverStation.Alliance -import edu.wpi.first.wpilibj.smartdashboard.Field2d -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.robot.utils.PID -import frc.robot.utils.RobotParameters.MotorParameters -import frc.robot.utils.RobotParameters.SwerveParameters -import frc.robot.utils.RobotParameters.SwerveParameters.PIDParameters -import frc.robot.utils.RobotParameters.SwerveParameters.Thresholds.SHOULD_INVERT -import frc.robot.utils.dash -import frc.robot.utils.dashPID - -/** The [SwerveSubsystem] class includes all the motors to drive the robot. */ -class SwerveSubsystem(photonvision: Photonvision) : SubsystemBase() { - private val poseEstimator: SwerveDrivePoseEstimator - private val field = Field2d() - - private val pidgey = Pigeon2(MotorParameters.PIDGEY_ID) - private val states: Array - private val modules = - arrayOf( - SwerveModule( - MotorParameters.FRONT_LEFT_DRIVE_ID, - MotorParameters.FRONT_LEFT_STEER_ID, - MotorParameters.FRONT_LEFT_CAN_CODER_ID, - SwerveParameters.Thresholds.CANCODER_VAL9, - ), - SwerveModule( - MotorParameters.FRONT_RIGHT_DRIVE_ID, - MotorParameters.FRONT_RIGHT_STEER_ID, - MotorParameters.FRONT_RIGHT_CAN_CODER_ID, - SwerveParameters.Thresholds.CANCODER_VAL10, - ), - SwerveModule( - MotorParameters.BACK_LEFT_DRIVE_ID, - MotorParameters.BACK_LEFT_STEER_ID, - MotorParameters.BACK_LEFT_CAN_CODER_ID, - SwerveParameters.Thresholds.CANCODER_VAL11, - ), - SwerveModule( - MotorParameters.BACK_RIGHT_DRIVE_ID, - MotorParameters.BACK_RIGHT_STEER_ID, - MotorParameters.BACK_RIGHT_CAN_CODER_ID, - SwerveParameters.Thresholds.CANCODER_VAL12, - ), - ) - - private val photonvision: Photonvision - - private val pid = - PID( - SmartDashboard.getNumber("AUTO: P", PIDParameters.DRIVE_PID_AUTO.p), - SmartDashboard.getNumber("AUTO: I", PIDParameters.DRIVE_PID_AUTO.i), - SmartDashboard.getNumber("AUTO: D", PIDParameters.DRIVE_PID_AUTO.d), - ) - private var velocity = 0.0 - - init { - pidgey.reset() - states = arrayOfNulls(4) - this.photonvision = photonvision - - poseEstimator = - SwerveDrivePoseEstimator( - SwerveParameters.PhysicalParameters.kinematics, - Rotation2d.fromDegrees(heading), - modulePositions, - Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5.0)), - VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30.0)), - ) - - AutoBuilder.configure( - { this.pose }, // Robot pose supplier - { pose: Pose2d? -> - this.newPose(pose) - }, // Method to reset odometry (will be called if your auto has a starting pose) - { this.autoSpeeds }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - { chassisSpeeds: ChassisSpeeds? -> - this.chassisSpeedsDrive(chassisSpeeds) - }, // Method that will drive the robot given ROBOT RELATIVE - PPHolonomicDriveController( // PPHolonomicController is the built-in path following - // controller for holonomic drive trains - PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - ), - PIDParameters.config, - { - val alliance = DriverStation.getAlliance() - alliance - .map { - if (SHOULD_INVERT) { - it == Alliance.Red - } else { - it != Alliance.Blue - } - } - .orElse(false) - }, - this, // Reference to this subsystem to set requirements - ) - } - - /** - * This method will be called once per scheduler run. It updates the robot's pose estimation and - * logs relevant data to the dashboard. - */ - override fun periodic() { - if (DriverStation.isTeleop()) { - photonvision.getEstimatedGlobalPose(poseEstimator.estimatedPosition)?.apply { - val timestamp = timestampSeconds - val estimatedPose = estimatedPose - val visionMeasurement2d = estimatedPose.toPose2d() - poseEstimator.addVisionMeasurement(visionMeasurement2d, timestamp) - SwerveParameters.Thresholds.currentPose = poseEstimator.estimatedPosition - } - } - - poseEstimator.update(pidgeyRotation, modulePositions) - - field.robotPose = poseEstimator.estimatedPosition - - dash( - "Pitch" to pidgey.pitch.valueAsDouble, - "Heading" to -pidgey.yaw.valueAsDouble, - "Yaw" to pidgey.yaw.valueAsDouble, - "Roll" to pidgey.roll.valueAsDouble, - "Robot Pose" to field, - ) - } - - /** - * Sets the drive speeds for the robot. - * - * @param forwardSpeed The forward speed. - * @param leftSpeed The left speed. - * @param turnSpeed The turn speed. - * @param isFieldOriented Whether the drive is field-oriented. - */ - fun setDriveSpeeds( - forwardSpeed: Double, - leftSpeed: Double, - turnSpeed: Double, - isFieldOriented: Boolean, - ) { - dash("Forward speed" to forwardSpeed, "Left speed" to leftSpeed, "Pidgey Heading" to heading) - - val speeds = - ChassisSpeeds(forwardSpeed, leftSpeed, turnSpeed).apply { - if (!isFieldOriented) toRobotRelativeSpeeds(pidgeyRotation) - } - - val states2 = SwerveParameters.PhysicalParameters.kinematics.toSwerveModuleStates(speeds) - SwerveDriveKinematics.desaturateWheelSpeeds(states2, MotorParameters.MAX_SPEED) - moduleStates = states2 - } - - val pidgeyRotation: Rotation2d - /** - * Gets the pidgey rotation. - * - * @return [Rotation2d] The current rotation of the [pidgey]. - */ - get() = pidgey.rotation2d - - val heading: Double - /** - * Gets the pidgey angle. - * - * @return [Double] The current angle of the [pidgey]. - */ - get() = -pidgey.yaw.valueAsDouble - - val pidgeyYaw: Double - /** - * Gets the pidgey yaw. - * - * @return [Double] The current yaw of the [pidgey]. - */ - get() = pidgey.yaw.valueAsDouble - - fun setInitialHeading() { - val alliance = DriverStation.getAlliance() - if (alliance.get() == Alliance.Red) { - pidgey.setYaw(27.4) - } else { - pidgey.setYaw(-27.4) - } - } - - /** Resets the pidgey(gyro) heading to 0. */ - fun resetPidgey() { - pidgey.reset() - } - - val pose: Pose2d - /** - * Gets the current pose of the robot. - * - * @return [Pose2d] The current pose of the robot. - */ - get() = poseEstimator.estimatedPosition - - /** Zeros the pose of the robot. */ - fun zeroPose() { - poseEstimator.resetPosition( - Rotation2d.fromDegrees(heading), - modulePositions, - Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)), - ) - } - - /** - * Resets the pose of the robot. - * - * @param pose The new pose to set. - */ - fun newPose(pose: Pose2d?) { - poseEstimator.resetPosition(Rotation2d.fromDegrees(heading), modulePositions, pose) - } - - val autoSpeeds: ChassisSpeeds - /** - * Gets the chassis speeds for autonomous driving. - * - * @return [ChassisSpeeds] The current chassis speeds. - */ - get() { - val k = SwerveParameters.PhysicalParameters.kinematics - // Necessary because a kotlin array is being passed into a java varargs parameter - val chassisSpeeds = - k::class.java.getMethod("toChassisSpeeds", Array::class.java) - return chassisSpeeds.invoke(k, moduleStates) as ChassisSpeeds - } - - val rotationPidggy: Rotation2d - /** - * Gets the rotation of the pidgey for PID control. - * - * @return [Rotation2d] The rotation of the pidgey for PID control. - */ - get() = Rotation2d.fromDegrees(-pidgey.rotation2d.degrees) - - /** - * Drives the robot using the given chassis speeds. - * - * @param chassisSpeeds The desired chassis speeds. - */ - fun chassisSpeedsDrive(chassisSpeeds: ChassisSpeeds?) { - // ChassisSpeeds speeds = - // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, getRotationPidggy()); - // SwerveModuleState[] newStates = SwerveGlobalValues.kinematics.toSwerveModuleStates(speeds); - // SwerveDriveKinematics.desaturateWheelSpeeds(newStates, MotorGlobalValues.MAX_SPEED); - - val newStates = - SwerveParameters.PhysicalParameters.kinematics.toSwerveModuleStates(chassisSpeeds) - moduleStates = newStates - } - - private var moduleStates: Array - /** - * Gets the current states of the swerve modules. - * - * @return SwerveModuleState[] The current states of the swerve modules. - */ - get() { - val moduleStates = emptyArray() - for (i in modules.indices) { - moduleStates[i] = modules[i].state - } - return moduleStates - } - /** - * Sets the desired module states. - * - * @param states SwerveModuleState[] The desired states for the swerve modules. - */ - set(states) { - for (i in states.indices) { - modules[i].state = states[i] - } - } - - val modulePositions: Array - /** - * Gets the current positions of the swerve modules. - * - * @return SwerveModulePosition[] The current positions of the swerve modules. - */ - get() { - val positions = arrayOfNulls(states.size) - - for (i in positions.indices) { - positions[i] = modules[i].position - } - - return positions - } - - /** Stops the robot. */ - fun stop() = modules.forEach { it.stop() } - - fun setAutoPID() = modules.forEach { it.setAUTOPID() } - - fun setTelePID() = modules.forEach { it.setTELEPID() } - - fun resetDrive() = modules.forEach { it.resetDrivePosition() } - - fun setCustomDrivePID() { - dashPID("Drive", pid, PIDParameters.DRIVE_PID_V_AUTO) { velocity = it } - modules.forEach { it.setDrivePID(pid, velocity) } - } -} diff --git a/src/main/java/frc/robot/utils/Dash.java b/src/main/java/frc/robot/utils/Dash.java new file mode 100644 index 0000000..2cd951e --- /dev/null +++ b/src/main/java/frc/robot/utils/Dash.java @@ -0,0 +1,55 @@ +package frc.robot.utils; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.utils.RobotParameters.SwerveParameters.Thresholds; +import kotlin.Pair; + +public class Dash { + /** + * Function to update values from the SmartDashboard. + * + * @param pairs The pairs of keys and values to update. + */ + @SafeVarargs + public static void dash(Pair... pairs) { + if (Thresholds.TEST_MODE) { + for (Pair pair : pairs) { + String key = pair.getFirst(); + Object value = pair.getSecond(); + if (value instanceof Number numberValue) { + SmartDashboard.putNumber(key, numberValue.doubleValue()); + } else if (value instanceof Boolean booleanValue) { + SmartDashboard.putBoolean(key, booleanValue); + } else if (value instanceof String stringValue) { + SmartDashboard.putString(key, stringValue); + } else if (value instanceof Sendable sendableValue) { + SmartDashboard.putData(key, sendableValue); + } else { + System.out.println("Jayden had a skill issue like always"); + throw new IllegalArgumentException("Unsupported type: " + value.getClass()); + } + } + } + } + + /** + * Function to update PIDV values from the SmartDashboard. + * + * @param pid The PID object to update. + * @param velocity The velocity to update. + * @param prefix The prefix for the SmartDashboard keys. + * @param changeV The function to change the velocity. + */ + public static void dashPID( + String prefix, PID pid, double velocity, java.util.function.DoubleConsumer changeV) { + pid.setP(SmartDashboard.getNumber(prefix + " Auto P", pid.getP())); + pid.setI(SmartDashboard.getNumber(prefix + " Auto I", pid.getI())); + pid.setD(SmartDashboard.getNumber(prefix + " Auto D", pid.getD())); + changeV.accept(SmartDashboard.getNumber(prefix + " Auto V", velocity)); + } + + public static Pair pairOf(String s, Object o) { + return new Pair<>(s, o); + } +} diff --git a/src/main/java/frc/robot/utils/Dash.kt b/src/main/java/frc/robot/utils/Dash.kt deleted file mode 100644 index 7637cc3..0000000 --- a/src/main/java/frc/robot/utils/Dash.kt +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.utils - -import edu.wpi.first.util.sendable.Sendable -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard - -/** - * Function to update values from the [SmartDashboard]. - * - * @param pairs The pairs of keys and values to update. - */ -fun dash(vararg pairs: Pair) { - if (RobotParameters.SwerveParameters.Thresholds.TEST_MODE) { - pairs.forEach { (key, value) -> - when (value) { - is Number -> SmartDashboard.putNumber(key, value.toDouble()) - is Boolean -> SmartDashboard.putBoolean(key, value) - is String -> SmartDashboard.putString(key, value) - is Sendable -> SmartDashboard.putData(key, value) - else -> { - print("Jayden had a skill issue like always") - throw IllegalArgumentException("Unsupported type: ${value::class.java}") - } - } - } - } -} - -/** - * Function to update PIDV values from the [SmartDashboard]. - * - * @param pid The PID object to update. - * @param velocity The velocity to update. - * @param prefix The prefix for the [SmartDashboard] keys. - */ -fun dashPID(prefix: String, pid: PID, velocity: Double, changeV: (Double) -> Unit) { - pid.p = SmartDashboard.getNumber("$prefix Auto P", pid.p) - pid.i = SmartDashboard.getNumber("$prefix Auto I", pid.i) - pid.d = SmartDashboard.getNumber("$prefix Auto D", pid.d) - changeV(SmartDashboard.getNumber("$prefix Auto V", velocity)) -} diff --git a/src/main/java/frc/robot/utils/Extensions.java b/src/main/java/frc/robot/utils/Extensions.java new file mode 100644 index 0000000..2847510 --- /dev/null +++ b/src/main/java/frc/robot/utils/Extensions.java @@ -0,0 +1,18 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + +/** Utility class for extension functions. */ +public class Extensions { + /** + * Converts a 2D translation to a 3D translation. + * + * @param translation2d The 2D translation to convert. + * @param z The z-coordinate for the 3D translation. + * @return The resulting 3D translation. + */ + public static Translation3d dimensionIncrease(Translation2d translation2d, double z) { + return new Translation3d(translation2d.getX(), translation2d.getY(), z); + } +} diff --git a/src/main/java/frc/robot/utils/Extensions.kt b/src/main/java/frc/robot/utils/Extensions.kt deleted file mode 100644 index 16877be..0000000 --- a/src/main/java/frc/robot/utils/Extensions.kt +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.utils - -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.geometry.Translation3d - -/** - * Extension function to convert a 2D translation to a 3D translation. - * - * @param z The z-coordinate for the 3D translation. - * @return [Translation3d] The resulting 3D translation. - * @receiver [Translation2d] The 2D translation to convert. - */ -fun Translation2d.to3D(z: Double): Translation3d = Translation3d(x, y, z) diff --git a/src/main/java/frc/robot/utils/LogitechGamingPad.java b/src/main/java/frc/robot/utils/LogitechGamingPad.java new file mode 100644 index 0000000..ccc6911 --- /dev/null +++ b/src/main/java/frc/robot/utils/LogitechGamingPad.java @@ -0,0 +1,259 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; + +/** A class representing a Logitech Gaming Pad. */ +public class LogitechGamingPad extends XboxController { + private final Joystick gamepad; + + /** Enum representing the DPad directions. */ + public enum DPad { + UP(0), + UP_RIGHT(45), + RIGHT(90), + DOWN_RIGHT(135), + DOWN(180), + DOWN_LEFT(225), + LEFT(270), + UP_LEFT(315); + + public final int angle; + + DPad(int angle) { + this.angle = angle; + } + } + + /** Enum representing the buttons on the gamepad. */ + public enum Button { + A(1), + B(2), + X(3), + Y(4), + LEFT_BUMPER(5), + RIGHT_BUMPER(6), + BACK(7), + START(8); + + public final int button; + + Button(int button) { + this.button = button; + } + } + + /** Enum representing the axes on the gamepad. */ + public enum Axis { + LEFT_ANALOG_X(0), + LEFT_ANALOG_Y(1), + RIGHT_ANALOG_X(4), + RIGHT_ANALOG_Y(5); + + public final int axis; + + Axis(int axis) { + this.axis = axis; + } + } + + /** Enum representing the triggers on the gamepad. */ + public enum Trigger { + LEFT(2), + RIGHT(3); + + public final int trigger; + + Trigger(int trigger) { + this.trigger = trigger; + } + } + + /** + * Constructor for the LogitechGamingPad. + * + * @param usbPort The USB port the gamepad is connected to. + */ + public LogitechGamingPad(int usbPort) { + super(usbPort); + this.gamepad = new Joystick(usbPort); + } + + /** + * Gets the value of the left trigger. + * + * @return The value of the left trigger. + */ + public double getLeftTriggerValue() { + return gamepad.getRawAxis(Trigger.LEFT.trigger); + } + + /** + * Gets the value of the left analog X axis. + * + * @return The value of the left analog X axis. + */ + public double getLeftAnalogXAxis() { + return gamepad.getRawAxis(Axis.LEFT_ANALOG_X.axis); + } + + /** + * Gets the value of the left analog Y axis. + * + * @return The value of the left analog Y axis. + */ + public double getLeftAnalogYAxis() { + return gamepad.getRawAxis(Axis.LEFT_ANALOG_Y.axis); + } + + /** + * Gets the value of the right trigger. + * + * @return The value of the right trigger. + */ + public double getRightTriggerValue() { + return gamepad.getRawAxis(Trigger.RIGHT.trigger); + } + + /** + * Gets the value of the right analog X axis. + * + * @return The value of the right analog X axis. + */ + public double getRightAnalogXAxis() { + return gamepad.getRawAxis(Axis.RIGHT_ANALOG_X.axis); + } + + /** + * Gets the value of the right analog Y axis. + * + * @return The value of the right analog Y axis. + */ + public double getRightAnalogYAxis() { + return gamepad.getRawAxis(Axis.RIGHT_ANALOG_Y.axis); + } + + @Override + public boolean getAButton() { + return gamepad.getRawButton(Button.A.button); + } + + @Override + public boolean getBButton() { + return gamepad.getRawButton(Button.B.button); + } + + @Override + public boolean getXButton() { + return gamepad.getRawButton(Button.X.button); + } + + @Override + public boolean getYButton() { + return gamepad.getRawButton(Button.Y.button); + } + + @Override + public boolean getBackButton() { + return gamepad.getRawButton(Button.BACK.button); + } + + @Override + public boolean getStartButton() { + return gamepad.getRawButton(Button.START.button); + } + + /** + * Checks if the DPad is pressed in a specific direction. + * + * @param index The index of the DPad direction. + * @return True if the DPad is pressed in the specified direction, false otherwise. + */ + public boolean checkDPad(int index) { + return 0 <= index && index <= 7 && (index * 45) == gamepad.getPOV(); + } + + /** + * Checks if the DPad is pressed at a specific angle. + * + * @param angle The angle to check. + * @param inDegrees Whether the angle is in degrees. + * @return True if the DPad is pressed at the specified angle, false otherwise. + */ + public boolean checkDPad(double angle, boolean inDegrees) { + double angdeg = inDegrees ? angle : Math.toDegrees(angle); + return (int) angdeg == gamepad.getPOV(); + } + + /** + * Gets the DPad value. + * + * @return The DPad value. + */ + public int getDPad() { + int pov = gamepad.getPOV(); + return pov == -1 ? pov : pov / 45; + } + + /** + * Gets the DPad value in degrees or radians. + * + * @param inDegrees Whether to return the value in degrees. + * @return The DPad value in degrees or radians. + */ + public double getDPad(boolean inDegrees) { + return inDegrees ? gamepad.getPOV() : Math.toRadians(gamepad.getPOV()); + } + + /** + * Checks if the DPad is pressed. + * + * @return True if the DPad is pressed, false otherwise. + */ + public boolean dPadIsPressed() { + return gamepad.getPOV() != -1; + } + + /** + * Sets the rumble amount for the gamepad. + * + * @param amount The rumble amount. + */ + public void setRumble(float amount) { + gamepad.setRumble(RumbleType.kLeftRumble, amount); + gamepad.setRumble(RumbleType.kRightRumble, amount); + } + + @Override + public double getRawAxis(int which) { + return gamepad.getRawAxis(which); + } + + @Override + public boolean getRawButton(int button) { + return gamepad.getRawButton(button); + } + + @Override + public int getPOV(int pov) { + return gamepad.getPOV(pov); + } + + /** + * Gets the twist value. + * + * @return The twist value. + */ + public double getTwist() { + return 0.0; + } + + /** + * Gets the throttle value. + * + * @return The throttle value. + */ + public double getThrottle() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/utils/LogitechGamingPad.kt b/src/main/java/frc/robot/utils/LogitechGamingPad.kt deleted file mode 100644 index 75506b1..0000000 --- a/src/main/java/frc/robot/utils/LogitechGamingPad.kt +++ /dev/null @@ -1,131 +0,0 @@ -package frc.robot.utils - -import edu.wpi.first.wpilibj.Joystick -import edu.wpi.first.wpilibj.XboxController - -class LogitechGamingPad(usbPort: Int) : XboxController(usbPort) { - private val gamepad = Joystick(usbPort) - - enum class DPad(val angle: Int) { - UP(0), - UP_RIGHT(45), - RIGHT(90), - DOWN_RIGHT(135), - DOWN(180), - DOWN_LEFT(225), - LEFT(270), - UP_LEFT(315), - } - - enum class Button(val button: Int) { - A(1), - B(2), - X(3), - Y(4), - LEFT_BUMPER(5), - RIGHT_BUMPER(6), - BACK(7), - START(8), - } - - enum class Axis(val axis: Int) { - LEFT_ANALOG_X(0), - LEFT_ANALOG_Y(1), - RIGHT_ANALOG_X(4), - RIGHT_ANALOG_Y(5), - } - - enum class Trigger(val trigger: Int) { - LEFT(2), - RIGHT(3), - } - - val leftTriggerValue: Double - get() = gamepad.getRawAxis(Trigger.LEFT.trigger) - - val leftAnalogXAxis: Double - get() = gamepad.getRawAxis(Axis.LEFT_ANALOG_X.axis) - - val leftAnalogYAxis: Double - get() = gamepad.getRawAxis(Axis.LEFT_ANALOG_Y.axis) - - val rightTriggerValue: Double - get() = gamepad.getRawAxis(Trigger.RIGHT.trigger) - - val rightAnalogXAxis: Double - get() = gamepad.getRawAxis(Axis.RIGHT_ANALOG_X.axis) - - val rightAnalogYAxis: Double - get() = gamepad.getRawAxis(Axis.RIGHT_ANALOG_Y.axis) - - override fun getAButton(): Boolean { - return gamepad.getRawButton(Button.A.button) - } - - override fun getBButton(): Boolean { - return gamepad.getRawButton(Button.B.button) - } - - override fun getXButton(): Boolean { - return gamepad.getRawButton(Button.X.button) - } - - override fun getYButton(): Boolean { - return gamepad.getRawButton(Button.Y.button) - } - - override fun getBackButton(): Boolean { - return gamepad.getRawButton(Button.BACK.button) - } - - override fun getStartButton(): Boolean { - return gamepad.getRawButton(Button.START.button) - } - - fun checkDPad(index: Int): Boolean { - return if (0 <= index && index <= 7) (index * 45) == gamepad.pov else false - } - - fun checkDPad(angle: Double, inDegrees: Boolean): Boolean { - var angle = angle - if (!inDegrees) angle = Math.toDegrees(angle) - return angle.toInt() == gamepad.pov - } - - val dPad: Int - get() { - val pov = gamepad.pov - return if (pov == -1) pov else pov / 45 - } - - fun getDPad(inDegrees: Boolean): Double { - return if (inDegrees) gamepad.pov.toDouble() else Math.toRadians(gamepad.pov.toDouble()) - } - - fun dPadIsPressed(): Boolean { - return gamepad.pov != -1 - } - - fun setRumble(amount: Float) { - gamepad.setRumble(RumbleType.kLeftRumble, amount.toDouble()) - gamepad.setRumble(RumbleType.kRightRumble, amount.toDouble()) - } - - override fun getRawAxis(which: Int): Double { - return gamepad.getRawAxis(which) - } - - override fun getRawButton(button: Int): Boolean { - return gamepad.getRawButton(button) - } - - override fun getPOV(pov: Int): Int { - return gamepad.getPOV(pov) - } - - val twist: Double - get() = 0.0 - - val throttle: Double - get() = 0.0 -} diff --git a/src/main/java/frc/robot/utils/PID.java b/src/main/java/frc/robot/utils/PID.java new file mode 100644 index 0000000..731645b --- /dev/null +++ b/src/main/java/frc/robot/utils/PID.java @@ -0,0 +1,214 @@ +package frc.robot.utils; + +/** A class representing a PID controller. */ +public class PID { + private double p; + private double i; + private double d; + private double f = 0.0; + private int s = 0; + + private double previousError = 0.0; + private double integral = 0.0; + private double setpoint = 0.0; + private double error = 0.0; + private double output = 0.0; + + /** + * Constructor for PID with P, I, and D values. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + */ + public PID(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + } + + /** + * Constructor for PID with P, I, D, F values and S value. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + * @param f The feedforward coefficient. + * @param s The S value. + */ + public PID(double p, double i, double d, double f, int s) { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.s = s; + } + + /** + * Constructor for PID with P, I, D, and F values. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + * @param f The feedforward coefficient. + */ + public PID(double p, double i, double d, double f) { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.s = -1; + } + + /** + * Updates the PID coefficients and S value. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + * @param f The feedforward coefficient. + * @param s The S value. + */ + public void updatePID(double p, double i, double d, double f, int s) { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.s = s; + } + + /** + * Updates the PID coefficients. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + * @param f The feedforward coefficient. + */ + public void updatePID(double p, double i, double d, double f) { + this.p = p; + this.i = i; + this.d = d; + this.f = f; + this.s = -1; + } + + /** + * Updates the PID coefficients. + * + * @param p The proportional coefficient. + * @param i The integral coefficient. + * @param d The derivative coefficient. + */ + public void updatePID(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + this.f = 0.0; + this.s = -1; + } + + /** + * Calculates the PID output based on the actual value and setpoint. + * + * @param actual The actual value. + * @param setpoint The target setpoint. + * @return The PID output. + */ + public double calculate(double actual, double setpoint) { + this.error = setpoint - actual; // Error = Target - Actual + this.integral += + (error + * 0.02); // Integral is increased by the error*time (which is 0.02 seconds using normal + double derivative = (error - this.previousError) / 0.02; + this.output = p * error + i * this.integral + d * derivative; + this.previousError = error; + return output; + } + + /** Resets the integral term. */ + public void resetI() { + this.integral = 0.0; + } + + // Getters and setters for the PID coefficients and other fields + public double getP() { + return p; + } + + public void setP(double p) { + this.p = p; + } + + public double getI() { + return i; + } + + public void setI(double i) { + this.i = i; + } + + public double getD() { + return d; + } + + public void setD(double d) { + this.d = d; + } + + public double getF() { + return f; + } + + public void setF(double f) { + this.f = f; + } + + public int getS() { + return s; + } + + public void setS(int s) { + this.s = s; + } + + public double getPreviousError() { + return previousError; + } + + public void setPreviousError(double previousError) { + this.previousError = previousError; + } + + public double getIntegral() { + return integral; + } + + public void setIntegral(double integral) { + this.integral = integral; + } + + public double getSetpoint() { + return setpoint; + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + public double getError() { + return error; + } + + public void setError(double error) { + this.error = error; + } + + public double getOutput() { + return output; + } + + public void setOutput(double output) { + this.output = output; + } +} diff --git a/src/main/java/frc/robot/utils/PID.kt b/src/main/java/frc/robot/utils/PID.kt deleted file mode 100644 index 5050c52..0000000 --- a/src/main/java/frc/robot/utils/PID.kt +++ /dev/null @@ -1,76 +0,0 @@ -package frc.robot.utils - -class PID { - var p: Double - var i: Double - var d: Double - var f: Double = 0.0 - var s: Int = 0 - - constructor(p: Double, i: Double, d: Double) { - this.p = p - this.i = i - this.d = d - } - - constructor(p: Double, i: Double, d: Double, f: Double, s: Int) { - this.p = p - this.i = i - this.d = d - this.f = f - this.s = s - } - - constructor(p: Double, i: Double, d: Double, f: Double) { - this.p = p - this.i = i - this.d = d - this.f = f - this.s = -1 - } - - fun updatePID(p: Double, i: Double, d: Double, f: Double, s: Int) { - this.p = p - this.i = i - this.d = d - this.f = f - this.s = s - } - - fun updatePID(p: Double, i: Double, d: Double, f: Double) { - this.p = p - this.i = i - this.d = d - this.f = f - this.s = -1 - } - - fun updatePID(p: Double, i: Double, d: Double) { - this.p = p - this.i = i - this.d = d - this.f = 0.0 - this.s = -1 - } - - var previous_error: Double = 0.0 - var integral: Double = 0.0 - var setpoint: Double = 0.0 - var error: Double = 0.0 - var output: Double = 0.0 - private set - - fun calculate(actual: Double, setpoint: Double): Double { - error = setpoint - actual // Error = Target - Actual - this.integral += - (error * .02) // Integral is increased by the error*time (which is .02 seconds using normal - val derivative = (error - this.previous_error) / .02 - this.output = p * error + i * this.integral + d * derivative - this.previous_error = error - return output - } - - fun resetI() { - this.integral = 0.0 - } -} diff --git a/src/main/java/frc/robot/utils/RobotParameters.java b/src/main/java/frc/robot/utils/RobotParameters.java new file mode 100644 index 0000000..a8fd8a9 --- /dev/null +++ b/src/main/java/frc/robot/utils/RobotParameters.java @@ -0,0 +1,145 @@ +package frc.robot.utils; + +import com.ctre.phoenix6.signals.InvertedValue; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import java.io.IOException; +import org.json.simple.parser.ParseException; + +/** Class containing global values for the robot. */ +@SuppressWarnings("ALL") +public class RobotParameters { + /** Class containing global values related to motors. */ + public static class MotorParameters { + // Motor CAN ID Values + public static final int FRONT_LEFT_STEER_ID = 1; + public static final int FRONT_LEFT_DRIVE_ID = 2; + public static final int FRONT_RIGHT_STEER_ID = 3; + public static final int FRONT_RIGHT_DRIVE_ID = 4; + public static final int BACK_LEFT_STEER_ID = 5; + public static final int BACK_LEFT_DRIVE_ID = 6; + public static final int BACK_RIGHT_STEER_ID = 7; + public static final int BACK_RIGHT_DRIVE_ID = 8; + public static final int FRONT_LEFT_CAN_CODER_ID = 9; + public static final int FRONT_RIGHT_CAN_CODER_ID = 10; + public static final int BACK_LEFT_CAN_CODER_ID = 11; + public static final int BACK_RIGHT_CAN_CODER_ID = 12; + public static final int PIDGEY_ID = 16; + + // Motor Property Values + public static final double MAX_SPEED = 5.76; + public static final double MAX_ANGULAR_SPEED = (14 * Math.PI) / 3; + public static final double ENCODER_COUNTS_PER_ROTATION = 1.0; + public static final double STEER_MOTOR_GEAR_RATIO = 150.0 / 7; + public static final double DRIVE_MOTOR_GEAR_RATIO = 5.9; + public static final double WHEEL_DIAMETER = 0.106; + public static final double SPEED_CONSTANT = 0.6; + public static final double AACORN_SPEED = 0.95; + public static final double SLOW_SPEED = 0.3; + public static final double TURN_CONSTANT = 0.3; + public static final double METERS_PER_REV = WHEEL_DIAMETER * Math.PI * 0.99; + public static double HEADING = 0.0; + + // Limit Values + public static final double DRIVE_SUPPLY_LIMIT = 45.0; + public static final double DRIVE_STATOR_LIMIT = 80.0; + public static final double DRIVE_SUPPLY_THRESHOLD = 30.0; + public static final double DRIVE_TIME_THRESHOLD = 0.25; + public static final double STEER_SUPPLY_LIMIT = 30.0; + public static final double STEER_SUPPLY_THRESHOLD = 30.0; + public static final double STEER_TIME_THRESHOLD = 0.25; + + // Motor Speed Manipulation Values + public static boolean SLOW_MODE = false; + public static boolean AACORN_MODE = true; + } + + /** Class containing global values related to the swerve drive system. */ + public static class SwerveParameters { + + /** Class containing PID constants for the swerve drive system. */ + public static class PIDParameters { + public static final PID STEER_PID_TELE = new PID(13.0, 0.000, 0.1, 0.0); + public static final PID STEER_PID_AUTO = new PID(15.0, 0.000, 0.1, 0.0); + public static final PID DRIVE_PID_AUTO = new PID(7.0, 0.0, 0.00); + public static final double DRIVE_PID_V_AUTO = 0.5; + public static final PID DRIVE_PID_TELE = new PID(1.5, 0.0, 0.0); + public static final double DRIVE_PID_V_TELE = 0.0; + public static final PID ROTATIONAL_PID = new PID(0.2, 0.0, 0.0, 0.0); + public static final PID PASS_ROTATIONAL_PID = new PID(0.1, 0.0, 0.0, 0.0); + public static PPHolonomicDriveController pathFollower = + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.00, 0.0), // translation + new PIDConstants(5.0, 0.0, 0.0) // rotation + ); + public static RobotConfig config; + + static { + try { + config = RobotConfig.fromGUISettings(); + } catch (IOException e) { + throw new RuntimeException("Failed to load robot config", e); + } catch (ParseException e) { + throw new RuntimeException("Failed to load robot config", e); + } + } + } + + /** Class containing physical dimensions and kinematics for the swerve drive system. */ + public static class PhysicalParameters { + public static final double ROBOT_SIZE = 0.43105229381; + public static final Translation2d FRONT_LEFT = new Translation2d(0.3048, 0.3048); + public static final Translation2d FRONT_RIGHT = new Translation2d(0.3048, -0.3048); + public static final Translation2d BACK_LEFT = new Translation2d(-0.3048, 0.3048); + public static final Translation2d BACK_RIGHT = new Translation2d(-0.3048, -0.3048); + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); + public static final double BASE_LENGTH_ERICK_TRAN = 0.3048 * 2; + } + + /** Class containing various thresholds and constants for the swerve drive system. */ + public static class Thresholds { + public static final double STATE_SPEED_THRESHOLD = 0.05; + public static final double CANCODER_VAL9 = -0.419189; + public static final double CANCODER_VAL10 = -0.825928 - 0.5; + public static final double CANCODER_VAL11 = -0.475098; + public static final double CANCODER_VAL12 = -0.032959 + 0.5; + public static final InvertedValue DRIVE_MOTOR_INVERETED = + InvertedValue.CounterClockwise_Positive; + public static final InvertedValue STEER_MOTOR_INVERTED = InvertedValue.Clockwise_Positive; + public static final double JOYSTICK_DEADBAND = 0.05; + public static final boolean USING_VISION = false; + public static final boolean FIELD_ORIENTATED = true; + public static final boolean AUTO_ALIGN = false; + public static final double LIMELIGHT_DEADBAND = 1.5; + public static final double MOTOR_DEADBAND = 0.05; + public static final boolean IS_FIELD_ORIENTED = true; + public static final boolean SHOULD_INVERT = false; + public static final double ENCODER_OFFSET = (0 / 360.0); + public static Pose2d currentPose = new Pose2d(0.0, 0.0, new Rotation2d(0.0)); + public static final int[] GREEN_LED = {0, 255, 0}; + public static final int[] ORANGE_LED = {255, 165, 0}; + public static final int[] HIGHTIDE_LED = {0, 182, 174}; + public static final double X_DEADZONE = 0.15 * 5.76; + public static final double Y_DEADZONE = 0.15 * 5.76; + public static final double OFF_BALANCE_ANGLE_THRESHOLD = 10.0; + public static final double ON_BALANCE_ANGLE_THRESHOLD = 5.0; + public static boolean TEST_MODE = true; + } + } + + /** Class containing constants for the PhotonVision system. */ + public static class PhotonVisionConstants { + public static final double CAMERA_ONE_HEIGHT_METER = 0.47; + public static final double CAMERA_ONE_ANGLE_DEG = 33.0; + public static final double OFFSET_TOWARD_MID_LEFT = -15.00; + public static final double CAMERA_TWO_HEIGHT_METER = 0.61; + public static final double CAMERA_TWO_ANGLE_DEG = 37.5; + public static final double OFFSET_TOWARD_MID_RIGHT = 15.0; + } +} diff --git a/src/main/java/frc/robot/utils/RobotParameters.kt b/src/main/java/frc/robot/utils/RobotParameters.kt deleted file mode 100644 index 0d7e607..0000000 --- a/src/main/java/frc/robot/utils/RobotParameters.kt +++ /dev/null @@ -1,132 +0,0 @@ -@file:Suppress("unused", "MemberVisibilityCanBePrivate") - -package frc.robot.utils - -import com.ctre.phoenix6.signals.InvertedValue -import com.pathplanner.lib.config.PIDConstants -import com.pathplanner.lib.config.RobotConfig -import com.pathplanner.lib.controllers.PPHolonomicDriveController -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.kinematics.SwerveDriveKinematics - -/** Object containing global values for the robot. */ -object RobotParameters { - /** Object containing global values related to motors. */ - object MotorParameters { - // Motor CAN ID Values - const val FRONT_LEFT_STEER_ID: Int = 1 - const val FRONT_LEFT_DRIVE_ID: Int = 2 - const val FRONT_RIGHT_STEER_ID: Int = 3 - const val FRONT_RIGHT_DRIVE_ID: Int = 4 - const val BACK_LEFT_STEER_ID: Int = 5 - const val BACK_LEFT_DRIVE_ID: Int = 6 - const val BACK_RIGHT_STEER_ID: Int = 7 - const val BACK_RIGHT_DRIVE_ID: Int = 8 - const val FRONT_LEFT_CAN_CODER_ID: Int = 9 - const val FRONT_RIGHT_CAN_CODER_ID: Int = 10 - const val BACK_LEFT_CAN_CODER_ID: Int = 11 - const val BACK_RIGHT_CAN_CODER_ID: Int = 12 - const val PIDGEY_ID: Int = 16 - - // Motor Property Values - const val MAX_SPEED: Double = 5.76 - const val MAX_ANGULAR_SPEED: Double = (14 * Math.PI) / 3 - const val ENCODER_COUNTS_PER_ROTATION: Double = 1.0 - const val STEER_MOTOR_GEAR_RATIO: Double = 150.0 / 7 - const val DRIVE_MOTOR_GEAR_RATIO: Double = 5.9 - const val WHEEL_DIAMETER: Double = 0.106 - const val SPEED_CONSTANT: Double = 0.6 - const val AACORN_SPEED: Double = 0.95 - const val SLOW_SPEED: Double = 0.3 - const val TURN_CONSTANT: Double = 0.3 - const val METERS_PER_REV: Double = WHEEL_DIAMETER * Math.PI * 0.99 - var HEADING: Double = 0.0 - - // Limit Values - const val DRIVE_SUPPLY_LIMIT: Double = 45.0 - const val DRIVE_STATOR_LIMIT: Double = 80.0 - const val DRIVE_SUPPLY_THRESHOLD: Double = 30.0 - const val DRIVE_TIME_THRESHOLD: Double = 0.25 - const val STEER_SUPPLY_LIMIT: Double = 30.0 - const val STEER_SUPPLY_THRESHOLD: Double = 30.0 - const val STEER_TIME_THRESHOLD: Double = 0.25 - - // Motor Speed Manipulation Values - var SLOW_MODE: Boolean = false - var AACORN_MODE: Boolean = true - } - - /** Object containing global values related to the swerve drive system. */ - object SwerveParameters { - /** Object containing PID constants for the swerve drive system. */ - object PIDParameters { - val STEER_PID_TELE: PID = PID(13.0, 0.000, 0.1, 0.0) - val STEER_PID_AUTO: PID = PID(15.0, 0.000, 0.1, 0.0) - val DRIVE_PID_AUTO: PID = PID(7.0, 0.0, 0.00) - const val DRIVE_PID_V_AUTO: Double = 0.5 - val DRIVE_PID_TELE: PID = PID(1.5, 0.0, 0.0) - const val DRIVE_PID_V_TELE: Double = 0.0 - val ROTATIONAL_PID: PID = PID(0.2, 0.0, 0.0, 0.0) - val PASS_ROTATIONAL_PID: PID = PID(0.1, 0.0, 0.0, 0.0) - var pathFollower: PPHolonomicDriveController = - PPHolonomicDriveController( - PIDConstants(5.0, 0.00, 0.0), // translation - PIDConstants(5.0, 0.0, 0.0), // rotation - ) - var config: RobotConfig = RobotConfig.fromGUISettings() - } - - /** Object containing physical dimensions and kinematics for the swerve drive system. */ - object PhysicalParameters { - const val ROBOT_SIZE: Double = 0.43105229381 - val FRONT_LEFT: Translation2d = Translation2d(0.3048, 0.3048) - val FRONT_RIGHT: Translation2d = Translation2d(0.3048, -0.3048) - val BACK_LEFT: Translation2d = Translation2d(-0.3048, 0.3048) - val BACK_RIGHT: Translation2d = Translation2d(-0.3048, -0.3048) - val kinematics: SwerveDriveKinematics = - SwerveDriveKinematics(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT) - const val BASE_LENGTH_ERICK_TRAN: Double = 0.3048 * 2 - } - - /** Object containing various thresholds and constants for the swerve drive system. */ - object Thresholds { - const val STATE_SPEED_THRESHOLD: Double = 0.05 - const val CANCODER_VAL9: Double = -0.419189 - const val CANCODER_VAL10: Double = -0.825928 - 0.5 - const val CANCODER_VAL11: Double = -0.475098 - const val CANCODER_VAL12: Double = -0.032959 + 0.5 - val DRIVE_MOTOR_INVERETED: InvertedValue = InvertedValue.CounterClockwise_Positive - val STEER_MOTOR_INVERTED: InvertedValue = InvertedValue.Clockwise_Positive - const val JOYSTICK_DEADBAND: Double = 0.05 - const val USING_VISION: Boolean = false - const val FIELD_ORIENTATED: Boolean = true - const val AUTO_ALIGN: Boolean = false - const val LIMELIGHT_DEADBAND: Double = 1.5 - const val MOTOR_DEADBAND: Double = 0.05 - const val IS_FIELD_ORIENTED: Boolean = true - const val SHOULD_INVERT = false - const val ENCODER_OFFSET: Double = (0 / 360).toDouble() - var currentPose = Pose2d(0.0, 0.0, Rotation2d(0.0)) - val GREEN_LED: IntArray = intArrayOf(0, 255, 0) - val ORANGE_LED: IntArray = intArrayOf(255, 165, 0) - val HIGHTIDE_LED: IntArray = intArrayOf(0, 182, 174) - const val X_DEADZONE: Double = 0.15 * 5.76 - const val Y_DEADZONE: Double = 0.15 * 5.76 - const val OFF_BALANCE_ANGLE_THRESHOLD: Double = 10.0 - const val ON_BALANCE_ANGLE_THRESHOLD: Double = 5.0 - var TEST_MODE: Boolean = true - } - } - - /** Object containing constants for the PhotonVision system. */ - object PhotonVisionConstants { - const val CAMERA_ONE_HEIGHT_METER: Double = 0.47 - const val CAMERA_ONE_ANGLE_DEG: Double = 33.0 - const val OFFSET_TOWARD_MID_LEFT: Double = -15.00 - const val CAMERA_TWO_HEIGHT_METER: Double = 0.61 - const val CAMERA_TWO_ANGLE_DEG: Double = 37.5 - const val OFFSET_TOWARD_MID_RIGHT: Double = 15.0 - } -}