Merge pull request #49 from 2491-NoMythic/offsetPreferences

2491-NoMythic · Feb 9, 2024 · a4ddad0 · a4ddad0
2 parents 869086f + ebeb1d3
commit a4ddad0
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -172,7 +172,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);
@@ -250,6 +250,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
@@ -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
@@ -118,10 +118,10 @@ public DrivetrainSubsystem(Lights lights, Boolean lightsExist) {
 		this.lightsExist = lightsExist;
 		this.limelight=Limelight.getInstance();
 
-		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()));
@@ -133,28 +133,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(
@@ -201,6 +201,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
@@ -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.