void LineFollow::ajusta_niveles()
{
int lectura=0;
for(int count=0; count<5; count++){
lectura=IRread(count);
if (lectura > sensor_blanco[count])
sensor_blanco[count]=lectura;
if (lectura < sensor_negro[count])
sensor_negro[count]=lectura;
}
}
uint16_t updateCarStatus(car *c)
{
uint16_t stat = 0;
stat |= IRread(); //obstacle on the sides?
//stat |= USread(); //obstacle front/right?
//updateAccel();
if(accel.z < -1100) //upside down?
stat |= UPSIDEDOWN;
c->status = stat;
angle.y = c->pos->teta;
return stat;
}
void LineFollow::runLineFollow(){
for(int count=0; count<5; count++)
{
lectura_sensor[count]=map(IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
acu+=lectura_sensor[count];
}
//Serial.println(millis());
if (acu > NIVEL_PARA_LINEA)
{
acu/=5;
int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
error = constrain(error,-100,100);
//Calculamos la correcion de velocidad mediante un filtro PD
int vel = (error * KP)/10 + (error-last_error)*KD;
last_error = error;
//Corregimos la velocidad de avance con el error de salida del filtro PD
int motor_left = constrain((robotSpeed + vel),-100,100);
int motor_right =constrain((robotSpeed - vel),-100,100);
//Movemos el robot
//motorsWritePct(motor_left,motor_right);
motorsWritePct(motor_left,motor_right);
//Esperamos un poquito a que el robot reaccione
delay(intergrationTime);
}
else
{
//Hemos encontrado una linea negra
//perpendicular a nuestro camino
//paramos el robot
motorsStop();
//y detenemos la ejecución del programa
//while(true);
reportActionDone();
//setMode(MODE_SIMPLE);
}
}
