How to make an autonomous routine

Technical Programming
1

I would like to schedule some commands and sequential commands in the getAuto method. I don’t know how to schedule or Chang commands, though.

2

For chaining together commands, you use Command Groups.

for the getAutonomousCommand() method in the Command Based Project Template, you just return a Command Object in that method and it will get run in Auto as long as you didn’t remove the functionality for it in your Robot class.

For more details about the Command Scheduler, there is also a page for that.

3

What gixxy said. You don’t need a set of explicit autonomous commands, necessarily, depending on how your code is set up.

We are pretty unsophisticated in what we’re capable of in terms of movement (no motion proviling or motion magic because we’re just behind enough to not be able to figure that stuff out fast enough), but have three autonomous routines, each one is a SequentialCommandGroup with nested ParallelRaceGroups leading the robot on a step-by-step set of instructions.

The ParallelRaceGroups in particular can be very useful for commands where IsFinished() returns false – once your primary SequentialCommandGroup gets to that point, it’ll freeze, waiting for the command to never end. So we have, for example, a ParallelRaceGroup(new Limelight_LED_On, new WaitCommand(0.02)) so that when the Wait ends, that sub-group ends, and it moves on to the next thing.

4

So how would you code it?

getAuto{
return Sequentialgroup (new Shooter,new Wait, new AutoDriveBack)
}

?

5

No. You create the command group as its own thing (if you’re using Java, right-click on commands then scroll down to “create new command” and then select the SequentialCommandGroup, just like you would creating a single command.) Assuming you named this SequentialCommandGroup something sensible, like Autonomous_1, then you’d have this:

getAuto{
return Autonomous_1
}

We have multiple autonomous modes, each triggered by a slider on the bottom of our joysticks, so ours looks something like

getAuto{
if (Joystick1Slider < 0.5) {
return Autonomous_1};
else if (Joystick2Slider < 0.5) {
return Autonomous_2};
else if (Joystick3Slider < 0.5) {
return Autonomous_3};
else {
return Autonomous_4};
}

6

Oh, Ok. Would I need to import it still and instantiate the command object? I assume you just left it out in that example because it’s assumed? Sorry for all the questions, just trying to make sure.

7

Right, you’d still have to instantiate it, and in the command object you’d have to import all the subsystems that each of the commands in the group use. Let me see if I can’t get a hold of our programming laptop so I can post an example.

8

This is our “play nice with others” (just back up and shoot) autonomous routine:

In RobotContainer we instantiate the command group with

public final Auto_Routine_1 auto_Routine_1 =
   new Auto_Routine_1(m_drivetrain, m_Limelight, shoot_Motor, feeder_motor, intake_Motor, agitator_motor);

and call it with

  public Command getAutonomousCommand() {
    if (joystick3.getRawAxis(3) <= 0) {
      return auto_Routine_1;
    }
   else if (leftJoystick.getRawAxis(3) <= 0) {
      return auto_Routine_2;
    }
   else if (rightJoystick.getRawAxis(3) <= 0) {
      return auto_Routine_3;
    }
    else return auto_Routine_1;
  }
}

Then command group auto_Routine_1:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;

import java.util.function.DoubleSupplier;

import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import frc.robot.Constants;
import frc.robot.subsystems.Agitator_Motor;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Feeder_Motor;
import frc.robot.subsystems.Limelight;
import frc.robot.subsystems.Shoot_Motor;
import frc.robot.subsystems.Intake_Motor;

// This is the first and simplest Auto Routine. It runs when the Slider axis on joystick3 is set to 1 or fully up.
// It also runs when all three are down

public class Auto_Routine_1 extends SequentialCommandGroup {
  
  public Auto_Routine_1(DriveTrain driveTrain, Limelight limelight, Shoot_Motor shoot_Motor, Feeder_Motor feeder_motor, Intake_Motor intake_Motor, Agitator_Motor agitator_motor, DoubleSupplier leftspeed, DoubleSupplier rightspeed) {
    super(
      new SequentialCommandGroup(
        new ParallelRaceGroup(
          new Limelight_LED_On(limelight), 
          new Shoot_Start(Constants.RPM_VALUE),
          new Drive_Back_2(Constants.speed, Constants.speed, driveTrain)),
        new ParallelRaceGroup(
          new Shoot_Motor_Command_Group(driveTrain, limelight, shoot_Motor, feeder_motor, agitator_motor, leftspeed, rightspeed),
          new Auto_Run_Intake(intake_Motor)
          )));
  }
}

Our more complicated routines are basically iterations of this same idea, with ParallelRaceGroups embedded into the SequentialCommandGroup.

There are probably better/more elegant ways to do things, but this worked for us.

9

In the “new command framework”, there are also decorators so you can pretty easily create command groups without making separate class files. It would be something like this:

Command getAuto() {
    return new ShootCommand(shooterSubsystem)
        .andThen(new WaitCommand(5))
        .andThen(new AutoDriveBackCommand(drivetrainSubsystem));
}
10

Good to know, thank you!

11

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.