Código Final - SLAVE

//SLAVE

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

bool sensor, sentido, touch, ativado;
int move_time,count, max_busca, x=0;
mutex semaforo;

#define velocidade 50

#define esquerda true
#define direita false

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

//MENSAGENS ENVIADAS
#define PEGUEI 3

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

// THREAD QUE REALIZA COMUNICACAO COM MASTER
task procura_master(){
int in;

// FICA SEMPRE AGUARDANDO CONTATO DO MASTER
while(true){
if (ReceiveRemoteNumber(INBOX, true, in) != STAT_MSG_EMPTY_MAILBOX) {
// O MASTER MANDOU O ESCRAVO INICIAR O RESGATE
if (in == VAI) {
// AINDA NÃO CAPTUREI NENHUM BONECO,
// VOU PROCURAR ALGUÉM
if ((x == 0) && (ativado == false)) {
TextOut(0,LCD_LINE1,"Indo ao resgate ...");
OnFwd(OUT_BC, velocidade);
ativado = true;
}
// JÁ TENHO UM BONECO CAPTURADO
// O MASTER SÓ ENTRARÁ EM CONTATO COM O SLAVE
// SE A DISTANCIA ENTRE ELES FOR PEQUENA,
// LOGO, SE HOUVER CONTATO, PODE SOLTAR O REFÉM
else if (x == 1) {
// ENTRA NA REGIAO MUTUAMENTE EXCLUSIVA,
// INTERROMPE OS MOTORES, SOLTA O BONECO,
// INFORMA AO MESTRE E SE POSICIONA PARA
// POSSIVEL NOVO RESGATE
Acquire(semaforo);
ativado = false;
Off(OUT_BC);

TextOut(0,LCD_LINE1,"Devolvendo boneco ...");
SendRemoteNumber(BT_CONN,OUTBOX,PEGUEI);
//SendRemoteString(BT_CONN,OUTBOX,PEGUEI);

// ABRE A GARRA
OnRev(OUT_A, 50);
Wait(500);
Off(OUT_A);
x=0;

// SE POSICIONA
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
Wait(500);
until (sensor){
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}

Wait(1000);
Release(semaforo);
}

}
// RECEBI ORDEM DE FINALIZACAO
if (in == PARA) {
TextOut(0,LCD_LINE1,"Resgate finalizado ...");
Stop(true);
}
}
}
}
}

// VERIFICA SENSORES
task testa_sensor(){
while(true){
sensor = SENSOR_3;
touch = SENSOR_1;
}
}

// REALIZA MOVIMENTACAO DO ROBÔ
task atua_motores(){
while(true){
Acquire(semaforo);
// SE NÃO RECEBEU ORDEM DE RESGATE, NÃO FAZ NADA
if (!ativado) {
Release(semaforo);
continue;
}

// ESTOU FORA DA LINHA :(
if (!sensor) {
while ((count < move_time) && (!sensor)) {
// viro para a esquerda
if (sentido) {
OnFwd(OUT_B, velocidade);
OnRev(OUT_C, velocidade);
}
// viro para a direita
else {
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}
count++;

// achei a linha
if (sensor) break;
}

// PROXIMA VEZ QUE SAIR DA LINHA
// VIVAREI NO SENTIDO OPOSTO
sentido = !sentido;

if (max_busca < 5)
move_time = move_time + 150;
else {
max_busca =0;
move_time = move_time + 2000;
sentido = esquerda;
}

max_busca++;
count = 0;
}
// ESTOU NA LINHA
else {
OnFwd(OUT_BC, velocidade);
count = 0;
move_time = 150;
max_busca = 0;

//sentido = !sentido;
}

Release(semaforo);
}
}

// REALIZA O RESGATE DO BONECO
task garra(){
while(true){
Acquire(semaforo);
if (touch == true && x==0){
Off(OUT_BC);
OnFwd(OUT_A, 35);
Wait(1000);
x=1;


OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
Wait(500); until (sensor){
OnFwd(OUT_C, velocidade);
OnRev(OUT_B, velocidade);
}

}
Release(semaforo);
}
}

task main()
{
max_busca = 0;
count = 0;
sentido = esquerda; //false = direita, true = esquerda
ativado = false;

SetSensorType(IN_1,SENSOR_TYPE_TOUCH); //sensor de toque
SetSensorMode(IN_1,SENSOR_MODE_BOOL);
SetSensorType(IN_3,SENSOR_TYPE_LIGHT_ACTIVE); //sensor de luz
SetSensorMode(IN_3,SENSOR_MODE_BOOL);

move_time = 150;

BTCheck(1);

// LEVANTA A GARRA
OnRev(OUT_A, 50);
Wait(500);
Off(OUT_A);

Precedes(testa_sensor, atua_motores, garra, procura_master);
}