diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a92fc06..a8b7986 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,11 +31,11 @@ public class RobotContainer { private void configureBindings() { driver.x().onTrue( - closedLoopTest.incrementPosition(-360) + closedLoopTest.setPosition(360) ); driver.b().onTrue( - closedLoopTest.incrementPosition(360) + closedLoopTest.setPosition(0) ); driver.y().onTrue( diff --git a/src/main/java/frc/robot/constants/ClosedLoopTestConstants.java b/src/main/java/frc/robot/constants/ClosedLoopTestConstants.java index 6563892..5de065f 100644 --- a/src/main/java/frc/robot/constants/ClosedLoopTestConstants.java +++ b/src/main/java/frc/robot/constants/ClosedLoopTestConstants.java @@ -1,15 +1,21 @@ package frc.robot.constants; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.MAXMotionConfig.MAXMotionPositionMode; public class ClosedLoopTestConstants { public static final double kPositionConversionFactor = 360.0/25.0; public static final double kVelocityConversionFactor = kPositionConversionFactor / 60.0; - public static final double kP = 0.001; - public static final double kI = 0; - public static final double kD = 0; - public static final double kF = 0; + public static final double kVeloP = 0.001; + public static final double kVeloI = 0; + public static final double kVeloD = 0; + public static final double kVeloF = 0; + + public static final double kPosP = 0.001; + public static final double kPosI = 0; + public static final double kPosD = 0; public static final double kAllowedClosedLoopError = 4; public static final double kMaxAcceleration = 180; @@ -23,14 +29,20 @@ public class ClosedLoopTestConstants { .velocityConversionFactor(kVelocityConversionFactor); motorConfig.closedLoop .pidf( - kP, - kI, - kD, - kF + kVeloP, + kVeloI, + kVeloD, + kVeloF, + ClosedLoopSlot.kSlot0 + ).pid( + kPosP, + kPosI, + kPosD, + ClosedLoopSlot.kSlot1 ); motorConfig.closedLoop.maxMotion .allowedClosedLoopError(kAllowedClosedLoopError) .maxAcceleration(kMaxAcceleration) - .maxVelocity(kMaxVelocity); + .maxVelocity(kMaxVelocity).positionMode(MAXMotionPositionMode.kMAXMotionTrapezoidal); } } diff --git a/src/main/java/frc/robot/subsystems/ClosedLoopTest.java b/src/main/java/frc/robot/subsystems/ClosedLoopTest.java index ac5a06d..e7cbec7 100644 --- a/src/main/java/frc/robot/subsystems/ClosedLoopTest.java +++ b/src/main/java/frc/robot/subsystems/ClosedLoopTest.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -37,10 +38,17 @@ public class ClosedLoopTest extends SubsystemBase { } public Command incrementPosition(double increment) { + return runOnce(() -> { + setPosition(encoder.getPosition() + increment); + }); + } + + public Command setPosition(double position) { return runOnce(() -> { controller.setReference( - encoder.getPosition() + increment, - ControlType.kMAXMotionPositionControl + position, + ControlType.kMAXMotionPositionControl, + ClosedLoopSlot.kSlot0 ); }); } @@ -49,7 +57,8 @@ public class ClosedLoopTest extends SubsystemBase { return runOnce(() -> { controller.setReference( velocity, - ControlType.kMAXMotionVelocityControl + ControlType.kMAXMotionVelocityControl, + ClosedLoopSlot.kSlot0 ); }); }