naming fixes and small changes

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

@@ -32,9 +32,9 @@ public final class Constants {
 
     public static final double stickDeadband = 0.1;
 
-    public static final class IntakeArm { // TODO: This must be tuned to specific robot
+    public static final class IntakeArmConstants { // TODO: This must be tuned to specific robot
         // mm
-        public static final int KMotorID = 0;
+        public static final int IntakeArmMotorID = 0;
         public static final double mmVelocity = 5.0;
         public static final double mmAcceleration = 10.0;
         public static final double mmJerk = 50;
@@ -51,13 +51,14 @@ public static final class IntakeArm { // TODO: This must be tuned to specific ro
         public static final double RevesrseSoftLimitThreshold = 0;
         // not mm
         public static final double minimumError = 0;
-        public static final double startingValue = 0;
+        public static final double intakeArmStartingValue = 0;
         public static final double zeroEncoderValue = 0;
-        public static final double intakeArmSpeed = 0;
+        public static final double intakeArmZeroSpeed = 0;
         public static final double intakeSetPoint = 0;
         public static final double trapSetPoint = 0;
         // switch
-        public static final int switchID = 0;
+        public static final int intakeArmSwitchID = 0;
+        public static final double AmpSetPoint = 0;
 
     }
 
@@ -273,7 +274,7 @@ public static class ShooterArmConstants {
         public static final double resetPose = 0.0;
         public static final double resetSpeed = -1.0;
 
-        public static final double startPose = 0.0;
+        public static final double shooterArmStartPose = 0.0;
 
         public static final InterpolationMap SHOOTER_ANGLE_INTERPOLATION_MAP = new InterpolationMap()
                 .put(1, 14100)
@@ -320,7 +321,7 @@ public static class ShooterConstants {
     }
 
     public static class IntakeConstants {
-        public static final int SwitchID = 1;
+        public static final int intakeNoteSensorID = 1;
         public static final int IntakeMotorID = 10;
         public static final double getNoteSpeed = 0.0;
         public static final double GroundIntakePose = 0.0;
@@ -330,7 +331,7 @@ public static class FeederConstants {
 
         public static final int FeederShootSpeed = 20;
         public static final int FeederMotorId = 3;
-        public static final int SwitchID = 0;
+        public static final int feederNoteSensorID = 0;
         public static final double FeederMotorSpeed = 0.8;
         public static final double getNoteSpeed = 0;
 
@@ -340,12 +341,15 @@ public static class TrapArmConstants {
         public static final int ArmMotorID = 10;
         public static final int InnerSwitchID = 0;
         public static final int OuterSwitchID = 10;
+        public static final double GoInSpeed = -0.1;
+        public static final double GoOutSpeed = 0.1;
 
     }
 
     public static class trapConstants {
         public static final int TRAP_MOTOR_ID = 0;
-
+        public static final int TrapNoteSensorID = 0;
+        public static final double TrapCollectSpeed = 0.1;
     }
 
     public static class climbingConstants {

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

@@ -8,7 +8,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.Utils.PathPlanner.PathPlannerHelper;
-import frc.robot.commands.SwerveCommands.*;
+import frc.robot.basicCommands.SwerveCommands.*;
 import frc.robot.subsystems.*;
 
 /**

src/main/java/frc/robot/aRobotOperations/CollectToFeeder.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.basicCommands.IntakeArmCommands.MoveIntakeArmToDegree;
+import frc.robot.basicCommands.IntakeCommands.IntakeSetSpeed;
+import frc.robot.basicCommands.feederCommands.FeedUntilNote;
+
+public class CollectToFeeder extends ParallelCommandGroup {
+  /** Creates a new MoveIntakeToCollectNote. */
+  public CollectToFeeder() {
+
+    addCommands(
+        new MoveIntakeArmToDegree(IntakeConstants.GroundIntakePose),
+        new IntakeSetSpeed(IntakeConstants.getNoteSpeed),
+        new FeedUntilNote().andThen(new IntakeSetSpeed(0)));
+  }
+}

src/main/java/frc/robot/aRobotOperations/CollectToIntake.java

@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import static frc.robot.Constants.IntakeArmConstants.*;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.basicCommands.IntakeArmCommands.MoveIntakeArmToDegree;
+import frc.robot.basicCommands.IntakeCommands.InatkeUntilNote;
+
+public class CollectToIntake extends ParallelCommandGroup {
+  /** Creates a new CollectToIntake. */
+  public CollectToIntake() {
+    addCommands(
+      new MoveIntakeArmToDegree(intakeSetPoint),
+      new InatkeUntilNote()
+    );
+  }
+}

src/main/java/frc/robot/aRobotOperations/IntakeToFeeder.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.basicCommands.IntakeArmCommands.MoveIntakeArmToDegree;
+import static frc.robot.Constants.IntakeArmConstants.*;
+
+public class IntakeToFeeder extends SequentialCommandGroup {
+
+  /** Creates a new FeederIntakeCommandGroup. */
+
+  public IntakeToFeeder() {
+
+    addCommands(
+        new MoveIntakeArmToDegree(intakeSetPoint),
+        new CollectToFeeder(),
+        new MoveIntakeArmToDegree(trapSetPoint));
+  }
+}

src/main/java/frc/robot/aRobotOperations/IntakeToTrap.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.IntakeArmConstants;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.basicCommands.IntakeArmCommands.MoveIntakeArmToDegree;
+import frc.robot.basicCommands.IntakeCommands.IntakeSetSpeed;
+import frc.robot.basicCommands.TrapArmCommands.goInSwitchCommand;
+import frc.robot.basicCommands.TrapCommands.TrapCollectUntilNote;
+
+public class IntakeToTrap extends SequentialCommandGroup {
+  /** Creates a new IntakeToTrap. */
+  public IntakeToTrap() {
+    addCommands(
+        new MoveIntakeArmToDegree(IntakeArmConstants.trapSetPoint)
+            .alongWith(new goInSwitchCommand()),
+        new TrapCollectUntilNote()
+            .alongWith(new IntakeSetSpeed(IntakeConstants.getNoteSpeed))
+    );
+  }
+}

src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.basicCommands.ShooterArmCommands.ShooterArmSpeakerAngle;
+import frc.robot.basicCommands.ShooterCommands.ShooterSpeakerSetSpeed;
+
+public class PrepareForShoot extends ParallelCommandGroup {
+  /** Creates a new PrepareForShoot. */
+  public PrepareForShoot() {
+
+    addCommands(
+      new ShooterArmSpeakerAngle(),
+      new ShooterSpeakerSetSpeed()
+    );
+  }
+}

src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java

@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.aRobotOperations;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.basicCommands.feederCommands.FeedToShooter;
+
+public class ShootSpeaker extends ParallelCommandGroup {
+  /** Creates a new ShootSpeaker. */
+  public ShootSpeaker() {
+    addCommands(
+      new PrepareForShoot(),
+      new FeedToShooter()
+    );
+  }
+}

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedLeftMotorClimbing.java → src/main/java/frc/robot/basicCommands/ClimbingCommands/SetSpeedLeftMotorClimbing.java

@@ -2,27 +2,24 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.ClimbingCommands;
+package frc.robot.basicCommands.ClimbingCommands;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.ClimbingSubsystem;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class SetSpeedLeftMotorClimbing extends InstantCommand {
   private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
   double speed;
+
   public SetSpeedLeftMotorClimbing(double speed) {
-      this.addRequirements(climbingSubsystem);
-      this.speed = speed;
-    // Use addRequirements() here to declare subsystem dependencies.
+    this.addRequirements(climbingSubsystem);
+    this.speed = speed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    this.climbingSubsystem.setSpeedClimbing(speed);
+    this.climbingSubsystem.setLeftSpeed(() -> speed);
 
   }
 }

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedMotorsClimbing.java → src/main/java/frc/robot/basicCommands/ClimbingCommands/SetSpeedMotorsClimbing.java

@@ -2,27 +2,24 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.ClimbingCommands;
+package frc.robot.basicCommands.ClimbingCommands;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.ClimbingSubsystem;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class SetSpeedMotorsClimbing extends InstantCommand {
   private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
   double speed;
+
   public SetSpeedMotorsClimbing(double speed) {
     this.addRequirements(climbingSubsystem);
     this.speed = speed;
-    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    this.climbingSubsystem.setSpeedClimbing(speed);
+    this.climbingSubsystem.setSpeedClimbing(() -> speed);
 
   }
 }

src/main/java/frc/robot/commands/ClimbingCommands/SetSpeedRightMotorClimbing.java → src/main/java/frc/robot/basicCommands/ClimbingCommands/SetSpeedRightMotorClimbing.java

@@ -2,28 +2,24 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.ClimbingCommands;
+package frc.robot.basicCommands.ClimbingCommands;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.ClimbingSubsystem;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class SetSpeedRightMotorClimbing extends InstantCommand {
   private final ClimbingSubsystem climbingSubsystem = ClimbingSubsystem.getInstance();
   double speed;
+
   public SetSpeedRightMotorClimbing(double speed) {
     this.addRequirements(climbingSubsystem);
     this.speed = speed;
-
-    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    this.climbingSubsystem.setSpeedClimbing(speed);
+    this.climbingSubsystem.setRightSpeed(() -> speed);
 
   }
 }

src/main/java/frc/robot/commands/IntakeArmCommands/IntakeArmSetSpeed.java → src/main/java/frc/robot/basicCommands/IntakeArmCommands/IntakeArmSetSpeed.java

@@ -2,25 +2,23 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.IntakeArmCommands;
+package frc.robot.basicCommands.IntakeArmCommands;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.IntakeArmSubsystem;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class IntakeArmSetSpeed extends InstantCommand {
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
   private double speed;
+
   public IntakeArmSetSpeed(double speed) {
-    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(intakeArm);
     this.speed = speed;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeArm.setSpeed(speed);
+    intakeArm.setSpeed(() -> speed);
   }
 }

src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java → src/main/java/frc/robot/basicCommands/IntakeArmCommands/MoveIntakeArmToDegree.java

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.IntakeArmCommands;
+package frc.robot.basicCommands.IntakeArmCommands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeArmSubsystem;

src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java → src/main/java/frc/robot/basicCommands/IntakeArmCommands/ZeroIntakeArm.java

@@ -2,23 +2,24 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands.IntakeArmCommands;
+package frc.robot.basicCommands.IntakeArmCommands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeArmSubsystem;
-import static frc.robot.Constants.IntakeArm.*;
+import static frc.robot.Constants.IntakeArmConstants.*;
 
-public class IntakeZero extends Command {
+public class ZeroIntakeArm extends Command {
   /** Creates a new SetSpeed. */
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
-  public IntakeZero() {
-    // Use addRequirements() here to declare subsystem dependencies.
+
+  public ZeroIntakeArm() {
+    addRequirements(intakeArm);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeArm.setSpeed(intakeArmSpeed);
+    intakeArm.setSpeed(() -> intakeArmZeroSpeed);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -29,7 +30,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    intakeArm.setSpeed(0);
+    intakeArm.setSpeed(() -> 0);
     intakeArm.setEncoder(zeroEncoderValue);
   }