#include pins.h

// Look after INPUT_GANTRY_S1 & INPUT_GANTRY_S2 input pins
// Position Y is stored in register #801 when sensor s1 is triggered 
// Position Y is stored in register #802 when sensor s2 is triggered
// If both sensors triggered, then 
//  - Motion stopped
//  - Position difference is calculated and stored into register #800

main()
{
 timer=0;

 message=PLCCMD_MOTION_CONTINUE;
 texit=timer+30;do{timer++;}while(timer<texit);

 mode_12=0;

 ready=0;

 s1_old=portget(INPUT_GANTRY_S1);
 s2_old=portget(INPUT_GANTRY_S2);

 do
 {
 timer++;

 s1=portget(INPUT_GANTRY_S1);
 s2=portget(INPUT_GANTRY_S2);

 mode1=mode_12&1;

 if (mode1==0)//if sensor 1 wasn't triggered
 {
  if (s1!=s1_old) //if triggered now
   {
     mode_12=mode_12|1; //set flag for sensor1
     position1=gvarget(5021+1);	//Save the Machine Y position
   };
 };

 mode2=mode_12&2;

 if (mode2==0)//if sensor 2 wasn't triggered
 {
  if (s2!=s2_old) //if triggered now
   {
     mode_12=mode_12|2; //set flag for sensor2
     position2=gvarget(5021+1);	//Save the Machine Y position
   };
 };

 }while(mode_12!=3);

 message=PLCCMD_MOTION_SKIP;
 timer=2;do{timer--;}while(timer>0);

 gvarset(801,position1); //send the position to myCNC software Register #801
 timer=50;do{timer--;}while(timer>0);
 gvarset(802,position2); //send the position to myCNC software Register #802
 timer=50;do{timer--;}while(timer>0);

 offset=position1-position2;
 gvarset(800,offset);
 timer=50;do{timer--;}while(timer>0);

 exit(99);
};