//******************************************************************
// enc32bit.cpp
// A sub-routine for 32-bit encoder
//                                            Chi-Mou Chao 3/22/2001
//******************************************************************
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <dos.h>
    
#include "enc3.h"

//------ENCODER300 variable ------------------------------   
long encoder_X;
long encoder_Y;
long encoder_Z;
unsigned int old_pos_X;
unsigned int old_pos_Y;
unsigned int old_pos_Z;                 
                 
//---------------------------------------------------------
void get_encoder32_isr()
{
unsigned int pos_X,pos_Y,pos_Z;   
int          delta_X,delta_Y,delta_Z;

   pos_X=ENC3_GET_ENCODER(X_axis);
   pos_Y=ENC3_GET_ENCODER(Y_axis);
   pos_Z=ENC3_GET_ENCODER(Z_axis);

   //---X axis encoder ----------------------
   if (((pos_X ^ old_pos_X) & 0xC000) == 0xC000)
     {
       if ((pos_X & 0xC000) != 0xC000)
          //65xxx -> 00xxx
          delta_X = (int) (pos_X + (65535 - old_pos_X) + 1);
       else
          //00xxx -> 65xxx
          delta_X = -(int)((65535-pos_X) + 1 +old_pos_X);
     }
   else
     {    //0 ~ 65535
          delta_X = (int)(pos_X - old_pos_X);
     }
   //---Y axis encoder ----------------------
   if (((pos_Y ^ old_pos_Y) & 0xC000) == 0xC000)
     {
       if ((pos_Y & 0xC000) != 0xC000)
          //65xxx -> 00xxx
          delta_Y = (int) (pos_Y + (65535 - old_pos_Y) + 1);
       else
          //00xxx -> 65xxx
          delta_Y = -(int)((65535-pos_Y) + 1 +old_pos_Y);
     }
   else
     {    //0 ~ 65535
          delta_Y = (int)(pos_Y - old_pos_Y);
     }
   //---Z axis encoder ----------------------
   if (((pos_Z ^ old_pos_Z) & 0xC000) == 0xC000)
     {
       if ((pos_Z & 0xC000) != 0xC000)
          //65xxx -> 00xxx
          delta_Z = (int) (pos_Z + (65535 - old_pos_Z) + 1);
       else
          //00xxx -> 65xxx
          delta_Z = -(int)((65535-pos_Z) + 1 +old_pos_Z);
     }
   else
     {    //0 ~ 65535
          delta_Z = (int)(pos_Z - old_pos_Z);
     }

   old_pos_X = pos_X;
   old_pos_Y = pos_Y;
   old_pos_Z = pos_Z; 
   
   encoder_X += delta_X;
   encoder_Y += delta_Y;
   encoder_Z += delta_Z;
}                       
//-----------------------------------------------------------------------
long ENC3_RESET_ENCODER32(unsigned char axis)
{                       

  old_pos_X=ENC3_GET_ENCODER(X_axis);
  old_pos_Y=ENC3_GET_ENCODER(Y_axis);
  old_pos_Z=ENC3_GET_ENCODER(Z_axis);
   
  switch (axis)
  {
    case X_axis: encoder_X=0; break;
    case Y_axis: encoder_Y=0; break;
    case Z_axis: encoder_Z=0; break;
  }        
  return 0;
}
//-----------------------------------------------------------------------
long ENC3_GET_ENCODER32(unsigned char axis)
{
  switch (axis)
  {
    case X_axis: return encoder_X;
    case Y_axis: return encoder_Y;
    case Z_axis: return encoder_Z;
  }
  return 0;
}