refactor(superstructure): Improve superstructure.

Gongoliers · Apr 9, 2024 · d6d1d9b · d6d1d9b
1 parent 368b47d
commit d6d1d9b
Show file tree

Hide file tree

Showing 14 changed files with 254 additions and 185 deletions.
diff --git a/simgui.json b/simgui.json
@@ -63,8 +63,7 @@
           },
           "Velocities": {
             "open": true
-          },
-          "open": true
+          }
         },
         "Climber": {
           "East": {

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -1,16 +1,12 @@
 package frc.robot.arm;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.PositionControllerIO;
 import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
 import frc.robot.RobotConstants;
 import frc.robot.arm.ArmConstants.ShoulderConstants;
-import frc.robot.superstructure.SuperstructureConstants;
-import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
@@ -25,17 +21,17 @@ public class Arm extends Subsystem {
   private final PositionControllerIOValues shoulderValues;
 
   /** Arm goal. */
-  private State setpoint, goal;
+  private ArmState setpoint, goal;
 
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
     shoulder = ArmFactory.createShoulder();
     shoulderValues = new PositionControllerIOValues();
     shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS);
 
-    setPosition(ShoulderAngleConstants.INITIAL.getRotations());
-    setpoint = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0);
-    goal = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0);
+    setPosition(new ArmState(ShoulderConstants.INITIAL_ANGLE));
+    setpoint = new ArmState(ShoulderConstants.INITIAL_ANGLE);
+    goal = new ArmState(ShoulderConstants.INITIAL_ANGLE);
   }
 
   /**
@@ -55,36 +51,33 @@ public static Arm getInstance() {
   public void periodic() {
     shoulder.update(shoulderValues);
 
-    setpoint = ShoulderAngleConstants.MOTION_PROFILE.calculate(
+    setpoint = new ArmState(ShoulderConstants.MOTION_PROFILE.calculate(
         RobotConstants.PERIODIC_DURATION,
-        setpoint,
-        goal);
+        setpoint.shoulderRotations(),
+        goal.shoulderRotations()));
 
-    shoulder.setSetpoint(setpoint.position, setpoint.velocity);
+    shoulder.setSetpoint(setpoint.shoulderRotations().position, setpoint.shoulderRotations().velocity);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
     PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues);
   }
 
-  public State getState() {
-    return new TrapezoidProfile.State(
-        shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond);
+  public ArmState getState() {
+    return new ArmState(new TrapezoidProfile.State(
+        shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond));
   }
 
-  public void setGoal(State goal) {
+  public void setGoal(ArmState goal) {
     this.goal = goal;
   }
 
   public boolean atGoal() {
-    return MathUtil.isNear(
-        getState().position,
-        goal.position,
-        SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations());
+    return getState().at(goal);
   }
 
-  public void setPosition(double positionRotations) {
-    shoulder.setPosition(positionRotations);
+  public void setPosition(ArmState armState) {
+    shoulder.setPosition(armState.shoulderRotations().position);
   }
 }
diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java
@@ -1,13 +1,15 @@
 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;
+import frc.lib.MotionProfileCalculator;
 import frc.lib.PIDFConstants;
 import frc.lib.controller.PositionControllerIO.PositionControllerIOConstants;
 
 /** Constants for the arm subsystem. */
 public class ArmConstants {
-
     /** Constants for the shoulder. */
     public static class ShoulderConstants {
         /** Shoulder's leader CAN. */
@@ -36,6 +38,32 @@ public static class ShoulderConstants {
             CONTROLLER_CONSTANTS.sensorToMechanismRatio = 39.771428571;
             CONTROLLER_CONSTANTS.absoluteEncoderOffsetRotations = Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07);
         }
-    }
 
+        public static final Rotation2d INITIAL_ANGLE = Rotation2d.fromDegrees(-26.45);
+
+        public static final Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(-26.45);
+
+        public static final Rotation2d SHOOT_ANGLE = Rotation2d.fromDegrees(-15);
+
+        public static final Rotation2d EJECT_ANGLE = Rotation2d.fromDegrees(0);
+
+        public static final Rotation2d AMP_ANGLE = Rotation2d.fromDegrees(90);
+
+        /** Tolerance of the shoulder. */
+        public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0);
+
+        /** Maximum speed of the shoulder in rotations per second. */
+        public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0);
+
+        /** Maximum acceleration of the shoulder in rotations per second per second. */
+        public static final double MAXIMUM_ACCELERATION =
+            MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1);
+
+        /** Maximum speed and acceleration of the shoulder . */
+        public static final TrapezoidProfile.Constraints CONSTRAINTS =
+            new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION);
+
+        /** Motion profile of the shoulder. */
+        public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
+    }
 }
diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java
@@ -0,0 +1,27 @@
+package frc.robot.arm;
+
+import java.util.Objects;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import frc.robot.arm.ArmConstants.ShoulderConstants;
+
+public record ArmState(State shoulderRotations) {
+
+    public ArmState {
+        Objects.requireNonNull(shoulderRotations);
+    }
+
+    public ArmState(Rotation2d shoulder) {
+        this(new State(shoulder.getRotations(), 0));
+    }
+
+  public boolean at(ArmState other) {
+    return MathUtil.isNear(
+        shoulderRotations.position,
+        other.shoulderRotations.position,
+        ShoulderConstants.TOLERANCE.getRotations());
+  }
+
+}
diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java
@@ -1,13 +1,11 @@
 package frc.robot.intake;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.VelocityControllerIO;
 import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
 import frc.robot.intake.IntakeConstants.BackRollerConstants;
 import frc.robot.intake.IntakeConstants.FrontRollerConstants;
-import frc.robot.intake.IntakeConstants.RollerConstants;
 
 /** Subsystem class for the intake subsystem. */
 public class Intake extends Subsystem {
@@ -21,7 +19,7 @@ public class Intake extends Subsystem {
   /** Roller values. */
   private final VelocityControllerIOValues frontRollerValues, backRollerValues;
 
-  private double rollerGoal;
+  private IntakeState setpoint, goal;
 
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
@@ -32,6 +30,9 @@ private Intake() {
     backRoller = IntakeFactory.createBackRoller();
     backRollerValues = new VelocityControllerIOValues();
     backRoller.configure(BackRollerConstants.CONTROLLER_CONSTANTS);
+
+    setpoint = new IntakeState(0, 0);
+    goal = new IntakeState(0, 0);
   }
 
   /**
@@ -52,8 +53,10 @@ public void periodic() {
     frontRoller.update(frontRollerValues);
     backRoller.update(backRollerValues);
 
-    frontRoller.setSetpoint(rollerGoal);
-    backRoller.setSetpoint(rollerGoal);
+    setpoint = goal;
+
+    frontRoller.setSetpoint(setpoint.frontRollerVelocityRotationsPerSecond());
+    backRoller.setSetpoint(setpoint.backRollerVelocityRotationsPerSecond());
   }
 
   @Override
@@ -62,22 +65,15 @@ public void addToShuffleboard(ShuffleboardTab tab) {
     VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues);
   }
 
-  public double getRollerVelocity() {
-    return (frontRollerValues.velocityRotationsPerSecond + backRollerValues.velocityRotationsPerSecond) / 2;
+  public IntakeState getState() {
+    return new IntakeState(frontRollerValues.velocityRotationsPerSecond, backRollerValues.velocityRotationsPerSecond);
   }
 
-  public void setGoal(double rollerVelocityRotationsPerSecond) {
-    this.rollerGoal = rollerVelocityRotationsPerSecond;
+  public void setGoal(IntakeState goal) {
+    this.goal = goal;
   }
 
   public boolean atGoal() {
-    return MathUtil.isNear(
-        frontRollerValues.velocityRotationsPerSecond,
-        rollerGoal,
-        RollerConstants.SPEED_TOLERANCE)
-    && MathUtil.isNear(
-        backRollerValues.velocityRotationsPerSecond,
-        rollerGoal,
-        RollerConstants.SPEED_TOLERANCE);
+    return getState().at(goal);
   }
 }
diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java
@@ -0,0 +1,26 @@
+package frc.robot.intake;
+
+import java.util.Objects;
+
+import edu.wpi.first.math.MathUtil;
+import frc.robot.intake.IntakeConstants.RollerConstants;
+
+public record IntakeState(double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) {
+
+    public IntakeState {
+        Objects.requireNonNull(frontRollerVelocityRotationsPerSecond);
+        Objects.requireNonNull(backRollerVelocityRotationsPerSecond);
+    }
+
+  public boolean at(IntakeState other) {
+    return MathUtil.isNear(
+        frontRollerVelocityRotationsPerSecond,
+        other.frontRollerVelocityRotationsPerSecond,
+        RollerConstants.SPEED_TOLERANCE)
+    && MathUtil.isNear(
+        backRollerVelocityRotationsPerSecond,
+        other.backRollerVelocityRotationsPerSecond,
+        RollerConstants.SPEED_TOLERANCE);
+  }
+
+}
diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java
@@ -1,6 +1,5 @@
 package frc.robot.shooter;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.VelocityControllerIO;
@@ -26,7 +25,7 @@ public class Shooter extends Subsystem {
   /** Flywheel values. */
   private final VelocityControllerIOValues flywheelValues;
 
-  private double serializerGoal, flywheelGoal;
+  private ShooterState setpoint, goal;
 
   /** Creates a new instance of the shooter subsystem. */
   private Shooter() {
@@ -37,6 +36,9 @@ private Shooter() {
     flywheel = ShooterFactory.createFlywheel();
     flywheelValues = new VelocityControllerIOValues();
     flywheel.configure(FlywheelConstants.CONTROLLER_CONSTANTS);
+
+    setpoint = new ShooterState(0, 0);
+    goal = new ShooterState(0, 0);
   }
 
   /**
@@ -57,8 +59,10 @@ public void periodic() {
     serializer.update(serializerValues);
     flywheel.update(flywheelValues);
 
-    flywheel.setSetpoint(flywheelGoal);
-    serializer.setSetpoint(serializerGoal);
+    setpoint = goal;
+
+    flywheel.setSetpoint(setpoint.flywheelVelocityRotationsPerSecond());
+    serializer.setSetpoint(setpoint.serializerVelocityRotationsPerSecond());
   }
 
   @Override
@@ -67,28 +71,16 @@ public void addToShuffleboard(ShuffleboardTab tab) {
     VelocityControllerIO.addToShuffleboard(tab, "Flywheel", flywheelValues);
   }
 
-  public double getFlywheelVelocity() {
-    return flywheelValues.velocityRotationsPerSecond;
+  public ShooterState getState() {
+    return new ShooterState(flywheelValues.velocityRotationsPerSecond, serializerValues.velocityRotationsPerSecond);
   }
 
-  public double getSerializerVelocity() {
-    return serializerValues.velocityRotationsPerSecond;
-  }
-
-  public void setGoal(
-      double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
-        this.flywheelGoal = flywheelVelocityRotationsPerSecond;
-        this.serializerGoal = serializerVelocityRotationsPerSecond;
+  public void setGoal(ShooterState goal)  {
+    this.goal = goal;
   }
 
   public boolean atGoal() {
-    return MathUtil.isNear(
-        getFlywheelVelocity(),
-        flywheelGoal,
-        FlywheelConstants.SPEED_TOLERANCE) && 
-    MathUtil.isNear(
-        getSerializerVelocity(),
-        serializerGoal,
-        FlywheelConstants.SPEED_TOLERANCE);
+    return getState().at(goal);
   }
+
 }
diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java
@@ -0,0 +1,20 @@
+package frc.robot.shooter;
+
+import java.util.Objects;
+
+import edu.wpi.first.math.MathUtil;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
+
+public record ShooterState(double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
+
+    public ShooterState {
+        Objects.requireNonNull(flywheelVelocityRotationsPerSecond);
+        Objects.requireNonNull(serializerVelocityRotationsPerSecond);
+    }
+
+    public boolean at(ShooterState other) {
+        return MathUtil.isNear(flywheelVelocityRotationsPerSecond, other.flywheelVelocityRotationsPerSecond, FlywheelConstants.SPEED_TOLERANCE) && MathUtil.isNear(serializerVelocityRotationsPerSecond, other.serializerVelocityRotationsPerSecond, SerializerConstants.SPEED_TOLERANCE);
+    }
+
+}