1
$\begingroup$

Do I need to use separate function in PID algorithm to make the bot go LEFT or RIGHT?

/**************************
  Author-
  Project- PID Code V-2
**************************/

float Kp = 0, Ki = 0, Kd = 0;
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0, previous_I = 0;
int sensor[5] = {0, 0, 0, 0, 0};
int initial_motor_speed = 100;

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);
void motor_left(void);
void motor_right(void);
void setup()
{
  pinMode(3, OUTPUT); //PWM Pin 1;9;EN PIN
  pinMode(5, OUTPUT); //PWM Pin 2;10;EN PIN
  pinMode(12, OUTPUT); //Left Motor Pin 1;4
  pinMode(13, OUTPUT); //Left Motor Pin 2;5
  pinMode(7, OUTPUT); //Right Motor Pin 1;6
  pinMode(8, OUTPUT); //Right Motor Pin 2;7

  Serial.begin(9600); //Enable Serial Communications
}

void loop()
{
  read_sensor_values();
  delay(15);
  calculate_pid();
  delay(15);
  motor_control();
}

void read_sensor_values()
{
  sensor[0] = digitalRead(A0);
  Serial.print("Sensor[0]:");
  Serial.println(sensor[0]);
  sensor[1] = digitalRead(A1);
  Serial.print("Sensor[1]:");
  Serial.println(sensor[1]);
  sensor[2] = digitalRead(A2);
  Serial.print("Sensor[2]:");
  Serial.println(sensor[2]);
  sensor[3] = digitalRead(A3);
  Serial.print("Sensor[3]:");
  Serial.println(sensor[3]);
  sensor[4] = digitalRead(A4);
  Serial.print("Sensor[4]:");
  Serial.println(sensor[4]);
  delay(3);

//  for(int i=0; i<5; i++)
//  {
//    sensor[i] = digitalRead(i);
//    Serial.print("Sensor[i]:");
//    Serial.println(sensor[i]);
//  }

  if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[4] == 0) && (sensor[4] == 1))
    error = 4;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[4] == 1) && (sensor[4] == 1))
    error = 3;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[4] == 1) && (sensor[4] == 0))
    error = 2;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[4] == 1) && (sensor[4] == 0))
    error = 1;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[4] == 0) && (sensor[4] == 0))
    error = 0;
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[4] == 0) && (sensor[4] == 0))
    error = -1;
  else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[4] == 0) && (sensor[4] == 0))
    error = -2;
  else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[4] == 0) && (sensor[4] == 0))
    error = -3;
  else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[4] == 0) && (sensor[4] == 0))
    error = -4;
  else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[4] == 0) && (sensor[4] == 0))
    if (error == -4) error = -5;
    else error = 5;
}

void calculate_pid()
{
  P = error;
  I = I + previous_I;
  D = error - previous_error;

  PID_value = (Kp * P) + (Ki * I) + (Kd * D);

  previous_I = I;
  previous_error = error;
}

void motor_control()
{
  // Calculating the effective motor speed:
  int left_motor_speed = initial_motor_speed - PID_value;
  int right_motor_speed = initial_motor_speed + PID_value;

  // Constraining the motor speed:
  constrain(left_motor_speed, 0, 255);
  constrain(right_motor_speed, 0, 255);

  analogWrite(3, initial_motor_speed - PID_value); //Left Motor Speed
  analogWrite(5, initial_motor_speed + PID_value); //Right Motor Speed

  //goForward:

  /*The pin numbers and high, low values are configurable depending on connections */
  digitalWrite(12, HIGH);
  digitalWrite(13, LOW);
  digitalWrite(7, HIGH);
  digitalWrite(8, LOW);
}
$\endgroup$
7
$\begingroup$

No, but you do need to calculate the P/I/D terms correctly. You have:

I = I + previous_I;

followed by:

previous_I = I;

With

I = 0;
previous_I = 0;

declared at the start. So your I term will always be zero here. What it should be is:

error = reference - feedback;
P_error = error;
I_error = I + (error*timeStep);
D_error = (error - previous_error)/timeStep;
prevous_error = error;

That is the most bullet-proof way to implement the PID loop. If your loop time is relatively stable, then you can absorb the time steps (the elapsed time between function calls) into the control gains:

P_term = P_gain * P_error;
I_term = I_gain * I_error;
D_term = D_gain * D_error;

Expanding this:

P_term = P_gain * (error);
I_term = I_gain * (<sum_0^k>(error*timeStep));
D_term = D_gain * ((error - previous_error)/timeStep);

I used a <sum_0^k> term (the "sum from zero to k") there to highlight the fact the integral term is a numeric integral composed of:

I_error = (error_0 * timeStep_0) + (error_1 * timeStep_1) + ... + (error_k * timeStep_k);

And if your timeStep is constant at every point, then you can factor it out and say:

I_error = timeStep*(error_0 + error_1 + ... + error_k);

So, regrouping the PID terms:

P_term = (P_gain) * error;
I_term = (I_gain*timeStep) * (<sum_0^k>error);
D_term = (D_gain/timeStep) * (error - previous_error);

At last, you can see that, if your time steps are relatively constant, you can say:

Effective_P_gain = P_gain;
Effective_I_gain = I_gain*timeStep;
Effective_D_gain = D_gain/timeStep;

P_error = error;
I_error = I_error + error;
D_error = error - previous_error;
previous_error = error;

Here you can see that I_error accumulates the error terms. I frequently see the timeStep being left off of the PID error terms, which I grudgingly accept because, as shown above, it's mathematically possible to do, but the problem is that:

  1. If you build modular code (which you should), then gain values you test when you're just doing motor tuning may fail when you add more detailed logic (or remove faulty logic) later in the development cycle. This is because addition or removal of program logic will alter the total time between function calls. The PID loop becomes highly sensitive to logic changes and thus also becomes very difficult to tune.
  2. You can't use empirical tuning methods like Ziegler-Nichols because those tuning methods require you to be using the true PID gains, not the modified gains. The timeStep term scales the integral and derivative gains, and the values diverge pretty quickly.

If your sample time is 10Hz, then your time step is (1/10) seconds, so integral gain is reduced by a factor of ten and your derivative gain is increased by a factor of ten, meaning they're a factor of ONE HUNDRED apart from one another. Ziegler-Nichols and others requires you to tune one gain until you see a particular system response, then set the remaining gains based on a formula that uses the first gain as an input.

If your gains aren't scaled correctly, those tuning methods will all fail. Now you're having twice the difficulty in tuning your PID controller:

  1. You can't use established tuning guidelines to tune the system, so you're guessing at the gains, then
  2. Once you do finally find good gains, your system becomes highly sensitive to changes in logic, which means you'll have to re-tune (entirely by guessing) again.

It's a huge time suck and a big distraction from the project.

| improve this answer | |
$\endgroup$
  • $\begingroup$ PS - Also, I just re-checked your code as I submitted the answer and noticed that your gains (Kp, Kd, and Ki) are all set to zero. So you're not actually developing any PID output because, even if you calculated the error terms correctly, you're ignoring them. Set the gains to non-zero values and see what happens. $\endgroup$ – Chuck Jul 18 '17 at 13:21
  • $\begingroup$ Yes i set everything to zero. What happens if I set them inside the loop?Will they work? I mean their value will be zero at first.But as the loop starts their values should be replaced by the new ones right? $\endgroup$ – Dinury Jul 19 '17 at 8:11
  • $\begingroup$ @MomshadDinury - The control error terms will update automatically. The control gain terms are fixed and do not change once you set them. This is what I am referring to when I talk about the hassle of manual tuning; you have to pick a set of control gains, then test the system behavior. If you're not getting the response you're looking for, then you have to change the control gains and try again. You keep doing this until you get a set of control gains that yields the response you want. $\endgroup$ – Chuck Jul 19 '17 at 13:23

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.