More temprorary teleop fixes from testing

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

@@ -66,6 +66,7 @@ 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(
         "WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory());
@@ -89,21 +90,17 @@ public Robot() {
     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.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
 
     m_codriver.a().onTrue(m_climber.climb());
-    m_codriver
-        .rightTrigger()
-        .onTrue(m_arm.goToSetpoint(Arm.Setpoint.kAmp).andThen(m_intake.outake(8)))
-        .onFalse(m_arm.goToSetpoint(Arm.Setpoint.kStowed));
-    m_codriver
-        .leftTrigger()
-        .onTrue(m_arm.goToSetpoint(Arm.Setpoint.kSpeaker).andThen(m_shooter.shoot()))
-        .onFalse(m_arm.goToSetpoint(Arm.Setpoint.kStowed));
+    m_codriver.rightBumper().whileTrue(m_intake.outake(8));
+    m_codriver.leftBumper().whileTrue(m_superstructure.shoot());
 
-    m_codriverX.whileTrue(m_intake.spinup(20)).onFalse(m_intake.stop());
+    m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop());
     m_scheduler
         .getDefaultButtonLoop()
         .bind(

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

@@ -22,16 +22,18 @@ public Superstructure(Shooter shooter, Intake intake, Index index) {
   }
 
   public Command intake() {
-    var lol =
-        m_index.m_intaking.onTrue(
-            Commands.parallel(
-                m_intake.intake(3).until(m_index.m_hasNote).andThen(m_intake.stop()),
-                m_index.intake(-3).until(m_index.m_hasNote).andThen(m_index.stop())));
-    return m_intake.intake(8).until(lol);
+    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 shooter() {
-    return m_shooter.spinup(500).andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot()));
+  public Command shoot() {
+    return m_shooter
+        .spinup(500)
+        .withTimeout(3)
+        .andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot()));
   }
 
   public Command stop() {

src/main/java/frc/robot/subsystems/Arm.java

@@ -54,10 +54,10 @@ public enum Setpoint {
 
   private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Arm");
   private final DoublePublisher output = driveStats.getDoubleTopic("Output").publish();
-  private final DoublePublisher ff_output = driveStats.getDoubleTopic("FF Output").publish();
-  private final DoublePublisher pid_output = driveStats.getDoubleTopic("PID Output").publish();
-  private final DoublePublisher pid_error = driveStats.getDoubleTopic("PID Error").publish();
-  private final DoublePublisher pid_setpoint = driveStats.getDoubleTopic("PID Setpoint").publish();
+  private final DoublePublisher ff_output = driveStats.getDoubleTopic("FF/Output").publish();
+  private final DoublePublisher pid_output = driveStats.getDoubleTopic("PID/Output").publish();
+  private final DoublePublisher pid_error = driveStats.getDoubleTopic("PID/Error").publish();
+  private final DoublePublisher pid_setpoint = driveStats.getDoubleTopic("PID/Setpoint").publish();
   private final DoublePublisher angle = driveStats.getDoubleTopic("Angle").publish();
   public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
 
@@ -79,12 +79,12 @@ private Command achievePosition(double position) {
           log_ff_output.append(ff_output);
           log_ff_position_setpoint.append(position);
           log_ff_velocity_setpoint.append((5676 / 250));
-          output.set(ff_output + pid_output);
           this.ff_output.set(ff_output);
           this.pid_output.set(0.5 + pid_output);
           this.pid_error.set(m_pid.getPositionError());
           this.pid_setpoint.set(m_pid.getSetpoint());
           m_primaryMotor.setVoltage(0.5 + pid_output * -1);
+          // m_primaryMotor.setVoltage(0.5 + pid_output);
         });
   }
 
@@ -137,10 +137,10 @@ public Command goToSetpoint(Setpoint setpoint) {
     log_setpoint.append(setpoint.name());
 
     switch (setpoint) {
-      case kAmp -> position = Math.PI / 2 + 0.1;
-      case kIntake -> position = 0;
-      case kSpeaker -> position = 0.2;
-      case kStowed -> position = 0;
+      case kAmp -> position = Math.PI / 2 + 4.8;
+      case kIntake -> position = 4.85;
+      case kSpeaker -> position = 4.85;
+      case kStowed -> position = 4.85;
     }
 
     return moveToPosition(position);

src/main/java/frc/robot/subsystems/Index.java

@@ -21,16 +21,17 @@ public class Index extends SubsystemBase {
   private final DigitalInput backBeamBreak = new DigitalInput(Constants.intakeBackBeambreak);
 
   public final Trigger m_hasNote = new Trigger(backBeamBreak::get);
-  public final Trigger m_intaking = new Trigger(frontBeambreak::get);
+  public final Trigger m_intaking = new Trigger(frontBeambreak::get).negate();
 
-  private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Index");
-  private final BooleanPublisher m_ntHasNote = driveStats.getBooleanTopic("Has Note").publish();
-  private final BooleanPublisher m_ntIntaking = driveStats.getBooleanTopic("Intaking").publish();
+  private final NetworkTable indexStats = NetworkTableInstance.getDefault().getTable("Index");
+  private final BooleanPublisher m_ntHasNote = indexStats.getBooleanTopic("Has Note").publish();
+  private final BooleanPublisher m_ntIntaking = indexStats.getBooleanTopic("Intaking").publish();
 
   public Index() {}
 
   public Command shoot() {
-    return run(() -> m_motor.setVoltage(-8)).until(m_hasNote.negate());
+    // return run(() -> m_motor.setVoltage(-8)).until(m_hasNote.negate());
+    return run(() -> m_motor.setVoltage(-8));
   }
 
   public Command intake(double voltage) {

src/main/java/frc/robot/subsystems/Shooter.java

@@ -9,6 +9,9 @@
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -27,22 +30,30 @@ public class Shooter extends SubsystemBase {
 
   private final DataLog m_log = DataLogManager.getLog();
   private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output");
+  private final NetworkTable shooterStats = NetworkTableInstance.getDefault().getTable("Shooter");
+  private final DoublePublisher m_ntPidError = shooterStats.getDoubleTopic("PID/Error").publish();
+  private final DoublePublisher m_ntPidOutput = shooterStats.getDoubleTopic("PID/Output").publish();
+  private final DoublePublisher m_ntRotationalVelocity =
+      shooterStats.getDoubleTopic("RotationalVelocity").publish();
 
   public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
 
   /** Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
-  public Shooter() {}
+  public Shooter() {
+    m_pid.setTolerance(0.05, 0.05);
+  }
 
   /** Acheives and maintains speed. */
   private Command achieveSpeeds(double speed) {
-    m_pid.reset();
     m_pid.setSetpoint(speed);
     return run(
         () -> {
           var output =
               m_pid.calculate(
                   -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()));
           log_pid_output.append(output);
+          m_ntPidError.set(m_pid.getVelocityError());
+          m_ntPidOutput.set(m_pid.getVelocityError());
           m_motor.setVoltage(output);
         });
   }
@@ -73,4 +84,9 @@ public Command maintain() {
   public Command shoot() {
     return spinup(500).andThen(maintain());
   }
+
+  @Override
+  public void periodic() {
+    m_ntRotationalVelocity.set(m_encoder.getVelocity());
+  }
 }