Many attempt at tuning Elevator values at 2/15 build session

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\infin\\Downloads",
+    "serverTeam": "2648"
+  }
+}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

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

@@ -9,12 +9,15 @@ import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.sysid.ElevatorSysID;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Manipulator;
 
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
@@ -35,6 +38,7 @@ public class RobotContainer {
 	private Drivetrain drivetrain;
 
 	private Elevator elevator;
+	//private ElevatorSysID elevator;
 
 	private Manipulator manipulator;
 
@@ -53,6 +57,7 @@ public class RobotContainer {
 		drivetrain = new Drivetrain();
 
 		elevator = new Elevator();
+		//elevator = new ElevatorSysID();
 
 		manipulator = new Manipulator();
 
@@ -64,12 +69,22 @@ public class RobotContainer {
 		autoChooser = AutoBuilder.buildAutoChooser();
 
 		configureButtonBindings();
+		//elevatorSysIDBindings();
 
 		configureNamedCommands();
 
 		configureShuffleboard();
 	}
 
+	/*private void elevatorSysIDBindings() {
+		elevator.setDefaultCommand(elevator.maintainPosition());
+
+		operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
+		operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
+		operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
+		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
+	}*/
+
 	private void configureButtonBindings() {
 		//Default commands
 		climberPivot.setDefaultCommand(
@@ -91,7 +106,7 @@ public class RobotContainer {
 
 		elevator.setDefaultCommand(
 			elevator.runManualElevator(
-				() -> -operator.getLeftY()
+				() -> -operator.getLeftY() * .5
 			)
 		);
 
@@ -134,8 +149,12 @@ public class RobotContainer {
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
 		*/
 
-		operator.povUp().whileTrue(
+		operator.povUp().onTrue(
-			elevator.goToSetpoint(20)
+			elevator.goToSetpoint(() -> 20)
+		);
+
+		operator.povDown().onTrue(
+			elevator.goToSetpoint(() -> 0)
 		);
 
 		/*
@@ -233,6 +252,10 @@ public class RobotContainer {
 			.withSize(1, 1)
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
+
+		sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
   }
 
 	public Command getAutonomousCommand() {
@@ -317,13 +340,13 @@ public class RobotContainer {
 		
 		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
@@ -335,14 +358,14 @@ public class RobotContainer {
 		if(!isL4){
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
-				elevator.goToSetpoint(elevatorPosition),
+				elevator.goToSetpoint(() -> elevatorPosition),
 				manipulatorPivot.goToSetpoint(armPosition));
 			
 		}else{
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
-				elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
-				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition));
+				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}

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

@@ -21,24 +21,18 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0.1;
+    public static final double kPositionControllerP = 3; //
     public static final double kPositionControllerI = 0;
-    public static final double kPositionControllerD = 0;
+    public static final double kPositionControllerD = 0.1;//0.35
      
-    public static final double kAllowedError = 0.5;
+    public static final double kAllowedError = 0.75;
-    
-    /* 
-    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.53; // calculated value
+    public static final double kFeedForwardG = .7;//1; // calculated value  .6
-    public static final double kFeedForwardV = 4.78; // calculated value
+    public static final double kFeedForwardV = 0.12; // calculated value  .12 INCHES NOT METERS
 
     public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 500.0; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    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;
     public static final double kL1Position = 0;
@@ -51,11 +45,11 @@ public class ElevatorConstants {
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 5.5;
-    public static final double kMaxHeight = 53.0;
+    public static final double kMaxHeight = 47.5; //actual is 53
 
     // 1, 7, 10 are the defaults for these, change as necessary
-    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDRampRate = .25;
-    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDStepVolts = 3;
     public static final double kSysIDTimeout = 10;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
@@ -76,10 +70,9 @@ public class ElevatorConstants {
             .idleMode(kIdleMode)
             .inverted(true);
         motorConfig.encoder
-            //.inverted(true)
+            .positionConversionFactor(kEncoderConversionFactor)
-            .positionConversionFactor(kEncoderConversionFactor);  
+            .velocityConversionFactor(kEncoderConversionFactor);
-        /*        
+        /*motorConfig.closedLoop
-        motorConfig.closedLoop
             .p(kPositionControllerP)
             .i(kPositionControllerI)
             .d(kPositionControllerD)
@@ -87,8 +80,7 @@ public class ElevatorConstants {
         motorConfig.closedLoop.maxMotion
             .maxAcceleration(kMaxAcceleration)
             .maxVelocity(kMaxVelocity)
-            .allowedClosedLoopError(kAllowedError);
+            .allowedClosedLoopError(kAllowedError);*/
-        */
     }
 
     public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();

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

@@ -15,6 +15,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -30,7 +32,7 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
+    private ProfiledPIDController pidController;
 
     private ElevatorFeedforward feedForward;
     
@@ -52,7 +54,7 @@ public class Elevator extends SubsystemBase {
         );
 
         elevatorMotor2.configure(
-            ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID), 
+            ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID), 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
@@ -65,10 +67,14 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new PIDController(
+        pidController = new ProfiledPIDController(
             ElevatorConstants.kPositionControllerP, 
             ElevatorConstants.kPositionControllerI,
-            ElevatorConstants.kPositionControllerD
+            ElevatorConstants.kPositionControllerD,
+            new TrapezoidProfile.Constraints(
+                ElevatorConstants.kMaxVelocity, 
+                ElevatorConstants.kMaxAcceleration
+            )
         );
         pidController.setTolerance(ElevatorConstants.kAllowedError);
 
@@ -115,8 +121,15 @@ public class Elevator extends SubsystemBase {
      */
     public Command runManualElevator(DoubleSupplier speed) {
         return run(() -> {
-            //elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
+            elevatorMotor1.set(
-            elevatorMotor1.set(speed.getAsDouble());
+                speed.getAsDouble()
+            );
+        });
+    }
+
+    public Command maintainPosition() {
+        return run(() -> {
+            elevatorMotor1.setVoltage(feedForward.calculate(0));
         });
     }
 
@@ -127,27 +140,34 @@ public class Elevator 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(
-            setpoint, 
+            setpoint.getAsDouble(), 
             0, 
             ElevatorConstants.kMaxHeight
         );
 
         return run(() -> {
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
-            );
             /* 
-            elevatorClosedLoop.setReference(clampedSetpoint,
+            if(pidController.atGoal()) {
-                SparkBase.ControlType.kMAXMotionPositionControl,
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
+            } else {
+*/
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(pidController.getSetpoint().velocity)
+                );
+            
+            
+            /*elevatorClosedLoop.setReference(
+                clampedSetpoint,
+                ControlType.kMAXMotionPositionControl,
                 ClosedLoopSlot.kSlot0,
                 feedForward.calculate(0)
-            );
+            );*/
-            */
+            
         });
 
     }
@@ -170,4 +190,12 @@ public class Elevator extends SubsystemBase {
     public boolean getBottomLimitSwitch() {
         return bottomLimitSwitch.get();
     }
+
+    public double getMotor1() {
+        return elevatorMotor1.getOutputCurrent();
+    }
+
+    public double getMotor2() {
+        return elevatorMotor2.getOutputCurrent();
+    }
 }

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

@@ -92,7 +92,7 @@ public class ManipulatorPivot extends SubsystemBase {
         return run(() -> {
             pivotClosedLoopController.setReference(clampedSetpoint,
                 SparkBase.ControlType.kMAXMotionPositionControl,
-                ClosedLoopSlot.kSlot0,
+                ClosedLoopSlot.kSlot1,
                 feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
         });
     }

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

@@ -1,8 +1,8 @@
 package frc.robot.subsystems.sysid;
 
-import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
 import edu.wpi.first.units.measure.MutLinearVelocity;
@@ -14,23 +14,17 @@ import frc.robot.constants.ElevatorConstants;
 import frc.robot.subsystems.Elevator;
 
 public class ElevatorSysID extends Elevator {
-    private MutVoltage appliedVoltage;
+    private MutVoltage appliedVoltage = Volts.mutable(0);;
 
-    private MutDistance elevatorPosition;
+    private MutDistance elevatorPosition = Inches.mutable(0);;
 
-    private MutLinearVelocity elevatorVelocity;
+    private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0);
 
     private SysIdRoutine routine;
 
     public ElevatorSysID() {
         super();
 
-        appliedVoltage = Volts.mutable(0);
-
-        elevatorPosition = Meters.mutable(0);
-
-        elevatorVelocity = MetersPerSecond.mutable(0);
-
         routine = new SysIdRoutine(
             ElevatorConstants.kSysIDConfig,
             new SysIdRoutine.Mechanism(
@@ -41,10 +35,10 @@ public class ElevatorSysID extends Elevator {
                             elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
-                            encoder.getPosition(), Meters
+                            encoder.getPosition(), Inches
                         ))
                         .linearVelocity(elevatorVelocity.mut_replace(
-                            encoder.getVelocity(), MetersPerSecond
+                            encoder.getVelocity(), InchesPerSecond
                         ));
                 }, 
                 this
@@ -52,6 +46,11 @@ public class ElevatorSysID extends Elevator {
         );
     }
 
+    @Override
+    public void periodic() {
+
+    }
+
     public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
         return routine.quasistatic(direction);
     }