kunshanhuatian/third/MotionCtrlCard/CMotorDriver.cpp

#define CARD_DLL_API _declspec(dllexport)

#include "CMotorDriver.h"
#ifndef _WINDOWS_H
#include <windows.h>
#define _WINDOWS_H
#endif
#include "gts.h"
#include "gtgl500.h"
#include <thread>

CMotorDriver::CMotorDriver()
{
	m_motor = new std::vector<MotorAttr>();
}


CMotorDriver::~CMotorDriver()
{
}

int CMotorDriver::Relmove(int motorId, double pos, bool waitFinished)
{
	double prfPos = 0.0;

	auto motorObj = std::find_if((*m_motor).begin(), (*m_motor).end(), [&](const MotorAttr &obj)
	{
		return obj.AxisId() == motorId;
	});
	if (motorObj == (*m_motor).end())
	{
		return 1;
	}
	pos *= motorObj->PulseTo1mm();

	short sRtn = GTN_GetPrfPos(m_gugaoCardCore, motorId, &prfPos); // 读取AXIS轴的规划位置
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_GetPrfPos is error");
		return 1;
	}

	prfPos += pos;
	prfPos = prfPos / motorObj->PulseTo1mm();

	AbsMove(motorId, motorObj->PulseTo1mm(), prfPos, motorObj->Acc(), motorObj->Dec(), motorObj->Vel(),true);
	return 0;
}

int CMotorDriver::AbsmoveMulti(std::vector<int> motorIdVec, std::vector<double> posVec, bool waitFinished)
{
	if (motorIdVec.size() != posVec.size()) {
		return 1;
	}

	if (!m_open)
		return 1;

	short sRtn;
	long mask = 0;
	for (unsigned int i = 0; i < motorIdVec.size(); i++)
	{
		unsigned short uirvMode;
		short  sRet = GTN_GetEcatAxisMode(m_gugaoCardCore, motorIdVec[i], &uirvMode);
		if (!sRet && uirvMode != 8)
		{
			sRet = GTN_SetEcatAxisMode(m_gugaoCardCore, motorIdVec[i], 8);
		}

		long lAxisStatus;
		sRet = GTN_GetSts(m_gugaoCardCore, motorIdVec[i], &lAxisStatus);
		if (sRet)
		{
			Logger::GetInstance()->WriteLog(0, "AbsMotion中的GTN_GetSts失败");
			return false;
		}
		if (!(lAxisStatus & (1 << 9)))//第9位为1时，电机使能
		{
			Logger::GetInstance()->WriteLog(0, "轴未使能");
			return false;
		}
		if (lAxisStatus & (1 << 10))//检查轴是否正在运动
		{
			Logger::GetInstance()->WriteLog(0, "轴正在运动");
			return false;
		}

		sRet = GTN_PrfTrap(m_gugaoCardCore, motorIdVec[i]); //切换为点位运动
		if (sRet)
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");

		// 读取点位运动参数
		TTrapPrm trap;
		sRet = GTN_GetTrapPrm(m_gugaoCardCore, motorIdVec[i], &trap);
		if (sRet)
		{
			Logger::GetInstance()->WriteLog(0, "SetPointMotionParam中的GTN_GetTrapPrm失败");
			return false;
		}

		auto motorObj = QueryMotor(motorIdVec[i]);
		if (motorObj.AxisId() > -1) {

			double resAcc = motorObj.Acc();
			double resDec = motorObj.Dec();
			double velStart = motorObj.Vel();

			TTrapPrm trap;
			trap.acc = motorObj.Acc();
			trap.dec = motorObj.Dec();
			trap.velStart = motorObj.Vel();
			trap.smoothTime = 50;

			sRet = GTN_SetTrapPrm(m_gugaoCardCore, motorIdVec[i], &trap); // 设置点位运动参数
			if (sRet)
			{
				Logger::GetInstance()->WriteLog(0, "GTN_SetTrapPrm is error");
				return 1;
			}

			sRet = GTN_SetVel(m_gugaoCardCore, motorIdVec[i], motorObj.Vel());
			if (sRet)
			{
				Logger::GetInstance()->WriteLog(0, "GTN_SetVel is error");
				return 1;
			}
		}

		sRtn = GTN_SetPos(m_gugaoCardCore, motorIdVec[i], (long)posVec[i]); // 设置AXIS轴的目标位置
		Sleep(100);
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: AbsmoveMulti -> GTN_SetPos is error");
			return 1;
		}

		mask |= 1 << (motorIdVec[i] - 1);
	}

	sRtn = GTN_Update(m_gugaoCardCore, mask);
	Sleep(100);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: AbsmoveMulti -> GTN_Update is error");
		return 1;
	}

	if (waitFinished)
	{
		for (unsigned int i = 0; i < motorIdVec.size(); i++)
		{
			long lAxisSts;
			do
			{
				// 读取AXIS轴的状态
				sRtn = GTN_GetSts(m_gugaoCardCore, motorIdVec[i], &lAxisSts);
				if (sRtn)
				{
					Logger::GetInstance()->WriteLog(0, "function: AbsmoveMulti -> GTN_GetSts is error");
					return 1;
				}
			} while (lAxisSts & (1 << 10));	// 等待AXIS轴规划停止
		}
	}
	return 0;
}

int CMotorDriver::RelmoveMulti(std::vector<int> motorIdVec, std::vector<double> posVec, bool waitFinished)
{
	return 0;
}

int CMotorDriver::Home(int motorId, bool waitFinished)
{
	/*if (!m_open)
		return 1;*/

	// 必须处于伺服使能状态， 切换到回零模式
	short sRtn = GTN_SetHomingMode(m_gugaoCardCore, motorId, 6);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
		sRtn = GTN_SetHomingMode(m_gugaoCardCore, motorId, 8);// 切换到位置控制模式
		if (sRtn)
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
		return 1;
	}

	double searchSpeed = 0.0;
	double indexSpeed = 0.0;
	double searchAcc = 0.0;
	short module = 1;
	auto motorObj = QueryMotor(motorId);
	if (motorObj.AxisId() > -1) {

		searchSpeed = motorObj.ReturnHomeStartVel();
		indexSpeed = motorObj.ReturnHomeEndVel();
		searchAcc = motorObj.ReturnAcc();

		for (auto & iter : (*m_motor)) {
			if (iter.AxisId() == motorId)
			{
				module = iter.ReturnHomeMode();
				break;
			}
		}
	}
	
	//设置回零参数
	sRtn = GTN_SetEcatHomingPrm(m_gugaoCardCore, motorId,
		module,//回零模式
		searchSpeed, //搜索开关速度，单位： 驱动器设置的用户速度单位
		indexSpeed, //搜索 index 标识速度，单位： 驱动器设置的用户速度单位
		searchAcc, //搜索加速度
		0, //原点偏移量
		0 //探针功能
	);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
		return 1;
	}

	// 启动回零
	sRtn = GTN_StartEcatHoming(m_gugaoCardCore, motorId);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
		return 1;
	}

	unsigned short sHomeSts;	// 回零状态
	while (true)
	{
		
		sRtn = GTN_GetEcatHomingStatus(m_gugaoCardCore, motorId, &sHomeSts); //读回零状态
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
			return 1;
		}

		if (sHomeSts == 3)
		{
			
			//@TODO下面这一行最开始有临界区保护
			auto& motorObj = QueryMotor(motorId);
			if (motorObj.AxisId() >= 0) {
				motorObj.IsHomed(true);
			}

			double encPos;
			if (GTN_GetEncPos(m_gugaoCardCore, motorId, &encPos) == 0) {
				//回零后编码器位置
				std::string str = "Encoder position after returning to zero: " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}
			
			double prfPos;
			if (GTN_GetPrfPos(m_gugaoCardCore, motorId, &prfPos) == 0) {
				//回零后规划位置
				std::string str = "Plan location after returning to zero: " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			sRtn = GTN_ClrSts(m_gugaoCardCore, motorId);
			if (sRtn)
				Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ClrSts is error");

			sRtn = GTN_ZeroPos(m_gugaoCardCore, motorId);//清零规划位置和实际位置，并进行零漂补偿。
			if (sRtn)
				Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ZeroPos is error");

			
			if (GTN_GetEncPos(m_gugaoCardCore, motorId, &encPos) == 0) {
				//回零后编码器位置
				std::string str = "Encoder position after clear(GTN_ZeroPos): " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			if (GTN_GetPrfPos(m_gugaoCardCore, motorId, &prfPos) == 0) {
				//回零后规划位置
				std::string str = "Plan location after clear(GTN_ZeroPos): " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			if (GTN_SetHomingMode(m_gugaoCardCore, motorId, 8))// 切换到位置控制模式
				Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");

			return 0;
		}
	}

	return 0;
}

int CMotorDriver::allAxisHome(std::vector<int> allAxisId, bool waitFinished)
{
	if (!m_open)
		return 0;
	{
		std::vector<std::thread> vThread;
		for (int& id : allAxisId)
		{
			std::thread t = std::thread([this, id]()
			{
				Home(id, true);
			});
			vThread.push_back(std::move(t));
		}
		for (std::thread& t : vThread)
		{
			if (t.joinable())
				t.join();
		}

		return 1;
	}

	short sRtn = 0;
	//short motorId = 1;
	//清除所有告警
	for (auto motorId : allAxisId)
	{

		short sRtn = GTN_ClrSts(m_gugaoCardCore, motorId);
		if (sRtn)
			Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ClrSts is error");

	}
	// 必须处于伺服使能状态， 切换到回零模式
	for (auto motorId : allAxisId)
	{
		short sRtn = GTN_SetHomingMode(m_gugaoCardCore, motorId, 6);
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
			sRtn = GTN_SetHomingMode(m_gugaoCardCore, motorId, 8);// 切换到位置控制模式
			if (sRtn)
				Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
			return 1;
		}
	}

	//设置回零参数
	for (auto motorId : allAxisId)
	{
		double searchSpeed = 0.0;
		double indexSpeed = 0.0;
		double searchAcc = 0.0;
		short module = 1;
		auto motorObj = QueryMotor(motorId);
		if (motorObj.AxisId() > 0) {

			searchSpeed = motorObj.ReturnHomeStartVel();
			indexSpeed = motorObj.ReturnHomeEndVel();
			searchAcc = motorObj.ReturnAcc();
			module = motorObj.ReturnHomeMode();
		}

		//设置回零参数
		sRtn = GTN_SetEcatHomingPrm(m_gugaoCardCore, motorObj.AxisId(),
			module,//回零模式
			searchSpeed, //搜索开关速度，单位： 驱动器设置的用户速度单位
			indexSpeed, //搜索 index 标识速度，单位： 驱动器设置的用户速度单位
			searchAcc, //搜索加速度
			0, //原点偏移量
			0 //探针功能
		);
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
			return 1;
		}
	}

	//下回零指令
	// 启动回零
	Sleep(20);
	for (auto motorId : allAxisId)
	{
		sRtn = GTN_StartEcatHoming(m_gugaoCardCore, motorId);

		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
			return 1;
		}

	}
	//判断等待回原
	int judgeFlag = 0;
	int n = 0;
	unsigned short* sHomeSts = new unsigned short[allAxisId.size()];	// 回零状态
	while (waitFinished)
	{
		if (judgeFlag == 0)
		{
			for (auto motorId : allAxisId)
			{
				if (sHomeSts[motorId - 1] == 0) {
					short sRtn = GTN_GetEcatHomingStatus(m_gugaoCardCore, motorId, &sHomeSts[motorId - 1]); //读回零状态
					if (sRtn)
					{
						Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
						return 1;
					}
					if (sHomeSts[motorId - 1] == 3)
						++n;
				}
			}
			if (n != allAxisId.size())
				continue;

			if (n == allAxisId.size())
			{
				for (auto motorId : allAxisId)
				{
					sRtn = GTN_SetHomingMode(m_gugaoCardCore, motorId, 8);
					if (GTN_SetHomingMode(m_gugaoCardCore, motorId, 8))// 切换到位置控制模式
						Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
					Sleep(10);
					sRtn = GTN_ClrSts(m_gugaoCardCore, motorId);
					judgeFlag = 1;

				}
			}
		}

		//判断所有轴是否回原
		if (judgeFlag == 1)
		{
			//清除所有告警
			for (auto motorId : allAxisId)
			{
				short sRtn = GTN_ClrSts(m_gugaoCardCore, motorId);
				if (sRtn)
					Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ClrSts is error");

				// 读取AXIS轴的状态
				long diPos = 0;
				double lrPrfPos = 0;
				sRtn = GTN_GetPos(m_gugaoCardCore, motorId, &diPos);
				sRtn = GTN_GetPrfPos(m_gugaoCardCore, motorId, &lrPrfPos, 1);
				Sleep(50);
				long lAxisSts = 0;
				sRtn = GTN_GetSts(m_gugaoCardCore, motorId, &lAxisSts);
				if (sRtn)
				{
					Logger::GetInstance()->WriteLog(0, "GTN_GetSts is error");
					return 1;
				}

				if (lAxisSts & (1 << 1))
				{
					Logger::GetInstance()->WriteLog(0, "Driver alarm flag");
				}
				else if (lAxisSts & (1 << 4))
				{
					Logger::GetInstance()->WriteLog(0, "跟随误差越限标志");
				}
				else if (lAxisSts & (1 << 5))
				{
					Logger::GetInstance()->WriteLog(0, "正限位触发标志");
				}
				else if (lAxisSts & (1 << 6))
				{
					Logger::GetInstance()->WriteLog(0, "负限位触发标志");
				}
				else if (lAxisSts & (1 << 7))
				{
					Logger::GetInstance()->WriteLog(0, "IO平滑停止触发标志");
				}
				else if (lAxisSts & (1 << 8))
				{
					Logger::GetInstance()->WriteLog(0, "IO急停触发标志");
				}
				else if (lAxisSts & (1 << 9))
				{
					Logger::GetInstance()->WriteLog(0, "电机使能标志");
				}
			}

			return 0;
		}
		Sleep(2);
	}
	return 1;

}

void CMotorDriver::ResetAxisErr(std::vector<int> axisId)
{
	for (auto& iter : axisId) {
		short sRtn = GTN_ClrSts(m_gugaoCardCore, iter); //清除驱动器报警标志、跟随误差越限标志、限位触发标志。
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "GTN_ClrSts is error");
			return;
		}
	}
	
}

int CMotorDriver::AddMotor(const MotorAttr & motor)
{
	m_motor->push_back(motor);
	return 0;
}

int CMotorDriver::DeleteMotor(int motorId)
{
	return 0;
}

int CMotorDriver::ModifyMotor(const MotorAttr & motor)
{
	return 0;
}

MotorAttr CMotorDriver::QueryMotor(int motorId)
{
	for (auto& iter : (*m_motor))
	{
		if (iter.AxisId() == motorId) {
			return iter;
		}
	}
	MotorAttr obj;
	obj.AxisId(-1);
	return obj;
}

void CMotorDriver::ClearAllAxis()
{
	m_motor->clear();
}

double CMotorDriver::GetCurPos(int motorId)
{
	double pos;
	{
		std::lock_guard<std::mutex> guard(m_getPosMutex);
		if (!m_open)
		{
			Logger::GetInstance()->WriteLog(0, "function:GetCurPos, device is not open");
			return 0;
		}
		// 读取AXIS轴的规划位置
		short sRtn = GTN_GetPrfPos(m_gugaoCardCore, motorId, &pos);
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function:GetCurPos, GTN_GetPrfPos is error");
			return 0;
		}

		for (auto & iter : (*m_motor)) {
			if (iter.AxisId() == motorId && iter.PulseTo1mm() > 0)
			{
				pos /= iter.PulseTo1mm();
				break;
			}
		}
	}
	return pos;
}

double CMotorDriver::GetCurEncoderPos(int iAxisId, int pulseTo1mm)
{
	if (!m_open)
	{
		Logger::GetInstance()->WriteLog(0, "function:GetCurPos, device is not open");
		return 0;
	}

	double pos;

	// 读取AXIS轴的规划位置
	short sRtn = GTN_GetAxisEncPos(m_gugaoCardCore, iAxisId, &pos);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function:GetCurPos, GTN_GetPrfPos is error");
		return 0;
	}

	for (auto & iter : (*m_motor)) {
		if (iter.AxisId() == iAxisId && iter.PulseTo1mm() > 0)
		{
			pos /= iter.PulseTo1mm();
			break;
		}
	}

	return pos;
}