Código Final - MASTER

//MASTER

#define BT_CONN 0
#define INBOX 1
#define OUTBOX 5

//NUMERO DE BONECOS A SEREM RESGATADOS
#define BONECOS 5

//DISTANCIA
#define DISTANCIA 200 //mm

//MENSAGENS ENVIADAS
#define VAI 1
#define PARA 2

//MENSAGENS RECEBIDAS
#define PEGUEI 3

// VERIFICA SE HÁ UMA CONEXAO BLUETOOTH ATIVADA
sub BTCheck(int conn){
if (!BluetoothStatus(conn)==NO_ERR){
TextOut(0,LCD_LINE2,"Impossivel conectar");
Wait(1000);
Stop(true);
}
}


// THREAD PRINCIPAL
// FAZ TODA COMUNICACAO COM ESCRAVO
task main(){
int contador = 0 ;

int in;

// ATIVA SENSOR ULTRASONICO
SetSensorLowspeed(IN_4);

// VERIFICA SE HÁ UMA CONEXAO BLUETOOTH ATIVADA
BTCheck(0);

TextOut(0,LCD_LINE1,"Master iniciado ...");
while(true){
// SE O FOR DETECADO ALGUMA COISA A UMA DISTANCIA INFERIOR A 'DISTANCIA'
// TENTA UMA COMUNICACAO
if (SensorUS(IN_4) < DISTANCIA) {
// CASO A QUANTIDADE DE BONECOS RECEBIDOS SEJA IGUAL
// OU SUPERIOR A DE BONECOS ESPERADOS, ENCERRA RESGATE
if (contador >= BONECOS) {
TextOut(0,LCD_LINE1,"Enviando comando de encerramento");
SendRemoteNumber(BT_CONN,OUTBOX,PARA);

Wait(1000);
Stop(true);

// CASO CONTRARIO, ENVIA A ORDEM DE RESGATE
} else {
TextOut(0,LCD_LINE1,"Enviando comando de iniciar resgate");
SendRemoteNumber(BT_CONN,OUTBOX,VAI);
}
}

// VERIFICA SE RECEBEU UMA RESPOSTA DO ESCRAVO
if (ReceiveRemoteNumber(INBOX, true, in) != STAT_MSG_EMPTY_MAILBOX) {
// O ESCRAVO RESPONDEU QUE DEVOLVEU UM BONECO
if (in == PEGUEI) {
TextOut(0,LCD_LINE1,"Boneco recebido");
contador++;
Wait(1000);
}
}
Wait(100);
}
}
}