Dynomotion Forum

Code: Select all

#include "KMotionDef.h"

#define AXISX 0
#define AXISYA 1
#define AXISYB 2
#define AXISZ 3

#define MPG_INPUT_AXIS 7    // Axis used to read hardware encoder (not necessarily the encoder channel)// Ось, используемая для чтения аппаратного энкодера (не обязательно канала энкодера)

#define TAU0  0.01			// smoothness factor (Low Pass Time constant seconds for MoveExp)// коэффициент гладкости (постоянная времени низкого прохода в секундах для MoveExp)
#define TAU1 0.015			// smoothness factor (Low Pass Time constant seconds for pre filter 1)// коэффициент гладкости (постоянная времени прохождения нижних частот в секундах для предварительного фильтра 1)
#define TAU2 0.015			// smoothness factor (Low Pass Time constant seconds for pre filter 2)// коэффициент гладкости (постоянная времени прохождения нижних частот в секундах для предварительного фильтра 2)
#define FINAL_TIME 1.0		// Set final dest after this amount of time with no change// Устанавливаем конечную цель по истечении этого времени без изменений

#define ENABLE_MPG 1050       // Switch/Button used to enable MPG Motion// Переключатель/кнопка, используемая для включения MPG Motion
#define FACTOR1 28
#define FACTOR10 30
#define FACTOR100 32

#define SELECTX 16
#define SELECTY 18
#define SELECTZ 29
#define SELECTA 30
#define Estop 7
#define MPG_Estop 31
#define Spindle_CW 27
#define Estop_Bit 1030
void ServiceMPG(void);

int main() 
{
	double T0, LastX=0, LastY=0, LastZ=0, LastA=0, Tau;
	
	KStepPresent=FALSE;      // enable KSTEP input multiplexing
	FPGA(KAN_TRIG_REG)=0;  	// Mux PWM0 to JP7 Pin5 IO 44 for KSTEP 

	FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80;  // set polarity and pulse length to 4us
	
	ch0->InputMode=NO_INPUT_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=26660;
	ch0->Accel=150000;
	ch0->Jerk=4e+06;
	ch0->P=0;
	ch0->I=0.01;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=200;
	ch0->MaxErr=1e+06;
	ch0->MaxOutput=200;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=0;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x110;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=1e+30;
	ch0->SoftLimitNeg=-1e+30;
	ch0->InputGain0=1;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=-1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=0;
	ch0->BacklashRate=0;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=1000000000;
	ch0->StepperAmplitude=20;

	ch0->iir[0].B0=1;
	ch0->iir[0].B1=0;
	ch0->iir[0].B2=0;
	ch0->iir[0].A1=0;
	ch0->iir[0].A2=0;

	ch0->iir[1].B0=1;
	ch0->iir[1].B1=0;
	ch0->iir[1].B2=0;
	ch0->iir[1].A1=0;
	ch0->iir[1].A2=0;

	ch0->iir[2].B0=0.000769;
	ch0->iir[2].B1=0.001538;
	ch0->iir[2].B2=0.000769;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;
    EnableAxisDest(0,0);

	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=26660;
	ch1->Accel=150000;
	ch1->Jerk=4e+06;
	ch1->P=0;
	ch1->I=0.01;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=200;
	ch1->MaxErr=1e+06;
	ch1->MaxOutput=200;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=0;
	ch1->InputChan1=0;
	ch1->OutputChan0=1;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x110;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=1e+30;
	ch1->SoftLimitNeg=-1e+30;
	ch1->InputGain0=1;
	ch1->InputGain1=1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=-1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_OFF;
	ch1->BacklashAmount=0;
	ch1->BacklashRate=0;
	ch1->invDistPerCycle=1;
	ch1->Lead=0;
	ch1->MaxFollowingError=1000000000;
	ch1->StepperAmplitude=20;

	ch1->iir[0].B0=1;
	ch1->iir[0].B1=0;
	ch1->iir[0].B2=0;
	ch1->iir[0].A1=0;
	ch1->iir[0].A2=0;

	ch1->iir[1].B0=1;
	ch1->iir[1].B1=0;
	ch1->iir[1].B2=0;
	ch1->iir[1].A1=0;
	ch1->iir[1].A2=0;

	ch1->iir[2].B0=0.000769;
	ch1->iir[2].B1=0.001538;
	ch1->iir[2].B2=0.000769;
	ch1->iir[2].A1=1.92076;
	ch1->iir[2].A2=-0.923833;
    EnableAxisDest(1,0);

	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=26660;
	ch2->Accel=150000;
	ch2->Jerk=4e+06;
	ch2->P=0;
	ch2->I=0.01;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+06;
	ch2->MaxOutput=200;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=0;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=0;
	ch2->MasterAxis=1;
	ch2->LimitSwitchOptions=0x110;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=1e+30;
	ch2->SoftLimitNeg=-1e+30;
	ch2->InputGain0=1;
	ch2->InputGain1=1;
	ch2->InputOffset0=0;
	ch2->InputOffset1=0;
	ch2->OutputGain=1;
	ch2->OutputOffset=0;
	ch2->SlaveGain=1;
	ch2->BacklashMode=BACKLASH_OFF;
	ch2->BacklashAmount=0;
	ch2->BacklashRate=0;
	ch2->invDistPerCycle=1;
	ch2->Lead=0;
	ch2->MaxFollowingError=1000000000;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=1;
	ch2->iir[0].B1=0;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0;
	ch2->iir[0].A2=0;

	ch2->iir[1].B0=1;
	ch2->iir[1].B1=0;
	ch2->iir[1].B2=0;
	ch2->iir[1].A1=0;
	ch2->iir[1].A2=0;

	ch2->iir[2].B0=1;
	ch2->iir[2].B1=0;
	ch2->iir[2].B2=0;
	ch2->iir[2].A1=0;
	ch2->iir[2].A2=0;
	EnableAxisDest(2,0);
	
	ch3->InputMode=NO_INPUT_MODE;
	ch3->OutputMode=STEP_DIR_MODE;
	ch3->Vel=20000;
	ch3->Accel=150000;
	ch3->Jerk=4e+06;
	ch3->P=0;
	ch3->I=0.01;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=200;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=200;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=0;
	ch3->InputChan1=0;
	ch3->OutputChan0=3;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x110;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=0;
	ch3->SoftLimitPos=1e+30;
	ch3->SoftLimitNeg=-1e+30;
	ch3->InputGain0=1;
	ch3->InputGain1=1;
	ch3->InputOffset0=0;
	ch3->InputOffset1=0;
	ch3->OutputGain=-1;
	ch3->OutputOffset=0;
	ch3->SlaveGain=1;
	ch3->BacklashMode=BACKLASH_OFF;
	ch3->BacklashAmount=0;
	ch3->BacklashRate=0;
	ch3->invDistPerCycle=1;
	ch3->Lead=0;
	ch3->MaxFollowingError=1000000000;
	ch3->StepperAmplitude=20;

	ch3->iir[0].B0=1;
	ch3->iir[0].B1=0;
	ch3->iir[0].B2=0;
	ch3->iir[0].A1=0;
	ch3->iir[0].A2=0;

	ch3->iir[1].B0=1;
	ch3->iir[1].B1=0;
	ch3->iir[1].B2=0;
	ch3->iir[1].A1=0;
	ch3->iir[1].A2=0;

	ch3->iir[2].B0=0.000769;
	ch3->iir[2].B1=0.001538;
	ch3->iir[2].B2=0.000769;
	ch3->iir[2].A1=1.92076;
	ch3->iir[2].A2=-0.923833;
	EnableAxisDest(3,0);
	
	DefineCoordSystem(0,1,3,-1);
	
//  Add a small amount of Coordinated Motion Path smoothing if desired
//	Tau = 0.001;  // seconds for Low Pass Filter Time Constant
//	KLP = exp(-TIMEBASE/Tau);
	KLP=0; // force to 0 to disable
//	printf("Tau=%f KLP=%f\n",Tau,KLP);
	SetBitDirection(Spindle_CW,1);            //настроим как выход 
	SetBitDirection(17,1);                    //настроим как выход 
	SetBitDirection(33,1);                    //настроим как выход 
	SetBitDirection(19,1);                    //Управление щеткой настроим как выход 
	SetBitDirection(21,1);                    //Управление замком держателя инструментов настроим как выход 
	SetBitDirection(23,1);                    //Управление пневатикой датчика поиска меток настроим как выход 
	ClearBit(ENABLE_MPG);
	ClearBit(Spindle_CW);
	ClearBit(23);                             //Поднимем датчик поиска меток
	ClearBit(21);                             //Закроем замок держателя инструмента
	ClearBit(1055);                           //Отменим готовность датчика к поиску метки
	ClearBit(19);                             //Поднимем щетку 
	ClearBit(1040);                           //Выключим режим New Tool
	ClearBit(1041);                           //Потушим бит освобождения шпинделя
	ClearBit(1024);                           //выключим светодиод оси Х 
	ClearBit(1026);                           //выключим светодиод оси Y 
	ClearBit(1025);                           //выключим светодиод оси Z
	ClearBit(1027);                           //сбросим флаг занято
    
	 chan[0].SoftLimitPos = 6e+09; 
     chan[0].SoftLimitNeg = -6e+09; 
     chan[1].SoftLimitPos = 6e+09; 
     chan[1].SoftLimitNeg = -6e+09; 
     chan[2].SoftLimitPos = 6e+09; 
     chan[2].SoftLimitNeg = -6e+09; 
     chan[3].SoftLimitPos = 6e+09; 
     chan[3].SoftLimitNeg = -6e+09;
	for (;;) // loop forever
	{
		WaitNextTimeSlice();
		
		if (!ReadBit(Estop) || !ReadBit(MPG_Estop))  //!chan[AXISX].Enable || !chan[AXISYA].Enable || !chan[AXISYB].Enable || !chan[AXISZ].Enable ||
		{
			SetBit(Estop_Bit);
			ClearBit(ENABLE_MPG);
			ClearBit(Spindle_CW);
			ClearBit(23);        //Поднимем датчик поиска меток
			ClearBit(1055);      //Отменим готовность датчика к поиску метки
			ClearBit(19);        //Поднимем щетку 
			ClearBit(1040);      //Выключим режим New Tool
			ClearBit(1041);      //Потушим бит освобождения шпинделя
			if (ch0->Enable)DisableAxis(0);
			if (ch1->Enable)DisableAxis(1);
			if (ch2->Enable)DisableAxis(2);
			if (ch3->Enable)DisableAxis(3);	
		}
		else ClearBit(Estop_Bit);
		/*
		if (!ReadBit(Estop) || !ReadBit(MPG_Estop))
		{
			SetBit(Estop_Bit);
			ClearBit(ENABLE_MPG);
			ClearBit(Spindle_CW);
			ClearBit(23);
			if (ch0->Enable)DisableAxis(0);
			if (ch1->Enable)DisableAxis(1);
			if (ch2->Enable)DisableAxis(2);
			if (ch3->Enable)DisableAxis(3);		
		}
		if (ReadBit(Estop) || ReadBit(MPG_Estop))ClearBit(Estop_Bit);
		*/
		
	    if (ReadBit(ENABLE_MPG))ServiceMPG();
	}

    return 0;
}
void ServiceMPG(void)
{
	static int Pos, FirstTime = TRUE;
	static int InMotion = FALSE, Axis,Axis_old, LastAxis = -1;
	static double LastChangeTime = 0, Target, Factor = 0,Factor_old;
	static double Target1, Target2, K1, K2, K1M, K2M;

	int Change1, NewPos;

	if (FirstTime)
	{
		Pos = chan[MPG_INPUT_AXIS].Position;
		K1 = exp(-2 * TIMEBASE / TAU1);	// filter coefficients// коэффициенты фильтра
		K2 = exp(-2 * TIMEBASE / TAU2);
		K1M = 1.0 - K1;
		K2M = 1.0 - K2;
		FirstTime = FALSE;
	}

	NewPos = chan[MPG_INPUT_AXIS].Position;
	Change1 = NewPos - Pos;
	Pos = NewPos;

	if ( !ReadBit(ENABLE_MPG) || JOB_ACTIVE)	// if not button pressed or Job Active ignore the encoder.
		Change1 = 0;
	
	if (ReadBit(FACTOR1))		// is X1 selected?
		Factor = 1;
	else if (ReadBit(FACTOR10))	// is X10 selected?
		Factor = 10;
	else if (ReadBit(FACTOR100))	// is X100 selected?
		Factor = 20;
	else
		Factor = 0.0;
  
	if (ReadBit(SELECTX))		// is x selected?
		Axis = 0;
	else if (ReadBit(SELECTY))	// is y selected?
		Axis = 1;
	else if (ReadBit(SELECTZ))	// is z selected?
		Axis = 3;
	else if (ReadBit(SELECTA))	// is A selected?
		Axis = 4;
    
	// Feedhold fully stopped ??// Feedhold полностью остановлен ??
	if (CS0_StoppingState == 4 && InMotion)
	{
		Change1 = 0;			// ignore any MPG change// игнорируем любые изменения MPG
		Jog(LastAxis, 0);
		InMotion = FALSE;
	}

	// check if the Axis just changed or we have been // проверяем, изменилась ли ось только что или мы были
	// converging to the target for a long time// долго приближаемся к цели
	if (Axis != LastAxis || (InMotion && Time_sec() > LastChangeTime + FINAL_TIME))
	{
		if (InMotion)
			Move(LastAxis, Target1);	//finalize any motion//завершаем любое движение

		LastAxis = Axis;
		InMotion = FALSE;
	}

	if (Change1)				// did we move?// мы переехали?
	{
		if (!InMotion)
			Target = Target1 = Target2 = chan[Axis].Dest;
		Target1 += Change1 * Factor;
		LastChangeTime = Time_sec();
		InMotion = TRUE;
	}

	if (InMotion)				// If moving// Если двигаться
	{
		if (Target1 > chan[LastAxis].SoftLimitPos)
			Target1 = chan[LastAxis].SoftLimitPos;
		if (Target1 < chan[LastAxis].SoftLimitNeg)
			Target1 = chan[LastAxis].SoftLimitNeg;
		Target2 = Target2 * K1 + Target1 * K1M;
		Target = Target * K2 + Target2 * K2M;
		MoveExp(Axis, Target, TAU0);
	}
}