removed velocity controllers on position mechanisms and added controller PID for elevator

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

@@ -89,17 +89,17 @@ public class RobotContainer {
 			)
 		);
 
-		elevator.setDefaultCommand(
+		//elevator.setDefaultCommand(
-			elevator.runAssistedElevator(operator::getLeftY)
+			//elevator.runAssistedElevator(operator::getLeftY)
-		);
+	//	);
 
 		manipulator.setDefaultCommand(
 			manipulator.defaultCommand()
 		);
 
-		manipulatorPivot.setDefaultCommand(
+		//manipulatorPivot.setDefaultCommand(
-			manipulatorPivot.runAssistedPivot(operator::getRightY)
+		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
-		);
+		//);
 
 		//Driver inputs
 		driver.start().whileTrue(

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

@@ -14,38 +14,42 @@ public class ElevatorConstants {
     public static final int kElevatorMotor1ID = 0;
     public static final int kElevatorMotor2ID = 0;
 
-    public static final int kTopLimitSwitchID = 0;
     public static final int kBottomLimitSwitchID = 0;
 
-    // 60/12 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
+    // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
-    public static final double kEncoderConversionFactor = 11/60 * Math.PI * 1.75 * 2;
+    public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;
 
-    public static final int kMotorAmpsMax = 40;
+    public static final int kCurrentLimit = 40;
 
     public static final double kPositionControllerP = 0;
     public static final double kPositionControllerI = 0;
     public static final double kPositionControllerD = 0;
+
+    public static final double kAllowedError = 0.25;
     
+    /* 
     public static final double kVelocityControllerP = 0;
     public static final double kVelocityControllerI = 0;
     public static final double kVelocityControllerD = 0;
+    */
 
     public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0.41; // calculated value
+    public static final double kFeedForwardG = 0.53; // calculated value
-    public static final double kFeedForwardV = 1.75; // calculated value
+    public static final double kFeedForwardV = 4.78; // calculated value
 
     public static final double kMaxVelocity = 0;
+    public static final double kMaxAcceleration = 0;
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 0;
+    public static final double kL2Position = 14.5;
-    public static final double kL3Position = 0;
+    public static final double kL3Position = 29.0;
-    public static final double kL4Position = 0;
+    public static final double kL4Position = 53.0;
     public static final double kL2AlgaePosition = 0;
     public static final double kL3AlgaePosition = 0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
-    public static final double kMaxHeight = 0;
+    public static final double kMaxHeight = 53.0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;
@@ -66,10 +70,19 @@ public class ElevatorConstants {
     
     static {
         motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
+            .smartCurrentLimit(kCurrentLimit)
             .idleMode(kIdleMode);
         motorConfig.encoder
             .positionConversionFactor(kEncoderConversionFactor)
             .velocityConversionFactor(kEncoderConversionFactor / 60.0);
+        motorConfig.closedLoop
+            .p(kPositionControllerP)
+            .i(kPositionControllerI)
+            .d(kPositionControllerD)
+            .velocityFF(0.0); // keep at zero for position pid
+        motorConfig.closedLoop.maxMotion
+            .maxAcceleration(kMaxAcceleration)
+            .maxVelocity(kMaxVelocity)
+            .allowedClosedLoopError(kAllowedError);
     }
 }

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

@@ -3,6 +3,9 @@ package frc.robot.subsystems;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -21,12 +24,14 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
+    private SparkClosedLoopController elevatorClosedLoop;
+
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
 
     private PIDController positionController;
-    private PIDController velocityController;
+    //private PIDController velocityController;
 
     private ElevatorFeedforward feedForward;
     
@@ -41,6 +46,8 @@ public class Elevator extends SubsystemBase {
             MotorType.kBrushless
         );
 
+        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
+
         elevatorMotor1.configure(
             ElevatorConstants.motorConfig, 
             ResetMode.kResetSafeParameters, 
@@ -65,11 +72,13 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kPositionControllerD
         );
 
+        /* 
         velocityController = new PIDController(
             ElevatorConstants.kVelocityControllerP, 
             ElevatorConstants.kVelocityControllerI, 
             ElevatorConstants.kVelocityControllerD
         );
+        */
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -123,7 +132,7 @@ public class Elevator extends SubsystemBase {
      * 
      * @param speed How fast the elevator moves
      * @return Sets motor voltage to move the elevator relative to the speed parameter
-     */
+     *
     public Command runAssistedElevator(DoubleSupplier speed) {
         return run(() -> {
             double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
@@ -137,6 +146,7 @@ public class Elevator extends SubsystemBase {
         }).until(
             () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
     }
+            */
     
     /**
      * Moves the elevator to a target destination (setpoint). 
@@ -153,6 +163,16 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kMaxHeight
         );
 
+        return run(() -> {
+
+            elevatorClosedLoop.setReference(clampedSetpoint,
+                SparkBase.ControlType.kMAXMotionPositionControl,
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(0)
+                );
+                
+        });
+/* 
         if (clampedSetpoint == 0) {
             return run(() -> {
                 double voltsOut = positionController.calculate(
@@ -181,6 +201,8 @@ public class Elevator extends SubsystemBase {
                 () -> positionController.atSetpoint() || bottomLimitSwitch.get()
             ).withTimeout(timeout);
         }
+            */
+
     }
 
     /**

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

@@ -11,6 +11,8 @@ public class Vision{
     private DoubleSubscriber cam1ClosestTag;
     private BooleanSubscriber cam1TagDetected;
 
+    private DoubleSubscriber cam1TimeStamp;
+
     private DoubleSubscriber framerate;
 
     public Vision(){
@@ -22,7 +24,6 @@ public class Vision{
         cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0);
 
         cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false);
-      //  cam2TagDetected = visionTable.getBooleanTopic("cam2_visible").subscribe(false);
 
         framerate = visionTable.getDoubleTopic("fps").subscribe(0.0);
     }
@@ -39,6 +40,10 @@ public class Vision{
         return cam1TagDetected.get();
     }
 
+    public double getCam1TimeStamp(){
+        return cam1GlobalPose.getLastChange();
+    }
+
     public double getFPS(){
         return framerate.get();
     }