//#include pins.h

#define INPUT_PROBE 13

#define ROLLBACK 2000

wait_move()
{
    do { code=gvarget(6060); }while(code!=0x4d);  //wait till motion finished
};

find_edgey()
{
  gvarset(8631,100); //acceleration time 50ms = 0.05s
  gvarset(8634,(2<<24)|speed); //Jog speed for Y
  gvarset(8635,2); //Jog in the Y+ direction
  timer=0;
  do
   {
      sensor0=portget(INPUT_PROBE);
      if ((timer&0xff)==0)   {     gvarset(8635,2);    };  //continue jog Y+ if the sensor is engaged
      timer++;
    }while(sensor0!=0);

  gvarset(8634,(2<<24)|speed_probe); //set lower probe speed for backwards motion for Y- direction
  gvarset(8635,2<<8); //Jog in Y- direction
  timer=0;
  do
   {
      sensor0=portget(INPUT_PROBE);
      if ((timer&0xff)==0)   {     gvarset(8635,2<<8);    }; //continue jog at probe speed until sensor is activated
      timer++;
    }while(sensor0==0);

   gvarset(8635,0); wait_move(); //stop jogging

};

find_edgex()
{
  gvarset(8631,100); //acceleration time 50ms = 0.05s
  gvarset(8634,(1<<24)|(speed*3)); //Jog speed for X
  gvarset(8635,1); //Jog in X+ direction
  timer=0;
  do
   {
      sensor0=portget(INPUT_PROBE);
      if ((timer&0xff)==0)   {     gvarset(8635,1);    };  //continue jog in X+ direction while sensor is ON
      timer++;
    }while(sensor0!=0);

  gvarset(8634,(1<<24)|(speed_probe*3)); //Jog speed for X
  gvarset(8635,1<<8); //X-
  timer=0;
  do
   {
      sensor0=portget(INPUT_PROBE);
      if ((timer&0xff)==0)   {     gvarset(8635,1<<8);    };
      timer++;
    }while(sensor0==0);

   gvarset(8635,0); wait_move();  //stop jogging

};



save_pos()
{ 

    gvarset(5730, 1);  timer=30;do{timer--;}while(timer>0); //save current position to calibration log

};


main()
{

  gvarset(5740, 1);  timer=30;do{timer--;}while(timer>0); //begin log

  gvarset(8631,100); //acceleration time 50ms = 0.05s
  gvarset(5539,1); //switch to fast g0moveA implementation


  gvarset(8632,44000); //speed X Position


  x0=gvarget(401)*50; //obtain user-inputted size of the working material (X-axis)
  y0=gvarget(402)*33; //obtain user-inputted size of the working material (Y-axis)

  g0moveA(0,0x1,x0); //move X 1/2 size
  do { code=gvarget(6060); }while(code==0);//wait till motion finished


  speed_probe=400;//gvarget(300);
  speed_fast=2000;//gvarget(301);
  speed=speed_fast;

  find_edgey();
  save_pos();

  gvarset(8632,44000); //speed
  g0moveA(0x3,0x1,300); //move X=0 (actually 3mm)
  do { code=gvarget(6060); }while(code==0);//wait till motion finished


  gvarset(8632,13000); //speed
  g0moveA(0,0x2,y0); //move Y 1/3 size incremental
  do { code=gvarget(6060); }while(code==0);//wait till motion finished

  find_edgex();
  save_pos();


  gvarset(8632,44000);
  g0moveA(0x3,0x1,300); //move X=0
  do { code=gvarget(6060); }while(code==0);//wait till motion finished

  gvarset(8632,13000);
  g0moveA(0,0x2,y0); //move Y 1/3 size incremental
  do { code=gvarget(6060); }while(code==0);//wait till motion finished

  find_edgex();
  save_pos();

  gvarset(5400,2); //switch tool to T2
  timer=500;do{timer--;}while(timer>0); //rotate based on 3 points

  gvarset(5740, 202);  timer=30;do{timer--;}while(timer>0); //rotate based on 3 points

  exit(99);  //normal exit 
};

};