ALGORITMO EN ARDUINO. Seguidor con PID

#include <QTRSensors.h>
/////////////////////////////////////////////////////////////////////////////////////
//************* Programa realizado por MARKO A. CABALLERO MORENO*************** //
// Solo para fines educativos //
// robot velocista mini FK BOT V 2.0

//

// micro motores pololu MP 10:1, sensores qtr 8rc, driver drv8833, arduino nano
//

05/12/2014

// ACTUALIZADO 29/3/2015

//

/////////////////////////////////////////////////////////////////////////////////////
#define NUM_SENSORS 8 //numero de sensores usados
#define TIMEOUT

2000 // tiempo de espera para dar resultado en uS

#define EMITTER_PIN 6

//pin led on

///////////////pines arduino a utilizar/////////////////////
#define led1 …ver más…

pagar sensores para indicar fin
//de calibracion delay(400); digitalWrite(led2,HIGH); //encender led 2 para indicar la
// espera de pulsacion de boton while(true) { int x=digitalRead(boton_1); //leemos y guardamos el valor
// del boton en variable x delay(100); if(x==0) //si se presiona boton
{
digitalWrite(led2,LOW); //indicamos que se presiono boton digitalWrite(led1,HIGH); //encendiendo led 1 delay(100); break; //saltamos hacia el bucle principal
}
}
}

void loop()
{
//pid(0, 120, 0.18, 4, 0.001);

pid(linea,velocidad,Kp,Ki,Kd); //funcion para algoritmo pid
//(tipo,flanco de comparacion)
//frenos_contorno(0,700);
frenos_contorno(linea,700); //funcion para frenado en curvas tipo
//0 para lineas negras, tipo 1 para lineas blancas
//flanco de comparación va desde 0 hasta 1000 , esto para ver
//si esta en negro o blanco
}

////////funciones para el control del robot//// void pid(int linea, int velocidad, float Kp, float Ki, float Kd)
{
position = qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, linea); //0 para linea
//negra, 1 para linea blanca proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error integral=integral + proporcional_pasado; //obteniendo integral derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo if (integral>1000) integral=1000; //limitamos la integral para no causar problemas if (integral<-1000) integral=-1000; salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki); if ( salida_pwm > velocidad )