Pid Turning with Interial Sensor

Programming Support VEXcode V5 Tech Support
1

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)
  {
    sensorvalue = ine.heading(rotationUnits::deg);

    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
2

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

3

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:

Screen Shot 2021-12-10 at 11.43.25 AM
Screen Shot 2021-12-10 at 11.43.25 AM814Ă—196 24.4 KB

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

3 Likes
4

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
5

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

6

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