Running Autonomous Command in Sequence with specific timing

Hi all,

I hope you are doing well in your respective competitions.

For our autonomous setup, we wanted to follow a path to the “shooting” position of the robot, and then once it arrives there, run the intake, then run the shooter, each for set period of time. Then once this is done, go to the “idle” trajectory. My autonomous code is within my robot container and is attached below.

public Command getAutonomousCommand() {
    var autoVoltageConstraint =
            new DifferentialDriveVoltageConstraint(
                    new SimpleMotorFeedforward(
                            Constants.AutonomousVariables.kS,
                            Constants.AutonomousVariables.kV,
                            Constants.AutonomousVariables.kA
                    ),
                    Constants.AutonomousVariables.kDriveKinematics,
                    Constants.AutonomousVariables.maxVoltage
            );

    TrajectoryConfig config =
            new TrajectoryConfig(
                    Constants.AutonomousVariables.kMaxSpeed,
                    Constants.AutonomousVariables.kMaxAcceleration
            ).setKinematics(Constants.AutonomousVariables.kDriveKinematics)
                    .addConstraint(autoVoltageConstraint);


    String redTrajectoryToShoot = "PathWeaver/Paths/Red Shoot.wpilib.json";
    String BlueTrajectoryToShoot = "PathWeaver/Paths/Blue Side Shoot.wpilib.json";
    String redIdleTrajectory = "PathWeaver/Paths/Red Shoot to idle.wpilib.json";
    String BlueIdleTrajectory = "PathWeaver/Paths/Blue Shoot to Idle.wpilib.json";

    Trajectory shoot = new Trajectory();
    Trajectory idle = new Trajectory();
    if (DriverStation.getAlliance() == DriverStation.Alliance.Red) {
      try {
        Path ShootTrajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(redTrajectoryToShoot);
        Path IdleTrajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(redIdleTrajectory);
        shoot = TrajectoryUtil.fromPathweaverJson(ShootTrajectoryPath);
        idle = TrajectoryUtil.fromPathweaverJson(IdleTrajectoryPath);
      } catch(IOError | IOException error) {
        DriverStation.reportError("I couldn't open the trajectory: " + redTrajectoryToShoot, error.getStackTrace());
      }
    } else {
      try {
        Path ShootTrajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(BlueTrajectoryToShoot);
        Path IdleTrajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(BlueIdleTrajectory);
        shoot = TrajectoryUtil.fromPathweaverJson(ShootTrajectoryPath);
        idle = TrajectoryUtil.fromPathweaverJson(IdleTrajectoryPath);
      } catch(IOError | IOException error) {
        DriverStation.reportError("I couldn't open the trajectory: " + BlueTrajectoryToShoot, error.getStackTrace());
      }
    }

    RamseteCommand ramseteCommand1 =
            new RamseteCommand(
                    shoot,
                    driveTrain::getPose,
                    new RamseteController(Constants.AutonomousVariables.kRameseteB, Constants.AutonomousVariables.kRameseteZeta),
                    new SimpleMotorFeedforward(
                            Constants.AutonomousVariables.kS,
                            Constants.AutonomousVariables.kV,
                            Constants.AutonomousVariables.kA),
                    Constants.AutonomousVariables.kDriveKinematics,
                    driveTrain::getSpeeds,
                    new PIDController(Constants.AutonomousVariables.kPDriveVel, 0, 0),
                    new PIDController(Constants.AutonomousVariables.kPDriveVel, 0, 0),
                    // RamseteCommand passes volts to the callback
                    driveTrain::tankDriveVolts,
                    driveTrain);
    RamseteCommand ramseteCommand2 =
            new RamseteCommand(
                    idle,
                    driveTrain::getPose,
                    new RamseteController(Constants.AutonomousVariables.kRameseteB, Constants.AutonomousVariables.kRameseteZeta),
                    new SimpleMotorFeedforward(
                            Constants.AutonomousVariables.kS,
                            Constants.AutonomousVariables.kV,
                            Constants.AutonomousVariables.kA),
                    Constants.AutonomousVariables.kDriveKinematics,
                    driveTrain::getSpeeds,
                    new PIDController(Constants.AutonomousVariables.kPDriveVel, 0, 0),
                    new PIDController(Constants.AutonomousVariables.kPDriveVel, 0, 0),
                    // RamseteCommand passes volts to the callback
                    driveTrain::tankDriveVolts,
                    driveTrain);

  }

Also attached is the definition for the shooter and intake commands.

private DriveTrain driveTrain = new DriveTrain();
  private Limelight limelight = new Limelight();
  private Intake intake = new Intake();
  private Shooter shooter = new Shooter();
  private Joystick stickL = new Joystick(Constants.LEFT_STICK);
  private Joystick stickR = new Joystick(Constants.RIGHT_STICK);

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
    // Configure the button bindings
    configureButtonBindings();
    configDefaultCommands();
  }

  private void configureButtonBindings() {

    JoystickButton shootBalls = new JoystickButton(stickR, 1);
    JoystickButton TargetTrack = new JoystickButton(stickR, 2);
    JoystickButton putBallsIn = new JoystickButton(stickR, 3);
    JoystickButton runShooterBottom = new JoystickButton(stickR, 8);
    JoystickButton runShooterTop = new JoystickButton(stickR, 9);
    JoystickButton visionProcessor = new JoystickButton(stickL, 8);
    JoystickButton driverCamera = new JoystickButton(stickL, 9);


    putBallsIn.whileHeld(new IntakeManipulator(intake, () -> -stickL.getRawAxis(3)));
    TargetTrack.whileHeld(new TrackTarget(driveTrain, limelight));
    shootBalls.whileHeld(new ShootBalls(shooter, () -> -stickR.getRawAxis(3)));
    runShooterBottom.whileHeld(new bottomShooter(shooter));
    runShooterTop.whileHeld(new topShooter(shooter, () -> -stickR.getRawAxis(3)));
    visionProcessor.whenPressed(new CameraMode0(limelight));
    driverCamera.whenPressed(new CameraMode1(limelight));

  }

If anybody could explain a viable way to run these commands in order (I assume it has something to do with a SequentialCommandGroup but I’m not really sure how to use it.

Any help would be amazing, thanks!

I’m guessing that you’ll want to do something like:

return new SequentialCommandGroup(
    ramseteCommand1,
    ramseteCommand2,
    new IntakeManipulator(intake, () -> -0.7).withTimeout(1.5),
    new ShootBalls(shooter, () -> -0.7).withTimeout(2.5));

See Convenience Features and Command Groups for more details.