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).

3 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.

4 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.

2 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