//##################################################################################################################################################################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 ); }
/*esta funcao tem de ser alterada pois ele tem de ver se encotra o farol a 360º */ void Ver_Farol() { setServoPos(0); readAnalogSensors(); if( (farolsen = readBeaconSens())==1) { andar_frente(); } }
/*esta funcao tem de ser alterada pois ele tem de ver se encotra o farol a 360º */ void Ver_Farol() { stop_Motors(); int pos = -15, cont=0; readAnalogSensors(); do{ setServoPos(pos); pos++; if(pos== (15)){ pos= -15; cont++; } wait(1); }while((readBeaconSens() ==0 ) && cont <= 1); printf("%d \n", pos); if(pos<0 && cont <2){ printf("1\n"); do{ setVel2 (-Velocidade, Velocidade); pos++; setServoPos(pos); wait(0.8); setVel2(0,0); // rotateRel_naive((double) pos ); }while((readBeaconSens())==0 && pos != 0); }else if(pos>0 && cont <2){ printf("2\n"); do{ setVel2 (Velocidade, -Velocidade); pos--; setServoPos(pos); wait(0.8); setVel2(0,0); // rotateRel_naive((double) (pos* (-1) ) ); }while((readBeaconSens())==0 && pos != 0); } setServoPos(0); }
int main (void) { //iniciçao da pic initPIC32 (); closedLoopControl( true ); setVel2(0, 0); T4CONbits.TCKPS=5; PR4=(PBCLK/32/3)-1;//fout 20MHz/(32*(62499+1))*10Hz,freq a 1 hz TMR4=0;//Reset timer T2 T4CONbits.TON=1; //Interrupts IFS0bits.T4IF=0;//reset do timer IPC4bits.T4IP=2;//priority IEC0bits.T4IE=1;//enable timer3 EnableInterrupts(); printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot printf("\r battery: %d ", batteryVoltage()); printStr("\n"); //desligar os leds para o inicio leds(0b1111); //leds off #if 0 while(1) { while(!tick40ms); tick40ms=0; readAnalogSensors(); } #endif while (TRUE) { TimeOut(); // timeOut => tb devia ir para uma inturrupcao //Sensor(); //leitura os sensores tem de se por numa interropecao //botao de estado if(startButton() == 1) // Botao start(preto) primido { estado = 1; resetCoreTimer(); leds(0x1); } else if(stopButton() == 1) //Botao stop(vermelho) primido { estado = 0; leds(0x0); } //funcoa de funcionameneto if(estado == 1) // robot fica em presesamento { Ver_Farol(); printf("D: %d F: %d E: %d LINHA: %d Farol: %d \r", sensor_dir,sensor_frente,sensor_esq,linha ,farolsen ); ANDAR2(); } //quando e primido o botao stop else if(stopButton() == 1) // deslica o funcionamento, nenhum led activo { Stop_robot(); } } return (0); }
void check_analog_sensors(){ if(internal_values.analog_sensors_updated == NO){ readAnalogSensors();// Fill in "analogSensors" structure internal_values.analog_sensors_updated = YES; } }
int main (void) { int countCiclos = 100; //iniciçao da pic initPIC32 (); closedLoopControl( true ); setVel2(0, 0); printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot printf("\r battery: %d ", batteryVoltage()); if(batteryVoltage() <94){ printf("_Bateria fraca, MUDAR Bateria\n"); } printStr("\n"); while(1) { while(!tick40ms); tick40ms=0; readAnalogSensors(); //state buttons if(startButton() == 1) // Botao start(preto) primido { estado = 1; enableObstSens(); leds(0x0); //leds off countCiclos = 100; } else if(stopButton() == 1) //Botao stop(vermelho) primido { printf("Red button pressed!!! \n"); estado = 0; disableObstSens(); } if(estado == 1) { TimeOut(); // timeOut => tb devia ir para uma inturrupcao Chegada_Farol(); if(countCiclos++ >= 100) { Ver_Farol(); countCiclos = 0; } Run_Beacon(); } if(estado == 2) { leds(0x1); Stop_robot(); } if(estado == 3) Fim(); else if(stopButton() == 1 || estado == 0) // deslica o funcionamento, nenhum led activo { Stop_robot(); } } return (0); }