p1240contidrive.cpp

基于PCI-1240运动控制卡的示例3-ContiDriv

/*
 ****************************************************************************************
 * Program        : P1240LINE.CPP                                                       *
 * Description    : PCI-1240 demo program for X & Y-Axis linear interpolation function. *
 * APIs used      : P1240MotDevOpen,P1240MotAxisParaSet,P1240MotLine,P1240MotAxisBusy   *
 *                  P1240MotDevClose                                                    *
 * Revision       : 1.00                                                                *
 * Date           : 09/01/2000                  Advantech Co., Ltd.                     *
 ****************************************************************************************
 */
#include <windows.h>
#include <windef.h>
#include <stdio.h>
#include <iostream.h>
#include <conio.h>
#include <winreg.h>
#include <math.h>

#include "..\..\..\include\ADS1240.h"
#define ID_LCPATH		1			// Assign the Buffer ID for put path data

void main()
{
  BYTE byBoard_ID,byTS,byRA,byArcDirection;
  DWORD dwSV,dwDV,dwMDV,dwAC,dwAK;
  DWORD dwReturnCode;
  DWORD			LCPoint_Num=9;
  MotMoveBuffer	PathData;
  DWORD	k,CurNum, PreCurNum=0;

	LONG lCenter1;
	LONG lCenter2;
	LONG lEnd1;
	LONG lEnd2;
	LONG lPulseX;
	LONG lPulseY;
	LONG lPulseZ;
	LONG lPulseU;

  byBoard_ID = 0;
  byTS = TCurveAcceleration;
  byRA = RelativeCoordinate;
  dwMDV = 200000;
  dwSV = 1000;
  dwDV = 40000;
  dwAC = 20000;
  dwAK = 619000;
  byArcDirection = CCW;

  printf(" PCI-1240 demo program for X & Y-Axis continue drive function.\n\n");
  printf(" T curve acceleration\n");
  printf(" Relative coordinate\n");
  printf(" MDV : %6d\n",dwMDV);
  printf(" SV  : %6d\n",dwSV);
  printf(" DV  : %6d\n",dwDV);
  printf(" AC  : %6d\n",dwAC);
  printf(" AK  : %6d\n",dwAK);

  // Device open
  dwReturnCode = P1240MotDevOpen(byBoard_ID);
  if (dwReturnCode != ERROR_SUCCESS)
  {
    printf("\n\n Program Fail %4x",dwReturnCode);
    printf("\n Board 0 doesn't exsit !");
    printf("\n Press any key to exit....");
    getch();
    exit(1);
  }

  // Set linear interpolation parameter
   dwReturnCode = P1240MotAxisParaSet(byBoard_ID,0,byTS,dwSV,dwDV,dwMDV,dwAC,dwAK);
  if (dwReturnCode != ERROR_SUCCESS)
    printf("\n\n Program Fail %4x",dwReturnCode);

  LONG Xpulses[10] = {20000,20000,20000,20000,-20000,-20000,-20000,20000,20000,2000};
  LONG Ypulses[10] = {15000,20000,0,-20000,20000,0,-20000,20000,0,-20000};
  lPulseX = Xpulses[0];
  lPulseY = Ypulses[0];
  dwReturnCode = P1240MotLine (byBoard_ID,XY_Axis,byRA,lPulseX,lPulseY,0,0);
  if (dwReturnCode != ERROR_SUCCESS)
  {
	  cout<<"Moving to the begginning spot failed!"<<endl;
	  cout<<dwReturnCode<<endl;
  }
	  cout<<"Moving to the begginning spot succeed!"<<endl;
  while(P1240MotAxisBusy(byBoard_ID,XY_Axis) == ERROR_SUCCESS)
  {
	;
  }
  cout<<"Moving to the begginning spot succeed!"<<endl;
START:
  cout<<"输入0非连续插补,输入1连续插补:"<<endl;
  int selection;
  cin>>selection;
  if (selection==0)
  {
	 for (int pi=1;pi<4;pi++)
	 {
		 lPulseX = Xpulses[pi];
		 lPulseY = Ypulses[pi];
		 dwReturnCode = P1240MotLine (byBoard_ID,XY_Axis,byRA,lPulseX,lPulseY,0,0);
		 if (dwReturnCode != ERROR_SUCCESS)
		{
			cout<<"Spot "<<pi<<" line interpolation failed!"<<endl;
			cout<<dwReturnCode<<endl;
		}
		while(P1240MotAxisBusy(byBoard_ID,XY_Axis) == ERROR_SUCCESS)
		{
			;
        }
/*	dwReturnCode = P1240MotLine(
		byBoard_ID, //Board ID number
		XY_Axis,    //Assign operation axis
		byRA,       //Assign movement operation is relative 
			          //or absolute coordinate
		lPulseX,    //The new position for X axis
		lPulseY,    //The new position for Y axis
		0,          //The new position for Z axis
		0);         //The new position for U axis*/
	 }
	 for (;pi<10;pi++)		//三点至五点圆弧插补
	 {
		 lEnd1 = Xpulses[pi+1];
		 lEnd2 = Ypulses[pi+1];
		 //double l= (Xpluse[i] - Xpluse[i+1]) * (Xpluse[i] - Xpluse[i+1]) + (Ypluse[i] - Ypluse[i+1]) * (Ypluse[i] - Ypluse[i+1]);
		 //double disdance = sqrt (l);	//由当前点到插补终点的距离
		 lCenter1 = (Xpulses[pi]+Xpulses[pi+1])/2;
		 lCenter2 = (Ypulses[pi]+Ypulses[pi+1])/2;
		 dwReturnCode = P1240MotArc (byBoard_ID,XY_Axis,byRA,byArcDirection,lCenter1,lCenter2,lEnd1,lEnd2);
		 if (dwReturnCode != ERROR_SUCCESS)
			 cout<<"Arc interpolation failed!"<<"The current spot is:"<<pi<<endl
				 <<"The error code is:"<<dwReturnCode<<endl;
		 while (P1240MotAxisBusy(byBoard_ID,XY_Axis) ==	ERROR_SUCCESS)
		 {
			 ;
		 }
	 }
  }

	else if (selection==1)
	{
		P1240InitialContiBuf(ID_LCPATH,LCPoint_Num);
  
		for(k=1; k<=LCPoint_Num; k++)
		{
			if (k<4)
			{
				PathData.dwEndPoint_ax1 = Xpulses[k];
				PathData.dwEndPoint_ax2 = Ypulses[k];
				PathData.dwEndPoint_ax3 = 0;
				PathData.dwCenPoint_ax1 = 0;
	   			PathData.dwCenPoint_ax2 = 0;
				PathData.wCommand = IPO_L2;
			}
			else
			{
				PathData.dwEndPoint_ax1 = Xpulses[k];
				PathData.dwEndPoint_ax2 = Ypulses[k];
				PathData.dwEndPoint_ax3 = 0;
				PathData.dwCenPoint_ax1 = (Xpulses[k]+Xpulses[k+1])/2;
	   			PathData.dwCenPoint_ax2 = (Ypulses[k]+Ypulses[k+1])/2;
				PathData.wCommand = IPO_CCW;
			}
/*      switch(k % 4)
	  {
		case 1 :

			PathData.dwEndPoint_ax1 = 0;
			PathData.dwEndPoint_ax2 = 20000;
			PathData.dwEndPoint_ax3 = 0;
			PathData.dwCenPoint_ax1 = 0;
			PathData.dwCenPoint_ax2 = 10000;
			PathData.wCommand = IPO_CW;
			break;
		case 2 :
			PathData.dwEndPoint_ax1 = 10000;
			PathData.dwEndPoint_ax2 = 0;
			PathData.dwEndPoint_ax3 = 0;
			PathData.dwCenPoint_ax1 = 0;
			PathData.dwCenPoint_ax2 = 0;
			PathData.wCommand = IPO_L2;
			break;
		case 3 :
			PathData.dwEndPoint_ax1 = 0;
			PathData.dwEndPoint_ax2 = -20000;
			PathData.dwEndPoint_ax3 = 0;
			PathData.dwCenPoint_ax1 = 0;
			PathData.dwCenPoint_ax2 = -10000;
			PathData.wCommand = IPO_CW;
			break;
		case 0 :
			PathData.dwEndPoint_ax1 = -10000;
			PathData.dwEndPoint_ax2 = 0;
			PathData.dwEndPoint_ax3 = 0;
			PathData.dwCenPoint_ax1 = 0;
			PathData.dwCenPoint_ax2 = 0;
			PathData.wCommand = IPO_L2;
			break;

	  }*/
   			dwReturnCode = P1240SetContiData(ID_LCPATH,		//Assign the Buffer ID for put path data
												&PathData,		// Path data to be put to buffer
												k);
			if (dwReturnCode != ERROR_SUCCESS)
				printf("\n\n Program Fail %4x",dwReturnCode);
		}

		dwReturnCode = P1240StartContiDrive(byBoard_ID,XY_Axis,ID_LCPATH);
		if (dwReturnCode != ERROR_SUCCESS)
			printf("\n\n Program Fail %4x",dwReturnCode);

		while(P1240MotAxisBusy(byBoard_ID,X_Axis))
		{
			P1240GetCurContiNum(byBoard_ID,&CurNum);
			if(CurNum != PreCurNum)
			{
				printf("\n Current Path_Number = %d",CurNum);
		        PreCurNum = CurNum;
			}
		}

		printf("\n Press any key to stop.");
		getch();

		P1240MotStop(byBoard_ID,XY_Axis,IMME_STOP);
		P1240FreeContiBuf(ID_LCPATH);
	}
	cout<<"If you want to quit,press 'q',if you want the machine moving in another mode,"<<endl
		<<"press the other key."<<endl;
	char s;
	cin>>s;
	if ((s=='q') || (s=='Q'))
	{
		dwReturnCode = P1240MotDevClose(byBoard_ID);
		if (dwReturnCode != ERROR_SUCCESS)
			printf("\n\n Program Fail %4x",dwReturnCode);
	}
	else //if ((s!='q') || (s!='Q'))
		goto START;
	
}