zonghe.cpp

此为两关节机器人末端轨迹规划和模糊控制的VC程序

//#include <winbase.h>       
#include <stdio.h>
#include <stdafx.h>
#include <string.h>
#include <iostream.h>
#include <stdlib.h>
#include <math.h>



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


int b[13][13]=                              //模糊控制查询表
{
	{-7,-7,-7,-7,-7,-7,-7,-4,-4,-2, 0, 0, 0},
    {-7,-7,-7,-7,-7,-7,-6,-4,-4,-2, 0, 0, 0},
    {-7,-7,-7,-7,-7,-6,-4,-4,-3,-2, 0, 0, 0},
    {-7,-7,-7,-7,-6,-4,-4,-3,-1,0, 1, 2, 2},
    {-6,-6,-6,-6,-4,-4,-2,-1,0, 1, 1, 2, 2},
    {-4,-4,-4,-4,-3,-3,-1,0, 1, 2, 2, 3, 3},
    {-4,-4,-3,-3,-2,-1,0, 1, 2, 3, 3, 4, 4},
    {-3,-3,-2,-2,-1, 0, 1, 3, 3, 4, 4, 4, 4},
    {-2,-2,-1,-1, 0, 1, 2, 4, 4, 6, 6, 6, 6},
    {-2,-2,-1, 0, 1, 3, 4, 4, 6, 7, 7, 7, 7},
    {0, 0, 0, 1, 3, 4, 4, 6, 7, 7, 7, 7, 7},
    {0, 0, 0, 2, 4, 4, 6, 7, 7, 7, 7, 7, 7},
    {0, 0, 0, 2, 4, 4, 7, 7, 7, 7, 7, 7, 7}
};

double u1=0,u2=0;//输出控制基量为0




/*=========pmac定义===========*/
	    HANDLE hMyDll;
		typedef BOOL (CALLBACK * OpenPmac )(DWORD);
        typedef BOOL (CALLBACK * ClosePmac )(DWORD);
        typedef int (CALLBACK * PmacGet )(DWORD,PCHAR,UINT,PCHAR);
		typedef void (CALLBACK * PmacSetVar)(DWORD,char,UINT,double);
		


	
		OpenPmac open;
		ClosePmac close;
		PmacGet pmacget;
		PmacSetVar pmacsetvar;
        
/*
typedef struct PID {

double setpoint;            //设定目标

double proportion;          //比例常数
double integral;            //积分常数
double derivative;          //微分常数

double lasterror;           //error[-1]
double preverror;           //error[-2]
double sumerror;            //sums of errors

}PID;
  */
		typedef struct fuzzylogic{
			double setpoint;        //设定目标
			
			double Ke;              //误差量化因子
			double Kc;              //误差变化的量化因子
			double Ku;              //输出量比例因子

			double preposition;
		}fuzzylogic;

/*
double PIDcalc(PID *pp,double nextpoint)
{
	double derror,error;
	error=pp->setpoint-nextpoint;               //偏差
	pp->sumerror+=error;                        //积分
	derror=pp->lasterror-pp->preverror;         //当前微分
	pp->preverror=pp->lasterror;
	pp->lasterror=error;
	return
		(pp->proportion*error                   //比例项
		+pp->integral*pp->sumerror              //积分项
		+pp->derivative*derror                  //微分项
		);
}
*/

/*========initialize PID structure================*/
/*
void PIDInit(PID *pp)   //初始化，各项赋0
{
   memset(pp,0,sizeof(PID));
}
*/
/*==========初始化fuzzylogic 结构============*/
		void fuzzylogicInit(fuzzylogic *ff)
		{
			memset(ff,0,sizeof(fuzzylogic)); //各项赋零
		}
/*====函数声明===*/
double fuzzylogiccalc1(fuzzylogic *,double);
double fuzzylogiccalc2(fuzzylogic *,double);
void function(double,double,double,double,double *,double *);//运动反解子函数
double sensor1(void);//从传感器(编码器)得到信号的函数（关节1）
void actuator1(double); //将运算结果送给执行器（DA）的函数（关节1）
double sensor2(void);//（关节2）
void actuator2(double);//（关节2）



void main()
{
/*======================================
=============直线插补程序===============
=========================================*/
double P0[2]={440,0};  //起始点
double Pe[2]={212,195}; //终止点
//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;
double N=floor(L/d+1);
printf("N=%8.3f\n",N);
//cout<<"L="<<L<<"\n";
double deltaX,deltaY;//X，Y轴的增量
deltaX=(Pe[0]-P0[0])/N;
deltaY=(Pe[1]-P0[1])/N;
//cout<<"deltaX="<<deltaX<<"\n";
//cout<<"deltaY="<<deltaY<<"\n";


double X[3002],Y[3002];   //存储各插值点坐标
X[0]=P0[0];
Y[0]=P0[1];
//cout<<"X[0]"<<X[0]<<"  "<<"Y[0]"<<Y[0];
for(int l=1;l<=N;l++)
{
	X[l]=X[l-1]+deltaX;
    Y[l]=Y[l-1]+deltaY;
	//cout<<"X Y\n";
    //cout<<X[l]<<" "<<Y[l]<<"\n";
}

/*==============================================
==================运动学反解====================
==============================================*/


double link1=260,link2=180;
double rot1[3002],rot2[3002];
double thitaA,thitaB;
for(int j=0;j<=N;j++)
{
	function(X[j],Y[j],link1,link2,&thitaA,&thitaB);
    rot1[j]=thitaA*5000/pi;//转化为脉冲数位置
	rot2[j]=thitaB*5000/pi;
	printf("rot2=%8.3f\n",rot2[j]);
    //printf("rot2=%8.3f\n",rot2[j]);
	//cout<<"rot1="<<rot1[j]<<"    "<<"rot2="<<rot2[j]<<"\n";
}
printf("rot301=%8.3f\n",rot1[301]);
/*
PID sPID1,sPID2;             //PID Control structure
double rOut1,rOut2;          //PID response(output)
double rIn1,rIn2;           //PID feedback(input)

PIDInit(&sPID1);        //各项赋零
PIDInit(&sPID2);
sPID1.proportion=500;       //设定关节1 PID参数
sPID1.integral=2;
sPID1.derivative=4000;

sPID2.proportion=60;       //设定关节2 PID参数
sPID2.integral=1;
sPID2.derivative=50;
*/

fuzzylogic sFuzzy1,sFuzzy2;
double rOut1,rOut2;          //fuzzylogic response(output)
double rIn1,rIn2;           //fuzzylogic feedback(input)

fuzzylogicInit(&sFuzzy1);   //各项赋零
fuzzylogicInit(&sFuzzy2);


sFuzzy1.Ke=1;     //设定关节1 量化 比例因子
sFuzzy1.Kc=40;//60;
sFuzzy1.Ku=80;//80;

sFuzzy2.Ke=2;        //设定关节2 量化 比例因子
sFuzzy2.Kc=20;
sFuzzy2.Ku=30;

/*================定时10毫秒程序====================*/

       LARGE_INTEGER litmp; 
       LONGLONG QPart1,QPart2;
       double dfMinus, dfFreq, dfTim; 
       QueryPerformanceFrequency(&litmp);
       dfFreq = (double)litmp.QuadPart;// 获得计数器的时钟频率

	   

/*===========PMAC初始化==================*/

	hMyDll = LoadLibrary("PComm32");//加载动态连接库，hMyDll为动态库句柄
    ASSERT (hMyDll!= NULL);
    open = (OpenPmac) GetProcAddress((HMODULE)hMyDll,"OpenPmacDevice");//取得函数地址并赋给函数指针
    close = (ClosePmac) GetProcAddress((HMODULE)hMyDll,"ClosePmacDevice");
    pmacget = (PmacGet) GetProcAddress((HMODULE)hMyDll,"PmacGetResponseA");
	pmacsetvar = (PmacSetVar) GetProcAddress((HMODULE)hMyDll,"PmacSetVariableDouble");
//	char i,m;
    if(open!=NULL)//调用动态库成功
	{	
		(*open)(1);//打开pmac设备1
		if((*open)(1)) //可以与PMAC通讯
		{
			    printf("communication Successfully!!!");
			    (* pmacsetvar)(1,'i',200,0);        //关闭2号口，以使可以写M202
				(* pmacsetvar)(1,'i',100,1);        //打开1号口，以使可以读M162
			    (* pmacsetvar)(1,'m',162,0);
				(* pmacsetvar)(1,'i',400,0);        //关闭4号口，以使可以写M402
				(* pmacsetvar)(1,'i',300,1);        //打开3号口，以使可以读M362
                (* pmacsetvar)(1,'m',362,0);
                
				//double servoperiod[4000];//用来测伺服周期
            for(int k=0;k<=N;k++)
			{	
			   QueryPerformanceCounter(&litmp);
               QPart1 = litmp.QuadPart;// 获得初始值
			   //if(k<N)
			  // {
			   sFuzzy1.setpoint=rot1[k];        //设定输入值
               sFuzzy2.setpoint=rot2[k];
			  // }
			  /* else
			   {  
				   sFuzzy1.setpoint=rot1[301];        //设定输入值
                   sFuzzy2.setpoint=rot2[301];
			   }*/
			   	   rIn1=sensor1();           //读传感器值
	               rOut1=fuzzylogiccalc1(&sFuzzy1,rIn1);     //执行PID运算
		           actuator1(rOut1);            //将运算结果送给执行器（DA）
                  
				   rIn2=sensor2();          
	               rOut2=fuzzylogiccalc2(&sFuzzy2,rIn2);    
		           actuator2(rOut2); 
               do
			   {				   
				  QueryPerformanceCounter(&litmp);
                  QPart2 = litmp.QuadPart;//获得中止值
                  dfMinus = (double)(QPart2-QPart1);
                  dfTim = dfMinus / dfFreq;// 获得对应的时间值，单位为秒
			      //servoperiod[k]=dfTim;
				  //printf("servoperiod=%8.9f\n",servoperiod[k]);
			   }while(dfTim<0.01);
			}
			(* pmacsetvar)(1,'i',200,1);//停止电机以防事故
			(* pmacsetvar)(1,'i',400,1); 
		    (*close)(1);    //关闭PMAC设备1
           FreeLibrary((HMODULE)hMyDll); //释放动态库
		}
		else//不能与PMAC通讯
		{
			printf("can not communication");
          /*  sFuzzy1.setpoint=2;       //调试用
		    printf("sFuzzy1=%8.8f\n",sFuzzy1.Ku);
			rOut1=fuzzylogiccalc1(&sFuzzy1,2.3); 
			printf("rOut1=%8.8f\n",rOut1);*/
		}
	}
	
	else    //调用动态库失败
	{
		printf("can not connect with pcomm32");
	}



}




/*=============子函数定义===============*/
		double fuzzylogiccalc1(fuzzylogic *ff,double nextpoint)
		{
			double e,ec;
			e=ff->setpoint-nextpoint;         //偏差???????????
            ec=(ff->preposition-nextpoint); //偏差变化???????????????
            ff->preposition=nextpoint;
			
                    //偏差e及偏差变化ec的模糊化
            int me0,mec0;
            if(ff->Ke*e>=0)
				 me0=int(ff->Ke*e+0.5);
            else
				 me0=int(ff->Ke*e-0.5);
            if(ff->Kc*e>=0)
				 mec0=int(ff->Kc*ec+0.5);
            else
				 mec0=int(ff->Kc*ec-0.5);

            int me=me0+6;
            int mec=mec0+6;
            if(me<0)me=0;
            if(me>12)me=12;
            if(mec<0)mec=0;
            if(mec>12)mec=12;
			//printf("me=%d\n",me);
            //printf("mec=%d\n",mec);
            u1=ff->Ku*b[me][mec]+u1;//控制量的变化量+当前控制量
	    	//printf("u=%8.8f\n",u);//调试用
			//printf("b=%d\n",b[me][mec]);
			
			return u1;
			
		}



		double fuzzylogiccalc2(fuzzylogic *ff,double nextpoint)
		{
			double e,ec;
			e=ff->setpoint-nextpoint;         //偏差???????????
            ec=(ff->preposition-nextpoint); //偏差变化???????????????
            ff->preposition=nextpoint;
			
                    //偏差e及偏差变化ec的模糊化
            int me0,mec0;
            if(ff->Ke*e>=0)
				 me0=int(ff->Ke*e+0.5);
            else
				 me0=int(ff->Ke*e-0.5);
            if(ff->Kc*e>=0)
				 mec0=int(ff->Kc*ec+0.5);
            else
				 mec0=int(ff->Kc*ec-0.5);

            int me=me0+6;
            int mec=mec0+6;
            if(me<0)me=0;
            if(me>12)me=12;
            if(mec<0)mec=0;
            if(mec>12)mec=12;
			//printf("me=%d\n",me);
            //printf("mec=%d\n",mec);
            u2=ff->Ku*b[me][mec]+u2;//控制量的变化量+当前控制量
	    	//printf("u=%8.8f\n",u);//调试用
			//printf("b=%d\n",b[me][mec]);
			
			return u2;
			
		}




void function(double Xe,double Ye,double l1,double l2,double * thita1,double * thita2)//Xe、Ye为末端坐标，l1、l2为杆长
{
	//double Xe,Ye,l1,l2;
	//double thita1,thita2;
	double CosThita2,CosPhi;
	double beta,phi;
    CosThita2=(Xe*Xe+Ye*Ye-l1*l1-l2*l2)/(2*l1*l2);
	* thita2=-acos(CosThita2);//在此只取thita2<0,thita1>0的解，也即只取第一象限的轨迹
    beta=atan(Ye/Xe);
    CosPhi=(Xe*Xe+Ye*Ye+l1*l1-l2*l2)/(2*l1*sqrt(Xe*Xe+Ye*Ye));
	phi=acos(CosPhi);
	* thita1=beta+phi;//弧度

}


double sensor1(void)          //定义从传感器(编码器)得到信号的函数(1号口)
{
	TCHAR buf1[255];
	double position1, position2;
	(* pmacget)(1,buf1,255,"m162");
	position1=atof(buf1);
    position2=position1/3072;      //将读出得值转换为脉冲数，1/[ix08*32]cts
	//printf("positionA=%8.2f\n",position2);
	return position2;
}

void actuator1(double rDelta)       //定义将运算结果送给执行器（DA）的函数(2号口)
{  
	//char m;
	if(rDelta<20480&&rDelta>-20480)                //20480由Ix69限制
	{
	    (* pmacsetvar)(1,'m',202,rDelta);
	}
	else if(rDelta>=20480)
	{
		(* pmacsetvar)(1,'m',202,20480);
	}
	else
	{
		(* pmacsetvar)(1,'m',202,-20480);
	}

/*	    TCHAR buf2[255];       //此下为调程序时方便，必要时可删去以减小伺服周期
	    double out; 
	    (* pmacget)(1,buf2,255,"m202");
	    out=atof(buf2);
	    printf("out=%8.2f\n",out);
*/
 
}



double sensor2(void)          //定义从传感器(编码器)得到信号的函数(3号口)
{
	TCHAR buf1[255];
	double position1, position2;
	(* pmacget)(1,buf1,255,"m362");
	position1=atof(buf1);
    position2=position1/3072;      //将读出得值转换为脉冲数，1/[ix08*32]cts
	//printf("positionB=%8.2f\n",position2);
	return position2;
}

void actuator2(double rDelta)       //定义将运算结果送给执行器（DA）的函数(4号口)
{  
	//char m;
	if(rDelta<20480&&rDelta>-20480)                //20480由Ix69限制
	{
	    (* pmacsetvar)(1,'m',402,rDelta);
	}
	else if(rDelta>=20480)
	{
		(* pmacsetvar)(1,'m',402,20480);
	}
	else
	{
		(* pmacsetvar)(1,'m',402,-20480);
	}
 
/*        TCHAR buf2[255];       //此下为调程序时方便，必要时可删去以减小伺服周期
	    double out; 
	    (* pmacget)(1,buf2,255,"m402");
	    out=atof(buf2);
        printf("outB=%8.2f\n",out);
*/
}