Merge pull request #35 from Gongoliers/3_18_2024_superstructure_state

3 18 2024 superstructure state

Author: haydenheroux

simgui-ds.json

@@ -98,11 +98,10 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     },
     {
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }

simgui.json

@@ -77,8 +77,7 @@
           },
           "Velocity": {
             "open": true
-          },
-          "open": true
+          }
         },
         "Shooter": {
           "Flywheel": {
@@ -88,6 +87,18 @@
             "open": true
           }
         },
+        "Superstructure": {
+          "Goal": {
+            "open": true
+          },
+          "Measurement": {
+            "open": true
+          },
+          "open": true,
+          "setpoint": {
+            "open": true
+          }
+        },
         "Swerve": {
           "Constants": {
             "open": true

src/main/deploy/pathplanner/autos/amp.full.auto

@@ -18,9 +18,22 @@
           }
         },
         {
-          "type": "named",
+          "type": "deadline",
           "data": {
-            "name": "shootNote"
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shootNote"
+                }
+              }
+            ]
           }
         },
         {

src/main/deploy/pathplanner/autos/amp.shootLeave.auto

@@ -18,8 +18,6 @@ public void robotInit() {
     robotContainer = RobotContainer.getInstance();
     swerve = Swerve.getInstance();
 
-    // new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home());
-
     new Trigger(this::isDisabled)
         .debounce(RobotConstants.DISABLE_COAST_DELAY)
         .onTrue(Commands.runOnce(() -> swerve.setBrake(false), swerve).ignoringDisable(true));

src/main/java/frc/robot/Robot.java

@@ -10,6 +10,7 @@
 import frc.robot.intake.Intake;
 import frc.robot.odometry.Odometry;
 import frc.robot.shooter.Shooter;
+import frc.robot.superstructure.Superstructure;
 import frc.robot.swerve.Swerve;
 
 /** Initializes subsystems and commands. */
@@ -24,6 +25,7 @@ public class RobotContainer {
   private final Intake intake;
   private final Odometry odometry;
   private final Shooter shooter;
+  private final Superstructure superstructure;
   private final Swerve swerve;
 
   private final CommandXboxController driverController, operatorController;
@@ -36,6 +38,7 @@ private RobotContainer() {
     intake = Intake.getInstance();
     odometry = Odometry.getInstance();
     shooter = Shooter.getInstance();
+    superstructure = Superstructure.getInstance();
     swerve = Swerve.getInstance();
 
     driverController = new CommandXboxController(0);
@@ -61,31 +64,26 @@ public static RobotContainer getInstance() {
 
   /** Initializes subsystem telemetry. */
   private void initializeTelemetry() {
-    Telemetry.initializeTabs(arm, auto, climber, intake, odometry, shooter, swerve);
+    Telemetry.initializeTabs(arm, auto, climber, intake, odometry, shooter, superstructure, swerve);
     SmartDashboard.putData("Mechanism", RobotMechanisms.getInstance().getMechanism());
   }
 
   /** Configures subsystem default commands. */
   private void configureDefaultCommands() {
-    arm.setDefaultCommand(arm.stow());
-    intake.setDefaultCommand(intake.stow());
     swerve.setDefaultCommand(swerve.driveWithController(driverController));
   }
 
   /** Configures controller bindings. */
   private void configureBindings() {
-    driverController.a().whileTrue(swerve.forwards());
-    driverController.b().whileTrue(swerve.sideways());
-    driverController.x().whileTrue(swerve.cross());
     driverController.y().onTrue(odometry.tare());
 
-    operatorController.leftTrigger().whileTrue(auto.intakePosition().andThen(auto.intakeNote()));
-    operatorController.leftBumper().whileTrue(intake.unstow().andThen(intake.outtake()));
+    operatorController.leftTrigger().onTrue(superstructure.intake());
+    operatorController.rightTrigger().onTrue(superstructure.shoot());
 
-    operatorController.rightTrigger().whileTrue(auto.shootPosition().andThen(shooter.spin()));
-    operatorController.rightBumper().whileTrue(shooter.serialize());
-
-    operatorController.a().whileTrue(arm.amp());
+    operatorController.a().onTrue(superstructure.amp());
+    operatorController.b().onTrue(superstructure.intake());
+    operatorController.x().onTrue(superstructure.stow());
+    operatorController.y().onTrue(superstructure.shoot());
   }
 
   /**

src/main/java/frc/robot/RobotContainer.java

@@ -10,11 +10,11 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.lib.InterpolatableColor;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
-import frc.robot.arm.ArmState;
 import frc.robot.intake.IntakeConstants.PivotMotorConstants;
 import frc.robot.intake.IntakeConstants.RollerMotorConstants;
 import frc.robot.shooter.ShooterConstants.FlywheelConstants;
 import frc.robot.shooter.ShooterConstants.SerializerConstants;
+import frc.robot.superstructure.SuperstructureState;
 
 /** Helper class for rendering robot mechanisms. */
 public class RobotMechanisms {
@@ -23,7 +23,7 @@ public class RobotMechanisms {
 
   private final Mechanism2d mechanism;
 
-  private MechanismLigament2d shoulder, shooter, serializer, intake;
+  private MechanismLigament2d shoulder, flywheel, serializer, pivot;
 
   private final double WIDTH =
       2
@@ -34,9 +34,12 @@ public class RobotMechanisms {
 
   private final Translation2d ORIGIN = new Translation2d(WIDTH / 2, 0);
 
+  public static final Translation2d SHOULDER_TO_ORIGIN =
+      new Translation2d(Units.inchesToMeters(-11.361), Units.inchesToMeters(7.721));
+
   private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray);
 
-  private final InterpolatableColor shooterColor =
+  private final InterpolatableColor flywheelColor =
       new InterpolatableColor(Color.kLightGray, Color.kSalmon);
   private final InterpolatableColor serializerColor =
       new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue);
@@ -47,12 +50,13 @@ private RobotMechanisms() {
     mechanism = new Mechanism2d(WIDTH, HEIGHT);
 
     initializeArmMechanism();
-    initializeFramePerimeterMechanisms();
     initializeIntakeMechanism();
+
+    initializeFramePerimeterMechanisms();
   }
 
   private void initializeArmMechanism() {
-    Translation2d armRootTranslation = ORIGIN.plus(ShoulderMotorConstants.SHOULDER_TO_ORIGIN);
+    Translation2d armRootTranslation = ORIGIN.plus(SHOULDER_TO_ORIGIN);
 
     double armThickness = Units.inchesToMeters(2) * 100;
 
@@ -67,7 +71,7 @@ private void initializeArmMechanism() {
                 0,
                 armThickness,
                 DEFAULT_COLOR));
-    shooter =
+    flywheel =
         shoulder.append(
             new MechanismLigament2d(
                 "wrist", Units.inchesToMeters(4.321), 0, armThickness, DEFAULT_COLOR));
@@ -103,7 +107,7 @@ private void initializeFramePerimeterMechanisms() {
   private void initializeIntakeMechanism() {
     double intakeThickness = Units.inchesToMeters(3) * 100;
 
-    intake =
+    pivot =
         mechanism
             .getRoot(
                 "intake",
@@ -126,9 +130,9 @@ public Mechanism2d getMechanism() {
     return mechanism;
   }
 
-  public void updateArm(ArmState state) {
-    Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulder().position);
-    Rotation2d wristRotation = Rotation2d.fromRotations(state.wrist().position);
+  public void updateSuperstructure(SuperstructureState state) {
+    Rotation2d shoulderRotation = Rotation2d.fromRotations(state.shoulderAngleRotations().position);
+    Rotation2d wristRotation = Rotation2d.fromRotations(state.wristAngleRotations().position);
 
     shoulder.setAngle(shoulderRotation);
 
@@ -138,32 +142,28 @@ public void updateArm(ArmState state) {
     Rotation2d shooterRotation = offsetWristRotation;
     Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180));
 
-    shooter.setAngle(shooterRotation);
-    serializer.setAngle(serializerRotation);
-  }
-
-  public void updateIntake(Rotation2d pivotAngle, double rollerVelocityRotationsPerSecond) {
-    Color color =
-        intakeColor.sample(
-            Math.abs(rollerVelocityRotationsPerSecond), 0, RollerMotorConstants.MAXIMUM_SPEED);
-
-    intake.setAngle(pivotAngle);
-    intake.setColor(new Color8Bit(color));
-  }
-
-  public void updateShooter(double velocityMetersPerSecond) {
-    Color color =
-        shooterColor.sample(
-            Math.abs(velocityMetersPerSecond), 0, FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED);
-
-    shooter.setColor(new Color8Bit(color));
-  }
+    pivot.setAngle(Rotation2d.fromRotations(state.pivotAngleRotations().position));
+    pivot.setColor(
+        new Color8Bit(
+            intakeColor.sample(
+                Math.abs(state.rollerVelocityRotationsPerSecond()),
+                0,
+                RollerMotorConstants.MAXIMUM_SPEED)));
 
-  public void updateSerializer(double velocityMetersPerSecond) {
-    Color color =
-        serializerColor.sample(
-            Math.abs(velocityMetersPerSecond), 0, SerializerConstants.MAXIMUM_TANGENTIAL_SPEED);
+    flywheel.setAngle(shooterRotation);
+    flywheel.setColor(
+        new Color8Bit(
+            flywheelColor.sample(
+                Math.abs(state.flywheelVelocityRotationsPerSecond()),
+                0,
+                FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED)));
 
-    serializer.setColor(new Color8Bit(color));
+    serializer.setAngle(serializerRotation);
+    serializer.setColor(
+        new Color8Bit(
+            serializerColor.sample(
+                Math.abs(state.serializerVelocityRotationsPerSecond()),
+                0,
+                SerializerConstants.MAXIMUM_TANGENTIAL_SPEED)));
   }
 }