# Implementing the inertial sensor to PID loop

I watched Connors PID tutorial (still confused) and I wanted to know how to implement the inertial sensor into the code. I also want to figure out what to put exactly into the autonomous to get it to turn.
Any help would be nice as well as examples. Thank you

PID is a generalized means of controlling a motor or collection of motors. Conceptually when one may want to use a PID, one has a target state in mind. The difference between the current state and that target state are fed into the PID which returns an output power level (based on tuning the PID’s constants).

The inertial sensor can provide “current state” information (e.g. it returns the heading of the robot). The roboteer may know what heading they would like the robot to drive at, either for some period of time or for some distance (one could also use a PID to control the distance driven, generally speaking using a motor’s integrated encoder or rotation sensors). One would then calculate the difference between the heading returned by the inertial sensor and the desired heading.

Good programming techniques abstract away “how to achieve something” from “what to achieve”. This makes re-using the “how to achieve something” (e.g. the PID algorithm) from the what (e.g. driving straight, or driving for distance).

6 Likes

There are Examples in VexCode Pro under the File menu. One is called called Accurate Turns (Inertial Sensor) that gives a good start to answering your question.

``````int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
Inertial20.calibrate();
// waits for the Inertial Sensor to calibrate
while (Inertial20.isCalibrating()) {
wait(100, msec);
}
// Turns the robot to the right
LeftMotor.spin(forward);
RightMotor.spin(reverse);
// Waits until the motor reaches a 90 degree turn and stops the Left and
// Right Motors.
waitUntil((Inertial20.rotation(degrees) >= 90.0));
LeftMotor.stop();
RightMotor.stop();
wait(1, seconds);
}
``````

The code above will keep running the WaitUntil loop until the robot turns past 90 degrees but it isn’t very accurate as the robot may coast a few degrees further.

Using Proportional (P)
If we set the motor power based on the Inertial sensor difference between where it is currently at and the destination then the motors will start to slow down as they get closer. This is a proportional loop (P).

``````//Power ratio - convert degrees to volts;
double ratio = 0.1;
double destination = 90;
double error;
error = destination - Inertial20.rotation(degrees);
LeftMotor.spin(forward,error * ratio, voltageUnits::volt);
RightMotor.spin(reverse,error * ratio, voltageUnits::volt);
``````

The problem with using only Proportional values is the motors keep slowing down as it gets closer to zero. If the robot still has 10 degrees yet to turn then the motor power is set to 1 (10 * .1) and the motors may stall.

PID
To fix the stalling issue with a P loop we add Integral and Derivative. then as the motors start to stall because the P is so low then the Integral (total Error) builds up and keeps them moving forward.

DIY
My suggestion is to start with the first example and then build on that as you gain experience. Use one motor (turn the right side only) to see how the robot behaves with your code. You can also `print to screen` the values of your variables so you can see value of the Inertial sensor and the power levels of your motors. PID requires a lot of testing.

The Easy Way
If you want a shortcut then you can always use the DriveTrain examples which automatically incorporate PID with the Inertial sensor. You can’t fine tune the performance but it’s a good starting option.

5 Likes

Building on this and what I mentioned earlier about a programming concept called “Separation of Concerns”, I would suggest that you eventually create (or use) a stand-alone PID class that encapsulates the PID logic from the application. For example, this seems to be a reasonable C++ implementation of PID:

``````MiniPID turnPID=MiniPID(1,0,0); // Tune these values
//set any other PID configuration options here.

double destination = 90;
double sensor_value = Inertial20.rotation(degrees);

while(fabs(sensor_value - destination) < .1){ // Some threshold
sensor_value = Inertial20.rotation(degrees);
double output=turnPID.getOutput(sensor_value,destination);
LeftMotor.spin(forward, output, voltageUnits::volt);
RightMotor.spin(reverse, output, voltageUnits::volt);

wait(20, msec);
}``````

This allows you to reuse the PID code for another application (say, driving for a certain distance). And it would even allow you to run multiple PIDs so that you can drive straight for a certain distance.

4 Likes

Thank you. I have one question though what and where do you put the code to enable it in autonomous.

Take a step back and write an autonomous without PID, then figure out how to layer in that complexity.

3 Likes

PID is a set of equations that when tuned properly can be very powerful in autonomous routines and skills. Below is a simple PID loop. It allows for turns to be corrected in an infinite number of ways. Remember that PID is only a concept, it can be applied to almost anything.

``````void TurnonPID(double gyroRequestedValue, double MaxspeedinRPM){
float gyroSensorCurrentValue;
float gyroError;
float gyroDrive;
float lastgyroError;
float gyroP;
float gyroD;

const float gyro_Kp = 0.573;
const float gyro_Ki = 0.4;
const float gyro_Kd = 0.18;

int TimeExit = 0;
double Threshold = 1.5;
while(1){
//Reads the sensor value and scale
gyroSensorCurrentValue = Inertial.rotation(vex::rotationUnits::deg);
Brain.Screen.setCursor(3, 1);

//calculates error
gyroError = gyroRequestedValue - gyroSensorCurrentValue;

//Exit loop
if(gyroError < Threshold and gyroError > -Threshold){
break;
}
else if(TimeExit == 10000){
Brain.Screen.clearScreen();
DriveStop();
resetDrive();
break;
}
else{
TimeExit = 0;
}

//calculate drive PID
gyroP = (gyro_Kp * gyroError);
static float gyroI = 0;
gyroI += gyroError * gyro_Ki;
if(gyroI > 1){
gyroI = 1;
}
if(gyroI < -1){
gyroI = -1;
}
gyroD = (gyroError - lastgyroError) * gyro_Kd;
gyroDrive = gyroP + gyroI + gyroD;

if(gyroDrive > MaxspeedinRPM){
gyroDrive = MaxspeedinRPM;
}
if(gyroDrive < -MaxspeedinRPM){
gyroDrive = -MaxspeedinRPM;
}

//Move Motors with PID
int powerValue = gyroDrive;
RightFront.spin(vex::directionType::rev,(powerValue), vex::velocityUnits::rpm);
LeftFront.spin(vex::directionType::rev, (powerValue), vex::velocityUnits::rpm);
RightBack.spin(vex::directionType::fwd, (powerValue), vex::velocityUnits::rpm);
LeftBack.spin(vex::directionType::fwd,  (powerValue), vex::velocityUnits::rpm);

lastgyroError = gyroError;
wait(50, vex::timeUnits::msec);
}
resetDrive();
DriveStop();
}```````
2 Likes

Thanks for the post, if you don’t mind, I’d like to provide some feedback, both for your benefit and the community’s.

I really like that you’ve made this able to handle any general request to turn the robot. This function signature `void TurnonPID(double gyroRequestedValue, double MaxspeedinRPM)` is very flexible. The code is also very clear, with variable names that describe their intention.

You’ve also done some excellent work thinking about exit conditions, especially with the timeout. A trivial suggestion for improvement would be to either add the timeout as another parameter for the user to set, or for the code to adjust the timeout based on the size of the initial turn and the movement speed.

A more substantial suggestion for improving this would be to “separate the concerns” of the PID calculation from the use of PID to implement the turning logic. When you go to write your “Drive Distance” function, you will find that you (largely) will copy-pasta this section of code:

``````//calculate drive PID
gyroP = (gyro_Kp * gyroError);
static float gyroI = 0;
gyroI += gyroError * gyro_Ki;
if(gyroI > 1){
gyroI = 1;
}
if(gyroI < -1){
gyroI = -1;
}
gyroD = (gyroError - lastgyroError) * gyro_Kd;
gyroDrive = gyroP + gyroI + gyroD;

if(gyroDrive > MaxspeedinRPM){
gyroDrive = MaxspeedinRPM;
}
if(gyroDrive < -MaxspeedinRPM){
gyroDrive = -MaxspeedinRPM;
}``````

Replacing variable names a bit, threshold values, etc. But the logic/calculation will be the same. Consider refactoring this to a function (or, better, a PID class). What this does, is, if you find a bug, or an improvement in this section of code, you only have to change it in one place and not in the X number of places you apply PID by copy-pasting this section of code

2 Likes

Hey! I wanted to try your example but im not sure what the resetDrive and DriveStop do. Im guessing DriveStop is just a function that brakes all 4 drive motors, but I can’t guess what resetDrive is.

while I don’t know. I can guess that it might reset the encoder values in the motors so when the next loop happens the current pos is always marked as 0

im also guessing it might be an Inertial.resetRotation(); ?

because I think this PID is for turning and not distance. Idk i’m very new to PID controllers

I have functions built to stop all the drive motors then reset the motor encoders. Drivestop stops the motors resetDrive resets the encoders

You might not want to do that because it causes the inertial to reset with out a proper calibration. It may not be anything official. But I found that it happens to be more accurate when you don’t reset the inertial after every turn.

1 Like

ah okay, so DriveStop would be something like FrontLeft.stop(braketype :: brake) *4 for each motor. What about reset?

I calibrated in my pre auton and currently im using this like waitUntil inertial rotation hits a desired degrees but im finding it to be a bit inconsistent

I program with the encoders in the motors. The encoders count the number of revolutions of the motor. Reseting them makes life easier because in my other functions I set a specific value the encoder should target and then it will go to it. Reseting it prevents the robot from going backward. It also makes it so you don’t need to do quick math when programming

1 Like

is coding with encoders like motor.spin(fwd, some double value, rev); ?
the problem i encountered with that is that if it doesnt hit the desired revs due to like hitting a mogo, It would just stop. I think I fixed it using a timeout function I made for all motors but im not sure if thats the best way.

I noticed in your PID controller you use rpm

I modified this PID loop so I can move forward and I based it off of the encoder values

oh, so you can set a target angle and distance im assuming?