PhysicalDevice/Machine/MachineryECOM/Trunk/DIOS.Dev.Machinery.Common/DIOS.Dev.Machinery.BaseModel/CBCTMotionModel.cpp

#include "stdafx.h"
#include "CBCTMotionModel.h"
#include "CBCTMotionStageArgs.h"
#include "ICircularController.h"
#include "ISwingController.h"
#include "ISensorEncoderController.h"
#include "IMachineryManager.h"
#include "CArmComponentNameDef.h"
#include "MotionStages.h"
#include "IPositionManager.h"
#include "IExposureController.h"
#include "FeedbackDefine.h"
#include "ConfigurerMotion.h"
#include "FluoroSwitchController.h"

using namespace DIOS::Dev::Detail::MachineryECOM;

CBCTMotionModel::CBCTMotionModel() 
:m_stageArgs(new CBCTMotionStageArgs()),
m_coordinates(nullptr),
m_machineryManager(nullptr),
m_circular(nullptr),
m_swing(nullptr),
m_circularencoder(nullptr),
m_swingencoder(nullptr),
m_exposure(nullptr),
m_fluoroSwitch(nullptr),
m_bUseCARMRotationScanning(false)
{
}


CBCTMotionModel::~CBCTMotionModel()
{
}

void CBCTMotionModel::ChangeStage(const std::string &stageName)
{
	m_StageName = stageName;
	OnMotionStage(stageName);
}

std::string CBCTMotionModel::GetStageName()
{
	return m_StageName;
}

IMotionStageArgs *CBCTMotionModel::GetStageArgs()
{
	return m_stageArgs;
}

void CBCTMotionModel::Initialize(IMachineryManager *machineryManager, IPositionManager *coordinates)
{
	m_coordinates = coordinates;
	m_machineryManager = machineryManager;

	m_circular = (ICircularController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR);
	m_swing = (ISwingController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING);
	m_circularencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER);
	m_swingencoder = (ISensorEncoderController*)machineryManager->Resove(CONTROLLER_CARM_TUBE_SWING_ENCODER);
	m_exposure = (IExposureController *)(machineryManager->Resove(CONTROLLER_EXPOSURE));
	m_fluoroSwitch = (IOutputController *)(machineryManager->Resove(CONTROLLER_FLUOROSWITCH));
	m_bUseCARMRotationScanning = ConfigurerMotion::GetUseSwingScanning();
}

void CBCTMotionModel::LoadMachineryParams(ResDataObject &params)
{
	m_MachineryParams = params;
}

void CBCTMotionModel::LoadModelParams(ResDataObject &params)
{
	m_ModelParams = params;
}

void CBCTMotionModel::SetTechnicalParams(ResDataObject &params)
{
	m_TechnicalParams = params;
}

void CBCTMotionModel::OnFeedbackMotionParams(ResDataObject &params)
{	
	std::string key((const char *)params["key"]);
	if (key == CARM_MODLE_FEEDBACK_KEY_CIRCULARENCODER)
	{
		m_FeedbackTubeCircularEncoders.push_back((int)params[key.c_str()]);
	}
}

BOOL CBCTMotionModel::GetMotionParams(ResDataObject &params)
{
	std::string key = (const char *)params["key"];
	if (key == "CircularResult")
	{
		params.clear();
		int count = 0;
		for (auto item : m_FeedbackTubeCircularEncoders)
		{
			float height = m_coordinates->ConvertSensorValue(m_circularencoder->Name(), item);
			char buffer[10];
			memset(buffer, 0, 10);
			sprintf_s(buffer, "%d", count);
			params.add(buffer, height * 100);
			++count;
		}
	}

	return TRUE;
}

void CBCTMotionModel::OnMotionStage(const std::string &stageName)
{
	if (stageName == CBCT_STAGE_CALCULATE_PARAMS)
	{
		MotionStageCalculateParams();
	}
	else if (stageName == CBCT_STAGE_SET_CBCT_SLICE)
	{
		MotionStageSetCBCTMotionSlice();
	}
	else if (stageName == CBCT_STAGE_MOVETO_END_POS)
	{
		MotionStageMovetoEndPos();
	}
	else if (stageName == CBCT_STAGE_CLEAR_PARAMS)
	{
		MotionStageClearParams();
	}
	else if (stageName == CBCT_STAGE_MOVETO_START_POS)
	{
		MotionStageMovetoStartPos();
	}
	else if (stageName == CBCT_STAGE_MOTION_ERROR_STOP)
	{
		MotionStageErrorStop();
	}
	else if (stageName == CBCT_STAGE_MOTION_ERROR_RECOVER)
	{
		MotionStageErrorRecover();
	}
	else if (stageName == CBCT_STAGE_MOTION_CHANGE_PFS)
	{

	}
	else if (stageName == CBCT_STAGE_START_EXPOSURE)
	{
		OnStartExposure();
	}
	else if (stageName == CBCT_STAGE_STOP_EXPOSURE)
	{
		OnStopExposure();
	}
}

void CBCTMotionModel::MotionStageClearParams()
{
	m_circularencoder->ClearTrigger();
	m_swingencoder->ClearTrigger();
}

void CBCTMotionModel::MotionStageCalculateParams()
{
	float acc = strtof((const char*)m_ModelParams["CBCTAcceleratingDistance"], nullptr);
	float brk = strtof((const char*)m_ModelParams["CBCTDeceleratingDistance"], nullptr);

	float startpos = strtof((const char *)m_TechnicalParams["CBCTStartPos"], nullptr);
	float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);
	int nproject = (int)m_TechnicalParams["CBCTProjectionNumber"];
	float scnAngle = strtof((const char *)m_TechnicalParams["CBCTScanAngle"], nullptr);
	int direction = (int)m_TechnicalParams["CBCTProjectionDirection"];

	m_startPos = startpos - (direction) * acc;

	m_endPos = m_startPos + (direction)*(scnAngle + brk);

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageCalculateParams]->[S:{$:f3} E:{$:f3}]", m_startPos, m_endPos);

	m_exposureTriggerSlice.clear();
	for (int i = 0; i < nproject; ++i)
	{
		float anglePerProject = scnAngle / (nproject * 1.0f);
		float triggerAngle = startpos - i * anglePerProject;
		m_exposureTriggerSlice.push_back(triggerAngle);
	}
}

void CBCTMotionModel::MotionStageSetCBCTMotionSlice()
{
	if (!m_bUseCARMRotationScanning)
	{
		//设置编码器触发同步使能
		auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		m_circularencoder->ActiveExposureTrigger(exposureTriggerID);

		//依次设置曝光点位关联同步信号
		int nSize = m_exposureTriggerSlice.size();
		for (int i = 0; i < nSize; ++i)
		{
			if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][SetExposureTrigger]->[Circular][Total:{$:d}, Cur:{$:d}][{$:f3}]", nSize, i, m_exposureTriggerSlice[i]);

			DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_CARM_TUBE_CIRCULAR_ENCODER, m_exposureTriggerSlice[i]);
			m_circularencoder->SetExposureTrigger(encoder);
		}

		//设置曝光信号触发编码器值回传
		auto sensortubeHeightID = m_circularencoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		m_exposure->ActiveExposureTubeHeightPositionAutoNotify(sensortubeHeightID);
	}
	else
	{
		auto exposureTriggerID = m_exposure->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		m_swingencoder->ActiveExposureTrigger(exposureTriggerID);

		int nSize = m_exposureTriggerSlice.size();
		for (int i = 0; i < nSize; ++i)
		{
			if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][SetExposureTrigger]->[Swing][Total:{$:d}, Cur:{$:d}][{$:f3}]", nSize, i, m_exposureTriggerSlice[i]);

			DWORD encoder = m_coordinates->ConvertPhysicsValue(CONTROLLER_CARM_TUBE_SWING_ENCODER, m_exposureTriggerSlice[i]);
			m_swingencoder->SetExposureTrigger(encoder);
		}

		auto sensortubeHeightID = m_swingencoder->GetInterfaceID(ID_COMMUNICATE_INTERFACE);
		m_exposure->ActiveExposureTubeAnglePositionAutoNotify(sensortubeHeightID);
	}
}

void CBCTMotionModel::MotionStageMovetoEndPos()
{
	if (!m_bUseCARMRotationScanning)
	{
		if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_CIRCULAR))
		{
			if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][MotionStageMovetoEndPos]->[CONTROLLER_CARM_TUBE_CIRCULAR is not ready status, need to reset first]");
			return;
		}

		float currentPosition = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);

		int direction = JudgeDirectionInTubeHeightAxis(currentPosition, m_endPos);

		if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageMovetoEndPos]->[Current Position:{$:f3} Target Position:{$:f3} Direction:{$:d}]",
			currentPosition, m_endPos, direction);

		MoveTubeCircular(direction, abs(currentPosition - m_endPos));
	}
	else
	{
		if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_SWING))
		{
			if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][MotionStageMovetoEndPos]->[CONTROLLER_CARM_TUBE_SWING is not ready status, need to reset first]");
			return;
		}

		float currentPosition = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);

		int direction = JudgeDirectionInTubeSwingAxis(currentPosition, m_endPos);

		if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageMovetoEndPos]->[Current Position:{$:f3} Target Position:{$:f3} Direction:{$:d}]",
			currentPosition, m_endPos, direction);

		RotateTubeSwing(direction, abs(currentPosition - m_endPos));
	}
}

void CBCTMotionModel::MotionStageMovetoStartPos()
{
	if (!m_bUseCARMRotationScanning)
	{
		TubeCircularToStartPosition();
	}
	else
	{
		TubeSwingToStartPostion();
	}
}

void CBCTMotionModel::MotionStageErrorStop()
{
	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageErrorStop]->[Enter]");

	m_circular->StopRotation();
	m_swing->StopSwing();

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][MotionStageErrorStop]->[Leave]");
}

void CBCTMotionModel::MotionStageErrorRecover()
{

}

void CBCTMotionModel::TubeSwingToStartPostion()
{
	if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_SWING))
	{
		if (gmotionLog) gmotionLog->Warn("[CBCTMotionModel][TubeSwingToStartPostion]->[CONTROLLER_CARM_TUBE_SWING is not ready status, need to reset first]");
		return;
	}

	float tubeAngle = m_coordinates->GetCurrentPhysical(CARM_TUBE_SWING);
	//auto swingStartPosition = 0.0f;
	auto swingStartPosition = m_startPos;

	int direction = JudgeDirectionInTubeSwingAxis(tubeAngle, swingStartPosition);

	float speed = 0.0f;
	if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
	{
		speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	}

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeSwingToStartPostion]->[S:{$:f3} T:{$:f3} D:{$:d} SP:{$:f3}]",
		tubeAngle, m_startPos, direction, speed);

	RotateTubeSwing(direction, abs(swingStartPosition - tubeAngle), speed);
}

void CBCTMotionModel::TubeCircularToStartPosition()
{
	if (!m_coordinates->IsSystemReady(CONTROLLER_CARM_TUBE_CIRCULAR))
	{
		if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeCircularToStartPosition]->[CONTROLLER_CARM_TUBE_CIRCULAR is not ready status, need to reset first]");
		return;
	}

	float tubeHeight = m_coordinates->GetCurrentPhysical(CARM_TUBE_CIRCULAR);

	int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, m_startPos);

	float speed = 0.0f;
	if (m_MachineryParams.GetFirstOf("MotionSpeed") > 0)
	{
		speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	}

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][TubeCircularToStartPosition]->[S:{$:f3} T:{$:f3} D:{$:d} SP:{$:f3}]", tubeHeight, m_startPos, direction, speed);

	MoveTubeCircular(direction, abs(m_startPos - tubeHeight), speed);
}

int  CBCTMotionModel::JudgeDirectionInTubeSwingAxis(float current, float target)
{
	auto positive = (int)atoi((const char *)m_MachineryParams["TubeRotateAxisPositiveDirection"]);
	auto direction = positive > 0 ? 1 : -1;

	if (current > target)
	{
		return -1 * direction;
	}

	if (current < target)
	{
		return 1 * direction;
	}

	return 0;
}

void CBCTMotionModel::RotateTubeSwing(int direction, float offset, float spd)
{
	float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);

	if (spd > 1e-3)
	{
		speed = spd;
	}

	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_CARM_TUBE_SWING, speed);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_SWING, offset);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][RotateTubeSwing]->[Step:{$:d} Period:{$:d} Direction:{$:d}]", step, period, direction);

	m_swing->Swing(direction, step, period);
}

int  CBCTMotionModel::JudgeDirectionInTubeHeightAxis(float current, float target)
{
	auto positive = (int)atoi((const char *)m_MachineryParams["TubeHeightAxisPositiveDirection"]);
	auto direction = positive > 0 ? 1 : -1;

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][JudgeDirectionInTubeHeightAxis]->[C:{$:f3} T:{$:f3} D:{$:d}]", current, target, direction);

	if (current > target)
	{
		return -1 * direction;
	}

	if (current < target)
	{
		return 1 * direction;
	}

	return 0;
}

void CBCTMotionModel::MoveTubeCircular(int direction, float offset, float spd)
{
	float speed = strtof((const char *)m_TechnicalParams["CBCTMotionSpeed"], nullptr);

	if (spd > 1e-3)
	{
		speed = spd;
	}

	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_CARM_TUBE_CIRCULAR, offset);
	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_CARM_TUBE_CIRCULAR, speed);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}

	m_circular->Rotate(direction, step, period);
}

void CBCTMotionModel::SwitchScanningComponents(int nSwitch)
{
	//false:滑轨滑动扫描   true:轴向转动扫描
	m_bUseCARMRotationScanning = nSwitch;

	if (gmotionLog) gmotionLog->Info("[CBCTMotionModel][SwitchScanningComponents]->[Using {$} for scanning]", m_bUseCARMRotationScanning ? "swing" : "circular");
}

void CBCTMotionModel::OnStartExposure()
{
	if (m_fluoroSwitch)
	{
		if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][OnStartExposure]->[Enable fluoro switch signal]");
		m_fluoroSwitch->OutputSignal(true);
	}
}

void CBCTMotionModel::OnStopExposure()
{
	if (m_fluoroSwitch)
	{
		if (gbusinessLog) gbusinessLog->Info("[CBCTMotionModel][OnStopExposure]->[Disable fluoro switch signal]");
		m_fluoroSwitch->OutputSignal(false);
	}
}

void CBCTMotionModel::SwitchWorkstation(string ws)
{
	m_CurWS = ws;
}