Yet another PID question

Projects Programming
1

Hey all,

So I have my program for a line follower, however I just can't get the tuning right. I have set kP until it oscillates however it seems that kD won't dampen it. I'm at a loss, I just can't figure it out. Can you take a look at my code and tell me where it is failing.
Main .ino

void loop() {
  // put your main code here, to run repeatedly:
  readSensorArrayIR();
  valueOfPID = calculatePID();
  motorControlWithPID();
  //delay(1000);
}

defines.h

/*-----------------------------------------------*/
/*----------------- PID Control -----------------*/
float kP = 13.2, kI = 0.01, kD = 20;
//float setPoint = 0;

float proportional = 0, intergral = 0, derivative = 0;
float error = 0, previousError = 0;
float valueOfPID = 0;
/*----------------- PID Control -----------------*/
/*-----------------------------------------------*/

/*-----------------------------------------------*/
/*---------------- Motor Control ----------------*/
#define LEFT_PWM 5
#define LEFT_IN1 8
#define LEFT_IN2 9
#define RIGHT_PWM 6
#define RIGHT_IN3 10
#define RIGHT_IN4 11

int leftBaseMotorSpeed = 80;
int leftMotorSpeed = 0;
int rightBaseMotorSpeed = 80;
int rightMotorSpeed = 0;
/*---------------- Motor Control ----------------*/
/*-----------------------------------------------*/

/*-----------------------------------------------*/
/*--------------- IR Sensor Array ---------------*/
#define IR_SENSOR_L A1
#define IR_SENSOR_LC A2
#define IR_SENSOR_C A5
#define IR_SENSOR_RC A3
#define IR_SENSOR_R A4

float sensorArrayIR[5] = { 0, 0, 0, 0, 0 };
//float calculatedPosition = 0;
/*--------------- IR Sensor Array ---------------*/
/*-----------------------------------------------*/

outputFunctions

/*-----------------------------------------------*/
/*---------------- Motor Control ----------------*/
void motorControlWithPID() {
  leftMotorSpeed = leftBaseMotorSpeed - valueOfPID;
  rightMotorSpeed = rightBaseMotorSpeed + valueOfPID;

  //constrain(leftMotorSpeed, 50, 150);
  //constrain(rightMotorSpeed, 50, 150);
  if (leftMotorSpeed < 0) {
    leftMotorSpeed = 0;
  }
  if (leftMotorSpeed > 150) {
    leftMotorSpeed = 150;
  }
  if (rightMotorSpeed < 0) {
    leftMotorSpeed = 0;
  }
  if (rightMotorSpeed > 150) {
    leftMotorSpeed = 150;
  }


  //Serial.println(leftMotorSpeed);
  //Serial.println(rightMotorSpeed);


  analogWrite(LEFT_PWM, leftMotorSpeed);
  digitalWrite(LEFT_IN1, LOW);
  digitalWrite(LEFT_IN2, HIGH);
  
  analogWrite(RIGHT_PWM, rightMotorSpeed);
  digitalWrite(RIGHT_IN3, LOW);
  digitalWrite(RIGHT_IN4, HIGH);
}
/*---------------- Motor Control ----------------*/
/*-----------------------------------------------*/

inputFunctions

/*-----------------------------------------------*/
/*--------------- IR Sensor Array ---------------*/
/*float readSensorArrayIR() {
  sensorArrayIR[0] = analogRead(IR_SENSOR_L);
  sensorArrayIR[1] = analogRead(IR_SENSOR_LC);
  sensorArrayIR[2] = analogRead(IR_SENSOR_C);
  sensorArrayIR[3] = analogRead(IR_SENSOR_RC);
  sensorArrayIR[4] = analogRead(IR_SENSOR_R);

  for (int i = 0; i < 5; i++) {
    calculatedPosition += sensorArrayIR[i];
  }

  return calculatedPosition;
}*/

int readSensorArrayIR() {
  sensorArrayIR[0] = digitalRead(IR_SENSOR_L);
  sensorArrayIR[1] = digitalRead(IR_SENSOR_LC);
  sensorArrayIR[2] = digitalRead(IR_SENSOR_C);
  sensorArrayIR[3] = digitalRead(IR_SENSOR_RC);
  sensorArrayIR[4] = digitalRead(IR_SENSOR_R);

  if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 1) { error = 4; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 1 && sensorArrayIR[4] == 1) { error = 3; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 1 && sensorArrayIR[4] == 0) { error = 2; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 1 && sensorArrayIR[3] == 1 && sensorArrayIR[4] == 0) { error = 1; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 1 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 0) { error = 0; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 1 && sensorArrayIR[2] == 1 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 0) { error = -1; Serial.println(error);}
  else if (sensorArrayIR[0] == 0 && sensorArrayIR[1] == 1 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 0) { error = -2; Serial.println(error);}
  else if (sensorArrayIR[0] == 1 && sensorArrayIR[1] == 1 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 0) { error = -3; Serial.println(error);}
  else if (sensorArrayIR[0] == 1 && sensorArrayIR[1] == 0 && sensorArrayIR[2] == 0 && sensorArrayIR[3] == 0 && sensorArrayIR[4] == 0) { error = -4; Serial.println(error);}

  return error;
}
/*--------------- IR Sensor Array ---------------*/
/*-----------------------------------------------*/

calculation

/*-----------------------------------------------*/
/*----------------- PID Control -----------------*/
float calculatePID() {
  proportional = error;
  intergral = intergral + error;
  derivative = error - previousError;

  previousError = error;

  Serial.println(derivative);

  return ((kP * proportional) + (kI * intergral) + (kD * derivative));
}
/*----------------- PID Control -----------------*/
/*-----------------------------------------------*/
2

Maybe the sign of Kd or the error term is incorrect.

Also, for the approach to work well, the time interval between calculations must be constant. I see nothing in the incomplete code that you posted which suggests attention to this important detail.

Try another PID library, like this one, which is known to work well.

3

jremington:
Also, for the approach to work well, the time interval between calculations must be constant. I see nothing in the incomplete code that you posted which suggests attention to this important detail.

Why would the loop function not be constant?

4

Pomachu:
Why would the loop function not be constant?

The interrupt service routine that handles timer 0 overflow runs.

Your code takes a different path then the last time loop ran.

Etcetera.

5

okay so I've found a weird bug. If kP == 1, the algorithm works as expect when the left sensor is active "1 0 0 0 0". However, when I set kP == 15 if the far left sensor "1 0 0 0 0" is active it will increase the motor speed to max rather than decrease. What is going on?