tuned Kg in terms of giant metal block

Author: CrolineCrois

src/main/java/frc/robot/Constants.java

@@ -70,12 +70,25 @@ public static class IntakeConstants {
     public static final int PIVOT_ID = 1;
     public static final int ROLLER_ID = 0;
     public static final int INTAKE_SENSOR_ID = 0;
+
     public static final double PIVOT_CONVERSION_FACTOR = 30. / (2 * Math.PI);
+
     public static final double ZERO_POSITION = 0;
     public static final double SOURCE_POSITION = 0; //TODO: change
     public static final double OUTTAKE_POSITION = 0; //TODO: change
     public static final double VERTICAL_POSITION = Units.degreesToRadians(90); //TODO: change
+
     public static final double PIVOT_TOLERANCE = .05; //TODO: change
 
+    public static final double PIVOT_KG = 1.2;
+    public static final double PIVOT_KS = 0;
+    public static final double PIVOT_KV = 0;
+
+    public static final double PIVOT_P = 2.1;
+    public static final double PIVOT_I = 0;
+    public static final double PIVOT_D = 0;
+
+
+
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -92,7 +92,7 @@ private void configureBindings() {
         new SetPivotZeroCommand(pivotSubsystem),
         new SetPivotVerticalCommand(pivotSubsystem),
         () -> (pivotSubsystem.getTargetState() == PivotState.VERTICAL)
-        )
+        ).withTimeout(3)
     );
 
     // aButton.onTrue(

src/main/java/frc/robot/commands/pivot/SetPivotVerticalCommand.java

@@ -15,7 +15,7 @@ public SetPivotVerticalCommand(PivotSubsystem pivotSubsystem) {
     @Override
     public void execute() {
         this.pivotSubsystem.setState(PivotState.VERTICAL);
-        System.out.println("setting vertical");
+        System.out.println("vertical" + pivotSubsystem.getPositionError());
     }
 
     @Override

src/main/java/frc/robot/commands/pivot/SetPivotZeroCommand.java

@@ -15,7 +15,7 @@ public SetPivotZeroCommand(PivotSubsystem pivotSubsystem) {
     @Override
     public void execute() {
         this.pivotSubsystem.setState(PivotState.ZERO);
-        System.out.println("setting horizaontal");
+        System.out.println("horizontal" + pivotSubsystem.getPositionError());
     }
 
     @Override

src/main/java/frc/robot/subsystems/intake/pivot/PivotSubsystem.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.intake.pivot;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -13,10 +14,18 @@
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 
 import static frc.robot.Constants.IntakeConstants.PIVOT_CONVERSION_FACTOR;
+import static frc.robot.Constants.IntakeConstants.PIVOT_D;
+import static frc.robot.Constants.IntakeConstants.PIVOT_I;
 import static frc.robot.Constants.IntakeConstants.PIVOT_ID;
+import static frc.robot.Constants.IntakeConstants.PIVOT_KG;
+import static frc.robot.Constants.IntakeConstants.PIVOT_KS;
+import static frc.robot.Constants.IntakeConstants.PIVOT_KV;
+import static frc.robot.Constants.IntakeConstants.PIVOT_P;
 import static frc.robot.Constants.IntakeConstants.PIVOT_TOLERANCE;
 
 
@@ -31,11 +40,10 @@ public class PivotSubsystem extends SubsystemBase{
     private SparkMaxConfig sparkMaxConfig;
     private SoftLimitConfig softLimitConfig;
 
-    private double p = 1; 
-    private double i = 0; 
-    private double d = 0;
+    private ArmFeedforward armFeedforward;
 
     private PivotState targetState;
+    private double gravityFF;
 
     public PivotSubsystem(){
 
@@ -53,7 +61,9 @@ public PivotSubsystem(){
                        .reverseSoftLimit(0);
 
         closedLoopConfig = new ClosedLoopConfig();
-        closedLoopConfig.pid(p, i, d);
+        closedLoopConfig.pid(PIVOT_P, PIVOT_I, PIVOT_D, ClosedLoopSlot.kSlot0);
+
+        armFeedforward = new ArmFeedforward(PIVOT_KS, PIVOT_KG, PIVOT_KV);
 
         sparkMaxConfig = new SparkMaxConfig();
         sparkMaxConfig.apply(closedLoopConfig)
@@ -65,6 +75,7 @@ public PivotSubsystem(){
         pivotPID = pivotMotor.getClosedLoopController();
 
         targetState = PivotState.ZERO;
+        gravityFF = 0;
 
     }
 
@@ -77,7 +88,8 @@ public PivotState getTargetState() {
     }
 
     public void setState(PivotState targetState) {
-        pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition);
+        gravityFF = armFeedforward.calculate(targetState.getTargetAngle(), 2);
+        pivotPID.setReference(targetState.getTargetAngle(), ControlType.kPosition, ClosedLoopSlot.kSlot0, gravityFF, ArbFFUnits.kVoltage);
         // System.out.println("state set to : " + targetState.getTargetAngle());
         this.targetState = targetState;
     }
@@ -89,5 +101,9 @@ public void setSpeed(double speed) {
     public boolean atState(PivotState state) {
         return Math.abs(getCurrentAngle() - state.getTargetAngle()) < PIVOT_TOLERANCE;
     }
+
+    public double getPositionError() {
+        return Math.abs(getCurrentAngle() - targetState.getTargetAngle());
+    }
     }