diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b798e6d5..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,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("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/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) { diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index c6d218c0..cc9a7a31 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), + 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, - Offsets.valueOf(Preferences.getString("FR", "AUGIE")).getValue(Positions.FR), + 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, - Offsets.valueOf(Preferences.getString("BL", "AUGIE")).getValue(Positions.BL), + 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, - Offsets.valueOf(Preferences.getString("BR", "AUGIE")).getValue(Positions.BR), + Rotation2d.fromRotations(Preferences.getDouble("BR offset", 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("BL offset", modules[2].findOffset()); + Preferences.setDouble("BR 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.