lineinterpolation.cpp

四自由度scara机械手臂的直线插补,完成两个关节的插补角度计算

#include <string.h>
#include <stdio.h>
#include <math.h>
#include <iostream.h>
#include <iomanip.h>

#define v 10        //插补沿直线速度5mm/s
#define Ts 0.2      //插补时间间隔20ms
#define pi 3.1415926

void reset();

void main()
{
/*======================================
=============直线插补程序===============
	=========================================*/
	double P0[2];  //起始点
	double Pe[2]; //终止点
	cout<<"input start point P0: ";
	cin>>P0[0]>>P0[1];
	cout<<"input end point Pe: ";
	cin>>Pe[0]>>Pe[1];
	//double L;            //轨迹长度
	//int N;                //插补总步数
	//float d;              //Ts间隔内行程
	double L=sqrt(pow(Pe[0]-P0[0],2)+pow(Pe[1]-P0[1],2));
	double d=v*Ts;
	int count=(int)floor(L/d+1);
	//cout<<"N="<<N<<"\n";
	//cout<<"L="<<L<<"\n";
	double deltaX,deltaY;//X，Y轴的增量
	deltaX=(Pe[0]-P0[0])/count;
	deltaY=(Pe[1]-P0[1])/count;
	//cout<<"deltaX="<<deltaX<<"\n";
	//cout<<"deltaY="<<deltaY<<"\n";
	
	double *X=new double[count+1];
	double *Y=new double[count+1];   //存储各插值点坐标
	X[0]=P0[0];
	Y[0]=P0[1];
	//cout<<"X[0]"<<X[0]<<"  "<<"Y[0]"<<Y[0];
	for(int i=1;i<=count;i++)
	{
		X[i]=X[i-1]+deltaX;
		Y[i]=Y[i-1]+deltaY;
		//cout<<"X Y\n";
		// cout<<X[i]<<" "<<Y[i]<<"\n";
	}
	
	/*==============================================
	==================运动学反解====================
	==============================================*/
	double arm1=200;
	double arm2=150;
	double vertical=50;
	double *angle1=new double[count+1];
	double *angle2=new double[count+1];
	double *A=new double[count+1];
	int k;
	for(k=0;k<=count;k++)
	{
 		A[k]=0;
 	}
	for(k=0;k<=count;k++)
	{
		angle1[k]=0;
	}
	for(k=0;k<=count;k++)
	{
		angle2[k]=0;
  	}
	k=0;
	for (int j=0;j<=count;j++)
	{
		A[j]=(arm1*arm1-arm2*arm2+X[j]*X[j]+Y[j]*Y[j])/(2*arm1*sqrt(X[j]*X[j]+Y[j]*Y[j]));
//		if(A[j]*A[j]>=1)
//		{
//			cout<<"error ";
//			break;
// 		}
		if(X[j]<0)
			angle1[j]=atan(A[j]/sqrt(1-A[j]*A[j]))-atan(X[j]/Y[j]);
		else
			angle1[j]=atan(-A[j]/sqrt(1-A[j]*A[j]))-atan(X[j]/Y[j]);
 		angle2[j]=atan(sqrt(X[j]*X[j]+Y[j]*Y[j])*cos(angle1[j]+atan(X[j]/Y[j]))/
 			         (sqrt(X[j]*X[j]+Y[j]*Y[j])*sin(angle1[j]+atan(X[j]/Y[j]))-arm1));
		if (angle1[j]<-2.0944||angle1[j]>2.0944||angle2[j]<-1.9199||angle2[j]>1.9199)
		{
			cout<<"out of range ";
			k--;
			break;
  		}
		k++;
 	
// 		cout<<A[j]<<endl;
	}
	if (k==j)
	{
		for(k=0;k<=count;k++)
			cout<<angle1[k]<<"    "<<angle2[k]<<endl;
	}

	if(X!=NULL)
	{
		delete []X;
	}
	if(Y!=NULL)
	{
		delete []Y;
	}
	if(A!=NULL)
	{
		delete []A;
	}
	if(angle1!=NULL)
	{
		delete []angle1;
	}
	if(angle2!=NULL)
	{
		delete []angle2;
 	}
}

void reset()
{
    double initialangle1=1/9*pi;
	double initialangle2=0;
}