//******************************************************************
//      DEMO2.cpp
//                                            Chi-Mou Chao 2/17/2001
// CPU version 2.0
//******************************************************************
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <dos.h>
    
#include "enc3.h"
#include "mstep2.h"

//------ENCODER300 variable ------------------------------   
#include "enc3.h"
unsigned int  x_value;
unsigned int  y_value;
unsigned int  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 get_encoder1()
{
int x,y;

  ENC3_SELECT(0x240);
  x_value = ENC3_GET_ENCODER(X_axis);
  y_value = ENC3_GET_ENCODER(Y_axis);
  z_value = ENC3_GET_ENCODER(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("%6d",x_value);
  gotoxy(15,2); cprintf("%6d",y_value);
  gotoxy(25,2); cprintf("%6d",z_value);
  gotoxy(x,y);
}

void get_encoder()
{
int x,y;
                   
  ENC3_SELECT(0x240);
  x_value = ENC3_GET_ENCODER(X_axis);
  y_value = ENC3_GET_ENCODER(Y_axis);
  z_value = ENC3_GET_ENCODER(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("%6d",x_value);
  gotoxy(15,2); cprintf("%6d",y_value);
  gotoxy(25,2); cprintf("%6d",z_value);
  gotoxy(x,y);
}
//--------------------------------------------------------------------
// Timer interrupt
// 1. trigger the original vector of INTR=0x08 by 18.Hz
// 2. get the PISO-PS300 information and status
//--------------------------------------------------------------------
void interrupt sampling_ISR(...)    
{
  disable();
  sampling_counter1 += sampling_time;
  if (sampling_counter1>65536L)
  {
     sampling_counter1-=65536L;
     old_handler();             //trigger original 0x08h (18.Hz)
  };

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


//####################################################################
void main()
{
int j,error,error_count;

  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("*** STEP-200 ROUND-ROBIN 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);

  set_timer();
  enable();
  get_encoder1();

  //----motion command------------------------
  error=0;
  error_count=0;
  cprintf("\r\n");

  cprintf("MSTEP2_INTP_LINE02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSTEP2_INTP_LINE02(CARD1,20000,-20000, 2040, 0);
     do {} while (MSTEP2_INTP_STOP()!=READY);
     do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     delay(5);
     get_encoder1();
     if ( (x_value!=20000) || (y_value!=-20000))
     {
	error=1;
	error_count++;
	cprintf("error x=%d, y=%d N=%d\r\n",x_value,y_value,j);
     };
     delay(5);
     ENC3_RESET_ENCODER(X_axis);
     ENC3_RESET_ENCODER(Y_axis);
     ENC3_RESET_ENCODER(Z_axis);
  }

  cprintf("MSTEP2_INTP_ARC02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSTEP2_INTP_ARC02(CARD1,20000,-20000, 100000, CW, 2040,0);
     do {} while (MSTEP2_INTP_STOP()!=READY);
     do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     delay(5);
     get_encoder1();
     if ( (x_value!=20000) || (y_value!=-20000))
     {
	error=1;
	error_count++;
	cprintf("error x=%d, y=%d N=%d\r\n",x_value,y_value,j);
     };
     delay(5);
     ENC3_RESET_ENCODER(X_axis);
     ENC3_RESET_ENCODER(Y_axis);
     ENC3_RESET_ENCODER(Z_axis);
  }

  cprintf("MSTEP2_INTP_CIRCLE02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSTEP2_INTP_CIRCLE02(CARD1,10000,-10000, CW, 2040,0);
     do {} while (MSTEP2_INTP_STOP()!=READY);
     do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //do {} while (MSTEP2_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     delay(5);
     get_encoder1();
     if ( (x_value!=0) || (y_value!=0))
     {
	error=1;
	error_count++;
	cprintf("error x=%d, y=%d N=%d\r\n",x_value,y_value,j);
     };
     delay(5);
     ENC3_RESET_ENCODER(X_axis);
     ENC3_RESET_ENCODER(Y_axis);
     ENC3_RESET_ENCODER(Z_axis);
  }

  MSTEP2_RESET_SYSTEM(CARD1);
  release_timer();
}