Changed elevator and manip pivot to regular pid controllers

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

@@ -15,15 +15,10 @@ 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;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -31,7 +26,6 @@ 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;
@@ -263,9 +257,6 @@ public class RobotContainer {
 
 		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/subsystems/Elevator.java

@@ -10,8 +10,7 @@ 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.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
-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;
@@ -25,12 +24,9 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private ProfiledPIDController pidControllerUp;
+    private PIDController pidController;
-    private ProfiledPIDController pidControllerDown;
 
     private ElevatorFeedforward feedForward;
-
-    private double currentPIDOut;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -61,28 +57,13 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidControllerUp = new ProfiledPIDController(
+        pidController = new PIDController(
-            ElevatorConstants.kUpControllerP, 
-            ElevatorConstants.kUpControllerI,
-            ElevatorConstants.kUpControllerD,
-            new TrapezoidProfile.Constraints(
-                ElevatorConstants.kMaxVelocity, 
-                ElevatorConstants.kMaxAcceleration
-            )
-        );
-
-        pidControllerDown = new ProfiledPIDController(
             ElevatorConstants.kDownControllerP, 
             ElevatorConstants.kDownControllerI,
-            ElevatorConstants.kDownControllerD,
+            ElevatorConstants.kDownControllerD
-            new TrapezoidProfile.Constraints(
-                ElevatorConstants.kMaxVelocity, 
-                ElevatorConstants.kMaxAcceleration
-            )
         );
 
-        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
+        pidController.setTolerance(ElevatorConstants.kAllowedError);
-        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -152,10 +133,6 @@ public class Elevator extends SubsystemBase {
         });
     }
 
-    public boolean eitherAtGoal() {
-        return pidControllerUp.atGoal() || pidControllerDown.atGoal();
-    }
-
     /**
      * Moves the elevator to a target destination (setpoint). 
      * 
@@ -170,27 +147,29 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kMaxHeight
         );
 
-        pidControllerDown.setGoal(clampedSetpoint);
-
         return run(() -> {
-            //pidControllerUp.reset(encoder.getPosition(), encoder.getVelocity());
+            elevatorMotor1.setVoltage(
-
+                pidController.calculate(
-            if(!pidControllerDown.atGoal()){
-                currentPIDOut = pidControllerDown.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
-                );
+                ) + feedForward.calculate(0)
-                System.out.println("CALCULATED");
-            }else{
-                currentPIDOut = 0;
-                System.out.println("SET ZERO");
-            }
-
-            elevatorMotor1.setVoltage(
-                currentPIDOut + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
             );
-        });
 
+            /*
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        clampedSetpoint
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }
+             */
+        });
     }
             
             /* 
@@ -247,8 +226,4 @@ public class Elevator extends SubsystemBase {
     public double getMotor2() {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
-
-    public double currentPIDOut(){
-        return currentPIDOut;
-    }
 }

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

@@ -11,8 +11,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.controller.PIDController;
-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;
@@ -24,7 +23,7 @@ public class ManipulatorPivot extends SubsystemBase {
 
     private ArmFeedforward feedForward;
 
-    private ProfiledPIDController pidController;
+    private PIDController pidController;
 
     public ManipulatorPivot() {
         pivotMotor = new SparkMax(
@@ -40,11 +39,10 @@ public class ManipulatorPivot extends SubsystemBase {
         
         encoder = pivotMotor.getAbsoluteEncoder();
 
-        pidController = new ProfiledPIDController(
+        pidController = new PIDController(
             ManipulatorPivotConstants.kPositionalP, 
             ManipulatorPivotConstants.kPositionalI, 
-            ManipulatorPivotConstants.kPositionalD, 
+            ManipulatorPivotConstants.kPositionalD
-            new TrapezoidProfile.Constraints(ManipulatorPivotConstants.kMaxVelocity, ManipulatorPivotConstants.kMaxAcceleration)
         );
         
         feedForward = new ArmFeedforward(
@@ -102,8 +100,6 @@ public class ManipulatorPivot extends SubsystemBase {
         );
 
         return run(() -> {
-            pidController.reset(encoder.getPosition(), encoder.getVelocity());
-
             pivotMotor.setVoltage(
                 pidController.calculate(
                     encoder.getPosition(),