manipulator pivot on controller pid

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

@@ -49,9 +49,6 @@ public class ManipulatorPivotConstants {
     /**The forward rotation limit of the arm */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 
-    public static final double kMagnetOffset = 0.0;
-    public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
-
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;
     public static final double kSysIDTimeout = 10;
@@ -73,15 +70,11 @@ public class ManipulatorPivotConstants {
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 
     static {
-        canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
-        canCoderConfig.MagnetSensor.MagnetOffset = kMagnetOffset;
-        // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
-        //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
 
         motorConfig
             .smartCurrentLimit(kMotorAmpsMax)
             .idleMode(kIdleMode);
-        motorConfig.encoder.positionConversionFactor(kPivotConversion);
+        motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
         motorConfig.closedLoop
             .p(kPositionalP)
             .i(kPositionalI)

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

@@ -1,17 +1,16 @@
 package frc.robot.subsystems;
 
-import java.util.function.DoubleSupplier;
-
-import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
 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;
@@ -19,13 +18,11 @@ import frc.robot.constants.ManipulatorPivotConstants;
 public class ManipulatorPivot extends SubsystemBase {
     protected SparkMax armMotor;
 
-    private CANcoder canCoder;
-
-    private PIDController positionController;
-    private PIDController velocityController;
-
     private ArmFeedforward feedForward;
 
+    private SparkClosedLoopController pivotClosedLoopController;
+    private SparkAbsoluteEncoder absoluteEncoder;
+
     public ManipulatorPivot() {
         armMotor = new SparkMax(
             ManipulatorPivotConstants.kArmMotorID, 
@@ -38,19 +35,9 @@ public class ManipulatorPivot extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
-        positionController = new PIDController(
-            ManipulatorPivotConstants.kPositionalP, 
-            ManipulatorPivotConstants.kPositionalI, 
-            ManipulatorPivotConstants.kPositionalD
-        );
+        pivotClosedLoopController = armMotor.getClosedLoopController();
+        absoluteEncoder = armMotor.getAbsoluteEncoder();
         
-        // TODO: Generate constants for continuous input range based on CANcoder configuration?
-        positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
-        positionController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
-
-        canCoder = new CANcoder(ManipulatorPivotConstants.kCANcoderID);
-        canCoder.getConfigurator().apply(ManipulatorPivotConstants.canCoderConfig);
-
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
             ManipulatorPivotConstants.kFeedForwardG, 
@@ -85,6 +72,7 @@ public class ManipulatorPivot extends SubsystemBase {
      * @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(), 
@@ -106,7 +94,7 @@ public class ManipulatorPivot extends SubsystemBase {
             armMotor.setVoltage(voltsOut);
         });
     }
-
+*/
     /**
      * Moves the arm to a target destination (setpoint)
      * 
@@ -122,16 +110,11 @@ public class ManipulatorPivot extends SubsystemBase {
         );
 
         return run(() -> {
-            double voltsOut = positionController.calculate(
-                getEncoderPosition(), 
-                clampedSetpoint
-            ) + feedForward.calculate(
-                getEncoderPosition(), 
-                getEncoderVelocity()
-            );
-
-            armMotor.setVoltage(voltsOut);
-        }).until(positionController::atSetpoint);
+            pivotClosedLoopController.setReference(clampedSetpoint,
+                SparkBase.ControlType.kMAXMotionPositionControl,
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
+        });
     }
 
     /**
@@ -140,15 +123,14 @@ public class ManipulatorPivot extends SubsystemBase {
      * @return CANCoder's position in radians
      */
     public double getEncoderPosition() {
-        return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
+        return absoluteEncoder.getPosition();
     }
-
     /**
      * Returns the CANCoder's velocity in radians per second
      * 
      * @return CANCoder's velocity in radians per second
      */
     public double getEncoderVelocity() {
-        return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
+        return absoluteEncoder.getVelocity();
     }
 }

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

@@ -9,11 +9,14 @@ public class Vision{
 
     private DoubleSubscriber blackRobotRelativePose;
     private DoubleSubscriber blackClosestTag;
+    private BooleanSubscriber blackTagDetected;
 
     private DoubleSubscriber blackFramerate;
 
     private DoubleSubscriber orangeRobotRelativePose;
     private DoubleSubscriber orangeClosestTag;
+    private BooleanSubscriber orangeTagDetected;
+
     private DoubleSubscriber orangeFramerate;
 
     public Vision(){
@@ -24,11 +27,13 @@ public class Vision{
 
         blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0);
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
+        blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
         orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0);
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
+        orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
@@ -45,6 +50,10 @@ public class Vision{
         return blackRobotRelativePose.getLastChange();
     }
 
+    public boolean getBlackTagDetected(){
+        return blackTagDetected.get();
+    }
+
     public double getBlackFPS(){
         return blackFramerate.get();
     }
@@ -61,6 +70,10 @@ public class Vision{
         return orangeRobotRelativePose.getLastChange();
     }
 
+    public boolean getOrangeTagDetected(){
+        return orangeTagDetected.get();
+    }
+
     public double getOrangeFPS(){
         return orangeFramerate.get();
     }