stuff works more tuning

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

@@ -19,6 +19,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -102,17 +103,17 @@ public class RobotContainer {
 				() -> true
 			)
 		);
-
+		/*
 		elevator.setDefaultCommand(
 			elevator.goToSetpoint(
 				() -> 0
 			)
 		);
-
+		*/
 		
 		manipulatorPivot.setDefaultCommand(
 			manipulatorPivot.runManualPivot(
-				() -> 0
+				() -> operator.getRightY() * 0.5
 			)
 		);
 		
@@ -140,28 +141,28 @@ public class RobotContainer {
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
-		*/
+		
 		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
 		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-		
+		*/
 		
 		operator.povUp().whileTrue(
-			elevator.goToSetpoint(() -> 50)
+			manipulatorPivot.goToSetpoint(() -> 0)
 		);
 
 		operator.povDown().whileTrue(
-			elevator.goToSetpoint(() -> 0)
+			manipulatorPivot.goToSetpoint(() -> (Units.degreesToRadians(110))
-		);
+		));
-
+		
 		/*
 		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
 
 		operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
 
- 
+		
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
@@ -267,6 +268,12 @@ public class RobotContainer {
 			.withSize(1, 1)
 			.withPosition(5, 1)
 			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
+
+		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
+
+
  	}
 
 	public Command getAutonomousCommand() {
@@ -317,7 +324,7 @@ public class RobotContainer {
 		// then the elevator, then the arm again
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
 			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
@@ -327,7 +334,7 @@ public class RobotContainer {
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
 			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		}
 	}
 
@@ -341,23 +348,24 @@ public class RobotContainer {
 	 * @return Moves the elevator and arm to the setpoints
 	 */
 	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
-		if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
+		
+		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
 				armPosition, 
 				0,
 				ManipulatorPivotConstants.kRotationLimit
 			);
-		}
+		}*/
 		
 		return Commands.either(
 			Commands.either(
-				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
-				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
 				() -> sequential
 			), 
 			Commands.either(
-				manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
-				manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
@@ -368,15 +376,15 @@ public class RobotContainer {
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition),
-				manipulatorPivot.goToSetpoint(armPosition));
+				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
-				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}
@@ -391,7 +399,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
 		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(armPosition));
+			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 	}
 
 	@SuppressWarnings("unused")

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -16,25 +16,25 @@ public class ManipulatorPivotConstants {
 
     public static final int kMotorCurrentMax = 40;
 
-    public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0;
+    public static final double kPivotConversion = 2 * Math.PI;
 
-    public static final double kPivotMaxVelocity = 0;
+    public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 0;
+    public static final double kPositionalP = 2.5;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
     public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
 
-    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
-    public static final double kFeedForwardG = 0.41; // calculated value 0.41
+    public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
     public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
-    public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
 
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0;
+    public static final double kEncoderOffset = 0.7815;
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
@@ -69,7 +69,8 @@ public class ManipulatorPivotConstants {
     static {
         motorConfig
             .smartCurrentLimit(kMotorCurrentMax)
-            .idleMode(kIdleMode);
+            .idleMode(kIdleMode)
+            .inverted(true);
         motorConfig.absoluteEncoder
             .positionConversionFactor(kPivotConversion)
             .inverted(false)

src/main/java/frc/robot/subsystems/Elevator.java

@@ -141,23 +141,40 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage to achieve the target destination
      */
     public Command goToSetpoint(DoubleSupplier setpoint) {
-        double clampedSetpoint = MathUtil.clamp(
-            setpoint.getAsDouble(), 
-            0, 
-            ElevatorConstants.kMaxHeight
-        );
 
+        return startRun(() -> {
+            
+        
+        pidController.setSetpoint(setpoint.getAsDouble());
         pidController.reset();
-
+        }, 
-        return run(() -> {
+        () -> {
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }
+        });
+        
+            
+            
+            
+            /*
             elevatorMotor1.setVoltage(
                 pidController.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
                 ) + feedForward.calculate(0)
             );
-
+*/
-            /*
+            /* 
             if (!pidController.atSetpoint()) {
                 elevatorMotor1.setVoltage(
                     pidController.calculate(
@@ -170,8 +187,8 @@ public class Elevator extends SubsystemBase {
                     feedForward.calculate(0)
                 );
             }
-             */
+             
-        });
+        });*/
     }
             
             /* 

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ManipulatorPivotConstants;
@@ -44,6 +45,8 @@ public class ManipulatorPivot extends SubsystemBase {
             ManipulatorPivotConstants.kPositionalI, 
             ManipulatorPivotConstants.kPositionalD
         );
+
+        pidController.disableContinuousInput();
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
@@ -92,24 +95,27 @@ public class ManipulatorPivot extends SubsystemBase {
      * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
-    public Command goToSetpoint(double setpoint) {
+    public Command goToSetpoint(DoubleSupplier setpoint) {
-        double clampedSetpoint = MathUtil.clamp(
+        return startRun(() -> {
-            setpoint, 
+            
-            0, 
+        
-            ManipulatorPivotConstants.kRotationLimit
+            pidController.setSetpoint(setpoint.getAsDouble());
-        );
+            pidController.reset();
-
+            }, 
-        return run(() -> {
+            () -> {
-            pivotMotor.setVoltage(
+                if (!pidController.atSetpoint()) {
-                pidController.calculate(
+                    pivotMotor.setVoltage(
-                    encoder.getPosition(), 
+                        pidController.calculate(
-                    clampedSetpoint
+                            encoder.getPosition(),
-                ) + feedForward.calculate(
+                            setpoint.getAsDouble()
-                    encoder.getPosition(), 
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
-                    0
+                    );
-                )
+                } else {
-            );
+                    pivotMotor.setVoltage(
-        });
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+            });
     }
 
     /**
@@ -118,7 +124,7 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Encoder's position in radians
      */
     public double getEncoderPosition() {
-        return encoder.getPosition();
+        return Units.radiansToDegrees( encoder.getPosition());
     }
     /**
      * Returns the encoder's velocity in radians per second
@@ -126,6 +132,14 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Encoder's velocity in radians per second
      */
     public double getEncoderVelocity() {
-        return encoder.getVelocity();
+        return Units.radiansToDegrees(encoder.getVelocity());
+    }
+
+    public double getCGPosition(){
+        return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
+    }
+
+    public double getPivotOutput(){
+        return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
     }
 }