PID Looping

To tune your PID values, it is a fairly straightforward but long process. If you have a tank drivetrain, firstly why are you even here, but secondly, you can just use SysID to find your PID values. For swerve drive it is more complicated, as we need a set of constants for turning and driving. Luckily, when you find the constants they will not change, unless something changes about your robot physically. So what even are PID constants? A PID controller is a big fancy equation, where you plug in three (sometimes four) values, and then a target position and a starting position, and it outputs the correct motor outputs to turn/move it to that point. Before we get to that though, lets get some definitions out of the way. E is error value, or in more simple terms, just how far away the robot is from the target point, whether that be rotational or translational. P is the proportional term, and it controls the robots movement until E has reached zero. I is our integral term, meaning that it controls the small bits of movement and adjustments after P is done with movement. For a simple of example of P and I, the example I generally use is: Imagine you have a apple hanging on a rubber band 1m from the side of a table. Your error in this case would be exactly 1m, assuming you want to get the apple up to the table. The P term moves the apple to around 0.95 or 0.97m, but not quite all the way there. The I term handles the rest of the movement to get your apple onto the table. The last term, D is our derivative term, and it is used to calculate and move back from overcorrections. Returning to our previous example, if the apple moves up to 1.4m, then the D term is responsible for deciding how far to move it back down so that it reaches exactly 1m. F is simply a constant, you can make it anything you want, but the WPILib PIDController class doesn't use it. Next, it's my favorite part! Formulas! Note that kP, kI, kD and kF simply represent your constants values for each.

P=kPE I=kIΣE D=kD*(d/dx)E F=kF Output=P+I+D(+F if you really want to) NOW ITS TIME TO ACTUALLY TUNE THESE VALUES FOR YOUR ROBOT!! THIS IS THE TUNING FOR THE TURNING (THAT'S LOWKEY A FIRE NAME)

Go into src/main/java/frc/robot/drivetrain/commands/TurnToAngle.java, and change the values of tP, tI and tD to zero. Create a google sheets document/excel sheet and create three columns named P, I, and D. Set your robot down on a flat surface, with enough space for it to spin. Enable your robot, deploy robot code, and hold the A button (or equivalent) on your controller. The first time, with all your values set to zero, the robot should spin in circles. Set tP to 0.00001, and write that down on your sheets document Slowly increase the value, I generally increase it by deleting a zero until the robot moves, and document these in your sheet. Once the robot turns, move tP up by very small values until the heading on smartdashboard is at 86-88. Now do the same process on the kD coordinate or the kI coordinate, whichever you like, but only change one at a time so you can pinpoint the effects of changes easily. Once you the value is close enough to 90 for your liking (I generally go for 90 on the dot because I hate myself), then you can stop with the rotation tuning and move on to the translation tuning. ITS THE TUNING FOR NOT TURNING!!!!!!

How to pass a value to the PID controller - To pass values to a PID controller we use our drive method: drive(xSpeed, ySpeed, rotation, fieldRelative, rateLimit). Into this method we input the value returned from PIDControllerName.calculate(currentValue, targetValue). This method will return a double. For example, if we want to turn the robot to 90° relative to its current position, we would make the target value 90, and the current value whatever the robot heading is. How a PID controller.calculate function works is, looks at the first value, and then it looks at the second value, and it does it's best to make them match. This is why we run the calculate function within periodic() or execute(), so that the current heading/position of the robot are constantly updated. So if we wanted it to turn to 90°, as previously stated, we would set the values of the calculate function to robot heading and 90°. This would mean that it creates a constantly adjusting number that gets smaller as we move towards the target angle. To understand this better, we need to know how the robot controller works. In this I will just be giving a simple explanation, if you want more detail go to the "Driving The Robot" section of this wiki. Basically the first three arguments (xSpeed, ySpeed, rot) the robot drive controller simply takes a double that controls how fast it moves in each. You cannot make it rotate to exactly 90° just using the rotation controller from drive, as this would simply make it rotate at 90 speed. So instead, we plug 90° into our calculate function, and it returns us a value that it thinks will move our robot to 90°. The beauty of running this in a periodic() or execute() is that the number that the calculate function returns is constantly changing, so it can move faster when it's further and slow down when it's closer, eventually coming to a full stop once it has reached the target point.