Robot PID testing

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

@@ -89,24 +89,28 @@ public class RobotContainer {
 			)
 		);
 
-		//elevator.setDefaultCommand(
+		elevator.setDefaultCommand(
-			//elevator.runAssistedElevator(operator::getLeftY)
+			elevator.runManualElevator(
-	//	);
+				() -> -operator.getLeftY()
-
+			)
-		//elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
-		elevator.setDefaultCommand(elevator.manualControl(() -> 0.0));
-
-		manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
-
-		manipulator.setDefaultCommand(
-			manipulator.defaultCommand()
 		);
 
-		//manipulatorPivot.setDefaultCommand(
+		
-		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
+		manipulatorPivot.setDefaultCommand(
-		//);
+			manipulatorPivot.setSpeed(
+				() -> 0
+			)
+		);
+		
+
+		manipulator.setDefaultCommand(
+			manipulator.runUntilCollected(
+				() -> 0
+			)
+		);
 
 		//Driver inputs
+		/*
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
@@ -128,16 +132,18 @@ public class RobotContainer {
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
+		*/
 
 		operator.povUp().whileTrue(
 			elevator.goToSetpoint(20)
 		);
 
-		//operator.a().whileTrue(elevator.manualControl(() -> 0.5));
-
-		//operator.b().whileTrue(elevator.manualControl(() ->-0.1));
-
 		/*
+		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
+
+		operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
+
+ 
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
@@ -200,36 +206,33 @@ public class RobotContainer {
 
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(0, 1)
+			.withPosition(0, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(2, 1)
+			.withPosition(2, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(2, 0)
+			.withPosition(2, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+			
+		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
+			.withSize(2, 1)
+			.withPosition(0, 1)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
-			.withSize(1, 1)
-			.withPosition(4, 1)
-			.withWidget(BuiltInWidgets.kBooleanBox);
-
-		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue);
-
-		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch);
-
-		sensorTab.addDouble("elevator position", elevator::getEncoderPosition);
-
-		/* 
-		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
 			.withSize(1, 1)
 			.withPosition(4, 0)
 			.withWidget(BuiltInWidgets.kBooleanBox);
-			*/
+
+		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
+			.withSize(1, 1)
+			.withPosition(4, 1)
+			.withWidget(BuiltInWidgets.kBooleanBox);
   }
 
 	public Command getAutonomousCommand() {
@@ -240,12 +243,13 @@ public class RobotContainer {
 	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
 	 * @return Moves the elevator and arm, then intakes coral
 	 */
+	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
 			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(() -> 1));
+			.andThen(manipulator.runUntilCollected(() -> .5));
 		}
 
 	/**
@@ -254,6 +258,7 @@ public class RobotContainer {
 	 * @param l2 Is the algae on L2? (True = L2, False = L3)
 	 * @return Moves the elevator and arm then intakes algae
 	 */
+	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
 			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
@@ -325,6 +330,7 @@ public class RobotContainer {
 		);
 	}
 
+	@SuppressWarnings("unused")
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
@@ -354,6 +360,7 @@ public class RobotContainer {
 			.andThen(manipulatorPivot.goToSetpoint(armPosition));
 	}
 
+	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));

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

@@ -21,7 +21,7 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0.3;
+    public static final double kPositionControllerP = 0.1;
     public static final double kPositionControllerI = 0;
     public static final double kPositionControllerD = 0;
 
@@ -74,9 +74,11 @@ public class ElevatorConstants {
         motorConfig
             .smartCurrentLimit(kCurrentLimit)
             .idleMode(kIdleMode)
-            .inverted(false);
+            .inverted(true);
         motorConfig.encoder
+            //.inverted(true)
             .positionConversionFactor(kEncoderConversionFactor);  
+        /*        
         motorConfig.closedLoop
             .p(kPositionControllerP)
             .i(kPositionControllerI)
@@ -86,6 +88,7 @@ public class ElevatorConstants {
             .maxAcceleration(kMaxAcceleration)
             .maxVelocity(kMaxVelocity)
             .allowedClosedLoopError(kAllowedError);
+        */
     }
 
     public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();

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

@@ -15,8 +15,6 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
-    //private DigitalInput cageLimitSwitch;
-
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
@@ -30,8 +28,6 @@ public class ClimberPivot extends SubsystemBase {
         );
 
         neoEncoder = pivotMotor.getEncoder();
-
-        //cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
     }
 
     public Command runPivot(double speed) {
@@ -53,15 +49,6 @@ public class ClimberPivot extends SubsystemBase {
         }).until(() -> neoEncoder.getPosition() >= setpoint);
     }
 
-    /**
-     * Returns the limit switch attached to the climber. Detects if the cage is present
-     * 
-     * @return Is the cage in the climber
-     */
-   // public boolean getCageLimitSwitch() {
-    //    return cageLimitSwitch.get();
-    //}
-
     public double getEncoderPosition() {
         return neoEncoder.getPosition();
     }

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

@@ -14,6 +14,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.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -23,12 +24,14 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
-    private SparkClosedLoopController elevatorClosedLoop;
+    //private SparkClosedLoopController elevatorClosedLoop;
 
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
 
+    private PIDController pidController;
+
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
@@ -48,27 +51,26 @@ public class Elevator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
-        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
-
         elevatorMotor2.configure(
             ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID), 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
+        //elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
+
         encoder = elevatorMotor1.getEncoder();
 
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        /* 
+        pidController = new PIDController(
-        velocityController = new PIDController(
+            ElevatorConstants.kPositionControllerP, 
-            ElevatorConstants.kVelocityControllerP, 
+            ElevatorConstants.kPositionControllerI,
-            ElevatorConstants.kVelocityControllerI, 
+            ElevatorConstants.kPositionControllerD
-            ElevatorConstants.kVelocityControllerD
         );
-        */
+        pidController.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -111,43 +113,15 @@ public class Elevator extends SubsystemBase {
      * @param speed The speed at which the elevator translates
      * @return Sets motor voltage to translate the elevator and maintain position
      */
-    public Command runManualElevator(double speed) {
+    public Command runManualElevator(DoubleSupplier speed) {
         return run(() -> {
-            elevatorMotor1.set(speed);
+            //elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
-        });
+            elevatorMotor1.set(speed.getAsDouble());
-    }
-
-    /**
-     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
-     * 
-     * @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;
-
-            double voltsOut = velocityController.calculate(
-                encoder.getVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(realSpeedTarget);
-
-            elevatorMotor1.setVoltage(voltsOut);
-        }).until(
-            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
-    }
-            */
-    
-    public Command manualControl(DoubleSupplier speed){
-        return run(() -> {
-            elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
-
         });
     }
 
     /**
      * Moves the elevator to a target destination (setpoint). 
-     * If the setpoint is 0, the elevator will creep down to hit the limit switch
      * 
      * @param setpoint Target destination of the subsystem
      * @param timeout Time to achieve the setpoint before quitting
@@ -161,44 +135,20 @@ public class Elevator extends SubsystemBase {
         );
 
         return run(() -> {
-
+            elevatorMotor1.setVoltage(
+                pidController.calculate(
+                    encoder.getPosition(),
+                    clampedSetpoint
+                ) + feedForward.calculate(0)
+            );
+            /*
             elevatorClosedLoop.setReference(clampedSetpoint,
                 SparkBase.ControlType.kMAXMotionPositionControl,
                 ClosedLoopSlot.kSlot0,
                 feedForward.calculate(0)
             );
-
-        });
-/* 
-        if (clampedSetpoint == 0) {
-            return run(() -> {
-                double voltsOut = positionController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout)
-                .andThen(Commands.either(
-                    runAssistedElevator(() -> 0),
-                    runAssistedElevator(() -> -.2), 
-                    bottomLimitSwitch::get
-                )).withTimeout(timeout);
-        } else {
-            return run(() -> {
-                double voltsOut = positionController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0);
-    
-                elevatorMotor1.setVoltage(voltsOut);
-            }).until(
-                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
-            ).withTimeout(timeout);
-        }
             */
+        });
 
     }
 

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

@@ -16,7 +16,6 @@ public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
     private DigitalInput coralBeamBreak;
-  //  private DigitalInput algaeBeamBreak; 
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -31,7 +30,6 @@ public class Manipulator extends SubsystemBase {
         );
 
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
-      //  algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
     /**
@@ -42,8 +40,7 @@ public class Manipulator extends SubsystemBase {
      */
     public Command defaultCommand() {
         return run(() -> {
-            //runUntilCollected(0.1);
+            runUntilCollected(() -> 0.1);
-            manipulatorMotor.set(0);
             });
     }
 
@@ -77,6 +74,19 @@ public class Manipulator extends SubsystemBase {
         }).until(() -> !coralBeamBreak.get());
     }
 
+    /**
+     * Runs the manipulator in a way that will bring the coral to a reliable holding position
+     * 
+     * @return Returns a command that will position the coral to a known location
+     */
+    public Command indexCoral() {
+        return run(() -> {
+            runUntilCollected(() -> 0.5)
+                .andThen(runManipulator(() -> .1, false))
+                .until(() -> getCoralBeamBreak());
+        });
+    }
+
     public boolean getCoralBeamBreak() {
         return coralBeamBreak.get();
     }

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

@@ -1,6 +1,9 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkMax;
+
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkAbsoluteEncoder;
 import com.revrobotics.spark.SparkBase;
@@ -66,35 +69,12 @@ public class ManipulatorPivot extends SubsystemBase {
         return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
     }
 
-    /**
+    public Command setSpeed(DoubleSupplier speed) {
-     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
-     * 
-     * @param speed The velocity at which the arm rotates
-     * @return Sets motor voltage to achieve the target velocity
-     */
-    /* 
-    public Command runAssistedPivot(DoubleSupplier speed) {
-        double clampedSpeed = MathUtil.clamp(
-            speed.getAsDouble(), 
-            -1, 
-            1
-        );
-
         return run(() -> {
-            double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
+            armMotor.set(speed.getAsDouble());
-
-            double voltsOut = velocityController.calculate(
-                getEncoderVelocity(), 
-                realSpeedTarget
-            ) + feedForward.calculate(
-                getEncoderPosition(), 
-                getEncoderVelocity()
-            );
-
-            armMotor.setVoltage(voltsOut);
         });
     }
-*/
+    
     /**
      * Moves the arm to a target destination (setpoint)
      *