diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/bin/main/frc/robot/Constants$JoystickConstants.class b/bin/main/frc/robot/Constants$JoystickConstants.class index da3bdb4..965439f 100644 Binary files a/bin/main/frc/robot/Constants$JoystickConstants.class and b/bin/main/frc/robot/Constants$JoystickConstants.class differ diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 50358f9..50de9b4 100644 Binary files a/bin/main/frc/robot/Constants$RobotConstants.class and b/bin/main/frc/robot/Constants$RobotConstants.class differ diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 311ccbf..90fed52 100644 Binary files a/bin/main/frc/robot/Constants$SuperstructureConstants.class and b/bin/main/frc/robot/Constants$SuperstructureConstants.class differ diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index 73f46c0..ed526e7 100644 Binary files a/bin/main/frc/robot/Constants$SwerveConstants.class and b/bin/main/frc/robot/Constants$SwerveConstants.class differ diff --git a/bin/main/frc/robot/Constants$VisionConstants.class b/bin/main/frc/robot/Constants$VisionConstants.class new file mode 100644 index 0000000..8844f75 Binary files /dev/null and b/bin/main/frc/robot/Constants$VisionConstants.class differ diff --git a/bin/main/frc/robot/Constants.class b/bin/main/frc/robot/Constants.class index 0c012e4..3b69068 100644 Binary files a/bin/main/frc/robot/Constants.class and b/bin/main/frc/robot/Constants.class differ diff --git a/bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class b/bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class new file mode 100644 index 0000000..9f9c6e9 Binary files /dev/null and b/bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class differ diff --git a/bin/main/frc/robot/IO/GyroIO.class b/bin/main/frc/robot/IO/GyroIO.class new file mode 100644 index 0000000..2693857 Binary files /dev/null and b/bin/main/frc/robot/IO/GyroIO.class differ diff --git a/bin/main/frc/robot/IO/GyroIOPigeon2.class b/bin/main/frc/robot/IO/GyroIOPigeon2.class new file mode 100644 index 0000000..528c80d Binary files /dev/null and b/bin/main/frc/robot/IO/GyroIOPigeon2.class differ diff --git a/bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class b/bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class new file mode 100644 index 0000000..dd9812c Binary files /dev/null and b/bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class differ diff --git a/bin/main/frc/robot/IO/ModuleIO.class b/bin/main/frc/robot/IO/ModuleIO.class new file mode 100644 index 0000000..68e6ef6 Binary files /dev/null and b/bin/main/frc/robot/IO/ModuleIO.class differ diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 08140ed..96f44d0 100644 Binary files a/bin/main/frc/robot/RobotContainer.class and b/bin/main/frc/robot/RobotContainer.class differ diff --git a/bin/main/frc/robot/commands/PathPlannerCommand.class b/bin/main/frc/robot/commands/PathPlannerCommand.class new file mode 100644 index 0000000..f2efd3d Binary files /dev/null and b/bin/main/frc/robot/commands/PathPlannerCommand.class differ diff --git a/bin/main/frc/robot/commands/TeleopSuperstructure.class b/bin/main/frc/robot/commands/TeleopSuperstructure.class index c2e609d..f5b8661 100644 Binary files a/bin/main/frc/robot/commands/TeleopSuperstructure.class and b/bin/main/frc/robot/commands/TeleopSuperstructure.class differ diff --git a/bin/main/frc/robot/commands/TeleopSwerve.class b/bin/main/frc/robot/commands/TeleopSwerve.class index af250cb..52abd9f 100644 Binary files a/bin/main/frc/robot/commands/TeleopSwerve.class and b/bin/main/frc/robot/commands/TeleopSwerve.class differ diff --git a/bin/main/frc/robot/subsystems/Elevator.class b/bin/main/frc/robot/subsystems/Elevator.class index 132ba38..4895b01 100644 Binary files a/bin/main/frc/robot/subsystems/Elevator.class and b/bin/main/frc/robot/subsystems/Elevator.class differ diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class index 2b7dcf7..1b119a9 100644 Binary files a/bin/main/frc/robot/subsystems/Grabber.class and b/bin/main/frc/robot/subsystems/Grabber.class differ diff --git a/bin/main/frc/robot/subsystems/Intake.class b/bin/main/frc/robot/subsystems/Intake.class index 07ecf05..dc684d4 100644 Binary files a/bin/main/frc/robot/subsystems/Intake.class and b/bin/main/frc/robot/subsystems/Intake.class differ diff --git a/bin/main/frc/robot/subsystems/LED.class b/bin/main/frc/robot/subsystems/LED.class index fb1aeaa..25c236b 100644 Binary files a/bin/main/frc/robot/subsystems/LED.class and b/bin/main/frc/robot/subsystems/LED.class differ diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class new file mode 100644 index 0000000..0fa0a12 Binary files /dev/null and b/bin/main/frc/robot/subsystems/Pathplanner.class differ diff --git a/bin/main/frc/robot/subsystems/Superstructure.class b/bin/main/frc/robot/subsystems/Superstructure.class index 4e7604f..7125660 100644 Binary files a/bin/main/frc/robot/subsystems/Superstructure.class and b/bin/main/frc/robot/subsystems/Superstructure.class differ diff --git a/bin/main/frc/robot/subsystems/Swerve.class b/bin/main/frc/robot/subsystems/Swerve.class index 54614ef..3e60c4b 100644 Binary files a/bin/main/frc/robot/subsystems/Swerve.class and b/bin/main/frc/robot/subsystems/Swerve.class differ diff --git a/bin/main/frc/robot/subsystems/SwerveDrive.class b/bin/main/frc/robot/subsystems/SwerveDrive.class new file mode 100644 index 0000000..91e3361 Binary files /dev/null and b/bin/main/frc/robot/subsystems/SwerveDrive.class differ diff --git a/bin/main/frc/robot/subsystems/SwerveDriveOld.class b/bin/main/frc/robot/subsystems/SwerveDriveOld.class new file mode 100644 index 0000000..981a5b1 Binary files /dev/null and b/bin/main/frc/robot/subsystems/SwerveDriveOld.class differ diff --git a/bin/main/frc/robot/subsystems/SwerveModule.class b/bin/main/frc/robot/subsystems/SwerveModule.class index 979055e..ccc67fb 100644 Binary files a/bin/main/frc/robot/subsystems/SwerveModule.class and b/bin/main/frc/robot/subsystems/SwerveModule.class differ diff --git a/bin/main/frc/robot/subsystems/SwerveModuleOld.class b/bin/main/frc/robot/subsystems/SwerveModuleOld.class new file mode 100644 index 0000000..0cd9490 Binary files /dev/null and b/bin/main/frc/robot/subsystems/SwerveModuleOld.class differ diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class new file mode 100644 index 0000000..6a5f16c Binary files /dev/null and b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class differ diff --git a/bin/main/frc/robot/subsystems/SwerveVision.class b/bin/main/frc/robot/subsystems/SwerveVision.class new file mode 100644 index 0000000..e72f9dc Binary files /dev/null and b/bin/main/frc/robot/subsystems/SwerveVision.class differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23b961c..7236c70 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,10 +4,19 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import frc.FSLib2025.swerve.SwerveModuleConstants; public class Constants { @@ -17,6 +26,7 @@ public static final class RobotConstants { public static final String CANBUS_NAME = "GTX7130"; public static final int DRIVE_CONTROLLER_PORT = 0; public static final int OPERATOR_BUTTONBOX_PORT = 1; + public static final double DRIVE_CONTROLLER_DEADBAND = 0.1; } public static final class SwerveConstants { @@ -26,23 +36,36 @@ public static final class SwerveConstants { public static final int PIGEON_ID = 40; - public static final double STEER_MOTOR_KP = 0.008; - public static final double STEER_MOTOR_KI = 0.05; - public static final double STEER_MOTOR_KD = 0.005; + public static final double DRIVE_MOTOR_KP = 0.0; + public static final double DRIVE_MOTOR_KI = 0.0; + public static final double DRIVE_MOTOR_KD = 0.0; + public static final double DRIVE_MOTOR_KS = 0.0; + public static final double DRIVE_MOTOR_KV = 0.0; + public static final double DRIVE_MOTOR_KA = 0.0; + + public static final double STEER_MOTOR_KP = 0.0005; + public static final double STEER_MOTOR_KI = 0.000015; + public static final double STEER_MOTOR_KD = 0.000001; public static final double STEER_MOTOR_WINDUP = 0.0; public static final int STEER_MOTOR_LIMIT = 0; public static final double DRIVE_MOTOR_GEAR_RATIO = 6.122449; - public static final double DRIVE_WHEEL_DIAMETERS = 4 * 0.0254; // meters + public static final double STEER_MOTOR_GEAR_RATIO = 150.0 / 7.0; + + public static final boolean IS_STEER_MOTOR_INVERTED = false; + + public static final double DRIVE_WHEEL_DIAMETERS = 0.0964511800486235; // meters public static final double DRIVE_WHEEL_PERIMETER = Math.PI * DRIVE_WHEEL_DIAMETERS; // meters - public static final Translation2d[] MODULE_TRANSLATOIN_METERS = new Translation2d[] { + public static Rotation2d gyroYaw = new Rotation2d(); + + public static final Translation2d[] MODULE_TRANSLATION_METERS = new Translation2d[] { new Translation2d(WHEEL_BASE / 2.0, WHEEL_BASE / 2.0), new Translation2d(WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), new Translation2d(-WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), new Translation2d(-WHEEL_BASE / 2.0, WHEEL_BASE / 2.0) }; - + public static final SwerveModuleConstants MOD0_CONSTANTS = new SwerveModuleConstants(); static { MOD0_CONSTANTS.DriveMotorId = 1; @@ -50,7 +73,7 @@ public static final class SwerveConstants { MOD0_CONSTANTS.CANcoderId = 0; MOD0_CONSTANTS.CANcoderOffset = -0.272461; } - + public static final SwerveModuleConstants MOD1_CONSTANTS = new SwerveModuleConstants(); static { MOD1_CONSTANTS.DriveMotorId = 11; @@ -58,7 +81,7 @@ public static final class SwerveConstants { MOD1_CONSTANTS.CANcoderId = 1; MOD1_CONSTANTS.CANcoderOffset = -0.123047; } - + public static final SwerveModuleConstants MOD2_CONSTANTS = new SwerveModuleConstants(); static { MOD2_CONSTANTS.DriveMotorId = 21; @@ -66,7 +89,7 @@ public static final class SwerveConstants { MOD2_CONSTANTS.CANcoderId = 2; MOD2_CONSTANTS.CANcoderOffset = 0.205322; } - + public static final SwerveModuleConstants MOD3_CONSTANTS = new SwerveModuleConstants(); static { MOD3_CONSTANTS.DriveMotorId = 31; @@ -75,15 +98,26 @@ public static final class SwerveConstants { MOD3_CONSTANTS.CANcoderOffset = -0.119141; } + public static final SwerveModuleConstants[] SWERVE_MODULE_CONSTANTS = new SwerveModuleConstants[] { + MOD0_CONSTANTS, + MOD1_CONSTANTS, + MOD2_CONSTANTS, + MOD3_CONSTANTS + }; + // motor configuration public static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = new TalonFXConfiguration(); static { + DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimit = 40; + DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; DRIVE_MOTOR_CONFIGURATION.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_MOTOR_GEAR_RATIO; } public static final SparkMaxConfig STEER_MOTOR_CONFIGURATION = new SparkMaxConfig(); static { - STEER_MOTOR_CONFIGURATION.inverted(false); + STEER_MOTOR_CONFIGURATION.smartCurrentLimit(30); + STEER_MOTOR_CONFIGURATION.voltageCompensation(12.0); + STEER_MOTOR_CONFIGURATION.inverted(IS_STEER_MOTOR_INVERTED); STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake); } } @@ -233,4 +267,14 @@ public static final class SuperstructureConstants { public static final class JoystickConstants { } + + public static final class VisionConstants { + public static final String CHASSIS_CAM_NAME = "mydickisbig"; + public static final Transform3d ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); + public static final AprilTagFieldLayout TAG_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + // placeholder values + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } } diff --git a/src/main/java/frc/robot/IO/GyroIO.java b/src/main/java/frc/robot/IO/GyroIO.java new file mode 100644 index 0000000..f88ea36 --- /dev/null +++ b/src/main/java/frc/robot/IO/GyroIO.java @@ -0,0 +1,28 @@ +package frc.robot.IO; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.math.geometry.Rotation2d; + + +public interface GyroIO { + + public static class GyroIOInputs implements LoggableInputs { + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; + @Override + public void toLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'toLog'"); + } + @Override + public void fromLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'fromLog'"); + } + } + + public default void updateInputs(GyroIOInputs inputs) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/IO/GyroIOPigeon2.java b/src/main/java/frc/robot/IO/GyroIOPigeon2.java new file mode 100644 index 0000000..bcd390c --- /dev/null +++ b/src/main/java/frc/robot/IO/GyroIOPigeon2.java @@ -0,0 +1,33 @@ +package frc.robot.IO; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +/** IO implementation for Pigeon2 */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = new Pigeon2(40,"GTX7130"); + private final StatusSignal yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/IO/ModuleIO.java b/src/main/java/frc/robot/IO/ModuleIO.java new file mode 100644 index 0000000..ced430c --- /dev/null +++ b/src/main/java/frc/robot/IO/ModuleIO.java @@ -0,0 +1,53 @@ +// 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.IO; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs implements LoggableInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + @Override + public void toLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'toLog'"); + } + + @Override + public void fromLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'fromLog'"); + } + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 321bda5..bb46f7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,30 +8,37 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.RobotConstants; +import frc.robot.IO.GyroIOPigeon2; +import frc.robot.commands.PathPlannerCommand; import frc.robot.commands.TeleopSuperstructure; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; import frc.robot.subsystems.Intake; import frc.robot.subsystems.LED; +import frc.robot.subsystems.PathPlanner; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.subsystems.SwervePoseEstimator; public class RobotContainer { private CommandXboxController m_Controller; private Joystick joystick; private static ButtonBox m_buttonbox; + // private final Elevator m_elevator; + // private final Grabber m_grabber; + // private final Intake m_intake; + // private final LED m_led; + // private final Superstructure m_superstructure; + private final SwerveDrive m_swerveD; + private final SwervePoseEstimator m_swervePE; + private final PathPlanner m_pathplanner; - private final Swerve m_swerve; - private final Elevator m_elevator; - private final Grabber m_grabber; - private final Intake m_intake; - private final LED m_led; - private final Superstructure m_superstructure; - - private final TeleopSwerve teleopSwerve; - private final TeleopSuperstructure teleopSuperstructure; + // private final TeleopSuperstructure teleopSuperstructure; + private final PathPlannerCommand pathPlannerCommand; + private static Command driveCommand; private final SendableChooser autoChooser; @@ -40,18 +47,33 @@ public RobotContainer() { joystick = new Joystick(RobotConstants.OPERATOR_BUTTONBOX_PORT); m_buttonbox = new ButtonBox(joystick); - m_swerve = Swerve.getInstance(); - m_elevator = Elevator.getInstance(); - m_grabber = Grabber.getInstance(); - m_intake = Intake.getInstance(); - m_led = LED.getInstance(); - m_superstructure = Superstructure.getInstance(); + m_swerveD = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); + m_swervePE = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); + m_pathplanner = PathPlanner.getInstance(); + // m_elevator = Elevator.getInstance(); + // m_grabber = Grabber.getInstance(); + // m_intake = Intake.getInstance(); + // m_led = LED.getInstance(); + // m_superstructure = Superstructure.getInstance(); - teleopSwerve = new TeleopSwerve(m_swerve, m_Controller); - teleopSuperstructure = new TeleopSuperstructure(); + // teleopSuperstructure = new TeleopSuperstructure(); + pathPlannerCommand = new PathPlannerCommand(m_pathplanner); + driveCommand = TeleopSwerve.joystickDrive( + m_swerveD, + m_swervePE, + () -> -m_Controller.getLeftX(), + () -> -m_Controller.getLeftY(), + () -> -m_Controller.getRightX()); - m_swerve.setDefaultCommand(teleopSwerve); - m_superstructure.setDefaultCommand(teleopSuperstructure); + m_swerveD.setDefaultCommand(driveCommand); + m_swervePE.setDefaultCommand(driveCommand); + // m_superstructure.setDefaultCommand(teleopSuperstructure); + m_pathplanner.setDefaultCommand(pathPlannerCommand); configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/PathPlannerCommand.java b/src/main/java/frc/robot/commands/PathPlannerCommand.java new file mode 100644 index 0000000..efb84a3 --- /dev/null +++ b/src/main/java/frc/robot/commands/PathPlannerCommand.java @@ -0,0 +1,37 @@ +// 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.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.PathPlanner; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class PathPlannerCommand extends Command { + + PathPlanner pathPlanner; + + public PathPlannerCommand(PathPlanner pathPlanner) { + this.pathPlanner = pathPlanner; + addRequirements(pathPlanner); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSuperstructure.java b/src/main/java/frc/robot/commands/TeleopSuperstructure.java index 2a5163a..45ba34d 100644 --- a/src/main/java/frc/robot/commands/TeleopSuperstructure.java +++ b/src/main/java/frc/robot/commands/TeleopSuperstructure.java @@ -1,6 +1,5 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.ButtonBox; import frc.robot.RobotContainer; diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index e426f25..8001b0b 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,67 +1,71 @@ + package frc.robot.commands; -import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; -import frc.robot.subsystems.Swerve; - -public class TeleopSwerve extends Command { - - private final Swerve swerve; - - private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); - private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); - private SlewRateLimiter rotLimiter = new SlewRateLimiter(3.0); - - private double xSpeed = 0.0; - private double ySpeed = 0.0; - private double rotSpeed = 0.0; - - private double reduction = 1; - - private CommandXboxController controller; - - public TeleopSwerve(Swerve swerve, CommandXboxController controller) { - this.swerve = swerve; - this.controller = controller; - addRequirements(swerve); - } +import frc.robot.subsystems.SwerveDrive; +import frc.robot.subsystems.SwervePoseEstimator; - @Override - public void execute() { +import java.util.function.DoubleSupplier; - if (controller.getHID().getAButtonPressed()) { - swerve.setOdometryPosition(new Pose2d()); - swerve.setGyroYaw(new Rotation2d()); - } +public class TeleopSwerve { + private TeleopSwerve() {} + - if (controller.getHID().getRightBumperButton()) { - reduction = 0.3; - } else { - reduction = 1; - } + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + SwerveDrive drive, + SwervePoseEstimator poseEstimator, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), RobotConstants.DRIVE_CONTROLLER_DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), RobotConstants.DRIVE_CONTROLLER_DEADBAND); - xSpeed = xLimiter.calculate(-controller.getLeftY() * reduction); - ySpeed = yLimiter.calculate(-controller.getLeftX() * reduction); - rotSpeed = rotLimiter.calculate(-controller.getRightX() * reduction); + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); - // square the input to inprove driving experience - xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); - rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); - swerve.drive( - new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED), - rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED, - true); - } + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * SwerveConstants.MAX_MODULE_SPEED * 0.3, + linearVelocity.getY() * SwerveConstants.MAX_MODULE_SPEED * 0.3, + omega * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * 0.3, + isFlipped + ? poseEstimator.getPERotation().plus(new Rotation2d(Math.PI)) + : poseEstimator.getPERotation())); + }, + drive); + } - @Override - public void end(boolean interrupted) { - swerve.drive(new Translation2d(), 0, false); - } -} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 476f1dc..7a9433d 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.SuperstructureConstants; @@ -31,13 +30,13 @@ public class Elevator extends SubsystemBase { // cancoder private final CANcoder cancoder; - private static Elevator mInstance = null; + private static Elevator m_Instance = null; public static Elevator getInstance() { - if (mInstance == null) { - mInstance = new Elevator(); + if (m_Instance == null) { + m_Instance = new Elevator(); } - return mInstance; + return m_Instance; } // shuffleboard diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index a9732cf..68596d7 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -8,14 +8,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.FSLib2025.state_machine.SuperstructureState; import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SuperstructureConstants; @@ -54,13 +50,13 @@ public class Grabber extends SubsystemBase { .withWidget(BuiltInWidgets.kTextView) .getEntry(); - private static Grabber mInstance = null; + private static Grabber m_Instance = null; public static Grabber getInstance() { - if (mInstance == null) { - mInstance = new Grabber(); + if (m_Instance == null) { + m_Instance = new Grabber(); } - return mInstance; + return m_Instance; } public Grabber() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 9dd1dbc..4ec64a2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -50,13 +50,13 @@ public class Intake extends SubsystemBase { .withWidget(BuiltInWidgets.kTextView) .getEntry(); - private static Intake mInstance = null; + private static Intake m_Instance = null; public static Intake getInstance() { - if (mInstance == null) { - mInstance = new Intake(); + if (m_Instance == null) { + m_Instance = new Intake(); } - return mInstance; + return m_Instance; } public Intake() { diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index d0af787..681ff8b 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -13,13 +13,13 @@ public class LED extends SubsystemBase { private static AddressableLED m_led; private static AddressableLEDBuffer m_ledBuffer; private final Timer timer = new Timer(); - private static LED mInstance = null; + private static LED m_Instance = null; public static LED getInstance() { - if (mInstance == null) { - mInstance = new LED(); + if (m_Instance == null) { + m_Instance = new LED(); } - return mInstance; + return m_Instance; } /** Creates a new LED. */ diff --git a/src/main/java/frc/robot/subsystems/Pathplanner.java b/src/main/java/frc/robot/subsystems/Pathplanner.java new file mode 100644 index 0000000..bbb1999 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Pathplanner.java @@ -0,0 +1,83 @@ +// 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.subsystems; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.IO.GyroIOPigeon2; + +public class PathPlanner extends SubsystemBase { + + private static PathPlanner m_instance = null; + + public static PathPlanner getInstance() { + if (m_instance == null) { + m_instance = new PathPlanner(); + } + return m_instance; + } + + private final SwerveDrive m_swerve = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); + private final SwervePoseEstimator m_poseEstimator = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); + + private Field2d m_field = new Field2d(); + + public PathPlanner() { + + RobotConfig config = null; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + if (config == null) { + throw new RuntimeException("Failed to load config"); + } + + AutoBuilder.configure( + m_poseEstimator::getPoseEstimatorPose, + m_poseEstimator::setPoseEstimatorPose, + m_poseEstimator::getRobotRelativeSpeeds, + (speeds, feedforwards) -> m_swerve.runVelocity(speeds), + new PPHolonomicDriveController( + new PIDConstants(10.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config, + () -> { + var alliance = DriverStation.getAlliance(); + if(alliance.isPresent()){ + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + } + ); + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + m_field.getObject("target").setPose(pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + m_field.getObject("path").setPoses(poses); + }); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a392d90..10fb24c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -18,13 +18,13 @@ public class Superstructure extends SubsystemBase { StateMachine mStateMachine; SuperstructureState mCommandedState; - private static Superstructure mInstance = null; + private static Superstructure m_Instance = null; public static Superstructure getInstance() { - if (mInstance == null) { - mInstance = new Superstructure(); + if (m_Instance == null) { + m_Instance = new Superstructure(); } - return mInstance; + return m_Instance; } /** Creates a new StateMachine. */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 2fce593..089f65e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,162 +1,163 @@ +// 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.subsystems; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; -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.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.IO.ModuleIO; import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; -public class Swerve extends SubsystemBase { - - private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); - - private SwerveModule[] modules = new SwerveModule[] { - new SwerveModule(0, SwerveConstants.MOD0_CONSTANTS), - new SwerveModule(1, SwerveConstants.MOD1_CONSTANTS), - new SwerveModule(2, SwerveConstants.MOD2_CONSTANTS), - new SwerveModule(3, SwerveConstants.MOD3_CONSTANTS) - }; - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); - private SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, getGyroYaw(), getModulePositions()); - - private Field2d field = new Field2d(); +/** Add your docs here. */ +public class Swerve implements ModuleIO { - private static Swerve mInstance; + private static Swerve m_instance = null; - public static Swerve getInstance() { - if (mInstance == null) { - mInstance = new Swerve(); + public static Swerve getInstance(int index) { + if (m_instance == null) { + m_instance = new Swerve(index); } - return mInstance; + return m_instance; } - public Swerve() { + private final TalonFX driveTalon; + // private final TalonFX turnTalon; + private final SparkMax turnSparkMax; + private final CANcoder cancoder; - RobotConfig config = null; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - e.printStackTrace(); - } - - if (config == null) { - throw new RuntimeException("Failed to load config"); - } - - AutoBuilder.configure( - this::getOdometryPosition, - this::setOdometryPosition, - this::getRobotRelativSpeeds, - (speeds, feedforwards) -> driveRobotRelative(speeds), - new PPHolonomicDriveController( - new PIDConstants(10.0, 0.0, 0.0), - new PIDConstants(5.0, 0.0, 0.0)), - config, - () -> { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this); - - PathPlannerLogging.setLogTargetPoseCallback((pose) -> { - field.getObject("target").setPose(pose); - }); - - PathPlannerLogging.setLogActivePathCallback((poses) -> { - field.getObject("path").setPoses(poses); - }); - - odometry.resetPose(new Pose2d()); - pigeon.reset(); - } - - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getGyroYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(moduleStates); - } - - public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { - ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); - SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); - } + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; - public Rotation2d getGyroYaw() { - return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); - } + private final StatusSignal turnAbsolutePosition; - public void setGyroYaw(Rotation2d yaw) { - pigeon.setYaw(yaw.getDegrees()); - } + private final Rotation2d absoluteEncoderOffset; - public Pose2d getOdometryPosition() { - return odometry.getPoseMeters(); - } + public Swerve(int index) { + if (index < 0 || index >= SwerveConstants.SWERVE_MODULE_CONSTANTS.length) { + throw new IllegalArgumentException("Invalid index: " + index); + } - public void setOdometryPosition(Pose2d pose) { - odometry.resetPosition(getGyroYaw(), getModulePositions(), pose); + driveTalon = new TalonFX(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].DriveMotorId, RobotConstants.CANBUS_NAME); + // turnTalon = new TalonFX(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].SteerMotorId, RobotConstants.CANBUS_NAME); + turnSparkMax = new SparkMax(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].SteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].CANcoderId); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].CANcoderOffset)); + + TalonFXConfiguration driveTalonConfig = SwerveConstants.DRIVE_MOTOR_CONFIGURATION; + driveTalon.getConfigurator().apply(driveTalonConfig); + setDriveMotorOutputMode(true); + + SparkMaxConfig turnSparkConfig = SwerveConstants.STEER_MOTOR_CONFIGURATION; + turnSparkMax.setCANTimeout(250); + turnSparkMax.configure(turnSparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + + BaseStatusSignal.setUpdateFrequencyForAll(100.0, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition + // turnVelocity, + // turnAppliedVolts, + // turnCurrent + ); + driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); } - public ChassisSpeeds getRobotRelativSpeeds() { - return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getGyroYaw()); + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition + // turnVelocity, + // turnAppliedVolts, + // turnCurrent + ); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble() / SwerveConstants.DRIVE_MOTOR_GEAR_RATIO); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble() / SwerveConstants.DRIVE_MOTOR_GEAR_RATIO); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + new Rotation2d(Units.rotationsToRadians(cancoder.getAbsolutePosition().getValueAsDouble())).plus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnSparkMax.getEncoder().getPosition() / SwerveConstants.STEER_MOTOR_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnSparkMax.getEncoder().getVelocity()) / SwerveConstants.STEER_MOTOR_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + // inputs.turnPosition = + // Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + // inputs.turnVelocityRadPerSec = + // Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + // inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + // inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; } - public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - positions[i] = modules[i].getModulePosition(); - } - return positions; + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); } - public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getModuleState(); - } - return states; + @Override + public void setTurnVoltage(double volts) { + // turnTalon.setControl(new VoltageOut(volts)); + turnSparkMax.setVoltage(volts); } - public void setModuleStates(SwerveModuleState[] desiredStates) { - if (desiredStates.length != 4) { - throw new IllegalArgumentException("desiredStates must have length 4"); - } - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); - for (SwerveModule mod : modules) { - mod.setDesiredState(desiredStates[mod.ModuleNumber]); - } + public void setDriveMotorOutputMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); } - @Override - public void periodic() { - odometry.update(getGyroYaw(), getModulePositions()); - field.setRobotPose(getOdometryPosition()); - - SmartDashboard.putData("Field", field); - SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); - SmartDashboard.putNumber("swerve odometry x", getOdometryPosition().getX()); - SmartDashboard.putNumber("swerve odometry y", getOdometryPosition().getY()); - } + // public void setTurnMotorOutputMode(boolean enable) { + // var config = new MotorOutputConfigs(); + // config.Inverted = + // SwerveConstants.IS_STEER_MOTOR_INVERTED + // ? InvertedValue.Clockwise_Positive + // : InvertedValue.CounterClockwise_Positive; + // config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + // turnTalon.getConfigurator().apply(config); + // } } diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..6c46a7a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems; + +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.ModuleIO; + +public class SwerveDrive extends SubsystemBase { + + private static SwerveDrive m_instance = null; + + public static SwerveDrive getInstance(ModuleIO FLModuleIO, ModuleIO FRModuleIO, ModuleIO RLModuleIO, ModuleIO RRModuleIO) { + if (m_instance == null) { + m_instance = new SwerveDrive(FLModuleIO, FRModuleIO, RLModuleIO, RRModuleIO); + } + return m_instance; + } + + private SwerveModule[] modules = new SwerveModule[4]; // FL FR RL RR + + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); + + public SwerveDrive(ModuleIO FLModuleIO, ModuleIO FRModuleIO, ModuleIO RLModuleIO, ModuleIO RRModuleIO) { + modules[0] = new SwerveModule(FLModuleIO, 0); + modules[1] = new SwerveModule(FLModuleIO, 1); + modules[2] = new SwerveModule(FLModuleIO, 2); + modules[3] = new SwerveModule(FLModuleIO, 3); + } + + @Override + public void periodic() { + for (var module : modules) { + module.periodic(); + } + + if(DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + } + + public void runVelocity(ChassisSpeeds speeds) { + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = m_kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, SwerveConstants.MAX_MODULE_SPEED); + + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + } + + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + public void stopWithButton() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = SwerveConstants.MODULE_TRANSLATION_METERS[i].getAngle(); + } + m_kinematics.resetHeadings(headings); + stop(); + } + + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getPosition(); + } + return positions; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveOld.java b/src/main/java/frc/robot/subsystems/SwerveDriveOld.java new file mode 100644 index 0000000..06ea175 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveDriveOld.java @@ -0,0 +1,163 @@ +// put here as a tribute to the old swerve drive code programmed by CHenry +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.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + +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.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +public class SwerveDriveOld extends SubsystemBase { + + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); + + private SwerveModuleOld[] modules = new SwerveModuleOld[] { + new SwerveModuleOld(0, SwerveConstants.MOD0_CONSTANTS), + new SwerveModuleOld(1, SwerveConstants.MOD1_CONSTANTS), + new SwerveModuleOld(2, SwerveConstants.MOD2_CONSTANTS), + new SwerveModuleOld(3, SwerveConstants.MOD3_CONSTANTS) + }; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); + private SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, getGyroYaw(), getModulePositions()); + + private Field2d field = new Field2d(); + + private static SwerveDriveOld mInstance; + + public static SwerveDriveOld getInstance() { + if (mInstance == null) { + mInstance = new SwerveDriveOld(); + } + return mInstance; + } + + public SwerveDriveOld() { + + RobotConfig config = null; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + if (config == null) { + throw new RuntimeException("Failed to load config"); + } + + AutoBuilder.configure( + this::getOdometryPosition, + this::setOdometryPosition, + this::getRobotRelativSpeeds, + (speeds, feedforwards) -> driveRobotRelative(speeds), + new PPHolonomicDriveController( + new PIDConstants(10.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config, + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + field.getObject("target").setPose(pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + field.getObject("path").setPoses(poses); + }); + + odometry.resetPose(new Pose2d()); + pigeon.reset(); + } + + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + ChassisSpeeds chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getGyroYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); + setModuleStates(moduleStates); + } + + public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); + SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(targetStates); + } + + public Rotation2d getGyroYaw() { + return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); + } + + public void setGyroYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } + + public Pose2d getOdometryPosition() { + return odometry.getPoseMeters(); + } + + public void setOdometryPosition(Pose2d pose) { + odometry.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + public ChassisSpeeds getRobotRelativSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getGyroYaw()); + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getModulePosition(); + } + return positions; + } + + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getModuleState(); + } + return states; + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + if (desiredStates.length != 4) { + throw new IllegalArgumentException("desiredStates must have length 4"); + } + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); + for (SwerveModuleOld mod : modules) { + mod.setDesiredState(desiredStates[mod.ModuleNumber]); + } + } + + @Override + public void periodic() { + odometry.update(getGyroYaw(), getModulePositions()); + field.setRobotPose(getOdometryPosition()); + + SmartDashboard.putData("Field", field); + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve odometry x", getOdometryPosition().getX()); + SmartDashboard.putNumber("swerve odometry y", getOdometryPosition().getY()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 1343af6..b2a049f 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -1,101 +1,123 @@ -package frc.robot.subsystems; +// 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. -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +package frc.robot.subsystems; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.FSLib2025.math.Maths; -import frc.FSLib2025.swerve.OnboardModuleState; -import frc.FSLib2025.swerve.SwerveModuleConstants; -import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.ModuleIO; +import frc.robot.IO.ModuleIO.ModuleIOInputs; +/** Add your docs here. */ public class SwerveModule { - public int ModuleNumber; - public SwerveModuleConstants ModuleConstants; + private static final double WHEEL_RADIUS = SwerveConstants.DRIVE_WHEEL_DIAMETERS / 2; // meters - private SparkMax steerMotor; - private TalonFX driveMotor; + private final ModuleIO io; + private final ModuleIOInputs inputs = new ModuleIOInputs(); + private final int index; - private CANcoder steerCANcoder; + private final SimpleMotorFeedforward driveFeedForward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; + private Double speedSetpoint = null; + private Rotation2d turnRelativeOffset = null; - private PIDController steerPID; + public SwerveModule(ModuleIO io, int index) { + this.io = io; + this.index = index; - private Rotation2d lastAngle; + driveFeedForward = new SimpleMotorFeedforward(SwerveConstants.DRIVE_MOTOR_KS, SwerveConstants.DRIVE_MOTOR_KV, SwerveConstants.DRIVE_MOTOR_KA); + driveFeedback = new PIDController(SwerveConstants.DRIVE_MOTOR_KP, SwerveConstants.DRIVE_MOTOR_KI, SwerveConstants.DRIVE_MOTOR_KD); + turnFeedback = new PIDController(SwerveConstants.STEER_MOTOR_KP, SwerveConstants.STEER_MOTOR_KI, SwerveConstants.STEER_MOTOR_KD); + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } - public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { - this.ModuleNumber = moduleNumber; - this.ModuleConstants = moduleConstants; + public void periodic() { + io.updateInputs(inputs); + + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(inputs.turnPosition.getRadians(), angleSetpoint.getRadians()) + ); + + if (speedSetpoint != null) { + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getError()); + + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedForward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec) + ); + } + } + } - steerPID = new PIDController( - SwerveConstants.STEER_MOTOR_KP, - SwerveConstants.STEER_MOTOR_KI, - SwerveConstants.STEER_MOTOR_KD); + public SwerveModuleState runSetpoint(SwerveModuleState state) { + state.optimize(getAngle()); - lastAngle = new Rotation2d(); + angleSetpoint = state.angle; + speedSetpoint = state.speedMetersPerSecond; - steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); - steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + return state; + } - driveMotor = new TalonFX(moduleConstants.DriveMotorId, RobotConstants.CANBUS_NAME); - driveMotor.getConfigurator().apply(SwerveConstants.DRIVE_MOTOR_CONFIGURATION); - driveMotor.setNeutralMode(NeutralModeValue.Brake); + public void runCharacterization(double volts) { + angleSetpoint = new Rotation2d(); - steerCANcoder = new CANcoder(moduleConstants.CANcoderId, RobotConstants.CANBUS_NAME); - CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; - cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; - cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - steerCANcoder.getConfigurator().apply(cancoderConfig); + io.setDriveVoltage(volts); + speedSetpoint = null; } - public void setDesiredState(SwerveModuleState desiredState) { - desiredState = OnboardModuleState.optimize(desiredState, getModuleState().angle); + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); - desiredState.angle = Math.abs(desiredState.speedMetersPerSecond) < 0.01 ? lastAngle : desiredState.angle; - double error = getModuleState().angle.getDegrees() - desiredState.angle.getDegrees(); - error = Maths.constrainAngleDegrees(error); - double steerOutput = steerPID.calculate(error); - setSteerMotor(steerOutput); - lastAngle = desiredState.angle; + angleSetpoint = null; + speedSetpoint = null; + } - double percentOutput = desiredState.speedMetersPerSecond / SwerveConstants.MAX_MODULE_SPEED; - percentOutput = Maths.clamp(percentOutput, -1, 1); - setDriveMotor(percentOutput); + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); } - private void setSteerMotor(double speed) { - steerMotor.set(speed); + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } } - private void setDriveMotor(double speed) { - driveMotor.set(speed); + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; } - public Rotation2d getSteerAngle() { - return Rotation2d.fromRotations(steerCANcoder.getAbsolutePosition().getValueAsDouble()); + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; } - public SwerveModulePosition getModulePosition() { - return new SwerveModulePosition( - driveMotor.getPosition().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, - getSteerAngle()); + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); } - public SwerveModuleState getModuleState() { - return new SwerveModuleState( - driveMotor.getVelocity().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, - getSteerAngle()); + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } -} \ No newline at end of file + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleOld.java b/src/main/java/frc/robot/subsystems/SwerveModuleOld.java new file mode 100644 index 0000000..b2068c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModuleOld.java @@ -0,0 +1,102 @@ +// put here as a tribute to the old swerve drive code programmed by CHenry +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.FSLib2025.math.Maths; +import frc.FSLib2025.swerve.OnboardModuleState; +import frc.FSLib2025.swerve.SwerveModuleConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +public class SwerveModuleOld { + public int ModuleNumber; + public SwerveModuleConstants ModuleConstants; + + private SparkMax steerMotor; + private TalonFX driveMotor; + + private CANcoder steerCANcoder; + + private PIDController steerPID; + + private Rotation2d lastAngle; + + public SwerveModuleOld(int moduleNumber, SwerveModuleConstants moduleConstants) { + this.ModuleNumber = moduleNumber; + this.ModuleConstants = moduleConstants; + + steerPID = new PIDController( + SwerveConstants.STEER_MOTOR_KP, + SwerveConstants.STEER_MOTOR_KI, + SwerveConstants.STEER_MOTOR_KD); + + lastAngle = new Rotation2d(); + + steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); + steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + driveMotor = new TalonFX(moduleConstants.DriveMotorId, RobotConstants.CANBUS_NAME); + driveMotor.getConfigurator().apply(SwerveConstants.DRIVE_MOTOR_CONFIGURATION); + driveMotor.setNeutralMode(NeutralModeValue.Brake); + + steerCANcoder = new CANcoder(moduleConstants.CANcoderId, RobotConstants.CANBUS_NAME); + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + steerCANcoder.getConfigurator().apply(cancoderConfig); + } + + public void setDesiredState(SwerveModuleState desiredState) { + desiredState = OnboardModuleState.optimize(desiredState, getModuleState().angle); + + desiredState.angle = Math.abs(desiredState.speedMetersPerSecond) < 0.01 ? lastAngle : desiredState.angle; + double error = getModuleState().angle.getDegrees() - desiredState.angle.getDegrees(); + error = Maths.constrainAngleDegrees(error); + double steerOutput = steerPID.calculate(error); + setSteerMotor(steerOutput); + lastAngle = desiredState.angle; + + double percentOutput = desiredState.speedMetersPerSecond / SwerveConstants.MAX_MODULE_SPEED; + percentOutput = Maths.clamp(percentOutput, -1, 1); + setDriveMotor(percentOutput); + } + + private void setSteerMotor(double speed) { + steerMotor.set(speed); + } + + private void setDriveMotor(double speed) { + driveMotor.set(speed); + } + + public Rotation2d getSteerAngle() { + return Rotation2d.fromRotations(steerCANcoder.getAbsolutePosition().getValueAsDouble()); + } + + public SwerveModulePosition getModulePosition() { + return new SwerveModulePosition( + driveMotor.getPosition().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + + public SwerveModuleState getModuleState() { + return new SwerveModuleState( + driveMotor.getVelocity().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java new file mode 100644 index 0000000..4e6e76f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -0,0 +1,126 @@ +// 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.subsystems; + +import java.util.function.Supplier; + +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.geometry.Twist2d; +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.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.GyroIO; +import frc.robot.IO.GyroIO.GyroIOInputs; + +public class SwervePoseEstimator extends SubsystemBase { + + private GyroIO gyroIO; + private final GyroIOInputs gyroInputs = new GyroIOInputs(); + + private static SwervePoseEstimator m_instance = null; + + public static SwervePoseEstimator getInstance(GyroIO gyroIO) { + if (m_instance == null) { + m_instance = new SwervePoseEstimator(gyroIO); + } + return m_instance; + } + + private SwerveDrive m_SwerveDrive = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); + private SwerveVision m_SwerveVision = SwerveVision.getInstance(); + + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); + private Rotation2d m_gyroYaw = new Rotation2d(); + private Supplier m_modulePos = () -> m_SwerveDrive.getModulePositions(); + private SwerveModulePosition[] m_lastModulePos = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( + m_kinematics, + m_gyroYaw, + m_lastModulePos, + new Pose2d() + ); + + private Field2d m_field = new Field2d(); + + public SwervePoseEstimator(GyroIO gyroIO) { + this.gyroIO = gyroIO; + m_poseEstimator.resetPosition(getGyroYaw(), m_SwerveDrive.getModulePositions(), getPoseEstimatorPose()); + } + + public Rotation2d getGyroYaw() { + return m_gyroYaw; + } + + public Pose2d getPoseEstimatorPose() { + return m_poseEstimator.getEstimatedPosition(); + } + + public void setPoseEstimatorPose(Pose2d pose) { + m_poseEstimator.resetPosition(getGyroYaw(), m_SwerveDrive.getModulePositions(), pose); + } + + public Rotation2d getPERotation() { + return getPoseEstimatorPose().getRotation(); + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_SwerveDrive.getModuleStates()), + getPERotation()); + } + + @Override + public void periodic() { + gyroIO.updateInputs(gyroInputs); + + SwerveModulePosition[] modulePos = m_modulePos.get(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePos[moduleIndex].distanceMeters + - m_lastModulePos[moduleIndex].distanceMeters, + modulePos[moduleIndex].angle); + m_lastModulePos[moduleIndex] = modulePos[moduleIndex]; + } + + if (gyroInputs.connected) { + m_gyroYaw = gyroInputs.yawPosition; + } else { + Twist2d twist = m_kinematics.toTwist2d(moduleDeltas); + m_gyroYaw = m_gyroYaw.plus(new Rotation2d(twist.dtheta)); + } + + m_poseEstimator.update(getGyroYaw(), m_SwerveDrive.getModulePositions()); + m_field.setRobotPose(getPoseEstimatorPose()); + SmartDashboard.putData("Field", m_field); + + Pose2d visionPose = m_SwerveVision.getLatestPose(); + if (visionPose != null) { + m_poseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp()); + } + + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve pose estimator x", getPoseEstimatorPose().getX()); + SmartDashboard.putNumber("swerve pose estimator y", getPoseEstimatorPose().getY()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveVision.java b/src/main/java/frc/robot/subsystems/SwerveVision.java new file mode 100644 index 0000000..770c228 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveVision.java @@ -0,0 +1,116 @@ +// 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.subsystems; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.VisionConstants; + +public class SwerveVision extends SubsystemBase { + + private static SwerveVision m_instance = null; + + public static SwerveVision getInstance() { + if (m_instance == null) { + m_instance = new SwerveVision(); + } + return m_instance; + } + + private final PhotonCamera camera; + private final PhotonPoseEstimator photonPoseEstimator; + private Matrix curStdDevs; + + public SwerveVision() { + camera = new PhotonCamera(VisionConstants.CHASSIS_CAM_NAME); + + photonPoseEstimator = new PhotonPoseEstimator(VisionConstants.TAG_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.ROBOT_TO_CAM); + photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + public Optional getVisionEstimatedPose() { + Optional visionEstPose = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEstPose = photonPoseEstimator.update(change); + updateEstimationStdDevs(visionEstPose, change.getTargets()); + } + return visionEstPose; + } + + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonPoseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + public Pose2d getLatestPose() { + Optional visionEst = getVisionEstimatedPose(); + + // Check if we have a valid vision estimate >w< + if (visionEst.isPresent()) { + return visionEst.get().estimatedPose.toPose2d(); + } + + return null; // No valid pose found ʕ•ᴥ•ʔ + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file