//--------------------------------------------------------
// DEMO2.cpp                                    2/25/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 "mservo3.h"

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

//----- 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 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;

//----- 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  = 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);
  }
  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()
{
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 main()
{
int j,error,error_count;

  disable();
  clrscr();

  //------set card 1 parameters-------------------------------------
  card1.address = 0x300;
  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.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        = NORMAL_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;

  //---- check SERVO-300 is exist or not ------------------------ 
  card1.exist=MSERVO3_REGISTRATION(CARD1, card1.address); 
  if (card1.exist!=YES)
  {
     cprintf("There is not exist any SERVO-300 card !");
     return;
  }  
  MSERVO3_RESET_SYSTEM(CARD1);  
  clrscr();

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

  cprintf("*** SERVO-300 ROUND-ROBIN test ***\r\n");

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

  cprintf("MSERVO3_INTP_LINE02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSERVO3_INTP_LINE02(CARD1,XY_plane, 20000,-20000, 2040, 0);
     do
     { //cprintf("%d full\r\n",j);
     } while (MSERVO3_INTP_STOP()!=READY);
     do {} while (MSERVO3_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     wait_count=2;
     while (wait_count) {}; //wait for correct position input in ISR
     if ( (card1.XC!=20000) || (card1.YC!=-20000))
     {
        error=1;
        error_count++;
	cprintf("error x=%ld, y=%ld N=%d\r\n",card1.XP,card1.YP,j);
     };
     MSERVO3_PRESET_POSITION(CARD1,X_axis,0);
     MSERVO3_PRESET_POSITION(CARD1,Y_axis,0);
  }

  cprintf("MSERVO3_INTP_ARC02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSERVO3_INTP_ARC02(CARD1,XY_plane,20000,-20000, 100000, CW, 2040,0);
     do {} while (MSERVO3_INTP_STOP()!=READY);
     do {} while (MSERVO3_IS_X_STOP(CARD1)==NO);
     //---------------------------------------------
     wait_count=2;
     while (wait_count) {}; //wait for correct position input in ISR
     if ( (card1.XC!=20000) || (card1.YC!=-20000))
     {
        error=1;
        error_count++;
	cprintf("error x=%ld, y=%ld N=%d\r\n",card1.XP,card1.YP,j);
     };
     MSERVO3_PRESET_POSITION(CARD1,X_axis,0);
     MSERVO3_PRESET_POSITION(CARD1,Y_axis,0);
  }

  cprintf("MSERVO3_INTP_CIRCLE02\r\n");
  for (j=0; j<400; j++)
  {
     //------------------------------------------------
     MSERVO3_INTP_CIRCLE02(CARD1,XY_plane,10000,-10000, CW, 2040,0);
     do {} while (MSERVO3_INTP_STOP()!=READY);
     do {} while (MSERVO3_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 x=%ld, y=%ld N=%d\r\n",card1.XP,card1.YP,j);
     };
     MSERVO3_PRESET_POSITION(CARD1,X_axis,0);
     MSERVO3_PRESET_POSITION(CARD1,Y_axis,0);
  }

  MSERVO3_RESET_SYSTEM(CARD1);
  release_timer();
}