// Programed by Jung, HyunRyong

// File name : line-follow.c

// Date :  2002. 12. 02



#include "eyebot.h"

#include <stdlib.h>



// center point of x axis

#define CEN_POINT 41



// global variables

colimage	g_matrix;      // keep color image

VWHandle 	vw_handle;     // handle of the motors

image		g_grey;        // keep binary image



// declare functions

void BinaryImage(colimage img, int threshold);

void Steer(int center, BOOL turn);





void BinaryImage(colimage img, int threshold)

{

   int i, j;

   int sum = 0, x_spos = 0, x_lpos = 0, c_point;

   int count = 0, t_count = 0;

   int start = 31;



   for(j = 0; j < imagerows; j++)

       for(i = 0; i < imagecolumns; i++)

       {

          // make grey image           

          sum = (img[j][i][0] + img[j][i][1] + img[j][i][2]) / 3;

		

          if(j == start + 1 && count == 0)

             start++;



          // make binary image

          if(sum > threshold)

          {

 	     g_grey[j][i] = 0;

             t_count++;

             

             if(j == start && count == 0)

             {

                x_spos = i;

                count++;

             }

             else if(j == start && count != 0)

             {

                x_lpos = i;

                count++; 

             }

          }

          else

             g_grey[j][i] = 255;

       }



   // display the binary image

   LCDPutGraphic(&g_grey);



   // find the center point

   if(count >= 2)

      c_point = (x_spos + x_lpos) * 0.5;

   else

      c_point = 41; 



   LCDSetPos(0, 0);

   LCDPrintf("%3d %3d %3d", x_spos, x_lpos, c_point);



   // cross hair

   LCDLine(c_point, start + 2, c_point, start - 2, 2);

   LCDLine(c_point - 2, start, c_point + 2, start, 2); 



   // if do not detect lane, turn around

   if(t_count == 0)

      Steer(c_point, 1);

   else

      Steer(c_point, 0);

}





void Steer(int center, BOOL turn)

{

   int gap;

 

   gap = CEN_POINT - center;

   

   if(gap > CEN_POINT)

      gap = CEN_POINT;

   if(gap < -CEN_POINT)

      gap = -CEN_POINT;

   

   LCDSetPos(2, 10);

   LCDPrintf("%3d", gap);



   // turn around

   if(turn)

   {

      LCDSetPos(3, 10);

      LCDPrintf("Back ");



        VWDriveStraight(vw_handle, 0.06, 0.3);

	VWDriveWait(vw_handle);



	VWSetSpeed(vw_handle, 0,0);

	OSWait(10);



        VWDriveStraight(vw_handle, -0.04, 0.3);

	VWDriveWait(vw_handle);



	VWSetSpeed(vw_handle, 0,0);

	OSWait(10);



      VWDriveTurn(vw_handle, 3.14, 1.2);

      VWDriveWait(vw_handle);

      OSWait(10);

   }

   else

   {

      // turn left

      if(gap > 5)

      {

          gap *= 0.073;

          VWSetSpeed(vw_handle, 0.05, 0.18 * gap);

          LCDSetPos(3, 10);

          LCDPrintf("Left ");

      }

      // turn right

      else if(gap < -5)

      { 

          gap *= 0.073;

          VWSetSpeed(vw_handle, 0.05, 0.18 * gap);

          LCDSetPos(3, 10);

          LCDPrintf("Right");

      }

      // go straight

      else

      {

          VWSetSpeed(vw_handle, 0.05, 0.00);

          LCDSetPos(3, 10);

          LCDPrintf("Go     ");

      }

   }

}





int main()

{

   int value;



   value = CAMInit(NORMAL);



   if(value == 255)

      LCDPrintf("Error in init LCD\n");

   else

   {

      // initialize the motors

      vw_handle = VWInit(VW_DRIVE, 1); 

      VWStartControl(vw_handle, 7.0, 0.3, 7.0, 0.1);



      LCDMenu("", "", "", "Exit");



      while(KEYRead() != KEY4)

      {

         // get a image from camera

         CAMGetColFrame(&g_matrix, FALSE);

         BinaryImage(g_matrix, 180);

      }

      VWStopControl(vw_handle);

      VWRelease(vw_handle);

   }

   return 0;

}