testing elevator

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

@@ -53,9 +53,10 @@ public class RobotContainer {
 
 		climberRollers = new ClimberRollers();
 
-		//vision = new Vision(drivetrain::getGyroValue);
 		drivetrain = new Drivetrain();
 
+		vision = new Vision(drivetrain::getGyroValue);
+
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
 
@@ -137,6 +138,8 @@ public class RobotContainer {
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
+
+		driver.y().onTrue(drivetrain.zeroHeading());
 		
 		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
 		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
@@ -188,7 +191,6 @@ public class RobotContainer {
 		operator.y().whileTrue(
 			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
 		);
-	
 
 	}
 
@@ -244,12 +246,22 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
+		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
 			.withSize(1, 1)
 			.withPosition(5, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
-		sensorTab.addDouble("Elevator error", elevator::getPIDError)
+		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
+			.withSize(1, 1)
+			.withPosition(5, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
+			.withSize(1, 1)
+			.withPosition(5, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
 			.withSize(1, 1)
 			.withPosition(5, 1)
 			.withWidget(BuiltInWidgets.kTextView);
@@ -381,10 +393,13 @@ public class RobotContainer {
 	 * @param armPosition The target rotation of the arm
 	 * @return Moves the elevator and arm to the setpoints
 	 */
-	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition))
+		.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 	}
 
 	@SuppressWarnings("unused")

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

@@ -43,14 +43,14 @@ public class ElevatorConstants {
     public static final double kL1Position = 0;
     public static final double kL2Position = 14.5;
     public static final double kL3Position = 29.0;
-    public static final double kL4Position = 53.0;
+    public static final double kL4Position = 52.0;
     public static final double kL4TransitionPosition = 40.0;
     public static final double kL2AlgaePosition = 22.0;
     public static final double kL3AlgaePosition = 39.0;
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
-    public static final double kMaxHeight = 47.5; //actual is 53
+    public static final double kMaxHeight = 52.0; //actual is 52
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;

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

@@ -248,9 +248,12 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.resetEncoders();
     }
 
-    /** Zeroes the heading of the robot. */
-    public void zeroHeading() {
-        gyro.reset();
+    /** Zeroes the heading of the robot. 
+     * @return */
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+        });
     }
 
     public double getGyroValue() {

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

@@ -24,7 +24,8 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
+    private PIDController pidControllerUp;
+    private PIDController pidControllerDown;
 
     private ElevatorFeedforward feedForward;
     
@@ -57,14 +58,23 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new PIDController(
+        pidControllerDown = new PIDController(
             ElevatorConstants.kDownControllerP, 
             ElevatorConstants.kDownControllerI,
             ElevatorConstants.kDownControllerD
         );
-        pidController.setSetpoint(0);
+        pidControllerDown.setSetpoint(0);
 
-        pidController.setTolerance(ElevatorConstants.kAllowedError);
+        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
+
+        pidControllerUp = new PIDController(
+            ElevatorConstants.kUpControllerP, 
+            ElevatorConstants.kUpControllerI,
+            ElevatorConstants.kUpControllerD
+        );
+        pidControllerUp.setSetpoint(0);
+
+        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -132,16 +142,24 @@ public class Elevator extends SubsystemBase {
 
         return startRun(() -> {
             
-        //pidController.reset();
-       // pidController.setSetpoint(encoder.getPosition());
+
         }, 
         () -> {
-            /*if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition()
-                    ) + feedForward.calculate(0)
-                );
+            /* 
+            if (!pidControllerUp.atSetpoint()) {
+                if(encoder.getPosition()>pidControllerUp.getSetpoint()){
+                    elevatorMotor1.setVoltage(
+                        pidControllerUp.calculate(
+                            encoder.getPosition()
+                        ) + feedForward.calculate(0)
+                    );
+                }else{
+                    elevatorMotor1.setVoltage(
+                        pidControllerDown.calculate(
+                            encoder.getPosition()
+                        ) + feedForward.calculate(0)
+                    );
+                }
             } else {
                 elevatorMotor1.setVoltage(
                     feedForward.calculate(0)
@@ -149,36 +167,10 @@ public class Elevator extends SubsystemBase {
             }*/
 
             elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
+                feedForward.calculate(0)
+            );
         });
         
-            
-            
-            
-            /*
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
-            );
-*/
-            /* 
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }
-             
-        });*/
     }
 
     /**
@@ -192,26 +184,27 @@ public class Elevator extends SubsystemBase {
 
         return startRun(() -> {
         
-        pidController.setSetpoint(setpoint.getAsDouble());
-        pidController.reset();
+        pidControllerUp.setSetpoint(setpoint.getAsDouble());
+        pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        pidControllerUp.reset();
+        pidControllerDown.reset();
         }, 
         () -> {
-            if (!pidController.atSetpoint()) {
+            double upOutput = pidControllerUp.calculate(getEncoderPosition());
+            double downOutput = pidControllerDown.calculate(getEncoderPosition());
+
+            if(setpoint.getAsDouble()>encoder.getPosition())
                 elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        setpoint.getAsDouble()
-                    ) + feedForward.calculate(0)
+                    upOutput + feedForward.calculate(0)
                 );
-            } else {
+            else{
                 elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
+                    downOutput + feedForward.calculate(0)
                 );
             }
-        }).until(() -> pidController.atSetpoint());
         
-            
-            
+        }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+  
             
             /*
             elevatorMotor1.setVoltage(
@@ -293,11 +286,19 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDSetpoint() {
-        return pidController.getSetpoint();
+    public double getPIDUpSetpoint() {
+        return pidControllerUp.getSetpoint();
     }
 
-    public double getPIDError() {
-        return pidController.getError();
+    public double getPIDUpError() {
+        return pidControllerUp.getError();
+    }
+
+    public double getPIDDownSetpoint() {
+        return pidControllerDown.getSetpoint();
+    }
+
+    public double getPIDDownError() {
+        return pidControllerDown.getError();
     }
 }

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

@@ -40,9 +40,9 @@ public class Vision{
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
         NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
-        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
-        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+        blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
 
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
@@ -59,24 +59,23 @@ public class Vision{
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
     
-    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
         Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
             new Rotation2d());
 
-        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
 
         Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
 
         Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
 
-        return new Pose2d(globalPose.getTranslation(), gyroAngle);
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
     }
 
     public Pose2d getBlackGlobalPose(){
         return relativeToGlobalPose2d(getBlackClosestTag(), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
     }
 
     public double getBlackRelativeX(){
@@ -109,8 +108,7 @@ public class Vision{
 
     public Pose2d getOrangeGlobalPose(){
         return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
     }
     
     public double getOrangeRelativeX(){