code is functional

src/main/java/frc/robot/Constants.java

@@ -13,25 +13,31 @@ public final class Constants {
   public static final int codriverport = 1;
 
   public static final int shooterPort = 31;
+  public static final int indexerPort = 21;
   public static final double shooterP = 0.5;
   public static final double shooterI = 0;
   public static final double shooterD = 0;
 
-  public static final int climberPort = 32;
+  public static final int climberPort = 10;
   public static final double climberP = 0.35;
   public static final double climberI = 0;
   public static final double climberD = 0;
 
-  public static final int intakePort = 35;
+  public static final int intakePort = 32;
+  public static final int intakeFrontBeambreak = 6;
+  public static final int intakeBackBeambreak = 5;
+  public static final double intakeP = 0.01;
+  public static final double intakeI = 0;
+  public static final double intakeD = 0;
 
-  public static final double armP = 21.0;
+  public static final double armP = 10.5;
   public static final double armI = 0;
-  public static final double armD = 0.015;
+  public static final double armD = 0;
   public static final double armS = 0;
   public static final double armG = 0;
   public static final double armV = 0;
   public static final double armA = 0;
-  public static final int armLeadPort = 21;
+  public static final int armLeadPort = 9;
   public static final int armFollowerPort = 26;
   public static final int armEncoderPort = 3;
 

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

@@ -12,24 +12,28 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandRobot;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.autos.Centre2541;
-import frc.robot.autos.Centre26541;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.autos.WompWompKieran;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Index;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
 import java.util.HashMap;
 
 public class Robot extends CommandRobot {
   private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport);
+  private final CommandXboxController m_codriver =
+      new CommandXboxController(Constants.codriverport);
 
   private final Arm m_arm = new Arm();
   private final Shooter m_shooter = new Shooter();
   private final Climber m_climber = new Climber();
   private final Intake m_intake = new Intake();
+  private final Index m_index = new Index();
+  private final Superstructure m_superstructure = new Superstructure(m_shooter, m_intake, m_index);
   private static final CommandSwerveDrivetrain m_drivetrain = TunerConstants.DriveTrain;
 
   private final SwerveRequest.FieldCentric m_drive =
@@ -40,15 +44,19 @@ public class Robot extends CommandRobot {
   private final SwerveRequest.SwerveDriveBrake m_brake = new SwerveRequest.SwerveDriveBrake();
   private final Telemetry m_logger = new Telemetry();
 
+  private final Trigger m_codriverX = m_codriver.x();
+
   public Robot() {
-    HashMap<Integer, String> aliases = new HashMap<>();
-    aliases.put(31, "Shooter");
-    aliases.put(32, "Climber");
-    aliases.put(35, "Intake");
-    aliases.put(21, "Arm Lead");
-    aliases.put(26, "Arm Follower");
-    URCL.start(aliases, false);
+    HashMap<Integer, String> urclAliases = new HashMap<>();
+    urclAliases.put(Constants.shooterPort, "Shooter");
+    urclAliases.put(Constants.climberPort, "Climber");
+    urclAliases.put(Constants.intakePort, "Intake");
+    urclAliases.put(Constants.indexerPort, "Index");
+    urclAliases.put(Constants.armLeadPort, "Arm Lead");
+    urclAliases.put(Constants.armFollowerPort, "Arm Follower");
+
     DataLogManager.start();
+    URCL.start(urclAliases, false);
     DriverStation.startDataLog(DataLogManager.getLog());
     SignalLogger.setPath(DataLogManager.getLogDir());
     SignalLogger.start();
@@ -58,15 +66,8 @@ public Robot() {
     m_scheduler.registerSubsystem(m_shooter);
     m_scheduler.registerSubsystem(m_climber);
     m_scheduler.registerSubsystem(m_intake);
+    m_scheduler.registerSubsystem(m_index);
 
-    m_autoChooser.addOption(
-        "Centre2_5_4_1 Blue", new Centre2541(m_drivetrain, false).followTrajectory());
-    m_autoChooser.addOption(
-        "Centre2_5_4_1 Red", new Centre2541(m_drivetrain, true).followTrajectory());
-    m_autoChooser.addOption(
-        "Centre2_6_5_4_1 Red", new Centre26541(m_drivetrain, true).followTrajectory());
-    m_autoChooser.addOption(
-        "Centre2_6_5_4_1 Blue", new Centre26541(m_drivetrain, false).followTrajectory());
     m_autoChooser.addOption(
         "WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory());
     m_autoChooser.addOption(
@@ -86,10 +87,31 @@ public Robot() {
                     .withRotationalRate(
                         Utils.deadzone(
                             -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));
+    m_intake.setDefaultCommand(m_superstructure.intake());
+    m_shooter.setDefaultCommand(m_shooter.stop());
+    m_index.setDefaultCommand(m_index.stop());
+    // m_arm.setDefaultCommand(m_arm.goToSetpoint(Arm.Setpoint.kStowed));
+    m_arm.setDefaultCommand(m_arm.manualControl(m_codriver::getLeftY));
 
 
     m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
-    m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
+    m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
+
+    m_codriver.a().onTrue(m_climber.climb());
+    m_codriver.rightBumper().whileTrue(m_intake.outake(8));
+    m_codriver.leftBumper().whileTrue(m_superstructure.shoot());
+
+    m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop());
+    m_scheduler
+        .getDefaultButtonLoop()
+        .bind(
+            () -> {
+              if (m_codriver.getHID().getYButton()) {
+                m_intake.removeDefaultCommand();
+              }
+            });
+    m_codriver.y().onTrue(m_superstructure.stop());
+    m_codriver.b().whileTrue(m_superstructure.outake());
   }
 
 }

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

@@ -0,0 +1,46 @@
+// Copyright (c) 2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.Index;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
+
+public class Superstructure {
+  private final Shooter m_shooter;
+  private final Intake m_intake;
+  private final Index m_index;
+
+  public Superstructure(Shooter shooter, Intake intake, Index index) {
+    m_shooter = shooter;
+    m_intake = intake;
+    m_index = index;
+  }
+
+  public Command intake() {
+    m_index.m_intaking.onTrue(
+        Commands.parallel(
+            m_intake.intake(5).until(m_index.m_hasNote).andThen(m_intake.stop()),
+            m_index.intake(-5).until(m_index.m_hasNote).andThen(m_index.stop())));
+    return m_intake.intake(8).until(m_index.m_intaking);
+  }
+
+  public Command shoot() {
+    return m_shooter
+        .spinup(500)
+        .withTimeout(3)
+        .andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot()));
+  }
+
+  public Command stop() {
+    return Commands.parallel(m_shooter.stop(), m_intake.stop(), m_index.stop());
+  }
+
+  public Command outake() {
+    return Commands.parallel(m_intake.outake(-8), m_index.outake(8));
+  }
+}

src/main/java/frc/robot/URCL.java

@@ -49,9 +49,12 @@ public class URCL {
   private static RawPublisher aliasesPublisher;
   private static Notifier notifier;
   private static final DataLog datalog = DataLogManager.getLog();
-  private static RawLogEntry persistentLogEntry = new RawLogEntry(datalog, "URCL/Raw/Persistent");
-  private static RawLogEntry periodicLogEntry = new RawLogEntry(datalog, "/URCL/Raw/Periodic");
-  private static RawLogEntry aliasLogEntry = new RawLogEntry(datalog, "/URCL/Raw/Aliases");
+  private static RawLogEntry persistentLogEntry =
+      new RawLogEntry(datalog, "URCL/Raw/Persistent", "", "URCLr2_persistent");
+  private static RawLogEntry periodicLogEntry =
+      new RawLogEntry(datalog, "/URCL/Raw/Periodic", "", "URCLr2_periodic");
+  private static RawLogEntry aliasLogEntry =
+      new RawLogEntry(datalog, "/URCL/Raw/Aliases", "", "URCLr2_aliases");
 
   /**
    * Start capturing data from REV motor controllers to NetworkTables. This method should only be

src/main/java/frc/robot/autos/OneNote.java

@@ -32,6 +32,6 @@ public void configureBindings() {
         .onTrue(
             m_shooter
                 .spinup(300)
-                .andThen(Commands.parallel(m_intake.pass(), m_shooter.maintain())));
+                .andThen(Commands.parallel(m_intake.spinup(200), m_shooter.maintain())));
   }
 }

src/main/java/frc/robot/autos/TwoNote.java

@@ -8,19 +8,20 @@
 import com.choreo.lib.Choreo;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.subsystems.Intake;
+// import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
 
 public class TwoNote implements Auto {
   Shooter m_shooter;
-  Intake m_intake;
+  // Intake m_intake;
 
   Command m_pathfollower;
 
   public TwoNote(
-      CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake, boolean isRed) {
+      // CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake, boolean isRed) {
+      CommandSwerveDrivetrain drivetrain, Shooter shooter, boolean isRed) {
     m_shooter = shooter;
-    m_intake = intake;
+    // m_intake = intake;
 
     m_pathfollower = drivetrain.followTrajectory("example-two-note", isRed);
   }
@@ -42,7 +43,7 @@ private Command shoot() {
   @Override
   public void configureBindings() {
     Choreo.event("first-shot").onTrue(shoot());
-    Choreo.event("intake").onTrue(m_intake.intake());
+    // Choreo.event("intake").onTrue(m_intake.spinUntilBeamBreak(200));
     Choreo.event("second-shot").onTrue(shoot());
   }
 }