//*********************************************************
//   Main.cpp                                      10/16/99
//*********************************************************
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <dos.h>
#include <string.h>
#include <bios.h>

#include "MSERVO3.h"

//----- define ------------------------------------------
#define CARD1 0
#define CARD2 1

#define COM_BACK	  BLUE
#define COM_TEXT	  WHITE
#define COM_UP_BACK	  WHITE
#define COM_UP_TEXT	  BLACK+BLINK
#define COM_ON_BACK	  RED
#define COM_ON_TEXT	  WHITE
#define SYS_ON_BACK	  MAGENTA
#define SYS_ON_TEXT	  WHITE
#define SYS_OFF_BACK	  CYAN
#define SYS_OFF_TEXT	  WHITE
#define SCREEN	        WHITE
#define SCREEN_BACK  BLUE


#define STOP_X         0
#define STOP_Y         1
#define STOP_Z         2
#define STOP_ALL       3
#define RESET_SYSTEM   4
#define BACK_HOME      5
#define PULSE_MOVE     6
#define INTP_PULSE     7
#define INTP_LINE      8
#define INTP_LINE02    9	
#define INTP_CIRCLE    10
#define INTP_CIRCLE02  11	
#define INTP_ARC       12 
#define INTP_ARC02     13
#define INTP_XYZ02     14		
#define CONSTANT_SPEED 15
#define DEC_STOP       16
#define MULTI_TEST     17
//--------------------------------------------------------
typedef struct
  {
	 //---- parameter ---------
	 unsigned int  address;
	 unsigned char exist;
	 unsigned char DDA;
	 unsigned char AD;
	 unsigned int  LSP;
	 unsigned int  HSP;
	 unsigned char home_speed;
	 unsigned char search_speed;
	 unsigned int  arc_speed;
	 unsigned char in_position;
	 unsigned int  lag_error; 
	 unsigned int  error_range;

	 unsigned char x_enable;
	 unsigned char x_mode;
	 unsigned char x_dir;
	 unsigned char x_son;
	 unsigned int  x_kp;
	 unsigned int  x_kd;

	 unsigned char y_enable;
	 unsigned char y_mode;
	 unsigned char y_dir;
	 unsigned char y_son;
	 unsigned int  y_kp;
	 unsigned int  y_kd;

	 unsigned char z_enable;
	 unsigned char z_mode;
	 unsigned char z_dir;
	 unsigned char z_son;
	 unsigned int  z_kp;
	 unsigned int  z_kd;

	 //---- information -------
	 unsigned char op;
	 unsigned char ip;
	 unsigned char msc;
	 unsigned char sys;
	 unsigned char ls;
	 unsigned char p1;
	 long          XC;
	 long          XP;
	 long          YC;
	 long          YP;
	 long          ZC;
	 long          ZP;
  } CardParameter;

typedef char string30[30];
typedef struct
	{
	  int	   max;
	  string30  name[30];
	  int	   x[30],y[30];
	} menutype;

//-----variable definition-----------------------
menutype      menu;
CardParameter card1,card2;
char	        screen_buffer[4096];
//####################################################################
//####################################################################
#define INTR 0x08
#define sampling_time 14318          // <1193180Hz>/14318=Hz(12ms) 1.5*8
unsigned long sampling_counter1=0;

void interrupt sampling_ISR(...);
void interrupt (*old_handler)(...);


void show_information();
void lamp_L(unsigned char lswitch,
				unsigned char x,
				unsigned char y,
				char show_text[10]);
void lamp_H(unsigned char lswitch,
				unsigned char x,
				unsigned char y,
				char show_text[10]);
//--------------------------------------------------------------------
void set_timer()
{
  disable();
	  old_handler = getvect(INTR);
	  setvect(INTR, sampling_ISR);
	  outp(0x43, 0x34);
	  outp(0x40,  sampling_time & 0x00ff);
	  outp(0x40, (sampling_time >> 8) );
  outportb(0x20,0x20);
  enable();
}
//--------------------------------------------------------------------
void release_timer()
{
  disable();
	 outp(0x43, 0x34);
	 outp(0x40, 0x00);
	 outp(0x40, 0x00);
	 setvect(INTR, old_handler);
	 outportb(0x20,0x20);
  enable();
}
//--------------------------------------------------------------------
void interrupt sampling_ISR(...)    // 10ms sample once
{
int cx,cy;

  cx=wherex();
  cy=wherey();
  _setcursortype(_NOCURSOR);
  disable();

  sampling_counter1 += sampling_time;
  if (sampling_counter1>65536L)
  {
	 sampling_counter1-=65536L;
	 old_handler();             // trigger original 0x08h (18.Hz)
  };

  //----- get card 1 information ----------------
//   card1.ip  =inportb(0x302);
//   card1.msc =inportb(0x301);

	 if (card1.exist==YES)
	 {
		card1.ip  = MSERVO3_DI(CARD1);
		card1.msc = MSERVO3_MSC(CARD1);

		MSERVO3_GET_CARD(CARD1);
		card1.sys   =MSERVO3_GET_SYS(CARD1);
		card1.ls    =MSERVO3_GET_LIMIT(CARD1);
		card1.p1    =MSERVO3_GET_P1(CARD1);
		card1.XC    =MSERVO3_GET_XC(CARD1);
		card1.XP    =MSERVO3_GET_XP(CARD1);
		card1.YC    =MSERVO3_GET_YC(CARD1);
		card1.YP    =MSERVO3_GET_YP(CARD1);
		card1.ZC    =MSERVO3_GET_ZC(CARD1);
		card1.ZP    =MSERVO3_GET_ZP(CARD1);
	 }

  show_information();
  gotoxy(cx,cy);
  _setcursortype(_NORMALCURSOR);
  outportb(0x20,0x20);
  enable();
}
//####################################################################
//--------------------------------------------------------------------
void lamp_L(unsigned char lswitch,
				unsigned char x,
				unsigned char y,
				char show_text[10])
{
  if (lswitch!=0)
  {
	textbackground(SYS_OFF_BACK);
	textcolor(SYS_OFF_TEXT);
  }
  else
  {
	textbackground(SYS_ON_BACK);
	textcolor(SYS_ON_TEXT);
  }
  gotoxy(x,y);
  cprintf(show_text);
}

void lamp_H(unsigned char lswitch,
				unsigned char x,
				unsigned char y,
				char     show_text[10])
{
  if (lswitch==0)
  {
	textbackground(SYS_OFF_BACK);
	textcolor(SYS_OFF_TEXT);
  }
  else
  {
	textbackground(SYS_ON_BACK);
	textcolor(SYS_ON_TEXT);
  }
  gotoxy(x,y);
  cprintf(show_text);
}
//-------------------------------------------------------------------
void show_information()
{
char str[20];

  lamp_L(card1.ip & 0x01,1,3, "/IP1");
  lamp_L(card1.ip & 0x02,1,4, "/IP2");
  lamp_L(card1.ip & 0x04,1,5, "/IP3");
  lamp_L(card1.ip & 0x08,1,6, "/IP4");
  lamp_L(card1.ip & 0x10,1,7, "/IP5");
  lamp_L(card1.ip & 0x20,1,8, "/IP6");
  lamp_L(card1.ip & 0x40,1,9, "/IP7");
  lamp_L(card1.ip & 0x80,1,10,"/IP8");

  lamp_L(card1.op & 0x01,5,3, "/OP1");
  lamp_L(card1.op & 0x02,5,4, "/OP2");
  lamp_L(card1.op & 0x04,5,5, "/OP3");
  lamp_L(card1.op & 0x08,5,6, "/OP4");
  lamp_L(card1.op & 0x10,5,7, "/OP5");
  lamp_L(card1.op & 0x20,5,8, "/OP6");
  lamp_L(card1.op & 0x40,5,9, "/OP7");

  lamp_L(card1.msc & 0x01,10,3, "/F1E");
  lamp_L(card1.msc & 0x02,10,4, "/F1F");
  lamp_L(card1.msc & 0x04,10,5, "/F2E");
  lamp_L(card1.msc & 0x08,10,6, "/F2H");
  lamp_L(card1.msc & 0x10,10,7, "/Xsp");
  lamp_L(card1.msc & 0x20,10,8, "/Ysp");
  lamp_L(card1.msc & 0x40,10,9, "/Zsp");
  lamp_L(card1.msc & 0x80,10,10,"/EMG");

  lamp_L(card1.ls & 0x01,15,3, "/XL+");
  lamp_L(card1.ls & 0x02,15,4, "/XL-");
  lamp_L(card1.ls & 0x04,15,5, "/YL+");
  lamp_L(card1.ls & 0x08,15,6, "/YL-");
  lamp_L(card1.ls & 0x10,15,7, "/ZL+");
  lamp_L(card1.ls & 0x20,15,8, "/ZL-");

  lamp_L(card1.p1 & 0x01,20,3, "/EMG");
  lamp_L(card1.p1 & 0x02,20,4, "/XI");
  lamp_L(card1.p1 & 0x04,20,5, "/YI");
  lamp_L(card1.p1 & 0x08,20,6, "/ZI");
  lamp_H(card1.p1 & 0x10,20,7, "C1 ");
  lamp_H(card1.p1 & 0x20,20,8, "C2 ");
  lamp_H(card1.p1 & 0x40,20,9, "C3 ");

  //-------------------------------------
  textbackground(SYS_OFF_BACK);
  textcolor(SYS_OFF_TEXT);

  ltoa( card1.XC, str, 10);
  gotoxy(6,23); cprintf("        ");
  gotoxy(6,23); cprintf(str);
  ltoa( card1.XP, str, 10);
  gotoxy(18,23); cprintf("        ");
  gotoxy(18,23); cprintf(str);

  ltoa( card1.YC, str, 10);
  gotoxy(6,24); cprintf("        ");
  gotoxy(6,24); cprintf(str);
  ltoa( card1.YP, str, 10);
  gotoxy(18,24); cprintf("        ");
  gotoxy(18,24); cprintf(str);

  ltoa( card1.ZC, str, 10);
  gotoxy(6,25); cprintf("        ");
  gotoxy(6,25); cprintf(str);
  ltoa( card1.ZP, str, 10);
  gotoxy(18,25); cprintf("        ");
  gotoxy(18,25); cprintf(str);
}
//####################################################################
//####################################################################
void unpressed_text(unsigned char x, unsigned char y,
			 string30 show_text)
{
  textbackground(COM_BACK);
  textcolor(COM_TEXT);
  gotoxy(x,y);
  cprintf(show_text);
  gotoxy(x,y);
}
//----------------------------------------------------------
void pressed_text(unsigned char x, unsigned char y,
			 string30 show_text)
{
  textbackground(COM_UP_BACK);
  textcolor(COM_UP_TEXT);
  gotoxy(x,y);
  cprintf(show_text);
  gotoxy(x,y);
}
//----------------------------------------------------------
void work_text(unsigned char x, unsigned char y,
			 string30 show_text)
{
  textbackground(COM_ON_BACK);
  textcolor(COM_ON_TEXT);
  gotoxy(x,y);
  cprintf(show_text);
  gotoxy(x,y);
}
//---------------------------------------------------------
void init_system_display()
{
int i;
	//---- print title ------
	textbackground(SYS_ON_BACK);
	textcolor(SYS_ON_TEXT);
	gotoxy(1,1);
	cprintf(
"              3 axis Servo Motor Control Card     version 2.0   C.C.M.02/03/01");

	window(1,2,27,25);
	textbackground(SYS_OFF_BACK);
	textcolor(SYS_OFF_TEXT);
	clrscr();
	//-------- show pannel
	window(1,1,80,25);
	textbackground(SYS_ON_BACK);
	textcolor(SYS_ON_TEXT);
	gotoxy(1,2);   cprintf("     I/O Information       ");
	gotoxy(1,11);   cprintf("      Motion Parameter     ");

	menu.max=18;
	for (i=0; i<12; i++)
	{
		menu.x[i]=30;
		menu.y[i]=2+i*2;
	}
	for (i=12; i<24; i++)
	{
		menu.x[i]=55;
		menu.y[i]=2+(i-12)*2;
	}

	strcpy(menu.name[0], " MSERVO3_STOP_X        ");
	strcpy(menu.name[1], " MSERVO3_STOP_Y        ");
	strcpy(menu.name[2], " MSERVO3_STOP_Z        ");
	strcpy(menu.name[3], " MSERVO3_STOP_ALL      ");
	strcpy(menu.name[4], " MSERVO3_RESET_SYSTEM  ");
	strcpy(menu.name[5], " MSERVO3_BACK_HOME     ");
	strcpy(menu.name[6], " MSERVO3_PULSE_MOVE    ");
	strcpy(menu.name[7], " MSERVO3_INTP_PULSE    ");
	strcpy(menu.name[8], " MSERVO3_INTP_LINE     ");
	strcpy(menu.name[9], " MSERVO3_INTP_LINE02   ");	
	strcpy(menu.name[10]," MSERVO3_INTP_CIRCLE   ");
	strcpy(menu.name[11]," MSERVO3_INTP_CIRCLE02 ");	
	strcpy(menu.name[12]," MSERVO3_INTP_ARC      "); 
	strcpy(menu.name[13]," MSERVO3_INTP_ARC02    ");
	strcpy(menu.name[14]," MSERVO3_INTP_XYZ02    ");		
	strcpy(menu.name[15]," MSERVO3_CONSTANT_SPEED");
	strcpy(menu.name[16]," MSERVO3_DEC_STOP      ");
	strcpy(menu.name[17]," Multi testing         ");	

	for (i=0; i<menu.max; i++)
	  unpressed_text (menu.x[i], menu.y[i], menu.name[i]);

	textbackground(SYS_OFF_BACK);
	textcolor(SYS_OFF_TEXT);
	gotoxy(1,12); cprintf("card1 exist =");
	gotoxy(15,12);
	if (card1.exist==YES) cprintf("YES");
	else                  cprintf("NO");

	gotoxy(1,13); cprintf("Address= %d",card1.address);
	gotoxy(1,14); cprintf("DDA    = %d",card1.DDA);
	gotoxy(1,15); cprintf("AD     = %d",card1.AD);
	gotoxy(1,16); cprintf("LSP    = %d",card1.LSP);
	gotoxy(1,17); cprintf("HSP    = %d",card1.HSP);
	gotoxy(1,18); cprintf("HomeSpd= %d",card1.home_speed);
	gotoxy(1,19); cprintf("SchSpd = %d",card1.search_speed);
	gotoxy(1,20); cprintf("ArcSpd = %d",card1.arc_speed);
	gotoxy(1,21); cprintf("InPos  = %d",card1.in_position);
	gotoxy(1,22); cprintf("LagErr = %d",card1.lag_error);

	gotoxy(15,13); cprintf("X enable=");
	gotoxy(25,13);
	if (card1.x_enable==YES) cprintf("YES");
	else                     cprintf("NO");
	gotoxy(15,14); cprintf("X mode=");
	gotoxy(23,14);
	if (card1.x_mode==SIMU_MODE) cprintf("SIMU");
	if (card1.x_mode==TEST_MODE) cprintf("TEST");
	if (card1.x_mode==CLOSE_MODE) cprintf("CLOSE");
	gotoxy(15,15); cprintf("X dir=");
	gotoxy(22,15);
	if (card1.x_dir==NORMAL_DIR) cprintf("NORMAL");
	else                         cprintf("REVERSE");

	gotoxy(15,16); cprintf("Y enable=");
	gotoxy(24,16);
	if (card1.y_enable==YES) cprintf("YES");
	else                     cprintf("NO");
	gotoxy(15,17); cprintf("Y mode=");
	gotoxy(22,17);
	if (card1.y_mode==SIMU_MODE) cprintf("SIMU");
	if (card1.y_mode==TEST_MODE) cprintf("TEST");
	if (card1.y_mode==CLOSE_MODE) cprintf("CLOSE");
	gotoxy(15,18); cprintf("Y dir=");
	gotoxy(21,18);
	if (card1.y_dir==NORMAL_DIR) cprintf("NORMAL");
	else                         cprintf("REVERSE");

	gotoxy(15,19); cprintf("Z enable=");
	gotoxy(24,19);
	if (card1.z_enable==YES) cprintf("YES");
	else                     cprintf("NO");
	gotoxy(15,20); cprintf("Z mode=");
	gotoxy(22,20);
	if (card1.z_mode==SIMU_MODE) cprintf("SIMU");
	if (card1.z_mode==TEST_MODE) cprintf("TEST");
	if (card1.z_mode==CLOSE_MODE) cprintf("CLOSE");
	gotoxy(15,21); cprintf("Z dir=");
	gotoxy(21,21);
	if (card1.z_dir==NORMAL_DIR) cprintf("NORMAL");
	else                         cprintf("REVERSE");

	gotoxy(2,23);  cprintf("XC=");
	gotoxy(14,23); cprintf("XP=");
	gotoxy(2,24);  cprintf("YC=");
	gotoxy(14,24); cprintf("YP=");
	gotoxy(2,25);  cprintf("ZC=");
	gotoxy(14,25); cprintf("ZP=");
}

//#############################################################
char *input_string(int x, int y, int max, char istr[30], int fch)
{
	int	 l,c,j;
	int	ch,ch1;

	l=strlen(istr);
	c=l;
	if (c>=max) c=max-1;
	ch=fch;
	disable();
	textbackground(BLACK); textcolor(WHITE);
	gotoxy(x,y); for (j=0; j<max; j++) cprintf(" ");
	gotoxy(x,y); cprintf(istr);
	gotoxy(x+c,y);
	enable();
  do {
	 switch (ch) {
	 case 0x4b00: c--; if (c<0) c=0;	    /* left */
		 break;
	 case 0x4d00: c++; if (c>l) c=l;   /* right */
				if (c>max-1) c=max-1;
		 break;
	 case 0x5300: if (l>0)		    /* del */
			{
			  for (j=c; j<l; j++) istr[j]=istr[j+1];
			  istr[j]='\0'; l=strlen(istr);
			}
		 break;
	 case 0x0e08: c--;			    /* backspace */
		 if (c<0) c=0;
		 else
			{
			  for (j=c; j<l; j++) istr[j]=istr[j+1];
			  istr[j]='\0'; l=strlen(istr);
			}
		 break;

	 default: ch1=ch&0x00ff;
		  if ((ch1>12)&(ch1<122))
		  {
			 if ( c>=strlen(istr))
		  {
		  istr[c]=ch1; istr[c+1]='\0'; c++;
		  }
			 else { istr[c]=ch1; c++;};

			 if (c>max-1) c=max-1;
		  }

		  break;
		 }
	  disable();
	  l=strlen(istr);
	  textbackground(0); textcolor(7);
	  gotoxy(x,y); for (j=0; j<max; j++) cprintf(" ");
	  textbackground(0); textcolor(7);
	  gotoxy(x,y); cprintf(istr);
	  gotoxy(x+c,y);
     enable();
	  do {
	} while (!bioskey(1));
	ch=bioskey(0);

	 } while (ch!=0x1c0d);
	 return (istr);
}
//----------------------------------------------------------------
long get_number(string30 show_text)
{
string30 tstr;
long temp;
  disable();
  gettext(20,11,60,13,screen_buffer);
  textbackground(WHITE);
  textcolor(BLACK);
  window(20,11,60,13);
  clrscr();
  window(1,1,80,25);

  gotoxy(22,12);
  cprintf(show_text);
  enable();
  strcpy(tstr,"");
  temp = atol(input_string(wherex(),wherey(),8,tstr,0) );

  window(1,1,80,25);
  puttext(20,11,60,13,screen_buffer);
  return temp;
}
//--------------------------------------------------------------------
void MessageBox(string30 show_text)
{
  disable();
  gettext(40,11,80,13,screen_buffer);
  textbackground(WHITE);
  textcolor(BLACK);
  window(40,11,80,13);
  clrscr();
  window(1,1,80,25);
  gotoxy(40,11); cprintf("ΙΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝ»");
  gotoxy(40,12); cprintf("Ί                                       Ί");
  gotoxy(40,13); cprintf("ΘΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΝΌ");

  gotoxy(42,12);
  cprintf(show_text);
  enable();

  do { delay(10); } while (!kbhit());
  bioskey(0);
  window(1,1,80,25);
  puttext(40,11,80,13,screen_buffer);
}
//--------------------------------------------------------------------
void set_parameter()
{
unsigned char config;

  MSERVO3_SET_NC(CARD1,NO);
  MSERVO3_SET_CONTROL_MODE(CARD1, card1.x_mode, card1.y_mode, card1.z_mode);
  MSERVO3_SET_VAR(CARD1, card1.DDA, card1.AD, card1.LSP,
						card1.HSP, card1.arc_speed);
  MSERVO3_SET_DEFDIR(CARD1, card1.x_dir, card1.y_dir, card1.z_dir);
  MSERVO3_SET_SERVO_ON(CARD1, card1.x_son, card1.y_son, card1.z_son);

  config=0;
  if (card1.x_enable==ON) config |= 0x01;
  if (card1.y_enable==ON) config |= 0x02;
  if (card1.z_enable==ON) config |= 0x04;
  MSERVO3_SET_CONFIG(CARD1, config, card1.in_position, card1.lag_error, card1.error_range);

  //---- config controller
  MSERVO3_SET_X_CONTROLLER(CARD1,card1.x_kp,card1.x_kd);
  MSERVO3_SET_Y_CONTROLLER(CARD1,card1.y_kp,card1.y_kd);
  MSERVO3_SET_Z_CONTROLLER(CARD1,card1.z_kp,card1.z_kd);
}
//####################################################################
void operation(unsigned char command)
{
unsigned char axis,dir,plane;
unsigned int  speed;
long	      pulse;
int	      pulsex,pulsey,pulsez;
long	      x,y,z,R;

 switch (command)
 {
  case STOP_X :
		MSERVO3_STOP(CARD1, X_axis);
		break;
  case STOP_Y :
		MSERVO3_STOP(CARD1, Y_axis);
		break;
  case STOP_Z :
		MSERVO3_STOP(CARD1, Z_axis);
		break;
  case STOP_ALL :
		MSERVO3_STOP_ALL(CARD1);
		break;
  case RESET_SYSTEM :
		MSERVO3_RESET_SYSTEM(CARD1);
		set_parameter();
		break;

  case BACK_HOME :
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };
                  
		axis=(unsigned char) get_number("Axis 1(X)2(Y)3(Z)= ");
		if ((axis!=1) && (axis!=2) && (axis!=3)) break;
		MSERVO3_BACK_HOME(CARD1,axis,card1.home_speed
				,card1.search_speed);
		break;
		
  case PULSE_MOVE :   
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
		{
                  MessageBox("Inhibit now!"); 
                  break;
                };  
		axis=(unsigned char) get_number("Axis 1(X)2(Y)3(Z)= ");
		pulse=(long) get_number("input pulse (+- 2^31 -1) = ");
		speed=(unsigned int) get_number("speed (<2040)= ");
		if ((axis!=1) && (axis!=2) && (axis!=3)) break;
		MSERVO3_PULSE_MOVE(CARD1,axis,pulse,speed);
		break;
		
  case INTP_PULSE :
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };

		pulsex=(int) get_number("X pulse number (-2047 ~ 2047) = ");
		pulsey=(int) get_number("Y pulse number (-2047 ~ 2047) = ");
		pulsez=(int) get_number("Z pulse number (-2047 ~ 2047) = ");
		if (abs(pulsex) > 2047 ) break;
		if (abs(pulsey) > 2047 ) break;
		if (abs(pulsez) > 2047 ) break;
		MSERVO3_INTP_PULSE(CARD1,pulsex,pulsey,pulsez);
		break;
		
  case INTP_LINE :  
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };
                  
		x=(long) get_number("X pulse number (+-2^31 -1)= ");
		y=(long) get_number("Y pulse number (+-2^31 -1)= ");
		speed=(unsigned int) get_number("speed (<2040)= ");
		MSERVO3_INTP_LINE01(CARD1,XY_plane,x,y,speed);
		break;
		
  case INTP_LINE02:
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };
                  
		plane=(unsigned char) get_number("Plane 1(X-Y)2(X-Z)3(Y-Z)= ");

		x=(long) get_number("X pulse number (+-2^31 -1)= ");
		y=(long) get_number("Y pulse number (+-2^31 -1)= ");
		speed=(unsigned int) get_number("speed (<2040)= ");

		MSERVO3_INTP_LINE02(CARD1,plane,x,y,speed,0);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		break;		
		
  case INTP_CIRCLE :
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };                                                             
                
		x=(long) get_number("center point X = ");
		y=(long) get_number("center point Y = ");
		dir =(unsigned char) get_number("0: CW/FW    1: CCW/BW = ");
		if ((dir!=CW) && (dir!=CCW)) break;
		speed=(unsigned int) get_number("speed (<2040)= ");
		MSERVO3_INTP_CIRCLE01(CARD1,XY_plane,x,y,dir,speed);
		break;

  case INTP_CIRCLE02 :
		if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
		{
		  MessageBox("Inhibit now!");
		  break;
		};
                                              
                plane=(unsigned char) get_number("Plane 1(X-Y)2(X-Z)3(Y-Z)= ");                              
		x=(long) get_number("center point X = ");
		y=(long) get_number("center point Y = ");
		dir =(unsigned char) get_number("0: CW/FW    1: CCW/BW = ");
		if ((dir!=CW) && (dir!=CCW)) break;
		speed=(unsigned int) get_number("speed (<2040)= ");
		MSERVO3_INTP_CIRCLE02(CARD1,plane,x,y,dir,speed,0);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		break;

  case INTP_ARC :
		if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
		{
		  MessageBox("Inhibit now!");
		  break;
		};
                
		x=(long) get_number("end point X = ");
		y=(long) get_number("end point Y = ");
		R=(long) get_number("Radius (+/-)= ");
		dir =(unsigned char) get_number("0: CW/FW    1: CCW/BW = ");
		if ((dir!=CW) && (dir!=CCW)) break;
		speed=(unsigned int) get_number("speed (<2040)= ");
		MSERVO3_INTP_ARC01(CARD1,XY_plane,x,y,R,dir,speed);
		break;

  case INTP_ARC02 : 
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
		     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };
                                               
                plane=(unsigned char) get_number("Plane 1(X-Y)2(X-Z)3(Y-Z)= ");                                               
		x=(long) get_number("end point X = ");
		y=(long) get_number("end point Y = ");
		R=(long) get_number("Radius (+/-)= ");
		dir =(unsigned char) get_number("0: CW/FW    1: CCW/BW = ");
		if ((dir!=CW) && (dir!=CCW)) break;
		speed=(unsigned int) get_number("speed (<2040)= ");
		MSERVO3_INTP_ARC02(CARD1,plane,x,y,R,dir,speed,0);
		do {} while (MSERVO3_INTP_STOP()!=READY);		
		break;
		
  case INTP_XYZ02:
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
		{
                  MessageBox("Inhibit now!"); 
                  break;
                };
                  
		x=(long) get_number("X pulse number (+-2^31 -1)= ");
		y=(long) get_number("Y pulse number (+-2^31 -1)= ");
		z=(long) get_number("Z pulse number (+-2^31 -1)= ");
		speed=(unsigned int) get_number("speed (<2040)= ");
		//MSERVO3_INTP_XYZ(CARD1,x,y,z,speed);
		MSERVO3_INTP_XYZ02(CARD1,x,y,z,speed,0); 
		do {} while (MSERVO3_INTP_STOP()!=READY);
		break;
		
  case CONSTANT_SPEED :
		axis=(unsigned char) get_number("Axis 1(X)2(Y)3(Z)= ");
		dir =(unsigned char) get_number("0: CW/FW    1: CCW/BW = ");
		speed=(unsigned int) get_number("speed (<2040)= ");
		if ((axis!=1) && (axis!=2) && (axis!=3)) break;
		if ((dir!=CW) && (dir!=CCW)) break;
		MSERVO3_CONSTANT_SPEED(CARD1,axis,dir,speed);
		break;
						      
  case DEC_STOP:
		axis=(unsigned char) get_number("Axis 1(X)2(Y)3(Z)= ");
		if ((axis!=1) && (axis!=2) && (axis!=3)) break;
		MSERVO3_DEC_STOP(CARD1,axis);
		break;
		
  case MULTI_TEST :                                      
                if (    (MSERVO3_IS_X_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Y_STOP(CARD1)==NO)
                     || (MSERVO3_IS_Z_STOP(CARD1)==NO))
                {
                  MessageBox("Inhibit now!"); 
                  break;
                };  
		//wait for others interpolation completed
		do {} while (MSERVO3_INTP_STOP()!=READY);

		//test XYZ01, no acceleration
		MSERVO3_INTP_XYZ02(CARD1,1000,1000,0,20,1);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		MSERVO3_INTP_XYZ02(CARD1,2000,1000,0,22,1);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		MSERVO3_INTP_XYZ02(CARD1,2000,2000,0,24,1);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		MSERVO3_INTP_XYZ02(CARD1,1000,2000,0,26,1);
                do {} while (MSERVO3_INTP_STOP()!=READY);
                
		do {} while (MSERVO3_IS_X_STOP(CARD1)==NO);
		delay(5000);

		//test WAIT_X, WAIT_Y, WAIT_Z
		MSERVO3_INTP_LINE02(CARD1, XY_plane, 10000,-10000,200,0);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		MSERVO3_INTP_LINE02(CARD1, XY_plane, -10000, 10000,200,0);
		do {} while (MSERVO3_INTP_STOP()!=READY);
		MSERVO3_INTP_XYZ02(CARD1, 5000, -10000, -40000, 200, 0);
		do {} while (MSERVO3_INTP_STOP()!=READY);

		do {} while (MSERVO3_IS_X_STOP(CARD1)==NO);
		do {} while (MSERVO3_IS_Y_STOP(CARD1)==NO);
		do {} while (MSERVO3_IS_Z_STOP(CARD1)==NO);
		MSERVO3_STOP_ALL(CARD1);
		delay(5000);

		MSERVO3_INTP_CIRCLE02(CARD1, XY_plane, 5000,-5000, CW, 200, 0);
		do {} while (MSERVO3_INTP_STOP()!=READY);

		do {} while (MSERVO3_IS_X_STOP(CARD1)==NO);
		do {} while (MSERVO3_IS_Y_STOP(CARD1)==NO);
		MSERVO3_STOP_ALL(CARD1);

		break;

  default: break;
 }
}


//####################################################################
void main()
{
int           c,oldc,chkey;

  disable();
  clrscr();

  //------set card 1 parameters-------------------------------------
  card1.address = 0x300;
  card1.DDA     = 4;
  card1.AD      = 20;
  card1.LSP     = 20;
  card1.HSP     = 2000;
  card1.home_speed   = 20;
  card1.search_speed = 2;
  card1.arc_speed    = 500;
  card1.in_position  = 10;
  card1.lag_error    = 5000;
  card1.error_range  = 500;

  card1.x_enable     = ON;
  card1.x_mode       = SIMU_MODE;
  card1.x_dir        = NORMAL_DIR;
  card1.x_son        = OFF;
  card1.x_kp         = 100;
  card1.x_kd         = 0;

  card1.y_enable     = ON;
  card1.y_mode       = SIMU_MODE;
  card1.y_dir        = REVERSE_DIR;
  card1.y_son        = OFF;
  card1.y_kp         = 100;
  card1.y_kd         = 0;

  card1.z_enable     = ON;
  card1.z_mode       = SIMU_MODE;
  card1.z_dir        = NORMAL_DIR;
  card1.z_son        = OFF;
  card1.z_kp         = 100;
  card1.z_kd         = 0;

  //###############################################################
  card1.exist=MSERVO3_REGISTRATION(CARD1, card1.address);
  MSERVO3_RESET_SYSTEM(CARD1);
  set_timer();
  init_system_display();

  //---- setting
  set_parameter();
  enable();


// operation -----------------------------------------
  textbackground(COM_UP_BACK);
  gotoxy(28,25);
  cprintf(" ESC:EXIT F7:STOP  Up,Down:move  Enter:select ");
  c=0; oldc=0;

  pressed_text(menu.x[c], menu.y[c], menu.name[c]);
  do
  {
	  do
		{
		  delay(50);
		//  display_information();
		} while (!kbhit());
	 chkey=bioskey(0);
	 switch (chkey)
		{
		case 0x4d00 : c +=12;
			 if (c>=menu.max) c -=12;   // right
			 break;
		case 0x4b00 : c -=12;
			 if (c<0)  c +=12;	       // left
			 break;
		case 0x5000 : c++;
			 if ( c>=menu.max ) c=0;    // down
			 break;
		case 0x4800 : c--;
			 if ( c<0 ) c=menu.max-1;   // up
			 break;

		case 0x1c0d : work_text(menu.x[c], menu.y[c], menu.name[c]);
//			 if (card1.exist==YES)
				 operation(c);		// enter
			 pressed_text(menu.x[c], menu.y[c], menu.name[c]);
			 break;
		case 0x4100 :  //F7
			 MSERVO3_STOP_ALL(CARD1);
			 break;
			 //*******************************************************
		default:   break;
		}
	 if (c!=oldc)
	 {
		unpressed_text(menu.x[oldc], menu.y[oldc], menu.name[oldc]);
		pressed_text(menu.x[c], menu.y[c], menu.name[c]);
		oldc=c;
	 };
  } while (chkey!=0x011b); // ESC

 MSERVO3_RESET_SYSTEM(CARD1);
 release_timer();
 textbackground(BLACK);
 clrscr();
 gotoxy(1,25);
}