From 2471ae351e6268eed295f704e97c92675c59eb0c Mon Sep 17 00:00:00 2001 From: "Krope(Christian)" <156374345+Kr0pe@users.noreply.github.com> Date: Thu, 11 Dec 2025 17:06:59 -0600 Subject: [PATCH] end of day --- .../robot/generic/subsystems/drive/Drive.java | 4 +++- .../subsystems/drive/DriveConstants.java | 22 +++++++++++++------ .../subsystems/drive/GyroIOPigeon2.java | 11 +++++++++- .../frc/robot/generic/util/SwerveBuilder.java | 13 +++++++---- .../frc/robot/outReach/RobotContainer.java | 2 +- 5 files changed, 38 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/generic/subsystems/drive/Drive.java b/src/main/java/frc/robot/generic/subsystems/drive/Drive.java index 8af171f..dac70ef 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/Drive.java @@ -183,6 +183,8 @@ public void periodic() { // Update gyro alert gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); + + Logger.recordOutput("Odometry/Robot", getPose()); } /** @@ -290,7 +292,7 @@ public double getFFCharacterizationVelocity() { } /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") + // @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } diff --git a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java index 9e94410..c02591c 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/DriveConstants.java @@ -60,15 +60,22 @@ public class DriveConstants { Underrotated → pointing toward the robot’s left (your right) */ public static final Rotation2d frontLeftZeroRotation = - new Rotation2d(-0.7853181997882291).minus(new Rotation2d(Degrees.of(16 + 5))); + new Rotation2d(-0.7853181997882291) + .minus(new Rotation2d(Degrees.of(16 + 5))) + .minus(Rotation2d.kCW_90deg); public static final Rotation2d frontRightZeroRotation = - new Rotation2d(-1.9785268942462368).minus(new Rotation2d(Degrees.of(-7 - 3))); + new Rotation2d(-1.9785268942462368) + .minus(new Rotation2d(Degrees.of(-7 - 3))) + .minus(Rotation2d.kCW_90deg); public static final Rotation2d backLeftZeroRotation = new Rotation2d(1.2279650529278354) .minus(new Rotation2d(Degrees.of(-8))) - .minus(Rotation2d.kCCW_90deg); + .minus(Rotation2d.kCCW_90deg) + .minus(Rotation2d.kCW_90deg); public static final Rotation2d backRightZeroRotation = - new Rotation2d(-0.8600462118731901).minus(new Rotation2d(Degrees.of(17.5 + 2))); + new Rotation2d(-0.8600462118731901) + .minus(new Rotation2d(Degrees.of(17.5 + 2))) + .minus(Rotation2d.kCW_90deg); // Device CAN IDs public static final int pigeonCanId = 13; @@ -90,7 +97,7 @@ public class DriveConstants { // Drive motor configuration public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(2); + public static final double wheelRadiusMeters = Units.inchesToMeters(1.981); public static final double driveMotorReduction = 5.14; // SDS mk4 with L4 gearing public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); @@ -101,10 +108,11 @@ public class DriveConstants { (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec // Drive PID configuration + // public static final double driveKp = 0.0; public static final double driveKd = 0.0; - public static final double driveKs = 0.0; - public static final double driveKv = 0.1; + public static final double driveKs = 0.11605; + public static final double driveKv = 0.11034; public static final double driveSimP = 0.05; public static final double driveSimD = 0.0; public static final double driveSimKs = 0.0; diff --git a/src/main/java/frc/robot/generic/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/generic/subsystems/drive/GyroIOPigeon2.java index ef92193..c55fe55 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/GyroIOPigeon2.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MountPoseConfigs; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; @@ -35,7 +36,15 @@ public class GyroIOPigeon2 implements GyroIO { private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); public GyroIOPigeon2() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon + .getConfigurator() + .apply( + new Pigeon2Configuration() + .withMountPose( + new MountPoseConfigs() + .withMountPosePitch(-0.8275826573371887) + .withMountPoseRoll(-2.518435001373291) + .withMountPoseYaw(1.0797953605651855))); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(odometryFrequency); yawVelocity.setUpdateFrequency(50.0); diff --git a/src/main/java/frc/robot/generic/util/SwerveBuilder.java b/src/main/java/frc/robot/generic/util/SwerveBuilder.java index c380b67..53cfa22 100644 --- a/src/main/java/frc/robot/generic/util/SwerveBuilder.java +++ b/src/main/java/frc/robot/generic/util/SwerveBuilder.java @@ -25,7 +25,8 @@ public class SwerveBuilder { * @param driveController the controller to bind buttons to, traditionally in port 0 * @return Drive subsystem, for further integration */ - public static Drive buildDefaultDrive(CommandXboxController driveController) { + public static Drive buildDefaultDrive( + CommandXboxController driveController, double speedMultiplier) { Drive drive; switch (Constants.currentMode) { case REAL: @@ -64,9 +65,9 @@ public static Drive buildDefaultDrive(CommandXboxController driveController) { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driveController.getLeftY(), - () -> -driveController.getLeftX(), - () -> driveController.getRightX())); + () -> driveController.getLeftY() * speedMultiplier, + () -> driveController.getLeftX() * speedMultiplier, + () -> driveController.getRightX() * speedMultiplier)); driveController .y() .onTrue( @@ -78,4 +79,8 @@ public static Drive buildDefaultDrive(CommandXboxController driveController) { .ignoringDisable(true)); return drive; } + + public static Drive buildDefaultDrive(CommandXboxController driveController) { + return buildDefaultDrive(driveController, 1); + } } diff --git a/src/main/java/frc/robot/outReach/RobotContainer.java b/src/main/java/frc/robot/outReach/RobotContainer.java index 09acff7..2965fae 100644 --- a/src/main/java/frc/robot/outReach/RobotContainer.java +++ b/src/main/java/frc/robot/outReach/RobotContainer.java @@ -51,7 +51,7 @@ public class RobotContainer implements AbstractRobotContainer { private final CANBus canBus = new CANBus(); // Subsystems - private final Drive drive = SwerveBuilder.buildDefaultDrive(controller); + private final Drive drive = SwerveBuilder.buildDefaultDrive(controller, 0.75); private final Turret turret; // Dashboard inputs