# Pid Turning with Interial Sensor

Hi, I am trying to create a PID code that includes the inertial sensor in order to create precise turning movements. I have run into the problem of my code not being able to tell properly when to turn left or right in order to drive the least amount of distance. For example if were to input 270 degrees I want it to automatically tell itself to instead of turning right 270 degrees to just turn left 90 degrees. But I keep running into the problem where it will only run one direction even with the code I implemented. And then when it reaches if told to go past a degree of 180 will just ocsilate at 180 degrees for an infinite amount of time. Here is the code.

``````double turnsetpoint;
void turnpid (double turnsetpoint)
{
while (pidswitch == true)
{

error = turnsetpoint - sensorvalue;
integral+= error;

derivative = error - preverror;

preverror = error;

Power = error*kP + integral*kI + derivative*kD;

printf("%+08.3g\n",error);

if (sensorvalue > setpoint && (sensorvalue - setpoint) <= 180){
LeftMotor.spin(forward, Power , pct);
RightMotor.spin(reverse, Power , pct);
LeftCenterMotor.spin(forward, Power, pct);
RightCenterMotor.spin(reverse, Power, pct);
BackLeftMotor.spin(forward, Power , pct);
BackRightMotor.spin(reverse, Power , pct);
}

else if (sensorvalue > setpoint && (sensorvalue - setpoint) > 180){
LeftMotor.spin(reverse, Power , pct);
RightMotor.spin(forward, Power , pct);
LeftCenterMotor.spin(reverse, Power, pct);
RightCenterMotor.spin(forward, Power, pct);
BackLeftMotor.spin(reverse, Power , pct);
BackRightMotor.spin(forward, Power , pct);
}

else if (sensorvalue < setpoint && error <= 180){
LeftMotor.spin(forward, Power , pct);
RightMotor.spin(reverse, Power , pct);
LeftCenterMotor.spin(forward, Power, pct);
RightCenterMotor.spin(reverse, Power, pct);
BackLeftMotor.spin(forward, Power , pct);
BackRightMotor.spin(reverse, Power , pct);
}

else if (sensorvalue < setpoint && error > 180){
LeftMotor.spin(reverse, Power , pct);
RightMotor.spin(forward, Power , pct);
LeftCenterMotor.spin(reverse, Power, pct);
RightCenterMotor.spin(forward, Power, pct);
BackLeftMotor.spin(reverse, Power , pct);
BackRightMotor.spin(forward, Power , pct);
}

if(fabs(error)<= 1){
pidswitch = false;
}

wait (15,msec);
}
LeftMotor.stop(brake);
RightMotor.stop(brake);
LeftCenterMotor.stop(brake);
RightCenterMotor.stop(brake);
BackLeftMotor.stop(brake);
BackRightMotor.stop(brake);
}
``````
1 Like

I have been using the Inertial sensor and when I code it to turn it is rotating left and right and not aligning.

here’s an idea. I know it’s in python, but it’s exactly what I use in my program. assuming heading is the inertial sensor heading and degrees is the input, this is the part that helps it turn the correct direction:

it basically switches the target and the inertial sensor reading from, say, 270 degrees to -90 degrees.

3 Likes

Very good! This is very close to the generalized solution. The case where this particular implementation fails is if `degrees` is more than 360. You may (reasonably) think that users wouldn’t ever use values more than 360 and be satisfied with the solution. Or, you can think about how this would always ensure that `degrees` is between -180 and 180 for any input

3 Likes

Ok I will implement it and try it the next time I get the chance

Also, once you figure out heading math, don’t forget to add an if() statement to clip the integral to prevent “integral windup”.