// line-follower + robot sledzacy
// projekt - Jordan 3.VI.2008r
// dla jednego czujnika z obsługą bluetootha (master)
#include "BTlib.nxc"
#define BT_CONN 1 // linia do ktorej jest podlaczony slave
#define MAILBOX 0

// tył tam gdzie czujnik
// silnik na porcie A - lewy
// silnik na porcie C - prawy

string in;
bool kierunek;
int ciemne = 480, predkosc = -40;
unsigned long czas, timer;
byte skok=400, cz1 = IN_1; /* cz1 - port do ktorego jest podpiety jest czujnik optyczny */

sub masterBTCheck(int conn){          /****************************/
   string cStr;                       /*     sprawdzenie czy      */
   cStr = NumToStr(conn);             /* mozliwa jest komunikacja */
   cStr = StrCat("na linii ",cStr);   /*      przez Bluetooth     */
   if (!BTCommCheck(conn)){           /****************************/
      TextOut(0,LCD_LINE2,"Polacz z robotem",true);
      TextOut(0,LCD_LINE3,"podazajacym");
      TextOut(0,LCD_LINE4,cStr,false);
      TextOut(0,LCD_LINE5,"w menu NXT",false);
      until(ButtonPressed(BTNCENTER, false));
      Stop(true);
   }
}

/***********************
*    PROGRAM GLOWNY    *
***********************/
task main() {

//     INICJALIZACJA   //
masterBTCheck(BT_CONN);  // czy jest komunikacja Bluetooth
SetSensor(cz1, SENSOR_LIGHT);
SetSensorType(cz1,SENSOR_TYPE_LIGHT_ACTIVE);
string tekst,out;
int czas;
// NORMALNA PRACA LIDERA //

byte cz2=IN_4;    // czujnik swiatla dzialajacy jako lampka kontrolna - nieobowiazkowa

  while(true) {
    ClearScreen();
    SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);
    //    BTSendMessage(BT_CONN,MAILBOX,out); //wyslanie wiadomosci

    /****** normalna jazda do przodu ******/
    tekst = StrToNum(Sensor(cz1));
    TextOut(10, LCD_LINE3,"cz1="+tekst,false);             /* A-lewy, C-prawy */

    in = BTReceiveMessage(BT_CONN, MAILBOX, TRUE);
    until (in=="ok") {
          Off(OUT_AC);
          in = BTReceiveMessage(BT_CONN, MAILBOX, TRUE);
    }

    OnFwd(OUT_AC,predkosc);
    Wait(50);
    /****** szukanie linni *****/
       czas=0;
       SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);
       while (Sensor(cz1) > ciemne) {
            if ((kierunek==false) && (Sensor(cz1) > ciemne)) {
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);
               Wait(1);                                        /***************************/
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);  /*  obraca robota w prawo  */
               OnRev(OUT_A,predkosc);                          /*      i szuka linii      */
               OnFwd(OUT_C,predkosc);                          /***************************/
               timer=0;
               until ( (Sensor(cz1) < ciemne) || (timer >= czas) ) {timer++; /* czyli czekaj */ };
               if (Sensor(cz1) > ciemne) { kierunek = true; };
               czas=czas+skok;
            }
            if ((kierunek==true) && (Sensor(cz1) > ciemne)) {
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);
               Wait(1);                                       /***************************/
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE); /*  obraca robota w lewo   */
               OnRev(OUT_C,predkosc);                         /*      i szuka linii      */
               OnFwd(OUT_A,predkosc);                         /***************************/
               timer=0;
               until ( (Sensor(cz1) < ciemne) || (timer >= czas) ) {timer++; /* czyli czekaj */ };
               if (Sensor(cz1) > ciemne) { kierunek = false; };
               czas=czas+skok;
            }
       }  // skoniec while (Sensor(cz1) > ciemne)

    /******* linia znaleziona, jedzie dalej ***/

  } // koniec glownej petli while(true)

}

// line-follower + robot sledzacy
// projekt - Jordan 3.VI.2008r
// dla jednego czujnika z obsługą bluetootha (slave)
#include "BTlib.nxc"
#define BT_CONN 0 // linia do ktorej jest podlaczony slave
#define MAILBOX 0

// tył tam gdzie czujnik
// silnik na porcie A - lewy
// silnik na porcie C - prawy

bool kierunek;
int odl_sledzenia=40; // max odleglosc na ktora moze oddalic sie master
unsigned long czas, timer;
byte skok=10, cz1 = IN_1; /*  cz1 - port do ktorego jest podpiety jest */
                          /*     ultradzwiekowy czujnik odleglosci     */

int predkosc() {           // dynamiczne przystosowanie predkosci do odleglosci
   int odl = SensorUS(cz1);
   if (odl > odl_sledzenia) return (-45); // <- predkosc przy rozgladaniu sie
   else { if (odl < 8) return (0);        // <- dla odleglosci < 6 zatrzymaj robota
          else return (odl * (-3)/2);       // <- normalna predkosc sledzenia
   }
}

sub slaveBTCheck(int conn){
   string cStr;
   cStr = NumToStr(conn);
   cStr = StrCat("na linni ",cStr);
   if (!BTCommCheck(conn)){
      TextOut(0,LCD_LINE2,"Polacz z robotem",true);
      TextOut(0,LCD_LINE3,"prowadzacym");
      TextOut(0,LCD_LINE4,cStr,false);
      TextOut(0,LCD_LINE5,"w menu NXT",false);
      until(ButtonPressed(BTNCENTER, false));   // czeka na nacisniecie srodkowego przycisku
      Stop(true);                               // konczy dzialanie programu
   }
}

/***********************
*    PROGRAM GLOWNY    *
***********************/

task main() {

//     INICJALIZACJA   //
slaveBTCheck(0); // czy jest komunikacja Bluetooth
SetSensorLowspeed(cz1);
string tekst,out;
int czuj;
byte cz2=IN_4;    // czujnik swiatla dzialajacy jako lampka kontrolna - nieobowiazkowa
// NORMALNA PRACA PODOAZAJACEGO //

  while(true) {
     ClearScreen();
    SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);
    if (SensorUS(cz1) < odl_sledzenia) BTSendMessage(BT_CONN,MAILBOX,"ok"); //wyslanie wiadomosci

    //    PODAZANIE ZAROBOTEM
    /* ---------------------- */
    tekst = StrToNum(SensorUS(cz1));
    TextOut(10, LCD_LINE3,"cz1="+tekst,false);       /* A-lewy, C-prawy */
    OnFwd(OUT_AC,predkosc());
    /****** szukanie linni *****/

       czas=0;
       while (SensorUS(cz1) > odl_sledzenia) {
            BTSendMessage(BT_CONN,MAILBOX,"nie_ok");
           
            if ((kierunek==false) && (SensorUS(cz1) > odl_sledzenia)) {
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);    /* mrygniecie dioda */
               Wait(1);
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);           /***************************/
               OnRev(OUT_C,predkosc());                                 /*  obraca robota w prawo  */
               OnFwd(OUT_A,predkosc());                                 /*      i szuka mastera    */
               timer=0;                                                 /***************************/
               until ( (SensorUS(cz1) < odl_sledzenia) || (timer >= czas) ) {timer++; };
               if (SensorUS(cz1) > odl_sledzenia) { kierunek = true; };
               czas=czas+skok;
            }
           
            if ((kierunek==true) && (SensorUS(cz1) > odl_sledzenia)) {
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);  /* mrygniecie dioda */
               Wait(1);
               SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);          /***************************/
               OnRev(OUT_A,predkosc());                                /*  obraca robota w lewo   */
               OnFwd(OUT_C,predkosc());                                /*      i szuka mastera    */
               timer=0;                                                /***************************/
               until ( (SensorUS(cz1) < odl_sledzenia) || (timer >= czas) ) {timer++; };
               if (SensorUS(cz1) > odl_sledzenia) { kierunek = false; };
               czas=czas+skok;
            }
           
       }  // skoniec while (SensorUS(cz1) > odl_sledzenia)

    /******* linia znaleziona, jedzie dalej ***/
  } // koniec petli while(true)

}