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