//Lathe Low gears
#include pins.h
#include vars.h

//#define input_lathe_low_gears 6
//#define input_lathe_high_gears 7
//#define output_lath_gears_low 3
//#define output_lath_gears_high 4

main()
{
  gvarset(9159,0);
  a=portget(input_lathe_low_gears);
  b=portget(input_lathe_high_gears);
  a_b=a^b;

  if (a_b==0)
   {
    gvarset(9159,1); //display error message
    message=PLCCMD_MOTION_BREAK;
    exit(99);
   };

  portclr(output_lathe_gears_high);//
  if (a!=0)
  {    //change gears
    gvarset(5715,1);//GVAR_LATHE_GEARS            5715
    timer=30; do { timer--; } while (timer>0);
    exit(99);
  };


  if (is_modbus!=0)
  {
    message=PLCCMD_MODBUS_SPINDLE_SPEED;
    command=spindle_low_speed;
    parameter=spindle_low_speed;
    timeout=timer+10; do { timer++; } while (timer<timeout);

    message=PLCCMD_MODBUS_SPINDLE_CMD;
    command=spindle_on_reg_value;
    parameter=spindle_on_reg_value;

    timeout=timer+10;
    do { timer++; } while (timer<timeout);
  };


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

  a0=gvarget(spindle_encoder);//Encoder#7; 9048+7=9055
  timer=10; do { timer--; } while (timer>0);

  ready=0;
  do {
    a1=gvarget(spindle_encoder);//Encoder#7; 9048+7=9055
    ad=a1-a0;
    a0=a1;

    timer=10; do { timer--; } while (timer>0);
    gvarset(7255,ad);
    if (ad>30)
      {
        if (ad<70)
          {
            ready=1;
          };
      };
   } while (ready==0);


  portset(output_lathe_gears_low);//3
  ready=0;
  wait_time=0;

  do{

    a=portget(input_lathe_low_gears);
    b=portget(input_lathe_high_gears);

    if (a!=0)
      {
        if (b==0)
          {
            ready=1;
          };
      };

    wait_time++;
    if (wait_time>5000)
      {
        ready=1;
      };
  } while (ready==0);

  //portclr(output_lathe_gears_low);//3

  message=PLCCMD_MODBUS_SPINDLE_CMD;
  command=spindle_off_reg_value;
  parameter=spindle_off_reg_value;

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


  if (wait_time>5000)
  {
    gvarset(9159,1); timer=30;do{timer--;}while(timer>0);
    message=PLCCMD_MOTION_BREAK;
    exit(99);
  };


  //change gears
  gvarset(5715,1);//GVAR_LATHE_GEARS            5715
  timer=30; do { timer--; } while (timer>0);

  exit(99);	//normal exit 

};