Less advanced sequential commands

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

@@ -3,7 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
-
+import frc.robot.Commands.SequentialCommands;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -20,22 +20,22 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final WristSubsystem wristSubsystem = new WristSubsystem();
-
+    private final SequentialCommands squentialCommand = new SequentialCommands(wristSubsystem);
   // private final WristCommand wristCommand = new WristCommand();
   private final CommandXboxController driverB =
       new CommandXboxController(Constants.Wrist.DRIVER_CONTROLLER_PORT);
   private boolean wristTriggerButton;
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
-
+    driverB.y().onTrue(new SequentialCommands(wristSubsystem));
     configureButtonBindings();
 
     //   } else if (driverB.leftTrigger().getAsBoolean()) {
-
-    driverB.y().onTrue(new WristCommand(wristSubsystem, 0));
-    driverB.a().onTrue(new WristCommand(wristSubsystem, 20));
-    driverB.b().onTrue(new WristCommand(wristSubsystem, 40));
+  
+    // driverB.y().onTrue(new WristCommand(wristSubsystem, 0));
+    // driverB.a().onTrue(new WristCommand(wristSubsystem, 20));
+    // driverB.b().onTrue(new WristCommand(wristSubsystem, 40));
     driverB.x().onTrue(new InstantCommand(() -> {}, wristSubsystem));
   }
 

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

@@ -0,0 +1,40 @@
+// 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.Commands;
+import frc.robot.Commands.WristCommand;
+import frc.robot.subsystems.WristSubsystem;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class SequentialCommands extends SequentialCommandGroup {
+  /** Creates a new TestCommand. */
+  public SequentialCommands(WristSubsystem wristSubsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addCommands(
+    new WristCommand(wristSubsystem, 0),
+    new WristCommand(wristSubsystem, 20),
+    new WristCommand(wristSubsystem, 40)
+    );
+  }
+
+  // // Called when the command is initially scheduled.
+  // @Override
+  // public void initialize() {}
+
+  // // Called every time the scheduler runs while the command is scheduled.
+  // @Override
+  // public void execute() {}
+
+  // // Called once the command ends or is interrupted.
+  // @Override
+  // public void end(boolean interrupted) {}
+
+  // // Returns true when the command should end.
+  // @Override
+  // public boolean isFinished() {
+  //   return false;
+  // }
+}

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

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

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

@@ -59,7 +59,11 @@ private double getCurrentAngle() {
     currentAngle = ticksToDegrees(wrist_motor.getSelectedSensorPosition());
     return currentAngle;
   }
+  public boolean nearTargetAngle(){
+    if(targetAngle-0.5<=getCurrentAngle() && getCurrentAngle()<=targetAngle+0.5)return true;
+    return false;
 
+  }    
   @Override
   public void periodic() {
     // calculates motor power