add basic gravity offset to elevator control

is very jank, going to improve it lots
Iron-Panthers · Nov 3, 2023 · 933541c · 933541c
1 parent a7ca866
commit 933541c
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -231,6 +231,8 @@ public static final class Setpoints {
 
     public static final double ZERO_MOTOR_POWER = -0.2;
     public static final double ZERO_STATOR_LIMIT = 25;
+
+    public static final double GRAVITY_OFFSET_PERCENT = 0.08;
   }
 
   public static final class Intake {

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -181,6 +181,13 @@ public double getPercentControl() {
     return percentControl;
   }
 
+  private double calculateGravityOffset() {
+    //maybe make it nonlinear, current model is very bad and not thought out
+    //interpolated tree map? prob not logarithmic but maybe
+    //
+    return (currentExtension / Elevator.MAX_EXTENSION_INCHES) * Elevator.GRAVITY_OFFSET_PERCENT;
+  }
+
   public double getTargetExtension() {
     return targetExtension;
   }
@@ -226,22 +233,16 @@ private void percentDrivePeriodic() {
 
     wristMotor.set(
         TalonFXControlMode.PercentOutput,
-        MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle), -0.25, 0.25));
+        MathUtil.clamp(wristController.calculate(currentWristAngle, targetAngle) + calculateGravityOffset(), -0.25, 0.25));
   }
 
   private void positionDrivePeriodic() {
     if (filterOutput < statorCurrentLimit) {
-      // || proxySensor.get() == false) {
       double motorPower = extensionController.calculate(currentExtension, targetExtension);
-      if (currentExtension < 20 && motorPower < 0) {
-        isInSlowZone = true;
-        motorPower = MathUtil.clamp(motorPower, -0.15, 0.15);
-        if (currentExtension < 6 && motorPower < 0) {
-          motorPower = MathUtil.clamp(motorPower, -0.075, 0.075);
-        }
-      } else {
-        rightMotor.set(TalonFXControlMode.PercentOutput, motorPower);
-      }
+      final double gravityOffset = calculateGravityOffset();
+
+      rightMotor.set(
+        TalonFXControlMode.PercentOutput, MathUtil.clamp(motorPower + gravityOffset, -0.8, 0.8));
     } else {
       rightMotor.set(TalonFXControlMode.PercentOutput, 0);
     }