Greetings I want to build a line follower
I purchased the materials are:
sensors QTR-8A (pololu)
10:1 Micro Metal Gearmotor MP (pololu)
2 Wheels (pololu)
1 ATMEGA 328
L293D motor driver
I have programmed a PID control but I need help finding the constants KP, KI, KD.
Actually the line follower is out of line.
The track is a black line white background.
Can someone help me and tell me why I can not control the line follower?
PD: The code is not mine and sorry my English, I from Ecuador.
#include <QTRSensors.h>
#define NUM_SENSORS 6 // numero de sensores Usados
#define NUM_SAMPLES_PER_SENSOR 4 // promedio de 4 lecturas por sensor
#define EMITTER_PIN 2 // pin emisor
// Sensores del 0-5 conectados en las entradas analogicas del 0-5
QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
int MOTOR1_PIN1 = 5;//5
int MOTOR1_PIN2 = 6;//6
int MOTOR2_PIN1 = 11;//11
int MOTOR2_PIN2 = 12;//12
unsigned int anterior_proporcional = 0;
long integral = 0;
void setup() {
delay(500);
pinMode(MOTOR1_PIN1, OUTPUT);
pinMode(MOTOR1_PIN2, OUTPUT);
pinMode(MOTOR2_PIN1, OUTPUT);
pinMode(MOTOR2_PIN2, OUTPUT);
int i;
pinMode(13, OUTPUT);
//pinMode(2, OUTPUT);
//digitalWrite(2,HIGH);
digitalWrite(13, HIGH); // Led Indica que se esta Calibrando el sistema
for (i = 0; i < 400; i++)
{
qtra.calibrate();
}
digitalWrite(13, LOW); // Apagado del Led indica terminado de calibrar
}
void loop() {
unsigned int position = qtra.readLine(sensorValues);
// El termino proporcional nos da el error lo restamos de 2500 para hacer el setpoint el centro
int proporcional = ((int)position) - 2500;
// Calcula la derivada (resta) y la integral (suma) de la prosicion porporcional
int derivativo = proporcional - anterior_proporcional;
integral += proporcional;
// Guardamos la ultima posicion
anterior_proporcional = proporcional;
int error = proporcional*0.305 + derivativo*350;// + integral*1/1000;// ; // + derivativo*1.5;
//Como valor maximo (max) le damos 255 que es nuestro pwm maximo y hacemos que despues de calcular el error
//No se sobrepase este valor
const int max = 255;
if(error > max)
error = max;
if(error < -max)
error = -max;
//finalmente seteamos los valores en la funcion creada para los motores (go)
if(error < 0)
//Aumenta la velocidad del Motor
go(max+error, max);
else
//Disminuye la velocidad del motor
go(max, max-error);
}
void go(int speedLeft, int speedRight) {
if (speedLeft > 0) {
analogWrite(MOTOR1_PIN1, speedLeft);
analogWrite(MOTOR1_PIN2, 0);
}
else {
analogWrite(MOTOR1_PIN1, 0);
analogWrite(MOTOR1_PIN2, -speedLeft);
}
if (speedRight > 0) {
analogWrite(MOTOR2_PIN1, speedRight);
analogWrite(MOTOR2_PIN2, 0);
}
else {
analogWrite(MOTOR2_PIN1, 0);
analogWrite(MOTOR2_PIN2, -speedRight);
}
}