//##################################################################################################################################################################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; }