PhysicalDevice/Machine/MachineryECOM/Trunk/DIOS.Dev.Machinery.Common/DIOS.Dev.Machinery.BaseComponent/TubeSwingController.cpp

#include "stdafx.h"
#include "TubeSwingController.h"
#include "ICommunicateEntity.h"
#include "IOInterfaceMapper.h"
#include "IServoDrive.h"

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

const float PULSE_DUTYCYCLE_MIN = 0.2f;
const float PULSE_DUTYCYCLE_MAX = 0.95f;
const float PULSE_DUTYCYCLE_DEF = 0.7f;

TubeSwingController::TubeSwingController()
:m_communicate(nullptr),
m_driveNumber(-1),
m_pulseInOneCircle(0),
m_pulseDutyCycle(PULSE_DUTYCYCLE_DEF),
m_servo(nullptr)
{
}


TubeSwingController::~TubeSwingController()
{
}

std::string TubeSwingController::CLASSID()
{
	return "FC053760-9422-42A5-B276-BFBBD3717053";
}

void TubeSwingController::Initialize(const std::string &name)
{
	SetName(name);
}

void TubeSwingController::AttachServoDrive(IServoDrive *servodrive)
{
	m_servo = servodrive;
}

void TubeSwingController::SetPulseOneCircle(unsigned short pulseonecircle)
{
	m_pulseInOneCircle = pulseonecircle;

	if (m_servo)
	{
		RS232_PARAM param;
		if (m_servo->MakePulseOnCirclePacket(pulseonecircle, param.rs485))
		{
			DoWrite485(param);
		}
	}
}

void TubeSwingController::SetPulseDutyCycle(float dutyCycle)
{
	m_pulseDutyCycle = dutyCycle;

	if (dutyCycle < PULSE_DUTYCYCLE_MIN || dutyCycle > PULSE_DUTYCYCLE_MAX)
	{
		m_pulseDutyCycle = PULSE_DUTYCYCLE_DEF;
	}
}

void TubeSwingController::OnCommunicationEstablished(ICommunicateEntity *communicate)
{
	m_communicate = communicate;
	if (!m_communicate)
	{
		return;
	}

	ActivePWMMode();
	InitializeServo();
}

void TubeSwingController::SetServoStatus(int svostatus)
{
	ServoOn(svostatus != 0);
}

void TubeSwingController::StopSwing()
{
	GPIO_DO_PARAM pwmparam;
	pwmparam.active_level = 0x00;
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_NONE, pwmparam);
}

void TubeSwingController::Swing(int direction, int steps, DWORD pwmperiod, float pulseDutyCycle)
{
	bool clockWise = direction > 0 ? false : true;
	SetSwingOrientation(clockWise);

	if (pulseDutyCycle < PULSE_DUTYCYCLE_MIN || pulseDutyCycle > PULSE_DUTYCYCLE_MAX)
	{
		pulseDutyCycle = m_pulseDutyCycle;
	}

	GPIO_DO_PARAM pwmparam;
	pwmparam.pwm_step.pwmstep_int = (int)steps;
	pwmparam.peroid.pwmperiod_short = (unsigned short)pwmperiod;
	pwmparam.effective.pwmperiod_short = (unsigned short)(pwmperiod * pulseDutyCycle);
	pwmparam.output_mode = (unsigned char)GOM_PWM;
	pwmparam.active_level = 0x01;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_DELETE, pwmparam);
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_MODE, pwmparam);

	ServoOn(TRUE);

	//持续运动不需要速度规划
	if (steps == -1)
	{
		m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
	}
	else
	{//速度规划
		int dectotal = m_pulseInOneCircle / 2;
		int acctotal = m_pulseInOneCircle / 2;

		if (steps <= (dectotal + acctotal))
		{
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
		}
		else
		{
			int decstep1 = 2 * dectotal / 3;
			int decstep2 = dectotal / 3;

			int decperiod1 = 3 * pwmperiod / 2;
			int decperiod2 = 3 * pwmperiod;

			int accstep1 = 2 * acctotal / 3;
			int accstep2 = acctotal / 3;

			int accperiod1 = 3 * pwmperiod / 2;
			int accperiod2 = 3 * pwmperiod;

			pwmparam.pwm_step.pwmstep_int = accstep2;
			pwmparam.peroid.pwmperiod_short = (unsigned short)accperiod2;
			pwmparam.effective.pwmperiod_short = (unsigned short)(accperiod2 * pulseDutyCycle);
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);

			pwmparam.pwm_step.pwmstep_int = accstep1;
			pwmparam.peroid.pwmperiod_short = (unsigned short)accperiod1;
			pwmparam.effective.pwmperiod_short = (unsigned short)(accperiod1 * pulseDutyCycle);
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);

			pwmparam.pwm_step.pwmstep_int = steps - dectotal - acctotal;
			pwmparam.peroid.pwmperiod_short = (unsigned short)pwmperiod;
			pwmparam.effective.pwmperiod_short = (unsigned short)(pwmperiod * pulseDutyCycle);
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
			pwmparam.pwm_step.pwmstep_int = decstep1;
			pwmparam.peroid.pwmperiod_short = (unsigned short)decperiod1;
			pwmparam.effective.pwmperiod_short = (unsigned short)(decperiod1 * pulseDutyCycle);
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
			pwmparam.pwm_step.pwmstep_int = decstep2;
			pwmparam.peroid.pwmperiod_short = (unsigned short)decperiod2;
			pwmparam.effective.pwmperiod_short = (unsigned short)(decperiod2 * pulseDutyCycle);
			m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
		}
	}

	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_NONE, pwmparam);
}

void TubeSwingController::SetSwingOrientation(bool clockWise)
{
	GPIO_DO_PARAM doparam;
	doparam.active_level = clockWise ? 0x0 : 0x1;
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_functionIds[ID_TUBE_ANGLE_DIRECTION], DO_ATTR_NONE, doparam);
}

void TubeSwingController::InitializeServo()
{
	if (m_servo)
	{
		RS232_PARAM param;
		if (m_servo->MakeServoOnEnablePacket(param.rs485))
		{
			DoWrite485(param);
		}

		ServoOn(FALSE);
	}
}

void TubeSwingController::ActivePWMMode()
{
	GPIO_DO_PARAM doparam;
	doparam.output_mode = (unsigned char)GOM_PWM;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_MODE, doparam);
}

void TubeSwingController::ServoOn(BOOL servoon)
{
	RS232_PARAM param;
	if (m_servo->MakeServoOnPacket(servoon, param.rs485))
	{
		DoWrite485(param);
	}
	else
	{
		if (m_functionIds.find(ID_TUBE_ANGLE_SERVO_ON) != m_functionIds.end()
			&& m_functionIds[ID_TUBE_ANGLE_SERVO_ON] > 0)
		{
			GPIO_DO_PARAM doparam;
			doparam.active_level = servoon ? 0x1 : 0x0;
			m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_functionIds[ID_TUBE_ANGLE_SERVO_ON], DO_ATTR_NONE, doparam);
		}
	}
}

void TubeSwingController::DoWrite485(RS232_PARAM &param)
{
	m_communicate->RS232_Ctrl(OP_WRITE, (RS232_ID)m_functionIds[ID_RS232], RS232_ATTR_NONE, param);

	Sleep(300);
}