Codigo Arduino (Seguidor de linea)

///---Define's---///

#define MAX_SPEED 200 //230 254

#define MED_SPEED 110 //105 135

#define OUT_LINE 16//valor dado para fuera de linea

#define KP 10 // 20 ir incrementando poco a poco y hasta donde no se

salga el carro ahi es.

#define KD 45 //55 que tan suave se acomoda

#define KI 1 //1 mantenerlo bajo

enum {HOME,STARTING,RUN}state = HOME;//se enumeran home, starting y run con 0,1 y2.

state

enum {CENTER,RIGHT,LEFT} out_state = CENTER;

int pin2 =

0,pin3=0,pin4=0,pin5=0,pin6=0,pin7=0,pin8=0,pin9=0,pin10=0,pin11=0,pin12=0,pin14=0,pi

n15=0,pin16=0,pin17=0,pin18=0,pin19=0;

//Local variables//

signed int error_actual = 0;

signed int error_anterior = 0;

int error_sum = 0;

int errores=0;

signed int speed_1 =0; //Motor 2 - left side

signed int speed_2 =0; //Motor 1 - right side

signed int proporcional = 0;

signed int derivativo = 0;

signed int integrativo = 0;

void setup() {

// Configuramos pines

pinMode(2, INPUT); //sensor

pinMode(3, OUTPUT); //PWM1

pinMode(4, INPUT); //Sensor

pinMode(5, INPUT); //sensor

pinMode(6, INPUT); //sensor

pinMode(7, INPUT); //sensor

pinMode(8, INPUT); //sensor

pinMode(9, OUTPUT); //Direccion PWM1

pinMode(10, OUTPUT); //PWM2

pinMode(11, OUTPUT); //Direccion PWM2

pinMode(12, INPUT); //sensor

pinMode(13, OUTPUT); //led

pinMode(14, INPUT); //sensor

pinMode(15, INPUT); //sensor

pinMode(16, INPUT); //sensor

pinMode(17, INPUT); //sensor

pinMode(18, INPUT); //S2

pinMode(19, INPUT); //S1

//Serial.begin(9600);

analogWrite(3,0); //PWM1 487Hz DC. de 0 a 255

analogWrite(10,0); //487Hz PWM2

digitalWrite(9, HIGH);//direccion PWM1

digitalWrite(11, HIGH); //Direccion PWM2

}

///---Functions---///

///Function to read error///

signed int Read_error (void)

{

int b0,b1,b2,b3,b4,b5,b6,b7,b8,b9,b10;

signed int error = 0;

b0=!(digitalRead(8));

b1=!(digitalRead(7));

b3=!(digitalRead(6));

b2=!(digitalRead(12));

b4=!(digitalRead(14));

b5=!(digitalRead(5));

b6=!(digitalRead(15));

b7=!(digitalRead(4));

b8=!(digitalRead(16));

b9=!(digitalRead(2));

b10=!(digitalRead(17));

if (b0||b1||b2||b3||b4||b5||b6||b7||b8||b9||b10) { //Out line - Black line (si

alguno lee linea entra)

/*Negative left sensor*/

error = (b1) ? (0-2) : error; //(0-2) operacion para que el

comilador vea un -2

error = (b3) ? (0-4) : error;

error = (b5) ? (0-6) : error;

error = (b7) ? (0-8) : error;

error = (b9) ? (0-10) : error;

error = (b1 && b3) ? (0-3) : error; //se leen sensores dobles al final

error = (b3 && b5) ? (0-5) : error;

error = (b5 && b7) ? (0-7) : error;

error = (b7 && b9) ? (0-9) : error;

/*Positive right sensor*/

error = (b2) ? 2 : error;

error = (b4) ? 4 : error;

error = (b6) ? 6 : error;

error = (b8) ? 8 : error;

error = (b10) ? 10 : error;

error = (b2 && b4) ? 3 : error;

error = (b4 && b6) ? 5 : error;

error = (b6 && b8) ? 7 : error;

error = (b8 && b10) ? 9 : error;

/*Neutral middle sensor*/

error = (b0) ? 0 : error;

error = (b0 && b1) ? (0-1) : error;

error = (b0 && b2) ? 1 : error;

out_state = ((error <= 4)&&(error >=(0-4))) ? CENTER : out_state; //fuera

de linea

out_state = ((error >= 5)&&(error <=10)) ? LEFT : out_state;

out_state = ((error <=(0-5))&&(error >=(0-10))) ? RIGHT: out_state;

return error;

}

else {

return OUT_LINE;

}

}

///Function to set speed of right motor///

...