ctrl.cpp

课题研究用的直线型一级倒立摆的源程序

#include <dos.h>
#include <bios.h>
#include <conio.h>
#include <stdio.h>
#include <graphics.h>

#include "face.h"
#include "sv_pci.h"
#include "draw.h"
#include "ctrl.h"

#include "userlib.h"
#include  "sv_pci.h"

void interrupt(*oldhandler)(...); //旧中断服务程序句柄定义
void interrupt handler(...);

extern int point_index;
extern void *image;
extern int mutex_event; //稳摆条件标志位

int times, times_draw, times_sign;


int start=0;     	/* 伺服控制状态标志 */
int safety = 0;  	/* 位置溢出标志 */

// 注：  电机编码器与运动控制卡的第一通道即第一轴相连
//       摆杆角度编码器与运动控制卡的第二通道即第二轴相连
double pos = 0;			        /* 1轴位置值，单位为m */
double pos0 = 0;		        /* 1轴上一个中断时间位置值 */

double angle = 0;		        /* 2轴角度值，单位为弧度 */
double angle0 = 0;		        /* 2轴上一个中断时间角度值 */

double swingup_ang = 0.18;
double energy = 0;

double ang_2pi = 0;

int pend = 0;			        /* 摆杆状态标志 */
int motion = 0;

float offset= 0;		        /* 位置偏移值 */
int times_flag = 0;		

double acc = 0;                         //初始化控制量：小车的速度和加速度
double vel = 0;			

double angleDot = 0;		        /* 摆杆角速度 */
double posDot = 0;      	        /* 小车的实际速度 */

double v = 0;
double v0[4] = {0, 0, 0, 0};
double v1[4] = {0, 0, 0, 0};

double sign_v = 0;
double pid_ang[5];
double pid_pos[5];

extern int ctrl_mode;

extern float respond_max_pos;
extern float respond_max_the;
extern int respond_start;		// 记录响应曲线开始标志
extern int respond_index;
extern RESPOND_CURVE respond[];
extern PARAM_INFO param_info[];
extern short rtn;
long actl_pos,apos1,apos2;
int pulse_sign = 0;
int index;
long vpos[400];


void interrupt handler(...) /*中断服务程序入口*/  
{

	disable();		                    /* 关闭中断*/

	if (mutex_event == 0)
		handle_pendulum();                  // 稳摆函数

	enable();                                    /* 打开中断，允许下次中断发生 */
}

int handle_safety() /*安全处理模块*/
{
	unsigned short swt;
	GT_GetSts(&swt);                             //查询控制卡状态

	if ((fabs(pos) >= LIMIT) ||                  //检查是否到达软限位位置或硬限位（限位开关）位置
	((swt & 0x20) == 0x20 ) ||
	((swt & 0x40) == 0x40))
	{
		safety = 1;                          //如果是，复位变量，不终止整个程序的运行
		motion = 0;
		offset = 0;
		vel = 0;
		acc = 0;
	}

	if (fabs(posDot) > MAX_SPEED){               //检查当前运动速度是否超过最高安全速度
		safety = ERR_2;                      //如果是，关伺服和控制器，返回-1，最终终止整个程序的运行
		rtn=GT_AxisOff();
		GT_Close();

		return -1;
	}
	
	switch (safety)
	{
	case ERR_1:                                  //检查急停按扭（空格键）是否按下
		rtn=GT_AxisOff();                    //如果是关掉伺服，返回-1，最终终止整个程序的运行     
		return -1;

	case 1:                                      //在小车到达软限位或硬限位的情况下，回到原点，再次起摆
		safety = 2;
		pend = 0;
		swingup_ang = 0.2;
		energy = 0;
		rtn=GT_Axis(1);
		rtn=GT_ClrSts();
		rtn=GT_SetVel(0);
		rtn=GT_SetAcc(0.3);
		rtn=GT_Update();

		/* return to origin */
		rtn=GT_Axis(1);
		rtn=GT_PrflT();
		rtn=GT_Update();

		rtn=GT_ClrSts();
		rtn=GT_SetVel(2);
		rtn=GT_SetAcc(0.25);
		rtn=GT_SetPos(0);
		rtn=GT_Update();

		return -1;

	case 2:

		unsigned short sts=0;
		rtn=GT_Axis(1);
		rtn=GT_GetSts(&sts);
		if ((sts & 0x400 )!=0x400)               //当小车回到原点运动完成时运动控制器状态字sts=0x400
		{
			safety = 0;
			offset = 0;
			vel = 0;
			acc = 0;
			rtn=GT_ClrSts();
			rtn=GT_SetVel(0);
			rtn=GT_SetAcc(0);
			rtn=GT_Update();
			if (start == 0)
			{
				rtn=GT_AxisOff();
			}
			else if (start == 5)
			{
				start = 4;
				rtn=GT_PrflV();
				rtn=GT_Update();
			}
		}
	       return -1;
	}
	return 0;
}

void enable_servo() /*使能伺服，并修改运动控制模式到速度控制模式*/                                  
{
	rtn=GT_AxisOn();
	rtn=GT_PrflV();
	rtn=GT_Update();
}

void handle_pendulum()
{
        /*times，times_draw分别为图形界面上绘制曲线和显示数据的采样时间*/	
	if (times < TIME_ONE)
		times ++;
	if (times_draw < TIME_TWO)
		times_draw ++;
		
        /*分别采集摆杆和驱动电机的当前位置，并将他们转化为相应的量纲。
          这些数据分别从运动控制器的2、1通道输入*/

	GT_Axis(2);
	rtn=GT_GetAtlPos(&actl_pos);//error_msg(rtn);
	angle =  -ENCODE2 * actl_pos;
	GT_Axis(1);
	rtn=GT_GetAtlPos(&actl_pos);//error_msg(rtn);
	pos = ENCODE1 * actl_pos;

	/* 根据上次控制周期的采样值和时钟周期计算个运动部件的速度，并更新变量*/
	posDot = (pos - pos0) / INTPERIOD;
	angleDot = (angle - angle0) / INTPERIOD;

	pos0 = pos;
	angle0 = angle;
	
	ang_2pi = angle;
	
	/* 规范化角度，使之在 0 to 2PI之间 */
	while (ang_2pi < 0)
	{ang_2pi += 2 * M_PI;};

	while (ang_2pi >= (2*M_PI))
	{ang_2pi -= 2 * M_PI;};

        /*进行安全检查，如果发生异常（如速度过大等），返回*/
	if ( handle_safety() == -1 )
		return;
	handle_sign();

	switch (start) //根据不同的操作命令，进行不同的处理
	{
		case 0:
			vel=0;
			acc=0;
			break;

		case 1:
			start = 2;
			enable_servo();                     
			return;

		case 3:
			if (first_move() == 0)
				start = 4;                  //进入控制状态
			return;

		case 4:
				linear_up();
			break;

		case 5:
				anti_cran();                //计算保持在该平衡位置所需的控制量
			break;

		default:
			break;
	}

	/*  输出控制值 */
	rtn=GT_ClrSts();                                   //清除控制器状态字
	rtn=GT_SetVel(vel);                                //设置所需的速度
	rtn=GT_SetAcc(acc);                                //设置控制加速度 
	rtn=GT_Update();                                   //刷新数据，施加控制
}

void linear_up() /*起摆能量控制模块*/

{
	if (fabs(ang_2pi-M_PI) <= ENTRY_ANGLE)             //检测倒立摆是否在平衡位置，如果是start=5
	{
		pend = 1;
		start = 5;
		acc = 0;
		vel = 0;
		return;
	}

	if ((ang_2pi < 0.05) || (2 * M_PI - ang_2pi < 0.05))
		energy = (JM * angleDot * angleDot) / 2;
		
	if (fabs(angleDot < 0.05))
	{
		if ((ang_2pi < M_PI) && (( 2 * ang_2pi / 3) >  swingup_ang))
		{
			swingup_ang = 2 * ang_2pi / 3;
		}
		else if ((2 * M_PI - ang_2pi < M_PI) && (( 2 * (2 * M_PI - ang_2pi) / 3 ) > swingup_ang))
			swingup_ang = 2 * (2 * M_PI - ang_2pi) / 3;
	}

	if ((ang_2pi < swingup_ang) || ((2 * M_PI - ang_2pi) < swingup_ang))
	{
		if (((ang_2pi < swingup_ang) || ((2 * M_PI - ang_2pi) < swingup_ang)) && (angleDot > 0))
		{
			vel = -2.2 * MAX_VEL * VEL_COF;
			v = -2 * (E_REF - energy );
		}
		else if (((ang_2pi < swingup_ang) || ((2 * M_PI - ang_2pi) < swingup_ang)) && (angleDot < 0))
		{
		        vel = 2.2 * (MAX_VEL * VEL_COF);
	        	v = 2 * (E_REF - energy );
		}
		acc = fabs(v * ACC_COF) ; 
	}
	else
	{
		if (v<0)
		{vel = -(MAX_VEL * VEL_COF); }
		else
		{vel =( MAX_VEL * VEL_COF);   }
	}


}

void anti_cran()   /*算法处理模块，得出控制量*/
{
	int i;
	float v_ctrl;

	switch (ctrl_mode)   //“ctrl_mode”为算法选择代号
	{

	case 0:  //------LQR控制算法处理模块-------------------------------
		v_ctrl = param_info[0].param_value[0] * (pos + offset + sign_v)
			+ param_info[0].param_value[1] * posDot
			+ param_info[0].param_value[2] * (ang_2pi - M_PI)
			+ param_info[0].param_value[3] * angleDot;
		v = -v_ctrl;

		break;

	case 1: //------PID控制算法处理模块--------------------------------
		v_ctrl = (param_info[1].param_value[0] + param_info[1].param_value[1] + param_info[1].param_value[2] / INTPERIOD) * pid_ang[4]
			-(param_info[1].param_value[0] + 2 * param_info[1].param_value[2] / INTPERIOD) * pid_ang[3]
			+ param_info[1].param_value[2] / INTPERIOD * pid_ang[2];

		v = - 0.3 * ( v0[3] + v_ctrl); 
	
		break;

	case 2:	//------传递函数控制算法处理模块---------------------------
		v_ctrl = param_info[2].param_value[0] * pid_ang[4]
			+ param_info[2].param_value[1] * pid_ang[3]
			+ param_info[2].param_value[2] * pid_ang[2]
			+ param_info[2].param_value[3] * pid_ang[1]
			+ param_info[2].param_value[4] * pid_ang[0]
			+ param_info[2].param_value[5] * v0[3]
			+ param_info[2].param_value[6] * v0[2]
			+ param_info[2].param_value[7] * v0[1]
			+ param_info[2].param_value[8] * v0[0];

		v =  - 0.03 * v_ctrl;
		break;
	}
	for (i=0; i<3; i++)
	{
	    v0[i] = v0[i+1];
	}
	v0[3] += v_ctrl;
/////////////////////将控制量转化为运动控制器可以接受的量纲////////////////////////////	

	acc = fabs(v * ACC_COF);  //摆系统控制量，值为正数
	
/* 运动控制卡速度控制方式下，是通过改变速度的正负来改变运动的方向，"vel"是
 一个事先设定的较大数值（常数），在控制过程中一般不会达到这个速度值*/
 
	if (v>0)    
		vel = (MAX_VEL_1 * VEL_COF);   
	if (v<0)
		vel = -(MAX_VEL_1 * VEL_COF);
		
}





//=====================起摆函数======================
/* 小车起摆来回运动控制 */
int first_move()
{
	unsigned short sts;
	switch(motion){
	case 1:
		motion = 2;

		rtn=GT_PrflT();          
		rtn=GT_ClrSts();
	   	rtn=GT_SetVel(2);
		rtn=GT_SetAcc(0.2);
		rtn=GT_SetPos(1000);
		rtn=GT_Update();
		return -1;

	case 2:
		rtn=GT_GetSts(&sts);
		if ((sts & 1 ) == 1)
		{
			motion = 3;
			rtn=GT_ClrSts();
			rtn=GT_SetPos(0);
			rtn=GT_Update();
		}
		return -1;

	case 3:
		rtn=GT_GetSts(&sts);
		if ((sts & 1 ) == 1)
		{
			rtn=GT_ClrSts();
			rtn=GT_SetVel(0);
			rtn=GT_SetAcc(0);
			rtn=GT_Update();
			rtn=GT_PrflV();
			rtn=GT_Update();

			start = 4;
			motion = 0;
		}
		return -1;
	}
	return 0;
}

void init_interrupt()   /*定时器中断初始化函数*/
{
	disable(); 		/* 关闭中断（在初始化中断过程中严禁产生中断） */
	/* 设置中断时间 */
	outportb(0x43,0x36);    /*写中断控制字：0X43为中断控制寄存器的内存映射地址
	                          写入0X36表示下面的字节用于设置2号实时时钟周期*/
	outportb(0x40,0xf6);   /* 写数据寄存器：0X40为数据寄存器内存映射地址，写入的数据为
	                         16位时钟细化比率，首先写入低字节0Xf6，随后写入高字节0X1b*/
	outportb(0x40,0x1b);    
	oldhandler = getvect(INTR);/*保存原有的中断服务程序句柄*/
	setvect(INTR, handler);  /*设置新的中断服务程序句柄*/
	enable();     /*打开中断*/
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*上述中断初始化代码创建一个6ms的定时器中断，即当打开中断后，将每隔6毫秒对摆系统进行一次实时控制。6毫秒的时间
   是通过向数据寄存器写入0X1bf6=7158获得的，因为计算机系统的2号实时时钟频率为1.193MHZ，1193000*0.006=7158*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////

void restore_interrupt()  /*中断恢复函数*/
{
	disable();  //关闭中断
	setvect(INTR,oldhandler);  //恢复旧的中断服务程序
	enable();  //打开中断
}

void handle_sign() /*扰动信号函数*/
{
	int i;
	float sign_fre = param_info[MAX_MENU].param_value[1];
	float sign_amp = param_info[MAX_MENU].param_value[2];

	switch ((int)param_info[MAX_MENU].param_value[0]){

	case 0:		// none sign
		sign_v = 0;
		break;

	case 1:		// impulse
		if (pulse_sign < 10){
			sign_v = sign_amp;
			pulse_sign ++;
		}
		else{
			param_info[MAX_MENU].param_value[0] = 0;
			sign_v = 0;
			pulse_sign = 0;
		}

		break;

	case 2:		// step
		sign_v = sign_amp;
		break;

	case 3:         //square
		if (2 * times_sign * sign_fre * INTPERIOD < 1){
			sign_v = sign_amp;
		}
		else
			sign_v = -sign_amp;

		break;

	case 4:		// sine
		sign_v = sign_amp * sin(times_sign * sign_fre * INTPERIOD * 2 * M_PI);
		break;

	case 5:		//sawtooth
		sign_v = sign_amp * (2 * times_sign * sign_fre * INTPERIOD - 1);
		break;
	}
	
		times_sign ++;
		if (times_sign * sign_fre * INTPERIOD > 1)
			times_sign = 0;
	if (respond_start == 1){
		respond[respond_index].point_the = (angle * 180 / M_PI  - 180);
		respond[respond_index].point_pos = pos;
		respond_max_pos = max(fabs(respond[respond_index].point_pos), respond_max_pos);
		respond_max_the = max(fabs(respond[respond_index].point_the), respond_max_the);

		respond_index ++;
		if (respond_index >= CURVE_POINT){
			respond_index = 0;
			respond_start = 0;
		}
	}
	for (i=0; i<4; i++){
		pid_ang[i] = pid_ang[i+1];
		}
	pid_ang[4] = ang_2pi - M_PI;
}