// Programa: Sigue el sonido // Equipo: Grisell Sofia Estrada

Anuncio
//
//
//
//
Programa: Sigue el sonido
Equipo:
Grisell Sofia Estrada Guardado
Patricia Espinosa de los monteros Riveros
Roberto Francisco Saldana Nuno
//_________________________________________________________
//Definimos OIDO como palabra a usar para utilizar el sensor de sonido
#define OIDO SensorValue(S3)
//Creamos nuestro primer task que es avanza, y su función es avanzar en linea recta
task avanza()
{
while( true )
{
//Seteamos ambos motores a que giren a la misma potencia en la misma dirección
motor[motorB] = 40;
motor[motorC] = 40;
}
}
//Este es el task para ubicar la fuente de sonido
task buscaSonido()
{
//Hacemos que las llantas nos se muevan por la inercia
bFloatDuringInactiveMotorPWM = false;
//Encendemos el display
bNxtLCDStatusDisplay = true;
while( true )
{
//Verificamos si hay sonido
if( SensorValue(S3) > 500 )
{
//Si sí hay sonido, el task se adueña del CPU
hogCPU();
motor[motorB] = 0;
motor[motorC] = 0;
el robot
//El robot da una vuelta completa en busca del sonido
nMotorEncoder[motorB] = 0;
nMotorEncoderTarget[motorB] = 720; //Aqui tiene que dar una vuelta completa
motor[motorB] = 30;
motor[motorC] = -30;
//wait10Msec(50);
while(nMotorRunState[motorB] != runStateIdle) // while Motor B is still running
(hasn't reached target yet):
{
nxtDisplayStringAt(0, 31, "%d, %d", SensorValue(S3), 0);
if(SensorValue(S3) < 500)
{
//Si mientras gira se encuentra con el sonido deja de girar
break;
}
}
//Libera el CPU y se repite el ciclo
releaseCPU();
}
}
}
//Pequeña rutina para buscar la fuente de sonido al inicio
task buscaFuente()
{
bFloatDuringInactiveMotorPWM = false;
bNxtLCDStatusDisplay = true;
int flag = 0;
while( true )
{
flag = 0;
if( SensorValue(S3) > 400 )//Si hay silencio
{
//Avanzará hacia adelante aproximada mente 1 metro
nMotorEncoder[motorB] = 0;
nMotorEncoderTarget[motorB] = 3600;
motor[motorB] = 50;
motor[motorC] = 50;
while(nMotorRunState[motorB] != runStateIdle) // while Motor B is still running
(hasn't reached target yet):
{
if(SensorValue(S3) < 400)
{
//Si mientas esta avanzando se encuentra con el sonido se va a detener
flag = 1;
break;
}
}
ciclo
if(flag != 1)//Sino encontró el sonido va a girar muy poco y va a volver a repetir el
{
vuelta completa el robot
}
else
nMotorEncoder[motorB] = 0;
nMotorEncoderTarget[motorB] = 40; //Aqui tiene que dar una
motor[motorB] = 30;
motor[motorC] = -30;
//Si encontró el sonido se acaba el ciclo
break;
}
}
//Cuando encuentra el sonido hace que inicien las task de avanza y de buscar sonido
StartTask(avanza);
StartTask(buscaSonido);
}
task main()
{
//Hacemos que las llantas no se muevan por la inercia
bFloatDuringInactiveMotorPWM = false;
bNxtLCDStatusDisplay = true;
//Iniciamos el task de buscar fuente
StartTask(buscaFuente);
while( true )
{
nxtDisplayStringAt(0, 31, "%d, %d", SensorValue(S3), 0);
}
}
Descargar