From 30dc6b6497bd96ff545b9a2e4f63635053212a1b Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 6 Feb 2024 12:10:48 -0600 Subject: [PATCH 1/5] changed offset system to have it all on Preferences, and also added methods to set the offsets at any time --- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/subsystems/DrivetrainSubsystem.java | 22 ++++++++++++------- .../frc/robot/subsystems/SwerveModule.java | 9 ++++++++ 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b798e6d5..fc6f74b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,6 +249,7 @@ private void configureBindings() { new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop())); new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber)); } + SmartDashboard.putData(new InstantCommand(driveTrain::setEncoderOffsets)); // //Intake bindings // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index c6d218c0..58644bca 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -117,10 +117,10 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { this.lights = lights; this.lightsExist = lightsExist; - Preferences.initString("FL", "AUGIE"); - Preferences.initString("FR", "AUGIE"); - Preferences.initString("BL", "AUGIE"); - Preferences.initString("BR", "AUGIE"); + Preferences.initDouble("FL offset", 0); + Preferences.initDouble("FR offset", 0); + Preferences.initDouble("BL offset", 0); + Preferences.initDouble("BR offset", 0); PathPlannerLogging.setLogActivePathCallback((poses) -> m_field.getObject("path").setPoses(poses)); SmartDashboard.putData("Field", m_field); SmartDashboard.putData("resetOdometry", new InstantCommand(() -> this.resetOdometry())); @@ -132,28 +132,28 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { FL_DRIVE_MOTOR_ID, FL_STEER_MOTOR_ID, FL_STEER_ENCODER_ID, - Offsets.valueOf(Preferences.getString("FL", "AUGIE")).getValue(Positions.FL), + new Rotation2d(Preferences.getDouble("FL", 0)), CANIVORE_DRIVETRAIN); modules[1] = new SwerveModule( "FR", FR_DRIVE_MOTOR_ID, FR_STEER_MOTOR_ID, FR_STEER_ENCODER_ID, - Offsets.valueOf(Preferences.getString("FR", "AUGIE")).getValue(Positions.FR), + new Rotation2d(Preferences.getDouble("FR", 0)), CANIVORE_DRIVETRAIN); modules[2] = new SwerveModule( "BL", BL_DRIVE_MOTOR_ID, BL_STEER_MOTOR_ID, BL_STEER_ENCODER_ID, - Offsets.valueOf(Preferences.getString("BL", "AUGIE")).getValue(Positions.BL), + new Rotation2d(Preferences.getDouble("BL", 0)), CANIVORE_DRIVETRAIN); modules[3] = new SwerveModule( "BR", BR_DRIVE_MOTOR_ID, BR_STEER_MOTOR_ID, BR_STEER_ENCODER_ID, - Offsets.valueOf(Preferences.getString("BR", "AUGIE")).getValue(Positions.BR), + new Rotation2d(Preferences.getDouble("BR", 0)), CANIVORE_DRIVETRAIN); odometer = new SwerveDrivePoseEstimator( @@ -200,6 +200,12 @@ public SwerveModuleState[] getModuleStates() { for (int i = 0; i < 4; i++) states[i] = modules[i].getState(); return states; } + public void setEncoderOffsets() { + Preferences.setDouble("FL offset", modules[0].findOffset()); + Preferences.setDouble("FR offset", modules[1].findOffset()); + Preferences.setDouble("BR offset", modules[2].findOffset()); + Preferences.setDouble("BL offset", modules[3].findOffset()); + } public Pose2d getPose() { return odometer.getEstimatedPosition(); } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 540a3a04..0e3b5a35 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -107,6 +107,15 @@ public Rotation2d getRotation() { public double getTargetAngle() { return m_desiredSteerAngle; } +/**finds the curernt encoder position, it removes the current offset so we just get the raw position + * @return + */ + public double findOffset() { + return MathUtil.inputModulus( + (m_steerEncoder.getPosition().getValue()+m_steerEncoderOffset.getRotations()), + -0.5, + 0.5); + } /** * Returns the target speed of the wheel. * @return The target speed of the wheel in meters/second. From 25122f20c4f5ec10238e22c64ae8e023ac3e6463 Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 6 Feb 2024 14:18:40 -0600 Subject: [PATCH 2/5] changed code that gets the offset, and also changed names of values from preferences --- .../java/frc/robot/subsystems/DrivetrainSubsystem.java | 8 ++++---- src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 58644bca..fbaa811f 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -132,28 +132,28 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { FL_DRIVE_MOTOR_ID, FL_STEER_MOTOR_ID, FL_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("FL", 0)), + new Rotation2d(Preferences.getDouble("FL offset", 0)), CANIVORE_DRIVETRAIN); modules[1] = new SwerveModule( "FR", FR_DRIVE_MOTOR_ID, FR_STEER_MOTOR_ID, FR_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("FR", 0)), + new Rotation2d(Preferences.getDouble("FR offset", 0)), CANIVORE_DRIVETRAIN); modules[2] = new SwerveModule( "BL", BL_DRIVE_MOTOR_ID, BL_STEER_MOTOR_ID, BL_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("BL", 0)), + new Rotation2d(Preferences.getDouble("BL offset", 0)), CANIVORE_DRIVETRAIN); modules[3] = new SwerveModule( "BR", BR_DRIVE_MOTOR_ID, BR_STEER_MOTOR_ID, BR_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("BR", 0)), + new Rotation2d(Preferences.getDouble("BR offset", 0)), CANIVORE_DRIVETRAIN); odometer = new SwerveDrivePoseEstimator( diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 0e3b5a35..d0005017 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -112,7 +112,7 @@ public double getTargetAngle() { */ public double findOffset() { return MathUtil.inputModulus( - (m_steerEncoder.getPosition().getValue()+m_steerEncoderOffset.getRotations()), + (m_steerEncoder.getPosition().getValue()-m_steerEncoderOffset.getRotations()), -0.5, 0.5); } From 4749c9240e609f628cc39bebb112fb31ebe34d4d Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 6 Feb 2024 19:23:29 -0600 Subject: [PATCH 3/5] Made code for offsetting all 4 modauls with one button, not working yet --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/SwerveModule.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc6f74b5..fba2a6b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -171,7 +171,7 @@ private void driveTrainInst() { () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(Z_AXIS), DEADBAND_NORMAL)); - driveTrain.setDefaultCommand(defaultDriveCommand); + driveTrain.setDefaultCommand(defaultDriveCommand); } private void shooterInst() { shooter = new ShooterSubsystem(ShooterConstants.SHOOTER_MOTOR_POWER); @@ -249,7 +249,7 @@ private void configureBindings() { new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop())); new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber)); } - SmartDashboard.putData(new InstantCommand(driveTrain::setEncoderOffsets)); + SmartDashboard.putData("set offsets", new InstantCommand(driveTrain::setEncoderOffsets)); // //Intake bindings // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index d0005017..0e3b5a35 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -112,7 +112,7 @@ public double getTargetAngle() { */ public double findOffset() { return MathUtil.inputModulus( - (m_steerEncoder.getPosition().getValue()-m_steerEncoderOffset.getRotations()), + (m_steerEncoder.getPosition().getValue()+m_steerEncoderOffset.getRotations()), -0.5, 0.5); } From fe163a7afe99d2ff54f698ca98e8876900fc8055 Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 6 Feb 2024 19:56:50 -0600 Subject: [PATCH 4/5] added offsets for current drivetrain --- src/main/java/frc/robot/settings/Constants.java | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index e76b08a9..6adfe51f 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -58,10 +58,10 @@ public enum Offsets{ BENH(0.153564), EVELYN(-0.111084), OMARIAHN(0.266846), - PHOEBE(-0.2458), //Moira FL - ROYCE(-0.0031), // Moira FR - ROWAN(0.3916), // Moira BL - QUINN(0.3557), //Moira BR + // PHOEBE(-0.2458), //Moira FL + // ROYCE(-0.0031), // Moira FR + // ROWAN(0.3916), // Moira BL + //QUINN(0.3557), //Moira BR // PHOEBE(0.253174), //Moira FL // ROYCE(-0.254639), // Moira FR // ROWAN(-0.113525), // Moira BL @@ -71,7 +71,12 @@ public enum Offsets{ WILLA(-0.152832), //Moira inverted FL OPAL(0.211419-0.25), //Moira inverted FR CLOVER(0.239746), //Moira inverted BL - NICHOLAS(-0.164551); //Moira inverted BR + NICHOLAS(-0.164551), //Moira inverted BR + QUINN(-0.394287), //FL + ROYCE(0.248291), //FR + PHOEBE(-0.486572), //BL + ROWAN(-0.360840); //BR + private double offset; Offsets(double value) { From ebeb1d32d8d91e363d7f5850ae79f1ab7d2cc83f Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 6 Feb 2024 21:15:07 -0600 Subject: [PATCH 5/5] changed preferences values to be from rotations, since that's how the offsets are. --- .../frc/robot/subsystems/DrivetrainSubsystem.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index fbaa811f..cc9a7a31 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -132,28 +132,28 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) { FL_DRIVE_MOTOR_ID, FL_STEER_MOTOR_ID, FL_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("FL offset", 0)), + Rotation2d.fromRotations(Preferences.getDouble("FL offset", 0)), CANIVORE_DRIVETRAIN); modules[1] = new SwerveModule( "FR", FR_DRIVE_MOTOR_ID, FR_STEER_MOTOR_ID, FR_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("FR offset", 0)), + Rotation2d.fromRotations(Preferences.getDouble("FR offset", 0)), CANIVORE_DRIVETRAIN); modules[2] = new SwerveModule( "BL", BL_DRIVE_MOTOR_ID, BL_STEER_MOTOR_ID, BL_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("BL offset", 0)), + Rotation2d.fromRotations(Preferences.getDouble("BL offset", 0)), CANIVORE_DRIVETRAIN); modules[3] = new SwerveModule( "BR", BR_DRIVE_MOTOR_ID, BR_STEER_MOTOR_ID, BR_STEER_ENCODER_ID, - new Rotation2d(Preferences.getDouble("BR offset", 0)), + Rotation2d.fromRotations(Preferences.getDouble("BR offset", 0)), CANIVORE_DRIVETRAIN); odometer = new SwerveDrivePoseEstimator( @@ -203,8 +203,8 @@ public SwerveModuleState[] getModuleStates() { public void setEncoderOffsets() { Preferences.setDouble("FL offset", modules[0].findOffset()); Preferences.setDouble("FR offset", modules[1].findOffset()); - Preferences.setDouble("BR offset", modules[2].findOffset()); - Preferences.setDouble("BL offset", modules[3].findOffset()); + Preferences.setDouble("BL offset", modules[2].findOffset()); + Preferences.setDouble("BR offset", modules[3].findOffset()); } public Pose2d getPose() { return odometer.getEstimatedPosition();