How to Integrate a PIDF Controller With Roadrunner
This recipe will assume you have a functioning PIDF controller that has already been tuned. If you do not, refer to integrating a custom PIDF controller.
Ingredients
- A PID or PIDF controller class
- Tuned PID(F) gains
- An OpMode or LinearOpMode
- A Finite State Machine
The Recipe
PID(F) Controller and gains
This recipe assumes you have 1) a PID(F) class that works and 2) tuned PID(F) gains. This recipe will not go over how to implement these; you should reference integrating a custom PIDF controller.
Finite State Machines
In short, a finite state machine is a code structure which allows code to run linearly while also having quasi-parallel actions running. The example we will be working with today is driving with Roadrunner while controlling linear slides. For a more indepth understanding of what finite state machines are, visit gm0.
You can work with Finite State Machines in either a LinearOpMode or an OpMode, either work.
For this recipe, we will be using a LinearOpMode.
To use an OpMode, move everything before the while loop into the init() function and everything in the while loop into the loop() function.
We will first have a full example and then break it down piece by piece.
This example is more like pseudocode than real code and is meant to demonstrate a methodology.
public class RoadRunnerPIDF extends LinearOpMode {
// the capitalization and snake_case is just convention because the values of an enum are constants
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
private STATES previousState = STATES.INIT;
private STATES currentState = STATES.INIT;
private int targetPosition = 0;
private TrajectorySequence forward;
private TrajectorySequence strafeLeft;
private TrajectorySequence backward;
private SampleMecanumDrive drive;
private DcMotorEx linearSlides;
private PIDFController PIDF;
public void runOpMode() {
/* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
The linear slides will simply be called linearSlides.
*/
// linear slide initialization code
// pidf initialization code
drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d());
forward = drive.TrajectorySequenceBuilder(new Pose2d())
.forward(10)
.build();
strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
.addDisplacementMarker(() -> {
targetPosition = 800;
})
.strafeLeft(10)
.build();
backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
.back(10)
.build();
waitForStart();
currentState = STATES.DRIVE_FORWARD;
while(opModeIsActive) {
switch (currentState) {
case (INIT):
break;
case (DRIVE_FORWARD):
if (previousState != currentState) {
// everything in here will run once when the state switches
drive.followTrajectoryAsync(forward)
previousState = STATES.DRIVE_FORWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
}
break;
case (STRAFE_LEFT_AND_LIFT_SLIDES):
if (previousState != currentState) {
// inside this trajectory sequence the targetPosition is set and the slides will start updating
drive.followTrajectorySequenceAsync(strafeLeft);
} else if (!drive.isBusy() && linearSlides.atTarget()) {
currentState = STATES.DRIVE_BACKWARD;
}
break;
case (DRIVE_BACKWARD):
if (previousState != currentState) {
drive.followTrajectorySequenceAsync(backward);
previousState = STATES.DRIVE_BACKWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STOP;
}
break;
case (STOP):
break;
}
// outside of the switch we update our slides, that way they are always receiving new information
drive.update();
double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
linearSlides.setPower(power);
}
}
}
Okay, let’s break this down piece by piece. First, what is an “enum” and why do we use them? Enums are a way to define a set of named constant values. They provide a convenient and readable way to work with predefined, named values in your code. Here, we used an enum to describe the various states the robot could be in.
public class RoadRunnerPIDF extends LinearOpMode {
// the capitalization and snake_case is just convention because the values of an enum are constants
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
private STATES previousState = STATES.INIT;
private STATES currentState = STATES.INIT;
private int targetPosition = 0;
private TrajectorySequence forward;
private TrajectorySequence strafeLeft;
private TrajectorySequence backward;
private SampleMecanumDrive drive;
private DcMotorEx linearSlides;
private PIDFController PIDF;
public void runOpMode() {
/* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
The linear slides will simply be called linearSlides.
*/
// linear slide initialization code
// pidf initialization code
drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d());
forward = drive.TrajectorySequenceBuilder(new Pose2d())
.forward(10)
.build();
strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
.addDisplacementMarker(() -> {
targetPosition = 800;
})
.strafeLeft(10)
.build();
backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
.back(10)
.build();
waitForStart();
currentState = STATES.DRIVE_FORWARD;
while(opModeIsActive) {
switch (currentState) {
case (INIT):
break;
case (DRIVE_FORWARD):
if (previousState != currentState) {
// everything in here will run once when the state switches
drive.followTrajectoryAsync(forward)
previousState = STATES.DRIVE_FORWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
}
break;
case (STRAFE_LEFT_AND_LIFT_SLIDES):
if (previousState != currentState) {
// inside this trajectory sequence the targetPosition is set and the slides will start updating
drive.followTrajectorySequenceAsync(strafeLeft);
} else if (!drive.isBusy() && linearSlides.atTarget()) {
currentState = STATES.DRIVE_BACKWARD;
}
break;
case (DRIVE_BACKWARD):
if (previousState != currentState) {
drive.followTrajectorySequenceAsync(backward);
previousState = STATES.DRIVE_BACKWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STOP;
}
break;
case (STOP):
break;
}
// outside of the switch we update our slides, that way they are always receiving new information
drive.update();
double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
linearSlides.setPower(power);
}
}
}
By using names with meaning like these, it is much clearer when writing and reading the code what each block does. It also means we don’t have to remember that state 0 means START and state 1 means DRIVE_FORWARD, etc.
Next, we initialize everything and build our trajectories.
The important one to note is creating strafeLeft, which includes slide movement.
public class RoadRunnerPIDF extends LinearOpMode {
// the capitalization and snake_case is just convention because the values of an enum are constants
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
private STATES previousState = STATES.INIT;
private STATES currentState = STATES.INIT;
private int targetPosition = 0;
private TrajectorySequence forward;
private TrajectorySequence strafeLeft;
private TrajectorySequence backward;
private SampleMecanumDrive drive;
private DcMotorEx linearSlides;
private PIDFController PIDF;
public void runOpMode() {
/* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
The linear slides will simply be called linearSlides.
*/
// linear slide initialization code
// pidf initialization code
drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d());
forward = drive.TrajectorySequenceBuilder(new Pose2d())
.forward(10)
.build();
strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
.addDisplacementMarker(() -> {
targetPosition = 800;
})
.strafeLeft(10)
.build();
backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
.back(10)
.build();
waitForStart();
currentState = STATES.DRIVE_FORWARD;
while(opModeIsActive) {
switch (currentState) {
case (INIT):
break;
case (DRIVE_FORWARD):
if (previousState != currentState) {
// everything in here will run once when the state switches
drive.followTrajectoryAsync(forward)
previousState = STATES.DRIVE_FORWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
}
break;
case (STRAFE_LEFT_AND_LIFT_SLIDES):
if (previousState != currentState) {
// inside this trajectory sequence the targetPosition is set and the slides will start updating
drive.followTrajectorySequenceAsync(strafeLeft);
} else if (!drive.isBusy() && linearSlides.atTarget()) {
currentState = STATES.DRIVE_BACKWARD;
}
break;
case (DRIVE_BACKWARD):
if (previousState != currentState) {
drive.followTrajectorySequenceAsync(backward);
previousState = STATES.DRIVE_BACKWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STOP;
}
break;
case (STOP):
break;
}
// outside of the switch we update our slides, that way they are always receiving new information
drive.update();
double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
linearSlides.setPower(power);
}
}
}
We used a displacement marker, which tells Roadrunner to run this code at the specified position along the trajectory.
The () -> {} is the lambda format for a one time use function.
The empty parentheses indicate that the function requires no arguments, and the curly braces denote the start of the function.
In this case, we’re just setting the targetPosition variable, but this marker could include setting servo position, reading sensors, or anything else really.
public class RoadRunnerPIDF extends LinearOpMode {
// the capitalization and snake_case is just convention because the values of an enum are constants
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
private STATES previousState = STATES.INIT;
private STATES currentState = STATES.INIT;
private int targetPosition = 0;
private TrajectorySequence forward;
private TrajectorySequence strafeLeft;
private TrajectorySequence backward;
private SampleMecanumDrive drive;
private DcMotorEx linearSlides;
private PIDFController PIDF;
public void runOpMode() {
/* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
The linear slides will simply be called linearSlides.
*/
// linear slide initialization code
// pidf initialization code
drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d());
forward = drive.TrajectorySequenceBuilder(new Pose2d())
.forward(10)
.build();
strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
.addDisplacementMarker(() -> {
targetPosition = 800;
})
.strafeLeft(10)
.build();
backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
.back(10)
.build();
waitForStart();
currentState = STATES.DRIVE_FORWARD;
while(opModeIsActive) {
switch (currentState) {
case (INIT):
break;
case (DRIVE_FORWARD):
if (previousState != currentState) {
// everything in here will run once when the state switches
drive.followTrajectoryAsync(forward)
previousState = STATES.DRIVE_FORWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
}
break;
case (STRAFE_LEFT_AND_LIFT_SLIDES):
if (previousState != currentState) {
// inside this trajectory sequence the targetPosition is set and the slides will start updating
drive.followTrajectorySequenceAsync(strafeLeft);
} else if (!drive.isBusy() && linearSlides.atTarget()) {
currentState = STATES.DRIVE_BACKWARD;
}
break;
case (DRIVE_BACKWARD):
if (previousState != currentState) {
drive.followTrajectorySequenceAsync(backward);
previousState = STATES.DRIVE_BACKWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STOP;
}
break;
case (STOP):
break;
}
// outside of the switch we update our slides, that way they are always receiving new information
drive.update();
double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
linearSlides.setPower(power);
}
}
}
So now we’re getting to the Finite State Machine (FSM) part. The first part of this case, which you’ll see in each part, is checking whether previous and current states are equal. This allows us to run code the first time it enters this state, like starting a trajectory (in this example). Then inside that same block, we also need to set the previous state to the one we’re in.
The else if just checks if we’re done with this state to detect when to move on.
This is the transition trigger.
In this case, it detects when the Roadrunner trajectory finishes.
The next case is the more interesting one.
public class RoadRunnerPIDF extends LinearOpMode {
// the capitalization and snake_case is just convention because the values of an enum are constants
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
private STATES previousState = STATES.INIT;
private STATES currentState = STATES.INIT;
private int targetPosition = 0;
private TrajectorySequence forward;
private TrajectorySequence strafeLeft;
private TrajectorySequence backward;
private SampleMecanumDrive drive;
private DcMotorEx linearSlides;
private PIDFController PIDF;
public void runOpMode() {
/* for the purpose of this recipe, I will be using linear slides with PIDF control to demonstrate.
The linear slides will simply be called linearSlides.
*/
// linear slide initialization code
// pidf initialization code
drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d());
forward = drive.TrajectorySequenceBuilder(new Pose2d())
.forward(10)
.build();
strafeLeft = drive.TrajectorySequenceBuilder(forward.end())
.addDisplacementMarker(() -> {
targetPosition = 800;
})
.strafeLeft(10)
.build();
backward = drive.TrajectorySequenceBuilder(strafeLeft.end())
.back(10)
.build();
waitForStart();
currentState = STATES.DRIVE_FORWARD;
while(opModeIsActive) {
switch (currentState) {
case (INIT):
break;
case (DRIVE_FORWARD):
if (previousState != currentState) {
// everything in here will run once when the state switches
drive.followTrajectoryAsync(forward)
previousState = STATES.DRIVE_FORWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STRAFE_LEFT_AND_LIFT_SLIDES;
}
break;
case (STRAFE_LEFT_AND_LIFT_SLIDES):
if (previousState != currentState) {
// inside this trajectory sequence the targetPosition is set and the slides will start updating
drive.followTrajectorySequenceAsync(strafeLeft);
} else if (!drive.isBusy() && linearSlides.atTarget()) {
currentState = STATES.DRIVE_BACKWARD;
}
break;
case (DRIVE_BACKWARD):
if (previousState != currentState) {
drive.followTrajectorySequenceAsync(backward);
previousState = STATES.DRIVE_BACKWARD;
} else if (!drive.isBusy()) {
currentState = STATES.STOP;
}
break;
case (STOP):
break;
}
// outside of the switch we update our slides, that way they are always receiving new information
drive.update();
double power = PIDF.calculate(linearSlides.getCurrentPosition(), targetPosition)
linearSlides.setPower(power);
}
}
}
Here we have the same structure. However, this time our transition trigger is finishing the Roadrunner trajectory and the linear slides reaching their target. Because this runs in a loop, once the displacement marker triggers and changes the targetPosition, the PID update that runs at the end of every loop will move the linear slides accordingly.
It is also important to note that when using async following, you must call drive.update() once every loop. This allows Roadrunner to track the robot’s movement and to ensure the motors are following the trajectory. Without it, the robot will not move.
Whew! You should now be able to integrate a PID(F) controller with Roadrunner trajectories.
This example was meant to be general and explain the structure and concepts needed to make PID(F) controllers work with Roadrunner. It will almost certainly require changes to make it work exactly how you wish, so don’t worry if your code doesn’t look exactly like this example!
State Factory
State Factory is a library which helps abstract a lot of the code of a finite state machine. It also helps ensure you don’t forget to write a break or an exit case.
This recipe will not cover the installation of State Factory. Please follow the instructions on their gitbook to install it.
So, we’re going to write the same finite state machine but this time using State Factory.
public class RoadRunnerPIDFSF extends LinearOpMode {
public enum STATES {
INIT,
DRIVE_FORWARD,
STRAFE_LEFT_AND_LIFT_SLIDES,
DRIVE_BACKWARD,
STOP;
}
SampleMecanumDrive drive;
DcMotorEx linearSlides;
PIDFController PIDF;
int targetPosition = 0;
public void runOpMode() {
// all the same initialization and trajectory building as above
StateMachine machine = new StateMachine()
.state(STATES.INIT) // creates a new state
.transition(() -> isStarted()) // condition to transition from this state to the next one
.state(STATES.DRIVE_FORWARD) // register a new state
.onEnter(() -> drive.followTrajectorySequenceAsync(forward)) // code to happen one time when entering this state
.transition(() -> !drive.isBusy())
.state(STATES.STRAFE_LEFT_AND_LIFT_SLIDES)
.onEnter(() -> drive.followTrajectorySequenceAsync(strafeLeft))
.transition(() -> (!drive.isBusy() && linearSlides.atTarget()))
.state(STATES.DRIVE_BACKWARD)
.onEnter(() -> drive.followTrajectorySequenceAsync(backward))
.transition(() -> !drive.isBusy())
.state(STATES.STOP)
.build();
// building this StateMachine doesn't actually do anything. We still need to run it
waitForStart();
machine.start(); // this starts the state machine, putting us into the first state
while(opModeIsActive()) {
machine.update();
drive.update();
double power = PIDF(linearSlides.getCurrentPosition(), targetPosition);
linearSlides.setPower(power);
}
}
}
These two examples both do the exact same thing. This introduction to State Factory was mostly meant to show how it can simplify writing FSMs.
Android studio may recommend changing something like () -> !drive.isBusy() to !drive::isBusy.
These are simply two different ways to write the same thing.
The double colon works like class/instance::method.
It is important to note that these were extremely simple FSMs and do not demonstrate their full capabilities. This was simply meant to show you a way to integrate RoadRunner and a PIDF controller.
This article was last modified:
On 2024-05-29 23:53:29 -08:00
By j5155
See it here:1e709464b6beff74feabee0cfc1a2e436a18c6ce