Merge branch '4_7_2024_refactor' of https://github.com/Gongoliers/Rob…

…ot2024 into 4_7_2024_refactor

src/main/java/frc/lib/controller/PositionControllerIO.java

@@ -14,7 +14,13 @@ public static class PositionControllerIOConstants {
      * Interpret counterclockwise rotation on the motor face as having positive velocity, if set to
      * true.
      */
-    public boolean ccwPositive = true;
+    public boolean ccwPositiveMotor = true;
+
+    /**
+     * Interpret counterclockwise rotation on the encoder as having positive velocity, if set to
+     * true.
+     */
+    public boolean ccwPositiveAbsoluteEncoder = true;
 
     /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */
     public boolean neutralBrake = false;

src/main/java/frc/lib/controller/PositionControllerIOTalonFX2.java

@@ -83,7 +83,7 @@ public void configure(PositionControllerIOConstants constants) {
     TalonFXConfiguration motorConfig = new TalonFXConfiguration();
 
     motorConfig.MotorOutput.Inverted =
-        constants.ccwPositive
+        constants.ccwPositiveMotor
             ? InvertedValue.CounterClockwise_Positive
             : InvertedValue.Clockwise_Positive;
     motorConfig.MotorOutput.NeutralMode =
@@ -110,7 +110,7 @@ public void configure(PositionControllerIOConstants constants) {
 
     encoderConfig.MagnetSensor.MagnetOffset = constants.absoluteEncoderOffsetRotations;
     encoderConfig.MagnetSensor.SensorDirection =
-        constants.ccwPositive
+        constants.ccwPositiveAbsoluteEncoder
             ? SensorDirectionValue.CounterClockwise_Positive
             : SensorDirectionValue.Clockwise_Positive;
 

src/main/java/frc/lib/controller/VelocityControllerIO.java

@@ -57,6 +57,16 @@ public static void addToShuffleboard(
     velocityController.addDouble(
         "Acceleration (dpsps)",
         () -> Units.rotationsToDegrees(values.accelerationRotationsPerSecondPerSecond));
+    velocityController.addDouble(
+        "Velocity (rps)", () -> values.velocityRotationsPerSecond);
+    velocityController.addDouble(
+        "Acceleration (rpsps)",
+        () -> values.accelerationRotationsPerSecondPerSecond);
+    velocityController.addDouble(
+        "Velocity (rpm)", () -> values.velocityRotationsPerSecond * 60);
+    velocityController.addDouble(
+        "Acceleration (rpmpm)",
+        () -> values.accelerationRotationsPerSecondPerSecond * 60);
     velocityController.addDouble("Voltage (V)", () -> values.motorVolts);
     velocityController.addDouble("Current (A)", () -> values.motorAmps);
   }

src/main/java/frc/robot/RobotConstants.java

@@ -47,5 +47,5 @@ public enum Subsystem {
       EnumSet.of(
           Subsystem.ARM, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE);
 
-  public static final Set<Subsystem> REAL_SUBSYSTEMS = ALL_SUBSYSTEMS;
+  public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.INTAKE);
 }

src/main/java/frc/robot/arm/ArmConstants.java

@@ -1,5 +1,6 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
@@ -25,25 +26,25 @@ public static class ShoulderConstants {
 
     static {
       PIDF.kS = 0.14;
-      PIDF.kG = 0.45;
+      PIDF.kG = 0.18;
       PIDF.kV = 4.0;
-      PIDF.kP = 20.0;
+      PIDF.kP = 8.0;
     }
 
     /** Shoulder's controller constants. */
     public static final PositionControllerIOConstants CONTROLLER_CONSTANTS =
         new PositionControllerIOConstants();
 
     static {
-      CONTROLLER_CONSTANTS.ccwPositive = false;
+      CONTROLLER_CONSTANTS.ccwPositiveMotor = true;
+      CONTROLLER_CONSTANTS.ccwPositiveAbsoluteEncoder = false;
       CONTROLLER_CONSTANTS.neutralBrake = true;
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571;
-      CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations =
-          Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07);
+      CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-173.135);
     }
 
     /** Maximum speed of the shoulder in rotations per second. */
-    public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0);
+    public static final double MAXIMUM_SPEED = Units.degreesToRotations(120.0);
 
     /** Maximum acceleration of the shoulder in rotations per second per second. */
     public static final double MAXIMUM_ACCELERATION =
@@ -55,5 +56,17 @@ public static class ShoulderConstants {
 
     /** Motion profile of the shoulder. */
     public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
+
+    public static final Rotation2d STOW = Rotation2d.fromDegrees(-25);
+
+    public static final Rotation2d SPEAKER = Rotation2d.fromDegrees(-15);
+
+    public static final Rotation2d PASS = Rotation2d.fromDegrees(0);
+
+    public static final Rotation2d EJECT = Rotation2d.fromDegrees(0);
+
+    public static final Rotation2d AMP = Rotation2d.fromDegrees(80);
+
+    public static final Rotation2d CLIMB = Rotation2d.fromDegrees(60);
   }
 }

src/main/java/frc/robot/arm/ArmState.java

@@ -4,23 +4,25 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
+import frc.robot.arm.ArmConstants.ShoulderConstants;
+
 import java.util.Objects;
 
 public record ArmState(State shoulderRotations) {
 
-  public static final ArmState INITIAL = new ArmState(Rotation2d.fromDegrees(-26.45));
+  public static final ArmState INITIAL = new ArmState(ShoulderConstants.STOW);
 
-  public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-26.45));
+  public static final ArmState STOW = new ArmState(ShoulderConstants.STOW);
 
-  public static final ArmState SPEAKER = new ArmState(Rotation2d.fromDegrees(-15));
+  public static final ArmState SPEAKER = new ArmState(ShoulderConstants.SPEAKER);
 
-  public static final ArmState PASS = new ArmState(Rotation2d.fromDegrees(0));
+  public static final ArmState PASS = new ArmState(ShoulderConstants.PASS);
 
-  public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(0));
+  public static final ArmState EJECT = new ArmState(ShoulderConstants.EJECT);
 
-  public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(90));
+  public static final ArmState AMP = new ArmState(ShoulderConstants.AMP);
 
-  public static final ArmState CLIMB = new ArmState(Rotation2d.fromDegrees(90));
+  public static final ArmState CLIMB = new ArmState(ShoulderConstants.CLIMB);
 
   public ArmState {
     Objects.requireNonNull(shoulderRotations);

src/main/java/frc/robot/intake/IntakeConstants.java

@@ -17,6 +17,7 @@ public static class FrontRollerConstants {
     static {
       PIDF.kS = 0.13;
       PIDF.kV = 0.1683;
+      PIDF.kP = 0.1;
     }
 
     /** Front roller's controller constants. */
@@ -25,22 +26,28 @@ public static class FrontRollerConstants {
 
     static {
       CONTROLLER_CONSTANTS.ccwPositive = false;
-      CONTROLLER_CONSTANTS.neutralBrake = true;
+      CONTROLLER_CONSTANTS.neutralBrake = false;
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 24.0 / 16.0;
     }
+
+    public static final double INTAKE_SPEED = 34;
+
+    /** Maximum speed of the roller in rotations per second. */
+    public static final double MAXIMUM_SPEED = 67;
   }
 
   /** Constants for the back roller. */
   public static class BackRollerConstants {
     /** Back roller's CAN. */
-    public static final CAN CAN = new CAN(50);
+    public static final CAN CAN = new CAN(40);
 
     /** Back roller's PIDF constants. */
     public static final PIDFConstants PIDF = new PIDFConstants();
 
     static {
       PIDF.kS = 0.13;
       PIDF.kV = 0.1759;
+      PIDF.kP = 0.1;
     }
 
     /** Back roller's controller constants. */
@@ -49,13 +56,12 @@ public static class BackRollerConstants {
 
     static {
       CONTROLLER_CONSTANTS.ccwPositive = false;
-      CONTROLLER_CONSTANTS.neutralBrake = true;
+      CONTROLLER_CONSTANTS.neutralBrake = false;
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 24.0 / 16.0;
     }
-  }
 
-  /** Constants for the roller motor. */
-  public static class RollerConstants {
+    public static final double INTAKE_SPEED = 34;
+
     /** Maximum speed of the roller in rotations per second. */
     public static final double MAXIMUM_SPEED = 67;
   }

src/main/java/frc/robot/intake/IntakeState.java

@@ -1,22 +1,25 @@
 package frc.robot.intake;
 
 import edu.wpi.first.math.MathUtil;
+import frc.robot.intake.IntakeConstants.BackRollerConstants;
+import frc.robot.intake.IntakeConstants.FrontRollerConstants;
+
 import java.util.Objects;
 
 public record IntakeState(
     double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) {
 
   public static final IntakeState IDLE = new IntakeState(0, 0);
 
-  public static final IntakeState INTAKE = new IntakeState(34, 34);
+  public static final IntakeState INTAKE = new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED);
 
   public IntakeState {
     Objects.requireNonNull(frontRollerVelocityRotationsPerSecond);
     Objects.requireNonNull(backRollerVelocityRotationsPerSecond);
   }
 
   public boolean at(IntakeState other) {
-    final double kToleranceRotationsPerSecond = 2.5;
+    final double kToleranceRotationsPerSecond = 1;
 
     return MathUtil.isNear(
             frontRollerVelocityRotationsPerSecond,

src/main/java/frc/robot/shooter/ShooterConstants.java

@@ -30,6 +30,14 @@ public static class SerializerConstants {
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 36.0 / 16.0;
     }
 
+    public static final double INTAKE_SPEED = 34;
+
+    public static final double PULL_SPEED = -20;
+
+    public static final double EJECT_SPEED = -44;
+
+    public static final double FEED_SPEED = 20;
+
     /** Maximum speed in rotations per second. */
     public static final double MAXIMUM_SPEED = 45.319;
   }
@@ -57,6 +65,12 @@ public static class FlywheelConstants {
       CONTROLLER_CONSTANTS.sensorToMechanismRatio = 28.0 / 16.0;
     }
 
+    public static final double SPEAKER_SPEED = 44;
+
+    public static final double PASS_SPEED = 44;
+
+    public static final double AMP_SPEED = 20;
+
     /** Maximum speed in rotations per second. */
     public static final double MAXIMUM_SPEED = 46.711;
   }

src/main/java/frc/robot/shooter/ShooterState.java

@@ -1,28 +1,31 @@
 package frc.robot.shooter;
 
 import edu.wpi.first.math.MathUtil;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
+
 import java.util.Objects;
 
 public record ShooterState(
     double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
 
   public static final ShooterState IDLE = new ShooterState(0, 0);
 
-  public static final ShooterState INTAKE = new ShooterState(0, 34);
+  public static final ShooterState INTAKE = new ShooterState(0, SerializerConstants.INTAKE_SPEED);
 
-  public static final ShooterState PULL = new ShooterState(0, -20);
+  public static final ShooterState PULL = new ShooterState(0, SerializerConstants.PULL_SPEED);
 
-  public static final ShooterState EJECT = new ShooterState(0, -44);
+  public static final ShooterState EJECT = new ShooterState(0, SerializerConstants.EJECT_SPEED);
 
-  public static final ShooterState SPEAKER_READY = new ShooterState(44, 0);
+  public static final ShooterState SPEAKER_READY = new ShooterState(FlywheelConstants.SPEAKER_SPEED, 0);
 
-  public static final ShooterState SPEAKER_SHOOT = new ShooterState(44, 20);
+  public static final ShooterState SPEAKER_SHOOT = new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FEED_SPEED);
 
-  public static final ShooterState PASS_READY = new ShooterState(44, 0);
+  public static final ShooterState PASS_READY = new ShooterState(FlywheelConstants.PASS_SPEED, 0);
 
-  public static final ShooterState PASS_SHOOT = new ShooterState(44, 20);
+  public static final ShooterState PASS_SHOOT = new ShooterState(FlywheelConstants.PASS_SPEED, SerializerConstants.FEED_SPEED);
 
-  public static final ShooterState AMP_SHOOT = new ShooterState(20, 20);
+  public static final ShooterState AMP_SHOOT = new ShooterState(FlywheelConstants.AMP_SPEED, SerializerConstants.FEED_SPEED);
 
   public ShooterState {
     Objects.requireNonNull(flywheelVelocityRotationsPerSecond);

src/main/java/frc/robot/superstructure/Superstructure.java

@@ -65,8 +65,12 @@ public void addToShuffleboard(ShuffleboardTab tab) {
     ShuffleboardLayout state = Telemetry.addColumn(tab, "State");
 
     state.addString(
-        "Running Command",
+        "State",
         () -> this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "NONE");
+
+    ShuffleboardLayout at = Telemetry.addColumn(tab, "At Goal?");
+
+    at.addBoolean("At Goal?", () -> at(goal));
   }
 
   private void addStateToShuffleboard(

src/main/java/frc/robot/superstructure/SuperstructureMechanism.java

@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.lib.InterpolatableColor;
 import frc.robot.RobotConstants;
-import frc.robot.intake.IntakeConstants.RollerConstants;
+import frc.robot.intake.IntakeConstants.FrontRollerConstants;
 import frc.robot.shooter.ShooterConstants.FlywheelConstants;
 import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
@@ -161,6 +161,6 @@ public void updateSuperstructure(SuperstructureState state) {
     rollers.setColor(
         new Color8Bit(
             ROLLERS_COLOR.sample(
-                Math.abs(averageRollerVelocity), 0, RollerConstants.MAXIMUM_SPEED)));
+                Math.abs(averageRollerVelocity), 0, FrontRollerConstants.MAXIMUM_SPEED)));
   }
 }