mainVidel.nxc

#define speed 30
#define step 50
#define discoveryTurn 5
#define CLEARLINE(L) \
        TextOut(0, L, "                               ");
              //9:31
// function to not forget OnFwd(Out_*) et OnRev(Out_*) 3000 ms = 45°

// Ultrasonic IN_4
// Servomotor A C
int distance[discoveryTurn+1];
int savedRotation = 0;

sub writeText(string sentence, int line, int column)
{
    int lineToUse = line;
    CLEARLINE(lineToUse);
    TextOut(column, lineToUse, sentence);
    return;
}

sub writeNum(int number, int line, int column)
{
    int lineToUse = line;
    CLEARLINE(lineToUse);
    NumOut(column, lineToUse, number);
    return;
}

sub trackingRotation(int number) {
    savedRotation += number;
}


sub discovery() // Discovering the surroundings
{
    OnFwd(OUT_A, speed);OnRev(OUT_C, speed); Wait(3700);Float(OUT_AC);

     Wait(3000);
     trackingRotation(-3700);

     for(int i=1;i <= discoveryTurn;i++)
     {
             //NumOut(i, 55, SensorUS(IN_4));
             OnRev(OUT_A, speed);OnFwd(OUT_C, speed);Wait(1300);Float(OUT_AC);
             trackingRotation(+1300);
             Wait(2000);
             distance[i] = SensorUS(IN_4);
             writeText("Probing distance sensors...", LCD_LINE2, 0);
             writeNum(distance[i], LCD_LINE3, 0);
     }
     
      //OnFwd(OUT_A, speed);OnRev(OUT_C, speed); Wait(2600);Float(OUT_AC);
      OnFwd(OUT_A, speed);OnRev(OUT_C, speed); Wait(savedRotation);Float(OUT_AC);

      return ;
}


task main()
{
     int keyHighestDistance = 1, highestDistance=1;
     int distanceToGoTillNear = 0;
     int angle = 0;
     while(true) {
       ClearScreen();
       writeText("How's life doing", LCD_LINE1, 0);

       SetSensorLowspeed(IN_4);
       discovery();

       for(int i=0; i<=discoveryTurn; i++)
       {
               writeNum(distance[i], LCD_LINE5, 13);
               writeNum(i, LCD_LINE6, 17);
               Wait(1000);
               if(distance[i] > highestDistance)
               {
                      if(distance[i] != 255) {
                              highestDistance = distance[i];
                              keyHighestDistance = i;
                              writeText("Trying out :", LCD_LINE7,0);
                              writeNum(i, LCD_LINE8, 13);
                              Wait(1000);
                      }
               }
       }
       //Max Distance and key of the direction
       ClearScreen();
       writeText("Using", LCD_LINE7,0);
       writeNum(keyHighestDistance, LCD_LINE8, 13);
       Wait(3000);
       ClearScreen();


       if(keyHighestDistance == 0) writeText("Error, stopping", LCD_LINE8, 0);
       else {
            writeText("Distance has been choosen !", LCD_LINE1,0);
            writeNum(keyHighestDistance, LCD_LINE2, 0);
            writeNum(highestDistance, LCD_LINE3, 3);
            Wait(1000);
            writeText("Going there!", LCD_LINE1,0);
            writeText("...", LCD_LINE2,0);
            if(keyHighestDistance == 1) {
                      OnFwd(OUT_A, speed);OnRev(OUT_C, speed);Wait(2400);Float(OUT_AC);
                      writeText("key 1", LCD_LINE3,0);
            }
            if(keyHighestDistance == 2) {
                      OnFwd(OUT_A, speed);OnRev(OUT_C, speed);Wait(1200);Float(OUT_AC);
                      writeText("key 2", LCD_LINE4,0);
            }
            if(keyHighestDistance == 4) {
                      OnRev(OUT_A, speed);OnFwd(OUT_C, speed);Wait(1200);Float(OUT_AC);
                      writeText("key 4", LCD_LINE5,0);
            }
            if(keyHighestDistance == 5) {
                      OnRev(OUT_A, speed);OnFwd(OUT_C, speed);Wait(2400);Float(OUT_AC);
                      writeText("key 5", LCD_LINE6,0);
            }

            Wait(1000);
            writeText("Done!", LCD_LINE4,0);
            Wait(1000);
            ClearScreen();
            writeText("Going there!", LCD_LINE1,0);

            distanceToGoTillNear = (highestDistance * 3) / 4;
            writeNum(distanceToGoTillNear, LCD_LINE2,5);
            writeText("...", LCD_LINE3,0);

            OnFwd(OUT_A, speed);OnFwd(OUT_C, speed);
            while(SensorUS(IN_4)>15) {
            writeNum(SensorUS(IN_4), LCD_LINE5,5)
            }
            Off (OUT_AC);

            writeText("Done !", LCD_LINE6,0);
            Wait(5000);
       }


        keyHighestDistance = 1;
        highestDistance = 1;
        distanceToGoTillNear = 0;
        for(int i = 0; i < discoveryTurn+1; i++) {
             distance[i] = 0;
        }
        savedRotation = 0;
      }

}