//--------------------------------------------------------
// DEMO2.cpp                                    2/24/2001
//                   
// CPU version 2.0
//
// Function:
//    test WAIT_X_STOP(),WAIT_IS_X_STOP()
//    round robin test
//--------------------------------------------------------
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <dos.h>
#include <bios.h>
#include "mstep3.h"

//----- define --------------------------------------------
#define CARD1 0

#define ROUNDS 100
//----- structure -----------------------------------------
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 x_mode;
	 unsigned char x_dir;
	 unsigned char x_son;
	 unsigned char y_mode;
	 unsigned char y_dir;
	 unsigned char y_son;
	 unsigned char z_mode;
	 unsigned char z_dir;
	 unsigned char z_son;

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

//----- variable ------------------------------------------
CardParameter card1;
long new_z,old_z;
int  wait_count=0;
//--------------------------------------------------------------------
#define INTR 0x08                  //timer interrupt number
#define sampling_time 11931        //<1193180Hz>/11931=Hz(10ms)
unsigned long sampling_counter1=0;
void interrupt sampling_ISR(...);
void interrupt (*old_handler)(...);

//--------------------------------------------------------------------
// set timer interrupt period as 2.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();
}
//--------------------------------------------------------------------
// 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(...)    // 2.5ms sample once
{
int x,y;

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

  //----- get card 1 information ----------------
  if (card1.exist==YES)
  {
    card1.ip  = MSTEP3_DI(CARD1);
    card1.msc = MSTEP3_MSC(CARD1);

    MSTEP3_GET_CARD(CARD1);
    card1.ls  =MSTEP3_GET_LIMIT(CARD1);
    card1.p1  =MSTEP3_GET_P1(CARD1);
    card1.XC  =MSTEP3_GET_XC(CARD1);
    card1.XP  =MSTEP3_GET_XP(CARD1);
    card1.YC  =MSTEP3_GET_YC(CARD1);
    card1.YP  =MSTEP3_GET_YP(CARD1);
    card1.ZC  =MSTEP3_GET_ZC(CARD1);
    card1.ZP  =MSTEP3_GET_ZP(CARD1);
  }
  if (wait_count) wait_count--;

  x=wherex(); y=wherey();
  gotoxy(5,2); cprintf("%6ld",card1.XP);
  gotoxy(15,2); cprintf("%6ld",card1.YP);
  gotoxy(25,2); cprintf("%6ld",card1.ZP);
  gotoxy(x,y);

  outportb(0x20,0x20);
  enable();
}
//--------------------------------------------------------------------
// set the operation parameter of PISO-PS300
//--------------------------------------------------------------------
void set_parameter()
{
  MSTEP3_SET_NC(CARD1,NO);
  MSTEP3_SET_CONTROL_MODE(CARD1, card1.x_mode, card1.y_mode, card1.z_mode);
  MSTEP3_SET_VAR(CARD1, card1.DDA, card1.AD, card1.LSP,
		 card1.HSP, card1.arc_speed);
  MSTEP3_SET_DEFDIR(CARD1, card1.x_dir, card1.y_dir, card1.z_dir);
  MSTEP3_SET_SERVO_ON(CARD1, card1.x_son, card1.y_son, card1.z_son);
}
//####################################################################
void main()
{
int j,error,error_count,temp1,temp2,temp3;

  disable();
  clrscr();

  //------set card 1 parameters-------------------------------------
  card1.address      = 0;     //0~15
  card1.DDA          = 1;
  card1.AD           = 50;
  card1.LSP          = 50;
  card1.HSP          = 2040;
  card1.home_speed   = 50;
  card1.search_speed = 5;
  card1.arc_speed    = 500;

  card1.x_mode       = DDA_CW_CCW|DDA_EN|ENC_CW_CCW|ENC_INTERNAL;
  card1.x_dir        = NORMAL_DIR;
  card1.x_son        = OFF;
  card1.y_mode       = DDA_CW_CCW|DDA_EN|ENC_CW_CCW|ENC_INTERNAL;
  card1.y_dir        = NORMAL_DIR;
  card1.y_son        = OFF;
  card1.z_mode       = DDA_CW_CCW|DDA_EN|ENC_CW_CCW|ENC_INTERNAL;
  card1.z_dir        = NORMAL_DIR;
  card1.z_son        = OFF;

  //---- check PISO-PS300/S300 is exist or not ------------------------
  card1.exist=MSTEP3_REGISTRATION(CARD1, card1.address);
  if (card1.exist!=YES)
  {
     cprintf("There is not exist any PISO-PS300/S300 card !");
     return;
  }
  MSTEP3_RESET_SYSTEM(CARD1);
  clrscr();

  //-------------------------------------------------------------------
  set_timer();
  set_parameter();
  enable();

  cprintf("*** PISO-PS300 ROUND-ROBIN test ***\r\n");

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

  cprintf("MSTEP3_INTP_XYZ02             \r\n");
  for (j=0; j<ROUNDS; j++)
  {
     //------------------------------------------------
     temp1=random(20000);
     temp2=random(20000);
     temp3=random(20000);
     MSTEP3_INTP_XYZ02(CARD1,temp1,-temp2, temp3, 2040, 0);
     do
     { //cprintf("%d full\r\n",j);
     } while (MSTEP3_INTP_STOP()!=READY);
     do {} while (MSTEP3_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     wait_count=2;
     while (wait_count) {}; //wait for correct position input in ISR
     if ( (card1.XC!=temp1) || (card1.YC!=-temp2) || (card1.ZC!=temp3))
     {
	error=1;
	error_count++;
	cprintf("error: N=%d, x=%ld, y=%ld, z=%ld               \r\n",
		j,card1.XP,card1.YP,card1.ZP);
     }
     else
     {
	cprintf("ok: N=%d, x=%ld, y=%ld, z=%ld                  \r",
		j,card1.XP,card1.YP,card1.ZP);
     }
     MSTEP3_PRESET_POSITION(CARD1,X_axis,0);
     MSTEP3_PRESET_POSITION(CARD1,Y_axis,0);
     MSTEP3_PRESET_POSITION(CARD1,Z_axis,0);
  }

  cprintf("MSTEP3_INTP_ARC02                     \r\n");
  for (j=0; j<ROUNDS; j++)
  {
     //------------------------------------------------
     temp1=random(20000)+1000;
     temp2=random(20000)+1000;
     MSTEP3_INTP_ARC02(CARD1,XY_plane,temp1,-temp2, 100000, CW, 2040,0);
     do {} while (MSTEP3_INTP_STOP()!=READY);
     do {} while (MSTEP3_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     wait_count=2;
     while (wait_count) {}; //wait for correct position input in ISR
     if ( (card1.XC!=temp1) || (card1.YC!=-temp2))
     {
	error=1;
	error_count++;
	cprintf("error: N=%d, x=%ld, y=%ld             \r\n",
		j,card1.XP,card1.YP);
     }
     else
     {
	cprintf("ok: N=%d, x=%ld, y=%ld                \r",
		j,card1.XP,card1.YP);
     }
     MSTEP3_PRESET_POSITION(CARD1,X_axis,0);
     MSTEP3_PRESET_POSITION(CARD1,Y_axis,0);
  }

  cprintf("MSTEP3_INTP_CIRCLE02                       \r\n");
  for (j=0; j<ROUNDS; j++)
  {
     temp1=random(20000)+2;
     temp2=random(20000)+2;
     //------------------------------------------------
     MSTEP3_INTP_CIRCLE02(CARD1,XY_plane,temp1,-temp2, CW, 2040,0);
     do {} while (MSTEP3_INTP_STOP()!=READY);
     do {} while (MSTEP3_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     wait_count=2;
     while (wait_count) {}; //wait for correct position input in ISR
     if ( (card1.XC!=0) || (card1.YC!=0))
     {
	error=1;
	error_count++;
	cprintf("error: N=%d, x=%ld, y=%ld             \r\n",
		j,card1.XP,card1.YP);
     }
     else
     {
	cprintf("ok: N=%d, x=%ld, y=%ld                \r",
		j,card1.XP,card1.YP);
     }
     MSTEP3_PRESET_POSITION(CARD1,X_axis,0);
     MSTEP3_PRESET_POSITION(CARD1,Y_axis,0);
  }

  MSTEP3_RESET_SYSTEM(CARD1);
  release_timer();
}