From 619b3f4b7f7a98c3026efbacf7ede8e08fbad1a5 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Tue, 11 Feb 2025 09:15:19 -0500 Subject: [PATCH] manipulator pivot on controller pid --- .../constants/ManipulatorPivotConstants.java | 9 +--- .../robot/subsystems/ManipulatorPivot.java | 54 +++++++------------ .../java/frc/robot/subsystems/Vision.java | 13 +++++ 3 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index b269e2b..1d98b37 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/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) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index dafa938..46d8ca3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/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(); } } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 8a70ac9..8d8b1b3 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/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(); }