Ho, I’m struggling to make a pd loop that allows my x drive to move straight, strafe and turn.

What sort of problems are you having? There’s a lot of PID templates on the forum, i’ll try and find some to link

Edit: Heres some nice ones i found

I’m having a hard time making it do those 3 motions at the same time. Sort of like what @sarah_97963A has done.

For that I would recommend watching this:

Although it’s titled as pure pursuit tutorial, I believe the first two episodes are about the “move to point function” and how to combine all three movements together.

From what I’ve watched it seems like he only talks about two motions.

We do this, you can look at our codebase: GitHub - cmwade/Caldwell-2020: The repository for 2775J's VRC Change Up code.

Essentially, at every point in time, the robot is driving (calculating each wheel output) straight to the desired point. It is also turning to the desired angle. The driving value and turning value is added for each wheel and the robot does both simultaneously.

Coder12, I must warn you, what you are describing involves a lot of math. Our team has succeeded in this task, though it took a number of months to develop. None the math is terribly advanced(geometry-trig-PID), but there is be a lot of calculation involved.

First things first, we need to define a coordinate system to describe where you want your robot to go. Since the robot is driving to a point on the field and rotating, robot-centric coordinates become pretty useless autonomous programming. In this case, you need to create field-centric coordinates. For example,

In this case, the close left side is 0 inches x 0 inches y and far right is 144 inches, 144 inches. The robot can turn from 0-360 degrees, with 90 degrees facing blue side and 0 facing the head ref side. Once you have this, you need to come up with a sensing system to allow the robot to figure out where it is on this coordinate system. This is commonly done using odometry. I would start with this document by team 5225a.

After that, you finally can start to make the PID (or PD) controller to move your robot to a field position. @2775Josh has described one such way to look at the robot. Each motion, drive, strafe and turning are added together to form the final motor speed at each point in time for the move.

These links all helped me develop my pure pursuit algorithm. It can be really quite simple and intuitive. Think of what you would do if you were a robot trying to follow a path –– the same steps are taken (roughly speaking) in this algorithm.

Generally, the algorithm is as follows:

- generate path –– (array of points, for example –– only done once)
- find robot’s current position
- find closest target point (based on the intersection of an imaginary circle with the path) –– the radius of this circle is variable based on how closely you want the robot to follow the path
- calculate power output based on robot’s current point to next target point (I did it by adding two PID controllers in parallel)
- send power to motors
- repeat until final point in the path is reached