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