I made a PID loop with a sort of straight line assist and I don't know if I did it right

Programming Support VEXcode Pro V5 Text Tech Support
1

So I made this PID loop, and I’m fairly confident that it will work on its own. However, I have incorporated what I believe to be a ‘straight line assist’ feature, and I am not sure if it will work. I do not have the robot right now, so I can’t really test it out. This is my first time making a PID loop from scratch, and I would really appreciate it if someone could point out things I did wrong, redundancies that could be taken out, and ways to make it more accurate. Any help or constructive criticism would be appreciated. Thanks in advance!

//pid loop starts

void drive(float target, float speed, float timeout){

Brain.Timer.reset();

FL.setPosition(0,degrees);

FR.setPosition(0,degrees);

BL.setPosition(0,degrees);

BR.setPosition(0,degrees);

Inertial.calibrate();

float kp = 0.06;

float ki = 0.01;

float kd = 0.9;

float error;

float prevError;

float totalError = 0;

float proportion;

float integral;

float derivative;

float rawPow;

float powerLeft;

float powerRight;

float turnl;

float turnr;

while(1){

//setting the error values

error = target;

prevError = error;

totalError += error;

//proportion value

proportion = kp*error;

//integral value

integral = totalError * ki;

//derivative value

derivative = (error - prevError) * kd;

//rawPow is the motor power without the straight line assist

rawPow = proportion + integral + derivative;

//this will make turnl equal to the inertial heading

if(Inertial.heading(degrees)>0){

turnl = Inertial.heading(degrees);

}

//this will make turnl equal to 0 if inertial heading is below zero

else{

turnl = 0;

}

//ditto

if(Inertial.heading(degrees)<0){

turnr = Inertial.heading(degrees);

}

//ditto

else{

turnr = 0;

}

//this is the pid power value plus the inertial heading, which corrects itself if it goes off course

powerLeft = rawPow + turnl;

//ditto

powerRight = rawPow + turnr;

//this is the speed cap

if(powerLeft>speed){

powerLeft = speed;

}

//ditto

if(powerRight>speed){

powerRight = speed;

}

//this is the timeout

if(Brain.Timer.value()>=timeout){

break;

}

if(error<4){
break;
}
//spinning the motors at the right voltages

if(rawPow>0){

FL.spin(fwd, powerLeft, vex::voltageUnits::volt);

FR.spin(fwd, powerRight, vex::voltageUnits::volt);

BL.spin(fwd, powerLeft, vex::voltageUnits::volt);

BR.spin(fwd, powerRight, vex::voltageUnits::volt);

}

if(rawPow<0){

FL.spin(reverse, powerLeft, vex::voltageUnits::volt);

FR.spin(reverse, powerRight, vex::voltageUnits::volt);

BL.spin(reverse, powerLeft, vex::voltageUnits::volt);

BR.spin(reverse, powerRight, vex::voltageUnits::volt);

}

wait(20,msec);

}

wait(20,msec);

}

void autonomous(void) {

drive(1500,50,1000);

}
2 Likes
2

I also just realized that I forgot to put a cap on the integral part… I will most likely just end up setting that to 0 and not using it anyways.

3
PID touch ups

In general, you should make separate functions for chunks of code that will likely be reused elsewhere in the program. This condenses the program and makes it more readable. So this chunk of code could get its own function –– void resetDrive (){...}. Additionally, when declaring variables, it’s generally good practice to include related variables on the same line and there’s no need for all the empty lines between each declaration (it will really go a long way when you’re debugging large programs). That’s all just knit picking though.

This should be error = target - currentPosition() –– using whatever sensor values you choose to use.

This can be condensed into this: integral += error*deltaTime;. You also need to include the change in the time between each cycle of the loop because the computer’s clock is not perfectly timed between iterations of the loop. Also, for the sake of math conventions, the integral calculation should not include the integral gain –– that should come last when you calculate the final output power. Otherwise, it’s just going to confuse you conceptually. You also need integral bounds.

Same issue as above. You need to include the change in time and multiplying the gain at this stage in the program doesn’t make conceptual sense. Derivative = (error -prevError)/deltaTime;.

The final output should read: rawPow = proportion * kp + integral * ki + derivative * kd;.

The turn correction logic:


if(Inertial.heading(degrees)>0){

turnl = Inertial.heading(degrees);

}

//this will make turnl equal to 0 if inertial heading is below zero

else{

turnl = 0;

}

//ditto

if(Inertial.heading(degrees)<0){

turnr = Inertial.heading(degrees);

}

//ditto

else{

turnr = 0;

}

//this is the pid power value plus the inertial heading, which corrects itself if it goes off course

powerLeft = rawPow + turnl;

//ditto

powerRight = rawPow + turnr;

//this is the speed cap

if(powerLeft>speed){

powerLeft = speed;

}

//ditto

if(powerRight>speed){

powerRight = speed;

}

This logic can be condensed into a single line. Think about what’s happening when the IMU is returning heading data. The sign will already change based on the direction the robot is facing. So, you can simply add and subtract those values to either side of the chassis.

Example:
setDrive(rawPow - IMU.heading(), rawPow + IMU.heading());
Pretend the left parameter sends power to the left side of the chassis and the right parameter to the right side of the chassis. So, let’s say the robot veers to the left (relative to the line you’re trying to follow), the IMU will return negative values.

So, that means you will be adding power to the left side of the chassis and subtracting power from the right side. When the left side of the chassis gets more power than the right side of the chassis, the robot will tend to turn rightwards. Notice that this is what would happen if the robot veers to the left –– the PID output will want to move to the right. This is the logic needed in order to combat veering (because the robot will want to turn against the resistance).

The final key to this puzzle is that as the robot approaches the correct heading, the IMU value will approach 0 and the PID output will be the only thing powering the chassis. In other words, if the robot is following the line correctly, nothing will be added or subtracted from the power output sent to either side of the chassis. Otherwise, well I already explained that.

Alternatively, you can do something a little more advanced. Because the chassis has two degrees of freedom, you can get even more stable behavior by combining two PID controllers. One that tracks linear distance and another that tracks veering. Summing the two will return a final output that produces really smooth behavior. You’ll likely find that simply setting the IMU as the turn correction power output might lead to oscillation because it is not tuned to your robot. You can also scale the IMU values up or down and in that way, it acts as a pLoop.

By the way, I was being very knit-picky about things like you asked. You will realize why this improves readibility after you implement what I mentioned. I’m happy to answer more questions :slight_smile:

12 Likes
4

I have heard of the term delta before, and I used to know very vaguely what it meant, but I have long forgotten. Can you please explain what delta time is?

I honestly don’t know how I forgot that. Just a mistake.

Why is this proportion * kp? Isn’t proportion just error*kp?

This makes a lot of sense and it condenses it down a lot. Thanks for that.

Can you explain this a little bit further for me please? This seems very useful.

Thank you for this.

3 Likes
5

Also, the timeout function in my code should work too, right? I think it should work.

6

“delta time” is just the change in time relative to the previous cycle of the loop. So deltaTime = currentTime - previousTime. This will always be a positive value. Delta means “change in …”.

It’s the same thing. proportion = error, I was just trying to be consistent with your naming conventions. I am not saying proportion = error * kp * kp. I don’t know if that answered your question.

Essentially, you run two PID loops in parallel (at the same time). One that keeps track of the linear power and one that keeps track of turn power. The target for the turn PID loop is the angle of the line the robot is following relative to the field (or previous position, your choice). The same IMU logic I explained applies where you send the linear power to both sides of the chassis and then add/subtract the turn PID output from either side. That’s a very basic overview of it. But just know, the amount of degree of freedom the system has is the same number of PID controllers that can be added in parallel to control the system.

This goes really in-depth into the concept though it uses very high-level jargon and discusses it on an abstract level. It’s an interesting read.

From the looks of it, yes. But you need to add the current time to the input from the parameter upon entering the function (not the loop).

2 Likes
7

How would I calculate current time and previous time?

8

The same way you find the current error and previous error except now you’re storing the time since the program started running instead of the position of the robot.

2 Likes
9

You should use gyro error. So something like you assign the value of the angle the robot is supposed to be heading minus the gyro heading.

I’m on a Vex IQ team and here is a simple kp adjustment program to drive straight.

pseudocode:

while(some condition):
  gyroerror = desired heading - getgyroheading(gyro)
  setMotorSpeed(Left,Velocity+gyroerror)
  setmotorSpeed(Right, Velocity-gyroerror)

you need to adjust this code accordingly

3 Likes
10

Here is the updated code using what you have suggested. I hope that I used delta time right.

//pid loop starts
void drive(float target, float speed, float timeout){
Brain.Timer.reset();
//calibrations
FL.setPosition(0,degrees); FR.setPosition(0,degrees); BL.setPosition(0,degrees); BR.setPosition(0,degrees);
Inertial.calibrate();
//setting variables
float kp = 0.06;
float ki = 0.01;
float kd = 0.9;
float error;
float prevError;
float proportion;
float integral = 0.0;
float derivative;
float rawPow;
float powerLeft;
float powerRight;
float currentTime;
float prevTime;
while(1){
//setting the error values
currentTime = Brain.Timer.value();
prevTime = currentTime;
error = target - ((FL.position(degrees)+ FR.position(degrees))/2);
prevError = error;

//proportion value
proportion = error/(currentTime-prevTime);

//integral value
integral += error*(currentTime - prevTime);

//derivative value
derivative = (error - prevError)/(currentTime - prevTime);

//rawPow is the motor power without the straight line assist
rawPow = proportion *kp + integral *ki + derivative * kd;

//this is the straight line assist
powerLeft = (rawPow + Inertial.heading(degrees));

//ditto
powerRight = (rawPow + Inertial.heading(degrees));

//this is the speed cap
if(powerLeft>speed){
powerLeft = speed;
}

//ditto
if(powerRight>speed){
powerRight = speed;
}

//this is the timeout
if(Brain.Timer.value()>=timeout){
break;
}

if(error<4){
break;
}
//spinning the motors at the right voltages
FL.spin(fwd, powerLeft, vex::voltageUnits::volt);
FR.spin(fwd, powerRight, vex::voltageUnits::volt);
BL.spin(fwd, powerLeft, vex::voltageUnits::volt);
BR.spin(fwd, powerRight, vex::voltageUnits::volt);

wait(20,msec);
}

wait(20,msec);
}

void autonomous(void) {
drive(1500,50,1000);
}
2 Likes
11

I see two potential bugs here.
a. The intertial sensor/gyro needs to be reset to zero when the loop starts or else the gyro error values would be off.
b.

depending on what your motor setup is, one should be + and the other should be - although you need to find out which one is correct.

[edit: i just saw your inertial calibration at the top. I’m not doing VRC yet but I’m pretty sure that resets the inertial sensor values]

12

Yes I do need to do that. I copy/pasted the second one and forgot to reverse it.

13

yeah that’s the point. I intentionally put the calibration outside of the while loop so that it does not reset in the middle of driving forwards. I may switch it to just resetting it to zero though because I think calibration takes a while.

1 Like
14

You shouldn’t re-calibrate the gyro every time you enter the function. Rather, calibrate the gyro in pre-auton. If your intent was to 0 the gyro, I believe there’s a specific function for that in Vex code. Otherwise, store the current gyro value upon entering the loop and subtract that stored value from the updated gyro value each time the loop iterates. That is essentially the same as zeroing the value except you preserve the absolute heading.

You could do this for further condensing float kp = 0.06, ki = 0.01, kd = 0.9; (and so on and so forth for the other variables). Again, this is really just preference, but it’s nice having ultra condensed code (and good practice).

Also, your gains are off –– kp should generally always be a magnitude in order higher than ki and kd since it does the bulk of the work. To see if this works for the system, analyze the settling times (the gains you currently have might accurately get the robot to the target, but it probably isn’t the most efficient route).

proportion is independent of time –– only integral and derivative depend on time. Sorry if I didn’t make that clear. If you haven’t taken calculus or physics yet then don’t worry about why that’s the case. It isn’t super relevant to how PID applies in this context. Also, since you used (currentTime-prevTime) 3 different times, you’re better off making a variable called deltaTime to store that value.

Make sure you take the absolute value of the power output and determine the sign of the power (positive or negative). So it should look like this: power = speed * sign;. The sign can be determined like this: power/fabs(power) which yields a value of 1 or -1.

Last thing, you need to set the drive motors to 0 after the loop exits. Otherwise, it will retain the last value of the power output from the PID loop (which will probably be small but not 0).

Everything else looks good to me (and I was still being knit picky about certain things). Nice job!

4 Likes
15

This is a really good example of what I was referring to by combining PID controllers

3 Likes
16

I haven’t tested this on the robot yet… i just put numbers in there because why not

That makes sense

17

Oh and for this… does the way I have it not work? I set all of the motors equal to 0 in the very beginning of the PID function.

18

You set the position of the motors equal to 0 which makes the movement relative to the position at the start of the function. But I’m saying that you need to set the velocity of the motor to 0 (or voltage –– whatever unit you choose to use). Otherwise, the motor will continue to spin after the loop has exited.

5 Likes
19

riiiiiight I forgot to put the motor.stop commands at the end of the loop. Thanks

20

@mvas one last thing (hopefully). I have just made a turning P loop using the inertial sensor value. I don’t need to waste my time making a full PID loop for that, do I? A P loop should get it there accurately and will do the job I think. This is the turning loop, and it looks like it should work. Just like last time, if there is anything I can condense to make it work better, I would like to know.

void turn(float left, float right, float speed){
FL.setPosition(0,degrees); FR.setPosition(0,degrees); BL.setPosition(0,degrees); BR.setPosition(0,degrees);
Inertial.setHeading(0,degrees);
float kp = 0.6;
float Lerror;
float Rerror;
float leftPow;
float rightPow;
float signL;
float signR;
while(1){
//calculating error
Lerror = left - Inertial.heading(degrees);
Rerror = right - Inertial.heading(degrees);

//calculating motor power
rightPow = Rerror * kp;
leftPow = Lerror * kp;

//calculating the sign for the right side
signR = rightPow/abs(int(rightPow));

//ditto
signL = leftPow/abs(int(leftPow));

//this is the speed cap
if(leftPow>speed){
leftPow = speed*signL;
}

//ditto
if(rightPow>speed){
rightPow = speed*signR;
}

if(Rerror<4 && Lerror<4){
break;
}

FL.spin(reverse, leftPow, vex::voltageUnits::volt);
FR.spin(fwd, rightPow, vex::voltageUnits::volt);
BL.spin(reverse, leftPow, vex::voltageUnits::volt);
BR.spin(fwd, rightPow, vex::voltageUnits::volt);

wait(20,msec);
}

FL.stop();
FR.stop();
BL.stop();
BR.stop();
wait(20,msec);
}