#include vars.h
#include pins.h


do_laser_probing()
{

  gvarset(8341,500); //AxisPlus Jog Speed 100mm/min
  gvarset(8342,5); //AxisPlus Jog Accel Time 0.005sec

  timer=0;
  slow=0;

  gvarset(8340,0-1); //AxisPlus Jog Negative Direction

  do{

  a=gvarget(7203);

  if (slow==0)
  {
    if (a<800)
    {
      gvarset(8341,50); //AxisPlus Jog Speed 100mm/min
      gvarset(8340,0-1); //AxisPlus Jog Negative Direction
      slow=1;
    };
  };

  if (a<16)
  {
    timer=200000;
  };

  timer++;

  }while(timer<100000);

  gvarset(8342,5); //AxisPlus Jog Accel Time 0.002sec
  gvarset(8340,0); //AxisPlus Jog Stop

  timer=0;
  do
   { 
      a=gvarget(8332); 
      timer++;
   }while(a!=0);

  pwm02=timer;

  gvarset(9717,0); //Set machine coordinate for AxisPlus to 0 

  timer=10; do { timer--; }while(time>0);


};

do_laser_calibration()
{
  gvarset(5740,999); //clear calibration (if any exists)
  timer=10; do { timer--; }while(time>0);

  timer=200; do { timer--; }while(time>0);


  timer=0;

  gvarset(5539,1);
  gvarset(8330,100); //Speed
  gvarset(8331,500); //Accel Time
  g0moveA(0x0,0x80,3000);//Axis Plus
  do
   { 
      a=gvarget(8332); //obtain the state of AxisPlus (can be idle, G0, jog, etc)
      timer++;
      if ((timer&0xf)==0)
      {
        gvarset(5731,0); //Save Position Log Every 32ms
      };
   }while(a!=0);



  gvarset(5740,998); //save calibration
  timer=10; do { timer--; }while(time>0);


};



main()
{

  gvarset(8030,0);     //THC Deactivate

  do_laser_probing();
  do_laser_calibration();

  exit(99);
};