working on algae wrist controller

TripleHelixProgramming · Feb 19, 2025 · eab2c55 · eab2c55
1 parent ed4a4fd
commit eab2c55
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -293,7 +293,7 @@ public static final class ClimberConstants {
 
   public static final class LedConstants {
     public static final int kLedPort = 0;
-    public static final int kLedBufferLength = 17;
+    public static final int kLedBufferLength = 40;
 
     public static final int kLEDsPerBlock = 2;
     public static final int kLEDsBetweenBlocks = 1;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -217,12 +217,12 @@ private void configureOperatorButtonBindings() {
     Trigger algaeMode = operator.leftBumper();
 
     // Test coral wrist motion
-    operator.back().onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode));
-    operator.start().onTrue(coralWrist.createSetAngleCommand(CoralWristState.L4));
+    // operator.back().onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode));
+    // operator.start().onTrue(coralWrist.createSetAngleCommand(CoralWristState.L4));
 
     // Test algae wrist motion
-    // operator.back().onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Floor));
-    // operator.start().onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
+    operator.back().onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Floor));
+    operator.start().onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
 
     // Test algae roller motion
     // operator.back().whileTrue(algaeRoller.createIntakeCommand());

diff --git a/src/main/java/frc/robot/elevator/AlgaeWrist.java b/src/main/java/frc/robot/elevator/AlgaeWrist.java
@@ -28,7 +28,7 @@ public class AlgaeWrist extends SubsystemBase {
   // TODO: Add ArmFeedforward
   // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/ArmFeedforward.html
   private final ProfiledPIDController controller =
-      new ProfiledPIDController(AlgaeWristConstants.kP, 0.0, 0.0, AlgaeWristConstants.kConstraints);
+      new ProfiledPIDController(AlgaeWristConstants.kP, AlgaeWristConstants.kI, 0.0, AlgaeWristConstants.kConstraints);
 
   private final EventLoop loop = new EventLoop();
   private AlgaeWristState targetState = AlgaeWristState.Unknown;
@@ -56,7 +56,7 @@ public AlgaeWrist() {
     motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
 
     controller.setTolerance(AlgaeWristConstants.kAllowableAngleError.in(Degrees));
-    // controller.setIZone();
+    controller.setIZone(AlgaeWristConstants.kIZone.in(Degrees));
     // controller.setIntegratorRange();
 
     // setDefaultCommand(createRemainAtCurrentAngleCommand());

diff --git a/src/main/java/frc/robot/elevator/CoralWrist.java b/src/main/java/frc/robot/elevator/CoralWrist.java
@@ -14,6 +14,7 @@
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -37,6 +38,7 @@ public class CoralWrist extends SubsystemBase {
 
   private final EventLoop loop = new EventLoop();
   private CoralWristState targetState = CoralWristState.Unknown;
+  private Angle targetAngle = targetState.angle;
 
   public CoralWrist() {
     // spotless:off
@@ -52,9 +54,6 @@ public CoralWrist() {
         .d(0.0)
         // .izone(),
         // .outputRange(-1, 1)
-        .positionWrappingEnabled(true)
-        .positionWrappingMinInput(0.0)
-        .positionWrappingMaxInput(204.0)
         .feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
 
     config.absoluteEncoder
@@ -86,6 +85,7 @@ public void periodic() {
     SmartDashboard.putNumber("Coral Wrist Angle", encoder.getPosition());
     SmartDashboard.putString("Coral Wrist Target State", getTargetState().name());
     // SmartDashboard.putNumber("Coral Wrist Target Angle", controller.getGoal().position);
+    SmartDashboard.putNumber("Coral Wrist Target Angle", targetAngle.in(Degrees));
     SmartDashboard.putNumber("Coral Wrist Applied Duty Cycle", motor.getAppliedOutput());
     SmartDashboard.putNumber("Coral Wrist Current", motor.getOutputCurrent());
   }
@@ -107,6 +107,7 @@ public Command createSetAngleCommand(CoralWristState state) {
         // initialize
         () -> {
           targetState = state;
+          targetAngle = state.angle;
           // controller.setGoal(targetState.angle.in(Degrees));
           controller.setReference(targetState.angle.in(Degrees), ControlType.kPosition);
         },

diff --git a/src/main/java/frc/robot/elevator/ElevatorConstants.java b/src/main/java/frc/robot/elevator/ElevatorConstants.java
@@ -159,9 +159,12 @@ public static final class AlgaeWristConstants {
     public static final double kPositionConversionFactor = 360.0;
     public static final double kPositionOffset = 322.0 / kPositionConversionFactor;
 
-    public static final double kP = 1.0;
-    private static final AngularVelocity kMaxVelocity = DegreesPerSecond.of(30.0);
-    public static final Time kTimeToMaxVelocity = Seconds.of(0.02);
+    public static final double kP = 0.001;
+    public static final double kI = 0.01;
+    public static final Angle kIZone = Degrees.of(120.0);
+
+    private static final AngularVelocity kMaxVelocity = DegreesPerSecond.of(150.0);
+    public static final Time kTimeToMaxVelocity = Seconds.of(0.5);
     public static final AngularAcceleration kMaxAcceleration = kMaxVelocity.div(kTimeToMaxVelocity);
     public static final Constraints kConstraints =
         new Constraints(