improve naming, delete some unnecessary stuff, left comments for fixes

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

@@ -578,7 +578,7 @@ public static final class Ports {
     public static final double MAX_HEIGHT = 50;
     /** FIXME */
     public static final double ELEVATOR_FEEDFORWARD = 0.01;
-    /**FIXME */
+    /** FIXME */
     public static final double AMP_HEIGHT = 20;
 
     public static final class Arm {

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

@@ -36,7 +36,6 @@
 import frc.robot.commands.AmpPreparationCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
-import frc.robot.commands.ElevatorHeightCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
@@ -52,8 +51,7 @@
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
-import frc.robot.commands.TargetLockCommand;
-import frc.robot.commands.UnloadShooterCommand;
+import frc.robot.commands.DrivebaseTargetLockCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -149,7 +147,7 @@ public RobotContainer() {
     NamedCommands.registerCommand(
         "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
     NamedCommands.registerCommand(
-        "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
+        "AutoDrivebaseAngle", new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
     NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
     NamedCommands.registerCommand(
         "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
@@ -229,7 +227,7 @@ public RobotContainer() {
             translationXSupplier,
             translationYSupplier,
             // anthony.rightBumper(),
-            anthony.leftBumper()));
+            ()-> false));
 
     rgbSubsystem.setDefaultCommand(
         new RGBCommand(
@@ -262,7 +260,7 @@ public RobotContainer() {
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
       driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
-      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
+      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getCurrentError());
       driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
     }
 
@@ -342,17 +340,17 @@ private void configureButtonBindings() {
         .leftBumper()
         .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-            .andThen(
-                new IntakeCommand(
-                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     jacob
         .leftBumper()
         .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-            .andThen(
-                new IntakeCommand(
-                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT
     anthony
@@ -365,6 +363,8 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
+
+    // FIXME should be right trigger is move elevator up, left trigger is move elevator down
     jacob
         .rightTrigger()
         .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem)));
@@ -374,30 +374,9 @@ private void configureButtonBindings() {
     jacob
         .y()
         .whileTrue(
-            new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
+            new DrivebaseTargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
                 .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
 
-    // jacob
-    //         .a()
-    //         .onTrue(
-    //                 new RotateAngleDriveCommand(
-    //                         drivebaseSubsystem,
-    //                         translationXSupplier,
-    //                         translationYSupplier,
-    //                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
-    //                         .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
-    //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem,
-    // ShooterMode.SHUTTLE)));
-
-    // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
-    // translationXSupplier,
-    // translationYSupplier, Setpoints.SPEAKER));
-
-    // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
-
-    // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
-    // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
-
     DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
 
     new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
@@ -421,22 +400,30 @@ private void configureButtonBindings() {
                             intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony.b().onTrue(
+    anthony
+        .b()
+        .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
-    jacob.b().onTrue(
-        new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
-    jacob.a().onTrue(
-        new RotateAngleDriveCommand(
-                drivebaseSubsystem,
-                translationXSupplier,
-                translationYSupplier,
-                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
-            .alongWith(
-                new AmpPreparationCommand(
-                    pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
+    jacob
+        .b()
+        .onTrue(
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+                .andThen(
+                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+    jacob
+        .a()
+        .onTrue(
+            new RotateAngleDriveCommand(
+                    drivebaseSubsystem,
+                    translationXSupplier,
+                    translationYSupplier,
+                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
+                .alongWith(
+                    new AmpPreparationCommand(
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     // SPEAKER FROM SUBWOOFER
     anthony
@@ -453,10 +440,10 @@ private void configureButtonBindings() {
                     drivebaseSubsystem,
                     translationXSupplier,
                     translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
-                        pivotSubsystem, elevatorSubsystem, shooterSubsystem))); 
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     DoubleSupplier rotation =
         exponential(
@@ -475,7 +462,7 @@ private void configureButtonBindings() {
                 translationXSupplier,
                 translationYSupplier,
                 rotationVelocity,
-                anthony.rightBumper()));
+                () -> false));
 
     new Trigger(
             () ->
@@ -488,7 +475,7 @@ private void configureButtonBindings() {
                 translationYSupplier,
                 anthony::getRightY,
                 anthony::getRightX,
-                anthony.rightBumper()));
+                () -> false));
   }
 
   /**

src/main/java/frc/robot/commands/AmpPreparationCommand.java

@@ -13,19 +13,17 @@
 public class AmpPreparationCommand extends SequentialCommandGroup {
   /** Creates a new AmpPreparationCommand. */
   public AmpPreparationCommand(
-    PivotSubsystem pivotSubsystem,
-    ElevatorSubsystem elevatorSubsystem,
-    ShooterSubsystem shooterSubsystem) {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      ShooterSubsystem shooterSubsystem) {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) {
       addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT));
-    }
-    else{
-      addCommands( 
+    } else {
+      addCommands(
           new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(
-          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
-        .andThen(
-          new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
+              .andThen(
+                  new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
+              .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
     }
   }
 }

src/main/java/frc/robot/commands/ArmAngleCommand.java

@@ -46,6 +46,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.nearTargetAngle();
+    return elevatorSubsystem.atTargetAngle();
   }
 }

src/main/java/frc/robot/commands/TargetLockCommand.java → src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java

@@ -17,7 +17,7 @@
  * This command takes a drive angle and a target x,y coordinate, and snaps the robot to face the
  * target. This is useful to lock the robot to fixed target with a button.
  */
-public class TargetLockCommand extends Command {
+public class DrivebaseTargetLockCommand extends Command {
   private final DrivebaseSubsystem drivebaseSubsystem;
 
   private final DoubleSupplier translationXSupplier;
@@ -28,7 +28,7 @@ public class TargetLockCommand extends Command {
   private double targetAngle;
 
   /** Creates a new TargetLockCommand. */
-  public TargetLockCommand(
+  public DrivebaseTargetLockCommand(
       DrivebaseSubsystem drivebaseSubsystem,
       DoubleSupplier translationXSupplier,
       DoubleSupplier translationYSupplier) {

src/main/java/frc/robot/commands/ElevatorHeightCommand.java

@@ -13,12 +13,6 @@
 public class ElevatorHeightCommand extends Command {
 
   ElevatorSubsystem elevatorSubsystem;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
   double height;
 
   public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
@@ -30,14 +24,13 @@ public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height)
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    elevatorSubsystem.setTargetHeight(height);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-
-    elevatorSubsystem.setTargetHeight(height);
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
@@ -46,6 +39,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.nearTargetHeight();
+    return elevatorSubsystem.atTargetHeight();
   }
 }

src/main/java/frc/robot/commands/LoadShooterCommand.java

@@ -19,6 +19,8 @@ public class LoadShooterCommand extends Command {
   ElevatorSubsystem elevatorSubsystem;
   PivotSubsystem pivotSubsystem;
 
+  /** Use PivotAndElevatorTransferCommand before running */
+
   public LoadShooterCommand(
       ShooterSubsystem shooterSubsystem,
       PivotSubsystem pivotSubsystem,
@@ -32,14 +34,17 @@ public LoadShooterCommand(
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
     elevatorSubsystem.setTargetHeight(0);
     pivotSubsystem.setTargetDegrees(20);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) {
+      shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java

@@ -4,17 +4,19 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
 
 public class PivotAndElevatorTransferPositionsCommand extends ParallelCommandGroup {
   /** Creates a new PivotAndElevatorTransferPositionsCommand. */
   public PivotAndElevatorTransferPositionsCommand(
-    PivotSubsystem pivotSubsystem,
-    ElevatorSubsystem elevatorSubsystem) {
-      addCommands(new PivotAngleCommand(pivotSubsystem, 20),
-        new ElevatorHeightCommand(elevatorSubsystem, 0));
+      PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
+    addCommands(
+        new PivotAngleCommand(pivotSubsystem, 20), new ElevatorHeightCommand(elevatorSubsystem, 0));
   }
 }
+
+  /* combine load and unload commands with this one - 
+  it should move everything into the correct position 
+  and either load or unload based on input from a button */

src/main/java/frc/robot/commands/PivotAngleCommand.java

@@ -36,6 +36,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return pivotSubsystem.isAtTargetDegrees();
+    return pivotSubsystem.atTargetDegrees();
   }
 }

src/main/java/frc/robot/commands/PivotManualCommand.java

@@ -19,7 +19,7 @@ public PivotManualCommand(PivotSubsystem pivotSubsystem, DoubleSupplier joystick
 
   @Override
   public void execute() {
-    pivotSubsystem.setPower(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
+    pivotSubsystem.setTargetDegrees(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
   }
 
   @Override

src/main/java/frc/robot/commands/RGBCommand.java

@@ -92,7 +92,7 @@ public void execute() {
 
     // ready to shoot = red
     if (isReadyToShootInSun()
-        && pivotSubsystem.isAtTargetDegrees()
+        && pivotSubsystem.atTargetDegrees()
         && Math.abs(drivebaseSubsystem.getAngularError()) < 2
         && readyToShootMsg.isEmpty()
         && visionSubsystem.getCanSeeSpeakerTags()) {
@@ -107,7 +107,7 @@ public void execute() {
         // input
         shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)
         || shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)
-        || pivotSubsystem.isAtTargetDegrees()) {
+        || pivotSubsystem.atTargetDegrees()) {
       readyToShootMsg.ifPresent(RGBMessage::expire);
       readyToShootMsg = Optional.empty();
     }

src/main/java/frc/robot/commands/SpeakerLockCommand.java

@@ -7,7 +7,7 @@
 public class SpeakerLockCommand extends ParallelCommandGroup {
   public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) {
     addCommands(
-        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
+        new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
         new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
   }
 }