Inertial sensor programming questions

I’m wondering if anyone can teach me how to make a pid loop for auto balancing with v5 inertial sensor. note this is my first year of vex and i’m the pro grammer for my team

How much do you already know about programming? If you don’t know much, I suggest you go through example codes in Vexcode V5 to learn a bit since it should be fairly easy to read the code. Also, try looking in the forum for any similar topics.

I guess, you can start with Conor’s PID tutorial: VEXCode PID Tutorial

Then, you can look at more PID references: PID course/tutorial - #4 by _Colossus

The code itself is going to be straight forward - you can copy it from one of the examples and use IMU accelerometer tilt as your input. It is tuning of PID coefficients that is going to be challenging. Robot with the platform are going to have so much inertia that you will need robust calculation of derivative to make it settles fast and reliable.


so actually I already watched that video and made the pid loop, but iI don’t know what to actually put in it to make the motors spin the right direction when the senor is tilted

also yes I went to a week long rookie camp at my school and learned lots about c++ coding. I can program driver control pre auton and autonomous, but I can’t quite figure out what to put in the pid loop

oh, and I use some sort of vexcode pro competition template that does not have smart config. plus it only has robot. config. vex.h and main.cpp,and i find it quite interesting that it does not have the other stuff

double kp=0.0;
double ki=0.0;
double kd=0.0;

int error;
int previouserror;
int derivative;
int totalerror;

bool enableabpid = true;
int ab_pid(){

int LrwPosition=Lrw.position(degrees);
int RrwPosition=Rrw.position(degrees);
int RfwPosition=Rrw.position(degrees);
int LfwPosition=Rrw.position(degrees);


return 1;


void autonomous(void) {
vex::task pid(ab_pid);

Starting from the basics, you don’t really need a PID loop if you’re just beginning now. PID might be a more advanced topic in coding and could be too big of a jump. When learning to code, you should learn why the code works and the process behind it. What I suggest you do first is to code the robot to move forward at a set velocity or voltage and wait until the inertial sensor is level.

1 Like

If we break down the basic steps in automatically balancing a robot, you want it to move forward and wait until the pitch of the inertial sensor is level with the ground. Here’s a diagram showing all the axes and their names:

Since we want need the robot to constantly update the value of the pitch given by the inertial sensor, we need to construct a while loop. At the same time, we can also create a function which is basically a named task for the robot, so you won’t have to manually rewrite the process every time you want to run it in the autonomous section of the code. The auto balancing function should be made outside of the autonomous section of the code.


We start out by creating a function outside of the autonomous task. In a competition, the referee or someone else running the competition would use a computer to signal the field to run the autonomous part of the code. Outside of the autonomous section, we can create a function and use it inside of the autonomous section in order to make it simpler. In the picture below, I named the function “autoBalance” there isn’t anything in the parentheses because we’re not inputting anything into the function; we’re simply telling just running the function in the code. Inside the function, you should see “while(1)”. The while starts the process. At the end of the process, you should see “return 1;”. The “return 1;” basically restarts the process again, creating a loop. We need a loop because the pitch of the robot changes throughout the process and needs to be updated. If we didn’t do that, all the robot would get is the pitch value at the start of the function. Inside the loop would be all of the instructions the robot follows.

Once we’re done with the function and loop, we put in the instructions. The basic instructions for the robot is to move forward until it’s balanced. In the loop, it moves the robot forward. So, the robot is moving up the ramp. Every cycle, which is about 20 milliseconds, the robot checks the pitch of the robot. Next, there is an if statement in the code. If the pitch isn’t equal to 0, meaning the robot isn’t balanced, it’ll skip over everything between the brackets of the if statement. After the code goes over the if statement, the function will loop back again and start from the beginning where it tells the robot to move forward. The loop will keep on happening until the if statement is run. Once the robot is level, the pitch should equal 0 in a perfect world. The inertial sensor should tell the robot that the pitch is 0 now, meaning it’s correctly balanced. When the loop is run, the if statement should be complete. Since the statement is now complete, everything inside the if statement bracket is executed. The robot should now stop and the “break;” stops the loop from repeating. Now we’ve finished making the function. All we have to do to use the function is to call on it in the autonomous. Your robot is now automatically balanced!



Well, assuming the robot did not overshoot even a bit :slight_smile:

Real world experimentation should determine what are allowable threshold for “balanced” and also how to adapt to other factors that might cause the robot not to balance, such as other objects on the platform, rings, mobile goals, and other robot.

Your explanation is great start for all to see how to work the problem through. Nice illustration too!


Thanks! Another thing to worry about in the real world is that the robot will, more likely than not, start oscillating near the balancing point because it only checks the pitch every 20 milliseconds and might not read pitch as 0 at that exact moment so it keeps overshooting and undershooting back and forth. This oscillation usually happens because you’re asking the robot to be too precise when it can’t be. You’re right when you mentioned that an allowable threshold for balanced. A larger threshold would allow the robot to be reasonably precise and to stop moving at the correct position instead of trying to be too precise and oscillating.


thank you guys very much. This helped he alot. Although I’m not exactly new to coding. :wink:

When I was learning PID, this document was really helpful:

1 Like

thanks, but for some reason I can’t open the file.

Maybe this would work:

thanks that worked. And

void pre_auton(void) {
// All activities that occur before the competition starts
}// end pre_auton

bool enableabpid = true;
int ab_pid(){


if (inertial9.orientation(pitch, degrees)== 0){


return 1;


hows this

Looks great! Don’t forget the break so the loop stops. Also I don’t think you need a semicolon after the last bracket and there shouldn’t be a bracket after “Rfw.stop(brakeType::hold);”