More work tuning the elevator

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

@@ -56,8 +56,8 @@ public class RobotContainer {
 
 		climberRollers = new ClimberRollers();
 
-		vision = new Vision(drivetrain::getGyroValue);
+		//vision = new Vision(drivetrain::getGyroValue);
-		drivetrain = new Drivetrain(vision);
+		drivetrain = new Drivetrain();
 
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
@@ -153,13 +153,15 @@ public class RobotContainer {
 		*/
 
 		operator.povUp().onTrue(
-			elevator.goToSetpoint(() -> 20)
+			elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal)
 		);
 
 		operator.povDown().onTrue(
-			elevator.goToSetpoint(() -> 0)
+			elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal)
 		);
 
+		operator.a().whileTrue(elevator.maintainPosition());
+
 		/*
 		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
 

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

@@ -21,17 +21,21 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 3; //
+    public static final double kUpControllerP = 4;//7; //
-    public static final double kPositionControllerI = 0;
+    public static final double kUpControllerI = 0;
-    public static final double kPositionControllerD = 0.1;//0.35
+    public static final double kUpControllerD = 0.35;//0.1;//0.35
+
+    public static final double kDownControllerP = 6;//7; //
+    public static final double kDownControllerI = 0;
+    public static final double kDownControllerD = 0.175;//0.1;//0.35
      
     public static final double kAllowedError = 0.75;
 
-    public static final double kFeedForwardS = 0;   /* kG too high - kG too low / 2 */
+    public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8;   /* kG too high - kG too low / 2  0.95, 0.2 */
-    public static final double kFeedForwardG = .7;  /* kG too high + kG too low / 2 */    // calculated value 0.6
+    public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
-    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
+    public static final double kFeedForwardV = 0.06;   // calculated value 0.12
 
-    public static final double kMaxVelocity = 120.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
     public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;

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

@@ -44,7 +44,7 @@ public class Drivetrain extends SubsystemBase {
     private Vision vision;
 
     /** Creates a new DriveSubsystem. */
-    public Drivetrain(Vision vision) {
+    public Drivetrain() {
         m_frontLeft = new MAXSwerveModule(
             DrivetrainConstants.kFrontLeftDrivingCanId,
             DrivetrainConstants.kFrontLeftTurningCanId,
@@ -115,6 +115,7 @@ public class Drivetrain extends SubsystemBase {
 
 
         // if the detected tags match your alliances reef tags use their pose estimates 
+        /* 
         if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
             m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
             
@@ -127,7 +128,7 @@ public class Drivetrain extends SubsystemBase {
         }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
             m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
         }
-        
+        */
         
         
     }

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

@@ -25,7 +25,8 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private ProfiledPIDController pidController;
+    private ProfiledPIDController pidControllerUp;
+    private ProfiledPIDController pidControllerDown;
 
     private ElevatorFeedforward feedForward;
     
@@ -60,16 +61,28 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new ProfiledPIDController(
+        pidControllerUp = new ProfiledPIDController(
-            ElevatorConstants.kPositionControllerP, 
+            ElevatorConstants.kUpControllerP, 
-            ElevatorConstants.kPositionControllerI,
+            ElevatorConstants.kUpControllerI,
-            ElevatorConstants.kPositionControllerD,
+            ElevatorConstants.kUpControllerD,
             new TrapezoidProfile.Constraints(
                 ElevatorConstants.kMaxVelocity, 
                 ElevatorConstants.kMaxAcceleration
             )
         );
-        pidController.setTolerance(ElevatorConstants.kAllowedError);
+
+        pidControllerDown = new ProfiledPIDController(
+            ElevatorConstants.kDownControllerP, 
+            ElevatorConstants.kDownControllerI,
+            ElevatorConstants.kDownControllerD,
+            new TrapezoidProfile.Constraints(
+                ElevatorConstants.kMaxVelocity, 
+                ElevatorConstants.kMaxAcceleration
+            )
+        );
+
+        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
+        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -114,9 +127,16 @@ public class Elevator extends SubsystemBase {
      */
     public Command runManualElevator(DoubleSupplier speed) {
         return run(() -> {
-            elevatorMotor1.set(
+            double desired = speed.getAsDouble();
-                speed.getAsDouble()
+
-            );
+            if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
+                elevatorMotor1.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
+            }
+            
         });
     }
 
@@ -132,6 +152,10 @@ public class Elevator extends SubsystemBase {
         });
     }
 
+    public boolean eitherAtGoal() {
+        return pidControllerUp.atGoal() || pidControllerDown.atGoal();
+    }
+
     /**
      * Moves the elevator to a target destination (setpoint). 
      * 
@@ -147,18 +171,36 @@ public class Elevator extends SubsystemBase {
         );
 
         return run(() -> {
-            pidController.reset(encoder.getPosition(), encoder.getVelocity());
+            //pidController.reset(encoder.getPosition(), encoder.getVelocity());
-
             elevatorMotor1.setVoltage(
-                pidController.calculate(
+                pidControllerUp.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
-                ) + feedForward.calculate(pidController.getSetpoint().velocity)
+                ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
             );
+            
         });
 
     }
             
+            /* 
+            if(encoder.getPosition() >= setpoint.getAsDouble()){
+                elevatorMotor1.setVoltage(
+                    pidControllerUp.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
+                );
+            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
+                elevatorMotor1.setVoltage(
+                    pidControllerDown.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
+                );
+            }
+                */
+
     /**
      * Returns the current encoder position
      * 
@@ -184,7 +226,7 @@ public class Elevator extends SubsystemBase {
      * @return Motor output current
      */
     public double getMotor1() {
-        return elevatorMotor1.getOutputCurrent();
+        return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
     }
 
     /**
@@ -193,6 +235,6 @@ public class Elevator extends SubsystemBase {
      * @return Motor output current
      */
     public double getMotor2() {
-        return elevatorMotor2.getOutputCurrent();
+        return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 }

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.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ManipulatorPivotConstants;
@@ -40,10 +41,10 @@ public class ManipulatorPivot extends SubsystemBase {
         encoder = pivotMotor.getAbsoluteEncoder();
 
         pidController = new ProfiledPIDController(
-            getEncoderPosition(), 
+            ManipulatorPivotConstants.kPositionalP, 
-            getEncoderVelocity(), 
+            ManipulatorPivotConstants.kPositionalI, 
-            getEncoderPosition(), 
+            ManipulatorPivotConstants.kPositionalD, 
-            null
+            new TrapezoidProfile.Constraints(ManipulatorPivotConstants.kMaxVelocity, ManipulatorPivotConstants.kMaxAcceleration)
         );
         
         feedForward = new ArmFeedforward(