slave_opt_uni.nxc

//SLAVE
#include "NXCDefs.h"
#define BT_CONN 1
# define INBOX 5
# define OUTBOX 1
# define IN_MBOX 5
# define OUT_MBOX 1
# define VEL 52
# define ESP 50
# define VELGIRO 30
# define cruce 45
# define camino 60
# define ESPB 20

int opt[], j, max;
int buscar = ESPB;
int i, espera = ESP, luz, luz_blanca, luz_negra, inc;
bool derecha;
bool fin, visitado;
int i_opt, ack, go;
int estado, estado_ant, x1, y1;

sub FIN() {
  while (true) {
    Off(OUT_BC);
    PlayTone(262, 400);
    Wait(500);
    PlayTone(294, 400);
    Wait(500);
    PlayTone(330, 400);
    Wait(500);
    PlayTone(294, 400);
    Wait(500);
    Wait(2000);
  }
}

sub BTCheck(int conn) {
  if (!BluetoothStatus(conn) == NO_ERR) {
    TextOut(5, LCD_LINE2, "Error");
    Wait(1000);
    Stop(true);
  }
}

sub ColocaI2() {
  OnFwd(OUT_B, VELGIRO);
  OnRev(OUT_C, VELGIRO);
  Wait(300);
  Off(OUT_BC);
}

sub Caminar(int tiempo) {
  OnFwd(OUT_B, VEL);
  OnFwd(OUT_C, VEL);
  Wait(tiempo);
  Off(OUT_BC);
}

sub Vuelta180() {
  OnFwd(OUT_C, VELGIRO);
  OnRev(OUT_B, VELGIRO);
  Wait(1250);
  Off(OUT_BC);
  for (i = 0; i < 250; i++) {
    OnFwd(OUT_C, VELGIRO);
    OnRev(OUT_B, VELGIRO);
    Wait(10);
    if ((Sensor(IN_3) < camino) && (Sensor(IN_3) > cruce)) {
      Off(OUT_BC);
      return;
    }
  }
}

sub Vuelta90D() {
  OnFwd(OUT_C, VELGIRO);
  OnRev(OUT_B, VELGIRO);
  Wait(470);
  Off(OUT_BC);
  for (i = 0; i < 80; i++) {
    OnFwd(OUT_C, VELGIRO);
    OnRev(OUT_B, VELGIRO);
    Wait(10);
    //if(Sensor(IN_3) < cruce){ FIN(); break;}
    if ((Sensor(IN_3) < camino) && (Sensor(IN_3) > cruce)) {
      Off(OUT_BC);
      return;
    }
  }
}

sub Vuelta90I() {
  OnFwd(OUT_B, VELGIRO);
  OnRev(OUT_C, VELGIRO);
  Wait(470);
  Off(OUT_BC);
  for (i = 0; i < 80; i++) {
    OnFwd(OUT_B, VELGIRO);
    OnRev(OUT_C, VELGIRO);
    Wait(10);
    if ((Sensor(IN_3) < camino) && (Sensor(IN_3) > cruce)) {
      Off(OUT_BC);
      return;
    }
  }
}

sub Recto2() {
  while (Sensor(IN_3) > camino) {
    OnFwd(OUT_C, VELGIRO);
    OnRev(OUT_B, VELGIRO);
    Wait(10);
  }
  Off(OUT_BC);
}

sub BuscarD() {
  for (i = 0; i < 25; i++) {
    OnFwd(OUT_C, VELGIRO - 7);
    OnRev(OUT_B, VELGIRO - 7);
    Wait(buscar);
    //if(SensorUS(IN_4) <= 12) break;
    if (Sensor(IN_3) < cruce) break;
    if ((Sensor(IN_3) < camino) && (Sensor(IN_3) > cruce)) {
      derecha = false;
      //OnFwd(OUT_BC, 15);
      //Wait(100);
      Float(OUT_BC);
      break;
    }
  }
}

sub BuscarI() {
  for (i = 0; i < 25; i++) {
    OnFwd(OUT_B, VELGIRO - 7);
    OnRev(OUT_C, VELGIRO - 7);
    Wait(buscar);
    //if(SensorUS(IN_4) <= 12) break;
    if (Sensor(IN_3) < cruce) break;
    if ((Sensor(IN_3) < camino) && (Sensor(IN_3) > cruce)) {
      derecha = true;
      //OnFwd(OUT_BC, 15);
      //Wait(100);
      Float(OUT_BC);
      break;
    }
  }
}

void Sigue_Linea() {
  while (true) {
    if (Sensor(IN_3) < cruce) break;
    if ((Sensor(IN_3) > cruce) && (Sensor(IN_3) < camino)) break;
    else {
      buscar = ESPB / 2;
      if (derecha) {
        BuscarD();
        if (Sensor(IN_3) > camino) {
          buscar = ESPB;
          BuscarI();
        }
      } else {
        BuscarI();
        if (Sensor(IN_3) > camino) {
          buscar = ESPB;
          BuscarD();
        }
      }
    }
  }
}

sub Optimo() {
  i_opt = -2;
  while (true) {
    eti: if (Sensor(IN_3) > camino) Sigue_Linea();
    if (Sensor(IN_3) < cruce) {
      OnFwd(OUT_BC, VEL);
      Wait(400);
      Off(OUT_BC);
      i_opt += 2;
      if (i_opt >= max - 4) FIN();
      if ((opt[i_opt] == opt[i_opt + 2]) && (opt[i_opt + 1] > opt[i_opt + 3])) {
        switch (estado) {
        case 0:
          //ColocaI();
          Vuelta180();
          estado = 3;
          goto eti;
          break;
        case 1:
          //ColocaI();
          Vuelta90D();
          estado = 3;
          goto eti;
          break;
        case 2:
          //ColocaI();
          Vuelta90I();
          estado = 3;
          goto eti;
          break;
        case 3:
          ColocaI2();
          Recto2();
          goto eti;
          break;
        }
      }
      if ((opt[i_opt] == opt[i_opt + 2]) && (opt[i_opt + 1] < opt[i_opt + 3])) {
        switch (estado) {
        case 0:
          ColocaI2();
          Recto2();
          goto eti;
          break;
        case 1:
          //ColocaI();
          Vuelta90I();
          estado = 0;
          goto eti;
          break;
        case 2:
          //ColocaI();
          Vuelta90D();
          estado = 0;
          goto eti;
          break;
        case 3:
          //ColocaI();
          Vuelta180();
          estado = 0;
          goto eti;
          break;
        }
      }
      if ((opt[i_opt] > opt[i_opt + 2]) && (opt[i_opt + 1] == opt[i_opt + 3])) {
        switch (estado) {
        case 0:
          //ColocaI();
          Vuelta90I();
          estado = 2;
          goto eti;
          break;
        case 1:
          //ColocaI();
          Vuelta180();
          estado = 2;
          goto eti;
          break;
        case 2:
          ColocaI2();
          Recto2();
          goto eti;
          break;
        case 3:
          //ColocaI();
          Vuelta90D();
          estado = 2;
          goto eti;
          break;
        }
      }
      if ((opt[i_opt] < opt[i_opt + 2]) && (opt[i_opt + 1] == opt[i_opt + 3])) {
        switch (estado) {
        case 0:
          //ColocaI();
          Vuelta90D();
          estado = 1;
          goto eti;
          break;
        case 1:
          ColocaI2();
          Recto2();
          goto eti;
          break;
        case 2:
          //ColocaI();
          Vuelta180();
          estado = 1;
          goto eti;
          break;
        case 3:
          //ColocaI();
          Vuelta90I();
          estado = 1;
          goto eti;
          break;
        }
      }
    } else Caminar(10);
  }
}

task main() {
  int in , i;
  max = 0;
  SetSensorLight(IN_3);
  ArrayInit(opt, 0, 200);
  BTCheck(0); //check master connection
  while (true) {
    TextOut(10, LCD_LINE1, "En espera");
    if (ReceiveRemoteNumber(IN_MBOX, true, in ) != STAT_MSG_EMPTY_MAILBOX) {
      opt[max] = in ;
      max++;
      SendResponseNumber(OUT_MBOX, 0xFF);
    }
    Wait(100); //take breath (optional)
    if ( in == 69) break;
  }
  TextOut(10, LCD_LINE1, "Coordenadas OK");
  Wait(2000);
  Optimo();
}