//##################################################################################################################################################################3
void Sensor()
{
//int sum_dir=0, sum_esq=0, sum_frente=0;
printf("Sensor\n");
//para garantir que funcionao
disableObstSens();
//disableLineSens();
//enableLineSens();
enableObstSens();
readAnalogSensors();
/*int i=0;
for( i=0; i<9; i++){
a_s_dir[i]=a_s_dir[i+1];
a_s_esq[i]=a_s_esq[i+1];
a_s_frente[i]=a_s_frente[i+1];
}
a_s_dir[9] = obstacleSensor(OBST_SENSOR_RIGHT);
a_s_esq[9] = obstacleSensor(OBST_SENSOR_LEFT);
a_s_frente[9]= obstacleSensor(OBST_SENSOR_FRONT);
*/
Chegada_Farol(); //verificao da chegada ao farol substitui => linha = lineSensor(LINE_SENSOR_CENTER);
#if 0
if((ciclos%10 )== 0){ //verificar isto pois nao sei se ele ve o farol so de 5 em 5 s
Ver_Farol();
}
#else
Ver_Farol();
#endif
/*// media dos sensores
for( i=0; i<9; i++){
sum_dir=+ a_s_dir[i];
sum_esq=+ a_s_esq[i];
sum_frente=+a_s_frente[i];
}
sensor_esq = sum_dir/10;
sensor_dir = sum_esq/10;
sensor_frente= sum_dir/10;*/
sensor_dir = obstacleSensor(OBST_SENSOR_RIGHT);
sensor_esq = obstacleSensor(OBST_SENSOR_LEFT);
sensor_frente= obstacleSensor(OBST_SENSOR_FRONT);
//printf("D: %d F: %d E: %d LINHA: %d Farol: %d \r", obstacleSensor(0), obstacleSensor(1), obstacleSensor(2), linha, farolsen );
}
//#############################################################################
void Run_Beacon ()
{
disableObstSens();
enableObstSens();
if(obstacleSensor(OBST_SENSOR_FRONT) > LIMIAR)
{
stop_Motors();
if(randomDecisionRotate() == 0)
{
Rodar_Sobre_Si_Dir();
wait(2);
}
else
{
Rodar_Sobre_Si_Esq();
wait(2);
}
}
if(obstacleSensor(OBST_SENSOR_RIGHT) > LIMIAR)
{
Vira_Dir();
}
if(obstacleSensor(OBST_SENSOR_LEFT) > LIMIAR)
{
Vira_esq();
}
if(obstacleSensor(OBST_SENSOR_FRONT) < LIMIAR && obstacleSensor(OBST_SENSOR_RIGHT) < LIMIAR && obstacleSensor(OBST_SENSOR_LEFT)< LIMIAR)
{
andar_frente();
}
if(obstacleSensor(OBST_SENSOR_FRONT) > LIMIAR && obstacleSensor(OBST_SENSOR_RIGHT) > LIMIAR && obstacleSensor(OBST_SENSOR_LEFT)> LIMIAR)
{
setVel2(0,0);
Rodar_Sobre_Si();
wait(1);
}
}
void calculateAverage(){
int temp=obstacleSensor(OBST_SENSOR_LEFT);
if(temp<155) temp=155;
vals[OBST_SENSOR_LEFT][pointer]=temp;
temp=obstacleSensor(OBST_SENSOR_FRONT);
if(temp<155) temp=155;
vals[OBST_SENSOR_FRONT][pointer]=temp;
temp=obstacleSensor(OBST_SENSOR_RIGHT);
if(temp<155) temp=155;
vals[OBST_SENSOR_RIGHT][pointer]=temp;
pointer = (pointer+1) % valsSize;
int i, j;
int tempVals[3][valsSize];
for (i = 0; i < valsSize; i++){
tempVals[OBST_SENSOR_LEFT][i]=vals[OBST_SENSOR_LEFT][i];
tempVals[OBST_SENSOR_FRONT][i]=vals[OBST_SENSOR_FRONT][i];
tempVals[OBST_SENSOR_RIGHT][i]=vals[OBST_SENSOR_RIGHT][i];
}
for (i = 1; i < valsSize; i++) {
for (j = 0; j < valsSize - 1; j++) {
if (tempVals[OBST_SENSOR_LEFT][j] > tempVals[OBST_SENSOR_LEFT][j + 1]) {
temp = tempVals[OBST_SENSOR_LEFT][j];
tempVals[OBST_SENSOR_LEFT][j] = tempVals[OBST_SENSOR_LEFT][j + 1];
tempVals[OBST_SENSOR_LEFT][j + 1] = temp;
}
if (tempVals[OBST_SENSOR_FRONT][j] > tempVals[OBST_SENSOR_FRONT][j + 1]) {
temp = tempVals[OBST_SENSOR_FRONT][j];
tempVals[OBST_SENSOR_FRONT][j] = tempVals[OBST_SENSOR_FRONT][j + 1];
tempVals[OBST_SENSOR_FRONT][j + 1] = temp;
}
if (tempVals[OBST_SENSOR_RIGHT][j] > tempVals[OBST_SENSOR_RIGHT][j + 1]) {
temp = tempVals[OBST_SENSOR_RIGHT][j];
tempVals[OBST_SENSOR_RIGHT][j] = tempVals[OBST_SENSOR_RIGHT][j + 1];
tempVals[OBST_SENSOR_RIGHT][j + 1] = temp;
}
}
}
//printf("--> %d %d %d %d %d %d %d - %d\n", tempVals[OBST_SENSOR_LEFT][0], tempVals[OBST_SENSOR_LEFT][1], tempVals[OBST_SENSOR_LEFT][2],
// tempVals[OBST_SENSOR_LEFT][3], tempVals[OBST_SENSOR_LEFT][4], tempVals[OBST_SENSOR_LEFT][5], tempVals[OBST_SENSOR_LEFT][6], tempVals[OBST_SENSOR_LEFT][valsSize/2]);
obstValFront = (6200 / (tempVals[OBST_SENSOR_FRONT][valsSize/2]-80)) - 7;
obstValLeft = (6200 / (tempVals[OBST_SENSOR_LEFT][valsSize/2]-80)) - 9;
obstValRight = (6200 / (tempVals[OBST_SENSOR_RIGHT][valsSize/2]-80)) - 8;
if(obstValFront>80) obstValFront = 80;
else if (obstValFront<0) obstValFront = 0;
if(obstValLeft>80) obstValLeft = 80;
else if (obstValLeft<0) obstValLeft = 0;
if(obstValRight>80) obstValRight = 80;
else if (obstValRight<0) obstValRight = 0;
}
