merge with vision stuff

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

@@ -19,6 +19,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -103,18 +104,14 @@ public class RobotContainer {
 				() -> true
 			)
 		);
-
+		
 		elevator.setDefaultCommand(
-			elevator.goToSetpoint(
+			elevator.maintainPosition()
-				() -> 0
-			)
 		);
-
+		
 		
 		manipulatorPivot.setDefaultCommand(
-			manipulatorPivot.runManualPivot(
+			manipulatorPivot.maintainPosition()
-				() -> 0
-			)
 		);
 		
 
@@ -125,7 +122,7 @@ public class RobotContainer {
 		);
 
 		//Driver inputs
-		/*
+		
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
@@ -141,52 +138,37 @@ 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().whileTrue(
-			elevator.goToSetpoint(() -> 50)
-		);
-
-		operator.povDown().whileTrue(
-			elevator.goToSetpoint(() -> 0)
-		);
-
-		/*
-		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
-
-		operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
-
- 
 		//Operator inputs
 		operator.povUp().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL4Position, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL3Position, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL2Position, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
-			moveManipulator(
+			safeMoveManipulator(
 				ElevatorConstants.kL1Position, 
 				ManipulatorPivotConstants.kL1Position
 			)
@@ -203,7 +185,11 @@ public class RobotContainer {
 		operator.b().onTrue(
 			algaeIntakeRoutine(false)
 		);
-		*/
+
+		operator.y().whileTrue(
+			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
+		);
+	
 
 	}
 
@@ -268,6 +254,12 @@ public class RobotContainer {
 			.withSize(1, 1)
 			.withPosition(5, 1)
 			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
+
+		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
+
+
  	}
 
 	public Command getAutonomousCommand() {
@@ -318,7 +310,7 @@ public class RobotContainer {
 		// then the elevator, then the arm again
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { 
 			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
 		} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
 			return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
@@ -328,7 +320,7 @@ public class RobotContainer {
 		// Catch all command that's safe regardless of arm and elevator positions
 		} else { 
 			return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-				.andThen(manipulatorPivot.goToSetpoint(armPosition));
+				.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 		}
 	}
 
@@ -342,23 +334,24 @@ public class RobotContainer {
 	 * @return Moves the elevator and arm to the setpoints
 	 */
 	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
-		if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
+		
+		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
 				armPosition, 
 				0,
 				ManipulatorPivotConstants.kRotationLimit
 			);
-		}
+		}*/
 		
 		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
@@ -369,15 +362,15 @@ public class RobotContainer {
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition),
-				manipulatorPivot.goToSetpoint(armPosition));
+				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
-				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
+				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
 				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
-				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}
@@ -392,7 +385,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
 		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(armPosition));
+			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 	}
 
 	@SuppressWarnings("unused")

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

@@ -49,7 +49,7 @@ public class ElevatorConstants {
     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 = 5.5;
+    public static final double kBracePosition = 0;
     public static final double kMaxHeight = 47.5; //actual is 53
 
     // 1, 7, 10 are the defaults for these, change as necessary

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

@@ -16,29 +16,29 @@ public class ManipulatorPivotConstants {
 
     public static final int kMotorCurrentMax = 40;
 
-    public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0;
+    public static final double kPivotConversion = 2 * Math.PI;
 
-    public static final double kPivotMaxVelocity = 0;
+    public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 0;
+    public static final double kPositionalP = 4;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
     public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
 
-    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
-    public static final double kFeedForwardG = 0.41; // calculated value 0.41
+    public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
     public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
-    public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
 
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0;
+    public static final double kEncoderOffset = 0.7815;
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(25.0);
+    public static final double kL2Position = Units.degreesToRadians(60.0);
     public static final double kL3Position = Units.degreesToRadians(60.0);
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
@@ -46,7 +46,7 @@ public class ManipulatorPivotConstants {
     public static final double kProcesserPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(60.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
     /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 
@@ -69,7 +69,8 @@ public class ManipulatorPivotConstants {
     static {
         motorConfig
             .smartCurrentLimit(kMotorCurrentMax)
-            .idleMode(kIdleMode);
+            .idleMode(kIdleMode)
+            .inverted(true);
         motorConfig.absoluteEncoder
             .positionConversionFactor(kPivotConversion)
             .inverted(false)

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

@@ -62,6 +62,7 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kDownControllerI,
             ElevatorConstants.kDownControllerD
         );
+        pidController.setSetpoint(0);
 
         pidController.setTolerance(ElevatorConstants.kAllowedError);
 
@@ -128,36 +129,42 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage based on feed forward calculation.
      */
     public Command maintainPosition() {
-        return run(() -> {
+
-            elevatorMotor1.setVoltage(feedForward.calculate(0));
+        return startRun(() -> {
+            
+        //pidController.reset();
+       // pidController.setSetpoint(encoder.getPosition());
+        }, 
+        () -> {
+            /*if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition()
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }*/
+
+            elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
         });
-    }
+        
-
+            
-    /**
+            
-     * Moves the elevator to a target destination (setpoint). 
+            
-     * 
+            /*
-     * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
-     * @return Sets motor voltage to achieve the target destination
-     */
-    public Command goToSetpoint(DoubleSupplier setpoint) {
-        double clampedSetpoint = MathUtil.clamp(
-            setpoint.getAsDouble(), 
-            0, 
-            ElevatorConstants.kMaxHeight
-        );
-
-        pidController.reset();
-
-        return run(() -> {
             elevatorMotor1.setVoltage(
                 pidController.calculate(
                     encoder.getPosition(),
                     clampedSetpoint
                 ) + feedForward.calculate(0)
             );
-
+*/
-            /*
+            /* 
             if (!pidController.atSetpoint()) {
                 elevatorMotor1.setVoltage(
                     pidController.calculate(
@@ -170,8 +177,65 @@ public class Elevator extends SubsystemBase {
                     feedForward.calculate(0)
                 );
             }
-             */
+             
-        });
+        });*/
+    }
+
+    /**
+     * Moves the elevator to a target destination (setpoint). 
+     * 
+     * @param setpoint Target destination of the subsystem
+     * @param timeout Time to achieve the setpoint before quitting
+     * @return Sets motor voltage to achieve the target destination
+     */
+    public Command goToSetpoint(DoubleSupplier setpoint) {
+
+        return startRun(() -> {
+        
+        pidController.setSetpoint(setpoint.getAsDouble());
+        pidController.reset();
+        }, 
+        () -> {
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) + feedForward.calculate(0)
+                );
+            } else {
+                elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
+            }
+        }).until(() -> pidController.atSetpoint());
+        
+            
+            
+            
+            /*
+            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)
+                );
+            }
+             
+        });*/
     }
             
             /* 

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

@@ -12,6 +12,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.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ManipulatorPivotConstants;
@@ -44,6 +45,9 @@ public class ManipulatorPivot extends SubsystemBase {
             ManipulatorPivotConstants.kPositionalI, 
             ManipulatorPivotConstants.kPositionalD
         );
+        pidController.setSetpoint(0);
+
+        pidController.disableContinuousInput();
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
@@ -92,24 +96,64 @@ public class ManipulatorPivot 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(
+        return startRun(() -> {
-            setpoint, 
+            
-            0, 
+            pidController.setSetpoint(setpoint.getAsDouble());
-            ManipulatorPivotConstants.kRotationLimit
+            pidController.reset();
-        );
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> pidController.atSetpoint());
+    }
 
-        return run(() -> {
+    public Command maintainPosition() {
-            pivotMotor.setVoltage(
+        return startRun(() -> {
-                pidController.calculate(
+            
-                    encoder.getPosition(), 
+        
-                    clampedSetpoint
+            pidController.reset();
-                ) + feedForward.calculate(
+            }, 
-                    encoder.getPosition(), 
+            () -> {
-                    0
+                /* 
-                )
+                if (!pidController.atSetpoint()) {
-            );
+                    pivotMotor.setVoltage(
-        });
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        pidController.getSetpoint()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            });
     }
 
     /**
@@ -118,7 +162,7 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Encoder's position in radians
      */
     public double getEncoderPosition() {
-        return encoder.getPosition();
+        return Units.radiansToDegrees( encoder.getPosition());
     }
     /**
      * Returns the encoder's velocity in radians per second
@@ -126,6 +170,14 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return Encoder's velocity in radians per second
      */
     public double getEncoderVelocity() {
-        return encoder.getVelocity();
+        return Units.radiansToDegrees(encoder.getVelocity());
+    }
+
+    public double getCGPosition(){
+        return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
+    }
+
+    public double getPivotOutput(){
+        return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
     }
 }