//******************************************************************
//      DEMO2.cpp
//                                            Chi-Mou Chao 3/22/2001
// The ENCODER-300 can be expand to 32-bits by software.
// This program demostrates a method that counts the 32-bits encoder
// by a 5ms timer interrupt.
//    ENC3_SELECT(0x240);
//    get_encoder32_isr();  //update the 32bit encoder
// The "get_encoder32_isr()" can be built in the timer interrupt
// routine and main program, which can update the 32-bits encoder
// value.
// The ENC3_RESET_ENCODER32(X_axis) will reset the 32-bits value.
//******************************************************************
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <dos.h>

#include "enc3.h"
#include "mstep2.h"

//------ENCODER300 variable ------------------------------
#include "enc32bit.h"
long  x_value;
long  y_value;
long  z_value;
unsigned char index;
unsigned char x_index;
unsigned char y_index;
unsigned char z_index;

#define  CARD1    0
//-------------define display color for color
unsigned char DDA,AD;
unsigned int  LSP,HSP;
unsigned char xmode,ymode;
unsigned char xdir,ydir;
unsigned char xson,yson;
                 
                 
//--------------------------------------------------------------------
#define INTR 0x08                  //timer interrupt number
#define sampling_time 5964         //<1193180Hz>/5964=Hz(5ms)
unsigned long sampling_counter1=0;
void interrupt sampling_ISR(...);
void interrupt (*old_handler)(...);

//--------------------------------------------------------------------
// set timer interrupt period as 5ms,
// and set the vector of INTR=0x08 as user's program address
//--------------------------------------------------------------------
void set_timer()
{
  disable();
	  old_handler = getvect(INTR);
	  setvect(INTR, sampling_ISR);
	  outp(0x43, 0x34);                    //modify timer
	  outp(0x40,  sampling_time & 0x00ff);
	  outp(0x40, (sampling_time >> 8) );
  outportb(0x20,0x20);
  enable();
}
//--------------------------------------------------------------------
// recover the vector of INTR=0x08,
// reset the timer
//--------------------------------------------------------------------
void release_timer()
{
  disable();
	 outp(0x43, 0x34);
	 outp(0x40, 0x00);
	 outp(0x40, 0x00);
	 setvect(INTR, old_handler);
	 outportb(0x20,0x20);
  enable();
}
//------------------------------------------------------------------
void display_encoder()
{
int x,y;

  ENC3_SELECT(0x240);
  x_value = ENC3_GET_ENCODER32(X_axis);
  y_value = ENC3_GET_ENCODER32(Y_axis);
  z_value = ENC3_GET_ENCODER32(Z_axis);
  index   = ENC3_GET_INDEX();
  x_index = index & 0x01;
  y_index = (index & 0x02) >> 1;
  z_index = (index & 0x04) >> 2;
  x=wherex(); y=wherey();
  gotoxy(5,2); cprintf("%8ld",x_value);
  gotoxy(15,2); cprintf("%8ld",y_value);
  gotoxy(25,2); cprintf("%8ld",z_value);
  gotoxy(x,y);
}
//--------------------------------------------------------------------
// Timer interrupt
// 1. trigger the original vector of INTR=0x08 by 18.Hz
//--------------------------------------------------------------------
void interrupt sampling_ISR(...)
{
  disable();
  sampling_counter1 += sampling_time;
  if (sampling_counter1>65536L)
  {
     sampling_counter1-=65536L;
     old_handler();             //trigger original 0x08h (18.Hz)
  };

  ENC3_SELECT(0x240);
  get_encoder32_isr();  //update the 32bit encoder

  display_encoder();
  outportb(0x20,0x20);
  enable();
}


//####################################################################
void main()
{
int ch;

  clrscr();
  //-------------------------------------------------------------
  DDA = 1;
  AD  = 50;
  LSP = 50;
  HSP = 2040;
  xmode = CW_CCW;
  ymode = CW_CCW;
  xdir = NORMAL_DIR;
  ydir = NORMAL_DIR;
  xson = ON;
  yson = ON;

  cprintf("*** ENCODER-300 32bit test ***\r\n");
  //----initial setting-----------------------
  MSTEP2_REGISTRATION(CARD1, 0x300);
  MSTEP2_RESET_SYSTEM(CARD1);
  MSTEP2_SET_VAR(CARD1, DDA, AD, LSP, HSP);
  MSTEP2_SET_DEFDIR(CARD1, xdir, ydir);
  MSTEP2_SET_MODE(CARD1, xmode, ymode);
  MSTEP2_SET_SERVO_ON(CARD1, xson, yson);
  MSTEP2_SET_NC(CARD1, NO);
  delay(100);

  //-------------------------------------------------------------
  ENC3_SELECT(0x240);
  ENC3_INIT_CARD( ENC_X1|ENC_CW_CCW,
		  ENC_X1|ENC_CW_CCW,
		  ENC_X1|ENC_CW_CCW);
  ENC3_RESET_ENCODER(X_axis);
  ENC3_RESET_ENCODER(Y_axis);
  ENC3_RESET_ENCODER(Z_axis);

  ENC3_RESET_ENCODER32(X_axis);
  ENC3_RESET_ENCODER32(Y_axis);
  ENC3_RESET_ENCODER32(Z_axis);

  set_timer();
  enable();

  //----motion command------------------------
  cprintf("\r\n");
  cprintf("output pulse by (STEP-200)MSTEP2_INTP_LINE02\r\n");

  MSTEP2_INTP_LINE02(CARD1,1000000,-1000000, 2040, 0);
  do {} while (MSTEP2_INTP_STOP()!=READY);
  do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
  //---------------------------------------------
  delay(5);
  cprintf("finish 32bit encoder test!!\r\n");

  ch=getch();
  MSTEP2_RESET_SYSTEM(CARD1);
  release_timer();
}