Im using arduíno pro mini with QRT sensors and trying tô make a LINE follower code using PID, some translations might bê helpfull for understanding:
ÚltimoErro : LastError
And the car wont drive in a straight line even when the sensors read a error = 0
Any tips on why the code isnt working i would apreciate
// Portas driver motor
#define PININ1 2
#define PININ2 4
#define PININ3 5
#define PININ4 7
#define PINENA 3
#define PINENB 6
// Portas led rgb
#define PINLEDR 9
#define PINLEDG 11
#define PINLEDB 10
// Portas sensor QTR
#define S1 A0
#define S2 A1
#define S3 A2
#define S4 A3
#define S5 A4
#define S6 A5
// Valores de ajustes para o seguidor de linha MIF
#define TRESHOLD 700 // Valor de referencia para cor da linha branca
#define RUNTIME 15500 // Valor para executar o percurso
void setup() {
Serial.begin(9600);
}
void loop() {
// TESTE 1°: leituta sensor
//int s[6];
//readSensors(true, s);
// TESTE 2°: motor esquerda
// motorOption('4',255,255);
//TESTE 3°: motor direita
// motorOption('6', 255, 255);
// TESTE 4°: seguidor de linha
followLineMEF();
// TESTE 5°: teste led RGB
// rgbControl(255,0,0,0);
motorControl();
}
int Kp = 1;
int Kd = 100;
int Ki = 1;
float integral = 0;
float derivada = 0;
float ultimoErro = 0;
int veloMotor = 0;
int speedLeft, speedRight;
void motorControl(void) {
int erro = followLineMEF();
integral = integral + erro;
derivada = erro - ultimoErro;
veloMotor = (Kp * erro) + (Ki * integral) + (Kd * derivada);
// Função para controle do driver de motor
// Definições das portas digitais
pinMode(PININ1, OUTPUT);
pinMode(PININ2, OUTPUT);
pinMode(PININ3, OUTPUT);
pinMode(PININ4, OUTPUT);
pinMode(PINENA, OUTPUT);
pinMode(PINENB, OUTPUT);
// Ajustes motor da esquerda
//frente
if (erro == 0) {
speedLeft = 255;
speedRight = 255;
digitalWrite (PININ4, HIGH);
digitalWrite (PININ3, LOW);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, HIGH);
digitalWrite (PININ2, LOW);
analogWrite (PINENB, speedRight);
}
//esquerda
else if (erro <= 0) {
if (veloMotor > 255) {
speedLeft = 255;
speedRight = 0;
digitalWrite (PININ3, HIGH);
digitalWrite (PININ4, LOW);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, LOW);
digitalWrite (PININ2, HIGH);
analogWrite (PINENB, speedRight);
} else {
speedLeft = 180 + veloMotor * -1;
speedRight = 180 + veloMotor;
digitalWrite (PININ3, HIGH);
digitalWrite (PININ4, LOW);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, LOW);
digitalWrite (PININ2, HIGH);
analogWrite (PINENB, speedRight);
}
}
// Caso direita TA INDO ACHO
else if (erro > 0) {
if (veloMotor > 255) {
speedLeft = 0;
speedRight = 255;
digitalWrite (PININ3, LOW);
digitalWrite (PININ4, HIGH);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, LOW);
digitalWrite (PININ2, HIGH);
analogWrite (PINENB, speedRight);
}
else {
speedRight = 180 + veloMotor;
speedLeft = veloMotor * -1 + 255;
digitalWrite (PININ3, HIGH);
digitalWrite (PININ4, LOW);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, HIGH);
digitalWrite (PININ2, LOW);
analogWrite (PINENB, speedRight);
}
}
else {
speedRight = 255;
speedLeft = 255;
digitalWrite (PININ4, LOW);
digitalWrite (PININ3, HIGH);
analogWrite (PINENA, speedLeft);
digitalWrite (PININ1, LOW);
digitalWrite (PININ2, HIGH);
}
ultimoErro = erro;
}
bool motorStop(long runtime, long currentTime) {
// Função de parada do robô
if (millis() >= (runtime + currentTime)) {
int cont = 0;
while (true) {
cont++;
}
return false;
}
return true;
}
void readSensors(bool readSerial, int *sensors) {
// Função para leitura dos sensores
sensors[0] = analogRead(S1);
sensors[1] = analogRead(S2);
sensors[2] = analogRead(S3);
sensors[3] = analogRead(S4);
sensors[4] = analogRead(S5);
sensors[5] = analogRead(S6);
if (readSerial) {
Serial.print(sensors[0]);
Serial.print(' ');
Serial.print(sensors[1]);
Serial.print(' ');
Serial.print(sensors[2]);
Serial.print(' ');
Serial.print(sensors[3]);
Serial.print(' ');
Serial.print(sensors[4]);
Serial.print(' ');
Serial.println(sensors[5]);
}
}
int followLineMEF(void) {
// Função para controle do seguidor de linha em modo de maquina de estado finita
bool flag = true;
long currentTime = millis();
int erro;
while (flag) {
// Flag para verificar a parada
flag = motorStop(RUNTIME, currentTime);
// Leitura sensores
int s[6];
readSensors(true, s);
// leitura do sensor (1 1 1 1 1 1)
if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
//motorControl(0);
erro = 0;
// leitura do sensor (0 1 1 1 1 0)
} else if ( s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(0);
erro = 0;
// leitura do sensor (0 0 1 1 0 0) //frente
} else if ( s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(0);
erro = 0;
// leitura do sensor (0 1 1 1 0 0)
} else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
// motorControl(1);
erro = 1;
// leitura do sensor (0 0 1 1 1 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD ) {
//motorControl(-1);
erro = -1;
// leitura do sensor (0 0 1 0 0 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(2);
erro = 2;
// leitura do sensor (0 0 0 1 0 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD ) {
//motorControl(-2);
erro = -2;
// leitura do sensor (0 1 1 0 0 0)
} else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(3);
erro = 3;
// leitura do sensor (0 0 0 1 1 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(-3);
erro = -3;
// leitura do sensor (1 1 1 0 0 0)
} else if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] <= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(4);
erro = 4;
// leit;ra do sensor (0 0 0 1 1 1)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] <= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
//motorControl(-4);
erro = -4;
// leitra do sensor (0 1 0 0 0 0)
} else if (s[0] >= TRESHOLD && s[1] <= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
// motorControl(5);
erro = 5;
// leitura do sensor (0 0 0 0 1 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] <= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(-5);
erro = -5;
// leitura do sensor (1 1 0 0 0 0)
} else if (s[0] <= TRESHOLD && s[1] <= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(6);
erro = 6;
// leitura do sensor (0 0 0 0 1 0)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] <= TRESHOLD && s[5] <= TRESHOLD) {
//motorControl(-6);
erro = -6;
// leitura do sensor (1 0 0 0 0 0)
} else if (s[0] <= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] >= TRESHOLD) {
//motorControl(7);
erro = 7;
// leitura do sensor (0 0 0 0 0 1)
} else if (s[0] >= TRESHOLD && s[1] >= TRESHOLD && s[2] >= TRESHOLD && s[3] >= TRESHOLD && s[4] >= TRESHOLD && s[5] <= TRESHOLD) {
//motorControl(-7);
erro = -7;
}
return erro;
}
}