Codigo seguimiento linea robot

Les parts de cada codi que s’han anat afegint estan en negreta.
1.1. Lectura dels sensors de color “sniffer”
Aquest primer procediment consisteix en fer una lectura dels sensors i crear una funció que sigui l’encarregada de representar al display la lectura de cada sensor.
També s’ha comprovat l’efecte d’histèresi produïts pels inversors Schmitt Trigger desplaçant els sensors lentament perl’espectre de grisos del full de la pràctica.
#include // Inclueix la llibreria LiquidCrystal

// Variables dels pins d'entrada/sortida declarades i inicialitzades
int Enable = 8;
int RS = 7;
int PinLCD1 = 6;
int PinLCD2 = 4;
int PinLCD3 = 0;
int PinLCD4 = 1;
int VBat = 4;
int snifferesq = A3; //sensor esquerra
int sniffercenesq = A2; //sensor centre esquerra
int sniffercendrt = A0;//sensor centre dreta
int snifferdrt = A1; //sensor dreta

int Numpolsesq;
int Numpolsdrt;

int Es;
int Dr;

const float pi = 3.14159265359;
int Numinterr = 0;
float Desplasament = (2.*pi*2.5*15.)/360.;
float TotalDesEsq = 0;
float TotalDesDrt = 0;

//Variables
float Bat = 0;
int s1, s2, s3, s4;
LiquidCrystal lcd(Enable, RS, PinLCD1, PinLCD2, PinLCD3, PinLCD4); //La funcióLiquidCrystal té 6 parametres.

/*
El primer parametre indica quina sortida de l'arduino esta connectada al port Rs de la LCD.
El segon parametres indica quina sortida de l'arduino esta connectada al porta Enable de la LCD.
Els altres 4 són els controls de la LCD.
*/

void setup() {

TCCR0A = B00100011;
TCCR0B = B00000011;
TCCR1A = B10100001;
TCCR1B = B00001011;
TCCR2A =B10000011;
TCCR2B = B00000100; //abans era: B00000011, s'ha trobar l'error i modificat
TIMSK2 = B00000001;

//configuració i pins d'entrada i sortida
attachInterrupt(0, ComptaEsquerra, CHANGE);
pinMode(2, INPUT);
attachInterrupt(1, ComptaDreta, CHANGE);
pinMode(3, INPUT);

pinMode(Enable, OUTPUT);
pinMode(RS, OUTPUT);
pinMode(PinLCD1, OUTPUT);
pinMode(PinLCD2,OUTPUT);
pinMode(PinLCD3, OUTPUT);
pinMode(PinLCD4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(10, OUTPUT);
pinMode(9, OUTPUT);
pinMode(11, OUTPUT);
pinMode(snifferesq, INPUT);
pinMode(sniffercenesq, INPUT);
pinMode(sniffercendrt, INPUT);
pinMode(snifferdrt, INPUT);

lcd.begin(16, 2); // El primer parametres és el numero de columnes que té la LCD i el segon el numero defiles que té la LCD.
}

void loop(){
//llegir_bateria(0,0);
//consigna(50,50);
//VelocitatRodes();
Lecturasensors();
Esciuresniffer();
}

void llegir_bateria(int col,int fil){
lcd.setCursor(col, fil); // Col•loca el cursor en el punt Columna 0, i en la fila 1
lcd.print("Bat: "); // Aquesta funció imprimeix el que escribim entre cometes i cadara impres tot el programa finsque parem l'arduino
//El potenciometre ha d'estar a 8.333k(homs)
Bat = ((analogRead(VBat)*11.)/1023.);
lcd.print(Bat); // Aquesta funcio escriura a la LCD el temps millis divit per 1000 cada volta de programar
lcd.print("V");
}

void VelocitatRodes(){
lcd.setCursor(0,1);
lcd.print("E: ");
lcd.print(TotalDesEsq);
lcd.print(" D: ");
lcd.print(TotalDesDrt);
}

voidconsigna(float Esquerra, float Dreta){ // Control de la velocitat de les rodes

if(Esquerra >= 0){
OCR1B = 0; //Esquerra endarrere
OCR0B = (Esquerra/100)*255; //Esquerra endavant
}else if(Esquerra < 0){
OCR1B = (-Esquerra/100)*255; //Esquerra endarrere
OCR0B = 0; //Esquerra endavant
}

if(Dreta >= 0){
OCR1A = (Dreta/100)*255; //Dreta endavant
OCR2A =0; //Dreta endarrere
}else if(Dreta < 0){
OCR1A = 0; //Dreta endavant
OCR2A = (-Dreta/100)*255; //Dreta endarrere
}
}

void ComptaEsquerra(){
Numpolsesq = Numpolsesq + 1;

if (Es == 1){
Numpolsesq = 0;
Es = 0;
}
}

void ComptaDreta(){
Numpolsdrt = Numpolsdrt + 1;

if (Dr == 1){
Numpolsdrt = 0;
Dr = 0;
}
}

ISR(TIMER2_OVF_vect)...