testing from 2/18

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

@@ -15,6 +15,7 @@ import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Manipulator;
+import frc.robot.subsystems.sysid.ElevatorSysID;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
@@ -30,6 +31,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 
 public class RobotContainer {
 	private ClimberPivot climberPivot;
@@ -108,8 +110,8 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runManualElevator(
+			elevator.goToSetpoint(
-				() -> -operator.getLeftY() * .5
+				() -> 0
 			)
 		);
 
@@ -144,24 +146,22 @@ 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().onTrue(
+		operator.povUp().whileTrue(
-			elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal)
+			elevator.goToSetpoint(() -> 50)
 		);
 
-		operator.povDown().onTrue(
+		operator.povDown().whileTrue(
-			elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal)
+			elevator.goToSetpoint(() -> 0)
 		);
 
-		operator.a().whileTrue(elevator.maintainPosition());
-
 		/*
 		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
 
@@ -231,7 +231,7 @@ public class RobotContainer {
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
-			.withWidget(BuiltInWidgets.kTextView);
+			.withWidget(BuiltInWidgets.kGraph);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
@@ -258,9 +258,14 @@ public class RobotContainer {
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
 
-		sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
+		sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
+			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
+		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
+			.withWidget(BuiltInWidgets.kGraph);
+
+		sensorTab.addDouble("PID output", elevator::currentPIDOut)
+		.withWidget(BuiltInWidgets.kGraph);
  	}
 
 	public Command getAutonomousCommand() {

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

@@ -18,23 +18,23 @@ public class ElevatorConstants {
 
     // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
     public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
-    public static final double kEncoderVelocityConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0 / 60;
+    public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kUpControllerP = 4;//7; //
+    public static final double kUpControllerP = 5.6;//7; //
     public static final double kUpControllerI = 0;
-    public static final double kUpControllerD = 0.35;//0.1;//0.35
+    public static final double kUpControllerD = 0.28;//0.28
 
-    public static final double kDownControllerP = 6;//7; //
+    public static final double kDownControllerP = 5.6;//7; //
     public static final double kDownControllerI = 0;
-    public static final double kDownControllerD = 0.175;//0.1;//0.35
+    public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
      
-    public static final double kAllowedError = 0.75;
+    public static final double kAllowedError = 1;
 
     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 = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
-    public static final double kFeedForwardV = 0.06;   // calculated value 0.12
+    public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
     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

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

@@ -29,6 +29,8 @@ public class Elevator extends SubsystemBase {
     private ProfiledPIDController pidControllerDown;
 
     private ElevatorFeedforward feedForward;
+
+    private double currentPIDOut;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -53,8 +55,6 @@ public class Elevator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
-        //elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
-
         encoder = elevatorMotor1.getEncoder();
 
         bottomLimitSwitch = new DigitalInput(
@@ -170,14 +170,24 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kMaxHeight
         );
 
-        return run(() -> {
+        pidControllerDown.setGoal(clampedSetpoint);
-            //pidController.reset(encoder.getPosition(), encoder.getVelocity());
 
-            elevatorMotor1.setVoltage(
+        return run(() -> {
-                pidControllerUp.calculate(
+            //pidControllerUp.reset(encoder.getPosition(), encoder.getVelocity());
+
+            if(!pidControllerDown.atGoal()){
+                currentPIDOut = pidControllerDown.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
-                ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
+                );
+                System.out.println("CALCULATED");
+            }else{
+                currentPIDOut = 0;
+                System.out.println("SET ZERO");
+            }
+
+            elevatorMotor1.setVoltage(
+                currentPIDOut + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
             );
         });
 
@@ -237,4 +247,8 @@ public class Elevator extends SubsystemBase {
     public double getMotor2() {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
+
+    public double currentPIDOut(){
+        return currentPIDOut;
+    }
 }

src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java

@@ -14,17 +14,23 @@ import frc.robot.constants.ElevatorConstants;
 import frc.robot.subsystems.Elevator;
 
 public class ElevatorSysID extends Elevator {
-    private MutVoltage appliedVoltage = Volts.mutable(0);;
+    private MutVoltage appliedVoltage;
 
-    private MutDistance elevatorPosition = Inches.mutable(0);;
+    private MutDistance elevatorPosition;
 
-    private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0);
+    private MutLinearVelocity elevatorVelocity;
 
     private SysIdRoutine routine;
 
     public ElevatorSysID() {
         super();
 
+        appliedVoltage = Volts.mutable(0);
+
+        elevatorPosition = Inches.mutable(0);
+
+        elevatorVelocity = InchesPerSecond.mutable(0);
+
         routine = new SysIdRoutine(
             ElevatorConstants.kSysIDConfig,
             new SysIdRoutine.Mechanism(