【运动控制】运动控制类实现

运动控制类实现：

#pragma once


#include <afx.h>

#include "Tool.h"
#include "Planning.h"


#define AXIS_SHEAR_ANGLE   360

class ServoControl
{
public:
	ServoControl();
	~ServoControl();

	void Set_SimulationMode(bool bSim);  // 设置仿真模式
	void Set_MotionPara();  // 设置运动参数

	int Servo_Init();   // 初始化
	int Servo_Close(); // 关闭
	int Servo_Enable(bool enable); // 使能
	int Servo_Halt();   // 暂停
	int Servo_SmoothStop(); // 平滑停止
	int Servo_EStop();  // 急停

	bool IfAllAxisStop();

	int Servo_AxisMove(Axis dMove);
	int Servo_AxisShear(Step dShear);

	int Get_JointPosition(OUT double gdPos[]); // 获取关节位置
	int Get_JointVel(OUT double gdVel[]);      // 获取关节速度
	CString Get_ServoErr(int err);     // 获取伺服错误信息
	int Clear_ServoErr();                      // 清除错误


private:
	unsigned short m_nCardNum; 
	bool m_bInitFlag;

	bool m_bFlag_Simulation;  // 仿真测试标志

	short m_nDir[5];

	// 缓冲区运动参数
	double m_dMaxV;
	ULONG m_lMaxAccTime;
	double m_dV_ini;
	double m_dV_end;
	double m_dPercent;
	double m_dInflection;
	USHORT m_iVelocity_Ratio_Enable;
};

#include "ServoControl.h"


#pragma comment(lib,"PMC.lib")



ServoControl::ServoControl()
{
	int i;
	for (i=0; i<5; i++)
	{
		m_nDir[i] = 1;
	}

	m_bFlag_Simulation = false;
	m_bInitFlag = false;
	m_nCardNum = 1;

	m_dMaxV = 50;
	m_lMaxAccTime = 500;
	m_dV_ini = 0;
	m_dV_end = 0;
	m_dPercent = 1;
	m_dInflection = 1;
	m_iVelocity_Ratio_Enable = 1;
}

ServoControl::~ServoControl()
{

}



int ServoControl::Servo_EStop()
{
	if (m_bFlag_Simulation)
	{
		return SYSTEM_OK;
	}

	int i;

	for (i=0; i<m_nCardNum; i++)
	{
		RETURN( PMC_Emergent_Stop(AXIS_8ALL, i) );   //紧急停止
	}

	return SYSTEM_OK;
}




bool ServoControl::IfAllAxisStop()
{
	if (m_bFlag_Simulation)
	{
		return true;
	}

	unsigned long uAxis;
	int i;

	for (i=0; i<m_nCardNum; i++)
	{
		PMC_Get_Execute_Axis(&uAxis, i) ;   //控制卡i上的轴是否停止
		if ((uAxis & AXIS_8ALL) != 0)
		{
			return false;
		}
	}

	return true;
}


int ServoControl::Servo_AxisMove(Axis dMove)
{
	if (m_bFlag_Simulation)
	{
		return SYSTEM_OK;
	}

	short rtn = 0;
	unsigned short uAxis = AXIS_1_S1 + AXIS_1_S2 + AXIS_1_O1 + AXIS_1_V + AXIS_1_O2 + AXIS_1_C1;

	// 本地卡0

	//PMC_AxisOn(AXIS_8ALL, 0);

	// 开辟缓冲区BANK0
	// 启动缓冲区
	RETURN (PMC_Start_Buffer(0, 1));			// 开辟 BANK为0，空间为 1*1024的缓冲区 ,卡号默认为 0

	rtn = PMC_Set_BufferProfile(m_dMaxV, m_lMaxAccTime, 
		m_dV_ini, m_dV_end, m_dPercent, m_dInflection, m_iVelocity_Ratio_Enable);

	rtn = PMC_Line6(uAxis, 
		dMove.S1, dMove.S2, dMove.O1, dMove.V, dMove.O2, dMove.C1,
		POSITION_OPPOSITE);

	ULONG number;
	rtn = PMC_End_Buffer(&number);
	rtn = PMC_Execute_Buffer(0);			// 启动BANK0 连续轨迹运动
	RETURN(rtn);


	return SYSTEM_OK;
}

int ServoControl::Servo_AxisShear(Step dShear)
{
	if (m_bFlag_Simulation)
	{
		return SYSTEM_OK;
	}

	short rtn = 0;
	unsigned short uAxis = 0;
	double ld_pos[5];
	int i;

	for (i=0; i<5; i++)
	{
		ld_pos[i] = 0;
	}

	if (dShear.Action_O1)
	{
		uAxis += AXIS_2_O1;
		m_nDir[0] = (m_nDir[0]) ? -1 : 1;
		ld_pos[0] = AXIS_SHEAR_ANGLE * m_nDir[0];
	}
	if (dShear.Action_V)
	{
		m_nDir[1] = (m_nDir[1]) ? -1 : 1;
		ld_pos[1] = AXIS_SHEAR_ANGLE * m_nDir[1];
	}
	if (dShear.Action_O2)
	{
		m_nDir[2] = (m_nDir[2]) ? -1 : 1;
		ld_pos[2] = AXIS_SHEAR_ANGLE * m_nDir[2];
	}
	if (dShear.Action_C1)
	{
		m_nDir[3] = (m_nDir[3]) ? -1 : 1;
		ld_pos[3] = AXIS_SHEAR_ANGLE * m_nDir[3];
	}
	if (dShear.Action_C2)
	{
		m_nDir[4] = (m_nDir[4]) ? -1 : 1;
		ld_pos[4] = AXIS_SHEAR_ANGLE * m_nDir[4];
	}

	// PMC_AxisOn(uAxis, 1);

	// 开辟缓冲区BANK0
	// 启动缓冲区
	rtn = PMC_Start_Buffer(0, 1);			// 开辟 BANK为0，空间为 1*1024的缓冲区 ,卡号默认为 0

	rtn = PMC_Set_BufferProfile(m_dMaxV, m_lMaxAccTime, 
		m_dV_ini, m_dV_end, m_dPercent, m_dInflection, m_iVelocity_Ratio_Enable);

	// 网络扩展卡0
	// 1号冲孔、V剪、2号冲孔、剪刀1
	uAxis = AXIS_2_O1 + AXIS_2_V + AXIS_2_O2 + AXIS_2_C1;
	rtn = PMC_RM_Line4(uAxis,
		ld_pos[0], ld_pos[1], ld_pos[2], ld_pos[3], 
		POSITION_OPPOSITE, 0, 0);

	// 网络扩展卡1
	// 剪刀2
	uAxis = 0x0001;
	rtn = PMC_RM_Update(uAxis, ld_pos[4], POSITION_OPPOSITE, 1, 0);


	ULONG number;
	rtn = PMC_End_Buffer(&number, 1);
	rtn = PMC_Execute_Buffer(0, 0, 1);			// 启动BANK0 连续轨迹运动
	RETURN(rtn);

	return SYSTEM_OK;
}



CString ServoControl::Get_ServoErr(int err)
{
	CString ls_str;

	switch(err)
	{
	case ERR_LIB_INIT_ERROR:
		ls_str = "初始化库失败，检查是否安装主动程序";
		break;
	case ERR_COMMUNICATE_ERROR:
		ls_str = "通讯出错";
		break;
	case ERR_CARD_NOT_EXIST:
		ls_str = "当前卡不存在，检查卡号";
		break;
	case ERR_CLOSE_CARD_FAILE:
		ls_str = "关闭卡失败";
		break;
	case ERR_SEND_COMMAND_ERROR:
		ls_str = "运动指令传输错误";
		break;
	case ERR_COMMAND_EXECUTING:
		ls_str = "指令正在执行";
		break;
	case ERR_VEL_PROFILE_PARA_ERROR:
		ls_str = "速度曲线参数设置错误";
		break;
	case ERR_AXIS_SET_ERROR:
		ls_str = "轴号设置错误";
		break;
	case ERR_ARC_INTPL_DIR_ERROR:
		ls_str = " 圆弧插补方向设置错误";
		break;
	case ERR_JOINT_MOVE_ERROR:
		ls_str = "多轴联动错误";
		break;
	case ERR_STOP_PROFILE_PARA_ERROR	:
		ls_str = "停止运动速度曲线参数错误";
		break;
	case ERR_VEL_MODE_PARA_ERROR:
		ls_str = "速度模式参数设置错误";
		break;
	case ERR_BUF_CREATE_PARA_ERROR:
		ls_str = "开辟缓冲区参数错误";
		break;
	case ERR_BUF_TERMINATE_PARA_ERROR:
		ls_str = "结束缓冲区错误";
		break;
	case ERR_BUF_NO_COMMAND:
		ls_str = "执行的缓冲区没有指令";
		break;
	case ERR_HOME_FAIL:
		ls_str = "零位寻找失败";
		break;
	case ERR_SET_PROSPECT_PROFILE_ERROR:
		ls_str = "设置前瞻参数错误";
		break;
	case ERR_SET_PROSPECT_MOVE_ERROR:
		ls_str = "设置前瞻运动错误";
		break;
	case ERR_COMMAND_WAIT_TIMEOUT:
		ls_str = "等待命令完成超时";
		break;
	case ERR_WAIT_ORDER_EVENT_TRIGGER_ERROR:
		ls_str = "表示等待命令出现事件触发";
		break;
	case ERR_DATA_CAPTURE_BUF_ERROR:
		ls_str = "数据捕捉缓失败存设置错误";
		break;
	case ERR_MEMORY_ALLOCATE_FAILE:
		ls_str = "内存开辟 ";
		break;
	case ERR_COMMAND_LENGTH_EXCEED:
		ls_str = "指令长度超出所开辟的内存";
		break;
	case ERR_GEAR_PARA_ERROR	:
		ls_str = "电子齿轮参数设置错误";
		break;
	case ERR_SERVO_ALARM:
		ls_str = "伺服报警而不能设置使能，需处理好伺服报警并清除状态位";
		break;
	case ERR_PARA_OVERFLOW:
		ls_str = "指令参数溢出 ";
		break;
	case ERR_EVENT_TRIGGER_ERR:
		ls_str = "表示指令运行的轴有事件触发被禁止运行";
		break;
	case ERR_WAIT_IO_INPUT_DELAY_ERROR:
		ls_str = "表示缓冲区等待输入IO超时";
		break;
	case ERR_PROCESS_EXCEED_ERROR:
		ls_str = "表示进程数超出范围";
		break;
	case ERR_CARD_TIMER_ERROR:
		ls_str = "表示控制卡定时器出错";
		break;
	case ERR_NETWORK_COMMUNICATE_ERROR:
		ls_str = "表示网络通讯错误";
		break;
	case ERR_ILLEGAL_INSTRUCTION_ERROR:
		ls_str = "表示非法指令运行";
		break;
	case ERR_HOME_THREAD_CREATE_FAILE:
		ls_str = "回零线程创建失败";
		break;
	case ERR_HOME_IN_PROCESS:
		ls_str = "正在回零中";
		break;
	case ERR_HOME_PROFILE_PARA_ERROR:
		ls_str = "回零参数设置错误";
		break;
	case ERR_IO_MODE_SET_ERROR:
		ls_str = "IO模式设置错误";
		break;
	default:
		break;
	}

	return ls_str;
}