Disable motion magic in ArmIO

Move profile in to subsystem
Mechanical-Advantage · Feb 14, 2024 · 0e9a34a · 0e9a34a
1 parent 96c5149
commit 0e9a34a
Show file tree

Hide file tree

Showing 16 changed files with 150 additions and 141 deletions.
diff --git a/build.gradle b/build.gradle
@@ -112,7 +112,7 @@ task(generateDriveTrajectories, dependsOn: "classes", type: JavaExec) {
     mainClass = "org.littletonrobotics.frc2024.subsystems.drive.trajectory.GenerateTrajectories"
     classpath = sourceSets.main.runtimeClasspath
 }
-compileJava.finalizedBy generateDriveTrajectories
+//compileJava.finalizedBy generateDriveTrajectories
 
 // Check robot type when deploying
 task(checkRobot, dependsOn: "classes", type: JavaExec) {

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
-import org.littletonrobotics.frc2024.commands.auto.AutoCommands;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
@@ -124,12 +123,19 @@ public RobotContainer() {
                   new ModuleIOSim(DriveConstants.moduleConfigs[2]),
                   new ModuleIOSim(DriveConstants.moduleConfigs[3]));
           flywheels = new Flywheels(new FlywheelsIOSim());
+
+          feeder = new Feeder(new FeederIOSim());
+          indexer = new Indexer(new IndexerIOSim());
+          intake = new Intake(new IntakeIOSim());
+          rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});
+
           arm = new Arm(new ArmIOSim());
+          superstructure = new Superstructure(arm);
         }
       }
     }
 
-    // No implementation for replay
+    // No-op implementation for replay
     if (drive == null) {
       drive =
           new Drive(
@@ -167,18 +173,15 @@ public RobotContainer() {
     }
     superstructure = new Superstructure(arm);
 
-    if (superstructure == null) {
-      superstructure = new Superstructure(arm);
-    }
     // Configure autos and buttons
     configureAutos();
     configureButtonBindings();
   }
 
   private void configureAutos() {
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
-    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
-    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
+    //    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
+    //    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
 
     // Set up feedforward characterization
     autoChooser.addOption(
@@ -202,18 +205,20 @@ private void configureAutos() {
   private void configureButtonBindings() {
     // Drive Commands
     drive.setDefaultCommand(
-        drive.run(
-            () ->
-                drive.setTeleopDriveGoal(
-                    -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX())));
+        drive
+            .run(
+                () ->
+                    drive.setTeleopDriveGoal(
+                        -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
+            .withName("Drive Teleop Input"));
 
-    // Aim
+    // Aim and Rev Flywheels
     controller
         .a()
-        .onTrue(Commands.runOnce(drive::setAutoAimGoal))
-        .onFalse(Commands.runOnce(drive::clearAutoAimGoal));
-    controller.a().whileTrue(Commands.parallel(flywheels.shoot(), superstructure.aim()));
-
+        .whileTrue(
+            Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
+                .alongWith(superstructure.aim(), flywheels.shoot())
+                .withName("Aim"));
     // Shoot
     Trigger readyToShoot =
         new Trigger(
@@ -228,22 +233,26 @@ private void configureButtonBindings() {
         .whileFalse(
             Commands.run(
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
-    controller.rightTrigger().and(readyToShoot).whileTrue(rollers.feedShooter());
-
+    controller
+        .rightTrigger()
+        .and(readyToShoot)
+        .onTrue(rollers.feedShooter().withTimeout(1.0).withName("Shoot"));
     // Intake Floor
     controller
         .leftTrigger()
         .whileTrue(
-            Commands.parallel(
-                superstructure.intake(),
-                Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake())));
+            superstructure
+                .intake()
+                .alongWith(rollers.floorIntake().onlyIf(superstructure::atGoal))
+                .withName("Floor Intake"));
     // Eject Floor
     controller
         .leftBumper()
         .whileTrue(
-            Commands.parallel(
-                superstructure.intake(),
-                Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor())));
+            superstructure
+                .intake()
+                .alongWith(rollers.ejectFloor().onlyIf(superstructure::atGoal))
+                .withName("Eject Floor"));
 
     controller
         .y()

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
@@ -11,7 +11,6 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.function.DoubleSupplier;
 import lombok.Getter;
@@ -52,7 +51,7 @@ public enum Goal {
     IDLING(idleLeftRpm, idleRightRpm),
     SHOOTING(shootingLeftRpm, shootingRightRpm),
     INTAKING(intakingRpm, intakingRpm),
-    EJECTing(ejectingRpm, ejectingRpm),
+    EJECTING(ejectingRpm, ejectingRpm),
     CHARACTERIZING(() -> 0.0, () -> 0.0);
 
     private final DoubleSupplier leftSetpoint;
@@ -122,28 +121,25 @@ public boolean atGoal() {
   }
 
   public Command stop() {
-    return runOnce(() -> setGoal(Goal.STOP)).andThen(Commands.idle()).withName("Flywheels Stop");
+    return runOnce(() -> setGoal(Goal.STOP)).withName("Flywheels Stop");
   }
 
   public Command idle() {
-    return runOnce(() -> setGoal(Goal.IDLING)).andThen(Commands.idle()).withName("Flywheels Idle");
+    return runOnce(() -> setGoal(Goal.IDLING)).withName("Flywheels Idle");
   }
 
   public Command shoot() {
     return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Flywheels Shooting");
   }
 
   public Command intake() {
     return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Flywheels Intaking");
   }
 
   public Command eject() {
-    return startEnd(() -> setGoal(Goal.EJECTing), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
+    return startEnd(() -> setGoal(Goal.EJECTING), () -> setGoal(Goal.IDLING))
         .withName("Flywheels Ejecting");
   }
 }
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java
@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers;
 
 import edu.wpi.first.math.MathUtil;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
@@ -8,7 +8,6 @@
 package org.littletonrobotics.frc2024.subsystems.rollers;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import lombok.Getter;
 import lombok.Setter;
@@ -90,30 +89,26 @@ public void periodic() {
   }
 
   public Command idle() {
-    return runOnce(() -> setGoal(Goal.IDLING)).andThen(Commands.idle()).withName("Rollers Idle");
+    return runOnce(() -> setGoal(Goal.IDLING)).withName("Rollers Idle");
   }
 
   public Command floorIntake() {
     return startEnd(() -> setGoal(Goal.FLOOR_INTAKING), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Rollers Floor Intake");
   }
 
   public Command stationIntake() {
     return startEnd(() -> setGoal(Goal.STATION_INTAKING), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Rollers Station Intake");
   }
 
   public Command ejectFloor() {
     return startEnd(() -> setGoal(Goal.EJECTING_TO_FLOOR), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Rollers Eject Floor");
   }
 
   public Command feedShooter() {
     return startEnd(() -> setGoal(Goal.FEEDING_TO_SHOOTER), () -> setGoal(Goal.IDLING))
-        .andThen(Commands.idle())
         .withName("Rollers Feed Shooter");
   }
 }
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java
@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.feeder;
 
 import edu.wpi.first.math.system.plant.DCMotor;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java
@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.indexer;
 
 import java.util.function.DoubleSupplier;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java
@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.indexer;
 
 import edu.wpi.first.math.system.plant.DCMotor;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java
@@ -1,3 +1,10 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
 package org.littletonrobotics.frc2024.subsystems.rollers.intake;
 
 import edu.wpi.first.math.system.plant.DCMotor;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -8,12 +8,10 @@
 package org.littletonrobotics.frc2024.subsystems.superstructure;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
 import lombok.Setter;
-import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -66,26 +64,21 @@ public boolean atGoal() {
   }
 
   public Command stow() {
-    return runOnce(() -> setGoal(SystemState.STOWING))
-        .andThen(Commands.idle())
-        .withName("Superstructure Stow");
+    return runOnce(() -> setGoal(SystemState.STOWING)).withName("Superstructure Stow");
   }
 
   public Command aim() {
     return startEnd(() -> setGoal(SystemState.AIMING), () -> setGoal(SystemState.STOWING))
-        .andThen(Commands.idle())
         .withName("Superstructure Aim");
   }
 
   public Command intake() {
     return startEnd(() -> setGoal(SystemState.INTAKING), () -> setGoal(SystemState.STOWING))
-        .andThen(Commands.idle())
         .withName("Superstructure Intake");
   }
 
   public Command stationIntake() {
     return startEnd(() -> setGoal(SystemState.STATION_INTAKING), () -> setGoal(SystemState.STOWING))
-        .andThen(Commands.idle())
         .withName("Superstructure Station Intake");
   }
 }