//##################################################################################################################################################################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 sensors_finish(){
disableObstSens();
disableGroundSens();
}
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);
}
