How do I add the inertial sensor into the drive PID for more accurate turning

I recently added the inertial sensor to my robot however I’m unsure how to get the inertial code into the program without conflicting with the main turning for PID.

int turnDifference = LeftTrainPosition - RightTrainPosition;

turnError = turnDifference - DesiredTurnValue;

turnDerivative = turnError - turnPrevError;

turnTotalError += turnError;

double turnMotorPower = Error * kP + derivative * kD + totalError * kI;

LeftTrain.spin(forward, LateralMotorPower + turnMotorPower, voltageUnits::volt);
RightTrain.spin(forward, LateralMotorPower - turnMotorPower, voltageUnits::volt);

I, personally, took the approach of writing two separate functions:

  • My “driving” function, which drives the robot forwards/backwards
  • My “turning” function, which turns the robot using the inertial sensor

If you are doing turning and driving independently of each other, this usually works pretty well, and allows me to use the inertial without affecting my drive PID. It also allows me to use different values for my turning and driving PID (which I haven’t had to do, but I have seen the more advanced teams do it.)

What you could do is keep your PID driving function as-is and make another function that uses the inertial to turn. Although, if I remember correctly, this might be the PID example that runs as a task, so you might have to get a little creative in order to get your turn PID to work since it works as one task. You could write another task that does the turning with inertial, if you want to do that.

If you need any more assistance, please let me know!

– Michael, 8349P

1 Like

double kP = 0.0;
double kI = 0.0;
double kD = 0.0;

//autonomous settings
int DesiredValue = 0;

int Error; //SensorValue - DesiredValue =position
int prevError = 0; //Position 20 mseconds ago
int derivative; //Error - prevError = speed
int totalError = 0;

bool ResetDriveSensors = false;

//varibles modified for use
bool EnableDrivePID = true;

int DrivePID(){

while(EnableDrivePID) {

//Reset Drive Encoders

if (ResetDriveSensors) {
  ResetDriveSensors = false;


//gets position of both drive trains
int LeftTrainPosition = LeftTrain.position(turns);
int RightTrainPosition = RightTrain.position(turns);

//Lateral Movement PID

//gets average of both drive trains
int averagePosition = (LeftTrainPosition + RightTrainPosition)/2;

Error = averagePosition - DesiredValue;

derivative = Error - prevError;

totalError += Error;

double LateralMotorPower = Error * kP + derivative * kD + totalError *kI;

LeftTrain.spin(forward, LateralMotorPower, voltageUnits::volt);
RightTrain.spin(forward, LateralMotorPower, voltageUnits::volt);

prevError = Error;

return 1;

void inertialRightTurning (float tr){
// waits for the Inertial Sensor to calibrate
while (Inertial20.isCalibrating()) {
wait(100, msec);
// turns the robot to the right
// Waits until the motor reaches a 90 degree turn and stops the Left and
// Right Motors.
waitUntil((Inertial20.rotation(turns) >= tr));
So could I do something like this only I define the turn velocity and run the task as only lateral movement and have the inertial sensor function run as something separate from the task its self?
also how do you change the drive PID from being a task and just run it as a function?

Yes, something like that should work in theory. You can change the drive PID from being a task to a function by passing in the distance as a parameter, and breaking from the loop when your robot is within a threshold (if you have well-tuned PID values, this is usually somewhere around 1-2 degrees on both sides.)

I see that you’re not using PID for your turns in this example, though. I have implemented it myself. Here are some tips:

  • Use your inertial heading in degrees for the sensor value
  • I’d recommend using a threshold on your turning PID too (a smaller one, preferably, as if you’re making a turn and then driving forward a long distance, even a few degrees can mess up your entire autonomous)
  • Your turn will be exactly the same as your drive, besides the fact that one of your sides will be flipped and you will be using your inertial sensor as the sensor instead of your motor encoders

If you need any more advice, please let me know!

– Michael, 8349P

1 Like

lol I didn’t realize I accidently put turns instead of degrees on the inertial sensor thank you.
I also got it running as a function now thank you for all your help

1 Like

Hi I’m new to PID but have some decent experience coding in Vex. I was just curious how you would code the drivetrain to have 4 motors not 2 and how you would change, for example the linear motion’s data to inches? Also could someone show me what a full autonomous run would look like in PID? Also how would I add the inertia sensor to the code?


Please don’t post the same question in multiple threads

sorry I’m just trying to better my chances of someone answering