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

#include "stdafx.h"
#include "ParkingModel.h"
#include "MotionStages.h"
#include "ParkingMotionStageArgs.h"
#include "IMachineryManager.h"
#include "ITubeAngleController.h"
#include "ITubeHeightController.h"
#include "IPositionManager.h"

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

ParkingModel::ParkingModel() 
:m_stageArgs(new ParkingMotionStageArgs()), 
m_coordinates(nullptr),
m_motorTubeAngle(nullptr),
m_motorTubeHeight(nullptr)
{
}


ParkingModel::~ParkingModel()
{
	if (m_stageArgs)
	{
		delete m_stageArgs;
		m_stageArgs = nullptr;
	}
}

std::string ParkingModel::GetStageName()
{
	return m_stageName;
}

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

void ParkingModel::ChangeStage(const std::string &stageName)
{
	m_stageName = stageName;
	OnMotionStage(stageName);
}

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

	m_motorTubeAngle = (ITubeAngleController *)(machineryManager->Resove(CONTROLLER_TUBE_ANGLE));
	m_motorTubeHeight = (ITubeHeightController *)(machineryManager->Resove(CONTROLLER_TUBE_HEIGHT));

	assert(m_motorTubeAngle);
	assert(m_motorTubeHeight);
}

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

void ParkingModel::LoadModelParams(ResDataObject &params)
{

}

void ParkingModel::SetTechnicalParams(ResDataObject &params)
{

}

void ParkingModel::OnFeedbackMotionParams(ResDataObject &params)
{

}

BOOL ParkingModel::GetMotionParams(ResDataObject &params)
{
	return TRUE;
}

void ParkingModel::OnMotionStage(const std::string &name)
{
	if (name == PARKING_STAGE_PARK_TO_HORIZONTAL)
	{
		OnMotionStageParkingHorizontal();
	}
	else if (name == PARKING_STAGE_PARK_TO_VERTICAL)
	{
		OnMotionStageParkingVertical();
	}
	else if (name == PARKING_STAGE_PARK_TO_TUBE_ANGLE_LOW_LIMIT)
	{
		OnMotionStageParkingTubeAngleLowLimit();
	}
	else if (name == PARKING_STAGE_PARK_TO_TUBE_HEIGHT_LOW_LIMIT)
	{
		OnMotionStageParkingTubeHeightLowLimit();
	}
	else if (name == PARKING_STAGE_STOP_MOVE)
	{
		OnMotionStageStopMove();
	}
}

void ParkingModel::OnMotionStageParkingHorizontal()
{
	AlignTubeHeightToDetector();
	RotateTubeToExposure();
}

void ParkingModel::OnMotionStageParkingVertical()
{
	RotateTubeToVertical();
}

void ParkingModel::OnMotionStageParkingTubeAngleLowLimit()
{
	RotateTubeToLowLimit();
}

void ParkingModel::OnMotionStageParkingTubeHeightLowLimit()
{
	MoveTubeHeightToLowLimit();
}

void ParkingModel::AlignTubeHeightToDetector()
{
	float detectorHeight = m_coordinates->GetCurrentPhysical(TOMO_DETECTOR_HEIGHT);
	float tubeHeight = m_coordinates->GetCurrentPhysical(TOMO_TUBE_HEIGHT);

	int direction = JudgeDirectionInTubeHeightAxis(tubeHeight, detectorHeight);

	MoveTubeHeight(direction, abs(detectorHeight - tubeHeight));
}

void ParkingModel::RotateTubeToExposure()
{
	float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
	float exposureAngle = GetExposureAngle();

	int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, exposureAngle);

	RotateTubeAngle(direction, tubeAngle);
}

void ParkingModel::RotateTubeToVertical()
{
	float tubeAngle = m_coordinates->GetCurrentPhysical(TOMO_TUBE_ANGLE);
	float verticalAngle = GetVerticalAngle();

	int direction = JudgeDirectionInTubeAngleAxis(tubeAngle, verticalAngle);

	RotateTubeAngle(direction, tubeAngle);
}

void ParkingModel::RotateTubeToLowLimit()
{
	auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]);

	auto currentangle = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_ANGLE);
	auto targetangle = m_coordinates->GetLandmarkPosition(TOMO_TUBE_ANGLE,LANDMARK_LOW);
	int direction = JudgeDirectionInTubeAngleAxis(currentangle, targetangle);

	if(gbusinessLog) gbusinessLog->Info("[ParkingModel][RotateTubeToLowLimit]->[{$:f6} {$:f6}]", currentangle, targetangle);

	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, abs(currentangle - targetangle));
	m_motorTubeAngle->Rotate(direction, steps, period);
}

void ParkingModel::MoveTubeHeightToLowLimit()
{
	float speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	auto currentheight = m_coordinates->GetCurrentAbsolutePhysical(TOMO_TUBE_HEIGHT);
	auto targetheight = m_coordinates->GetLandmarkPosition(TOMO_TUBE_HEIGHT,LANDMARK_LOW);
	int direction = JudgeDirectionInTubeHeightAxis(currentheight, targetheight);
	auto steps = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, abs(currentheight - targetheight));
	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}
	m_motorTubeHeight->Move(direction, steps, period);
}

void ParkingModel::MoveTubeHeight(int direction, float offset)
{
	float speed = (float)atof((const char *)m_MachineryParams["MotionSpeed"]);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_HEIGHT, offset);
	auto period = m_coordinates->ConvertMotorSpeed(CONTROLLER_TUBE_HEIGHT, speed);
	if (period < 50 || (period - 50) < 30)
	{
		period = 50;
	}
	m_motorTubeHeight->Move(direction, step, period);
}

void ParkingModel::RotateTubeAngle(int direction, float offset)
{
	auto period = (DWORD)atoi((const char *)m_MachineryParams["TubeAnglePeriodP0"]);
	auto step = m_coordinates->ConvertMotorStepValue(CONTROLLER_TUBE_ANGLE, offset);
	m_motorTubeAngle->Rotate(direction, step, period);
}

float ParkingModel::GetExposureAngle()
{
	return (float)atof((const char *)m_MachineryParams["TubeAngleOfRAD"]);
}

float ParkingModel::GetVerticalAngle()
{
	return (float)atof((const char *)m_MachineryParams["TubeAngleOfVertical"]);
}

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

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

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

	return 0;
}

int ParkingModel::JudgeDirectionInTubeAngleAxis(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 ParkingModel::OnMotionStageStopMove()
{
	m_motorTubeAngle->StopRotation();
	m_motorTubeHeight->StopMove();
}

void ParkingModel::SwitchScanningComponents(int nSwitch)
{

}

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