Hello all,
I have a vexiq v2 robot. It does not drive straight due to motor issues. I am interested in making a pid loop, but all of the ones I see online have things with motor rotation, motor velocity, motor rotation in degrees, turn to x angle, and more. I just have drivetrain controls. Could someone give me an example of a pid loop to make the robot drive straight in blocks? Thank you all so much I am new to coding and robots in general so any help would be much appreciated. Attached is an image of code online that I don’t have the blocks for.
1 Like
The way a PID loop works is that it takes an error and gives you the motor output to minimize that error. Normally that error is a rotation sensor on your motors. I don’t think those exist in iq so alternatively you can use a inertial sensor and for the error just take the heading - your goal heading.
1 Like
I have an inertial sensor, how would I make a code that does this? Thanks.
2 Likes
Sorry I probably should have elaborated more.
watch this video. It explains how PID’s work. This video is amazing but one side note is that having an integral term is kinda overkill for vex so you dont need it. I know someone whose integral term was 10^-9 so you dont really need it.
Pseudo code for this would be
kp = A number
kd = a number
Turn_speed = a number(0-100)
drive_speed = turn_speed - 100
loop{
error = Current_heading - goal heading
Derivative = prev_error - error
left_Motor.spin(drive_speed + turn_speed * (error * kp + Derivative * Kd))
RIght_Motor.spin(-(drive_speed + turn_Speed * (error * kp + Derivative * Kd)))
}
And then you want to tune KP and KD until it works
Also a short note. I have left side as positive and right as negative but depending on how IQ inertial sensors work that might be backwards
it should be noted that this only will go in one direction and you will have to switch the target heading in order to turn. Knowing when to do this is pretty hard without a rotation sensor and all of this code is pretty complicated so you might want to just set one side of youre drive to be slower and one side to be faster and just tune that manually.
If you still want to use PID’s and have any questions i can explain. The pseudo code is a bit messy
2 Likes
A simple way that I solved this problem is I had two seperate motor powers for each side. The main drive pid controlled the base motor power. Then a secondary pid(the correction pid) added power to the right side based on the difference between the left and right positions. In code this would look something like this : (this is in c++)
double RError = Labsoulute-Rabsoulute ;
if (fabs(RError) > .2){
RmotorPower = (RError * dLkp) + LmotorPower;
}
else{
RmotorPower = LmotorPower;
}
How the code works:
First it calculates the error by subtracting the measure of the Leftside from the meause of the RightSide (double RError = Labsoulute-Rabsoulute )
It then checks if this error is significant enough to correct (abs value is greater than .2 inches)(if (fabs(RError) > .2){)
Then if it is it calculates the power based on the error in angle and the motorPower of the base pid
RmotorPower = (RError * dLkp) + LmotorPower;
If it isn’t significant enough to correct it just drives straight else{ RmotorPower = LmotorPower; }
This will require some tuning with the kP value, if you do decide to go this route, I will be happy to help.
2 Likes
