|
|
@ -88,16 +88,32 @@ void loop() { |
|
|
|
} |
|
|
|
Serial.println(pos); |
|
|
|
*/ |
|
|
|
|
|
|
|
/*
|
|
|
|
*El error generado de 500 a 4000, se mapea a un valor |
|
|
|
*aceptable de -255 a 255 para establecer un valor adecuado |
|
|
|
*del error en función de la salida de los motores |
|
|
|
*/ |
|
|
|
int error=map(pos,500,4000,-255,255); |
|
|
|
Serial.print(pos); |
|
|
|
Serial.print("->"); |
|
|
|
Serial.println(error); |
|
|
|
|
|
|
|
luces(false,false); |
|
|
|
|
|
|
|
/*
|
|
|
|
* TODO: se debe agregar el código para incluir los valores de |
|
|
|
* D e I, pues actualmante solo tutiliza control P |
|
|
|
*/ |
|
|
|
float pid=kp*error; |
|
|
|
acelerar(70+pid,70-pid); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Funcion que controla la velocidad de cada motor, por lo |
|
|
|
* que se envia el PWM que será aplicado a cada motor, |
|
|
|
* acepta cualquier valor, pero esta validado para que solo |
|
|
|
* se pueda utilizar un rango de 0 - 255, si envias otro |
|
|
|
* valor, por defecto se coloca el extremo correspondiente |
|
|
|
*/ |
|
|
|
void acelerar(int motIzq, int motDer){ |
|
|
|
if(motIzq<0){ |
|
|
|
analogWrite(PWMB,0); |
|
|
@ -119,6 +135,11 @@ void acelerar(int motIzq, int motDer){ |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Función que permite controlar dos led's uno derecho y |
|
|
|
* el otro en la izquierda del seguidor |
|
|
|
*/ |
|
|
|
void luces(boolean izq,boolean der){ |
|
|
|
digitalWrite(LED_IZQ,izq); |
|
|
|
digitalWrite(LED_DER,der); |
|
|
@ -142,6 +163,9 @@ void leerRAW(){ |
|
|
|
* encontrar la posición de la linea negra en todo momento |
|
|
|
* es similar a la función de la libreria QTR, por lo que regresará |
|
|
|
* un valor entre 0 y 4000 |
|
|
|
* |
|
|
|
* NOTA: despues de llamar a esta función pueces ver los |
|
|
|
* valores en el arreglo llamado "valores" |
|
|
|
*/ |
|
|
|
int getPosicion(){ |
|
|
|
leerDigital(); |
|
|
@ -168,6 +192,8 @@ int getPosicion(){ |
|
|
|
* linea es multipo de 500 |
|
|
|
* |
|
|
|
* 500 1000 1500 2000 2500 3000 3500 4000 |
|
|
|
* |
|
|
|
*NOTA: si no se detencta linea, se regresa un 0 |
|
|
|
*/ |
|
|
|
void leerDigital(){ |
|
|
|
//LEER LOS VALORES DE LOS SENSORES
|
|
|
@ -179,11 +205,9 @@ void leerDigital(){ |
|
|
|
|
|
|
|
/*
|
|
|
|
* Esta funcion tiene como objetivo encontrar el valor mas alto |
|
|
|
* y mas bajo de los sensores, para establecer su umbral, por el momento solo |
|
|
|
* encuentra los valores bajos (blancos) y queda pendiente encontrar |
|
|
|
* los valores negros para obtener su promedio para el umbral. |
|
|
|
* |
|
|
|
* TODO:reacomodar esta función |
|
|
|
* y mas bajo de los sensores, para establecer su umbral, lo cual |
|
|
|
* se consigue sacando el promedio de el valor mas bajo (blanco) |
|
|
|
* y el mas alto (negro) |
|
|
|
*/ |
|
|
|
void calibracion(){ |
|
|
|
int min[]={1023,1023,1023,1023,1023,1023,1023,1023}; |
|
|
|