Duble servo on axis

Welcome to the Dynomotion forum

Moderators: dynomotion, TomKerekes, sfzeller

Post Reply
Al4nse
Posts: 7
Joined: Wed Jan 24, 2018 9:37 am

Duble servo on axis

Post by Al4nse » Wed Feb 14, 2018 11:55 am



Hello. We have such a problem. What could it be?

User avatar
TomKerekes
Posts: 17
Joined: Mon Dec 04, 2017 1:49 am

Re: Duble servo on axis

Post by TomKerekes » Wed Feb 14, 2018 6:54 pm

I don't know. you would have to provide information in order for anyone to help you. What is your configuration? What have you tested?
Regards,

Tom Kerekes
Dynomotion, Inc.

Al4nse
Posts: 7
Joined: Wed Jan 24, 2018 9:37 am

Re: Duble servo on axis

Post by Al4nse » Thu Feb 15, 2018 8:05 am

Code: Select all

#include "KMotionDef.h"
void main()
{
	float setup[4];	
	//Инициализация Konnect
	InitAux();
	AddKonnect_Aux0(0,&VirtualBits,VirtualBitsEx); // Konnect on JP4
	//DoRS232Cmds = TRUE;
	// Активация серводрайверов S-ON
	SetBit(48);
	SetBit(50);
	SetBit(52);
	SetBit(54);
	
	
	// Параметры оси Y(мастер)
	ch0->InputMode=ENCODER_MODE;   // режим ОС по энкодеру
	ch0->OutputMode=DAC_SERVO_MODE; // вывод сигнала задания в ЦАП, т.е. +/-10В
	ch0->Vel=100000; //99471.8; //132629; //99471.8;		   	   // максимальная скорость двигателя в импульсах
	ch0->Accel=1000000; //298413; //663145; //1.32629e+06; //198944;			   // максимальное ускорение 
	ch0->Jerk=10000000;			   // максимальный толчок
	ch0->P=0.9; //0.52;//1.07; //0.65;//0.4; //1.07; !!0.9				   // пропорциональная составляющая ПИД
	ch0->I=0.002; //0.0015; //0.0027;				  !!0.002   // интегральная составляющая
	ch0->D=0;					    // диф. составляющая
	ch0->FFAccel=0;				    // ускорение предзадания, или добавка в сигнал
	ch0->FFVel=0.0085; //0;	 //0.0085			    // скорость предзадания
	ch0->MaxI=2047;				    // максимальная величина обработки сигнала
	ch0->MaxErr=10000;			    // максимальная ошибка рассогласования позиции 
	ch0->MaxOutput=2047;		    // максимальный уровень сигнала
	ch0->DeadBandGain=1;		    // усиление мертвой зоны
	ch0->DeadBandRange=0;		    // величина мертвой зоны в импульсах
	ch0->InputChan0=0;			    // канал №0 входящих сигналов от датчика ОС
	ch0->InputChan1=0; 			    // второй канал для датчика ОС, резольвера например
	ch0->OutputChan0=0; 		    // канал №0 для выходящих сигналов, в нашем случае +/-10В
	ch0->OutputChan1=0; 		    // канал для подчиненной оси
	ch0->MasterAxis=-1;			    // включение/выкл. подчиненной оси
	ch0->LimitSwitchOptions=0x123;  // режим работы концевиков
	ch0->LimitSwitchNegBit=1025;    // входной сигнал - концевика
	ch0->LimitSwitchPosBit=1024;    // входной сигнал + концевика
	ch0->SoftLimitPos=1e+09; 	    // софтлимит положит. в импульсах
	ch0->SoftLimitNeg=-1e+09; 	    // софтлимит отриц. в импульсах
	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=0.00025;
	ch0->Lead=0;
	ch0->MaxFollowingError=10000;
	ch0->StepperAmplitude=100;
	
	//установки IIR фильтров
	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=1;
	ch0->iir[2].B1=0;
	ch0->iir[2].B2=0;
	ch0->iir[2].A1=0;
	ch0->iir[2].A2=0;
	

	
	// Параметры оси Y(слэйв)    
	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=DAC_SERVO_MODE;
	ch1->Vel=100000; //99471.8; //132629; //99471.8;
	ch1->Accel=1000000; //298413; //663145; //1.32629e+06; //198944;
	ch1->Jerk=10000000;
	ch1->P=0.9; //0.52; //1.07; //0.65; //0.4;  //1.07;
	ch1->I=0.002; //0.0015;//0.0027;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0.0085; //0;
	ch1->MaxI=2047;
	ch1->MaxErr=10000;
	ch1->MaxOutput=2047;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=1;
	ch1->OutputChan0=1;
	ch1->OutputChan1=1;
	ch1->MasterAxis=0;
	ch1->LimitSwitchOptions=0x123;
	ch1->LimitSwitchNegBit=1028;
	ch1->LimitSwitchPosBit=1024;
	ch1->SoftLimitPos=1e+09;
	ch1->SoftLimitNeg=-1e+09;
	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=0.00025;
	ch1->Lead=0;
	ch1->MaxFollowingError=10000;
	ch1->StepperAmplitude=100;

	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=1;
	ch1->iir[2].B1=0;
	ch1->iir[2].B2=0;
	ch1->iir[2].A1=0;
	ch1->iir[2].A2=0;
	
	// Параметры оси X
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=DAC_SERVO_MODE;
	ch2->Vel=100000; //99471.8; //132629; //132629;
	ch2->Accel=1000000; //298413; //663145; //1326290;
	ch2->Jerk=10000000;
	ch2->P=1.4; //0.65; //0.697;//1.1; !!1.4; //
	ch2->I=0.002; //0.0023;//0.00290;//0; !!0.002
	ch2->D=0; //0;
	ch2->FFAccel=0;
	ch2->FFVel=0.00745; //0; //0.00745
	ch2->MaxI=2047;
	ch2->MaxErr=1e+09;
	ch2->MaxOutput=2047;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=2;
	ch2->OutputChan0=2;
	ch2->OutputChan1=2;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x123;
	ch2->LimitSwitchNegBit=1026;
	ch2->LimitSwitchPosBit=1027;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	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=0.00025;
	ch2->Lead=0;
	ch2->MaxFollowingError=10000;
	ch2->StepperAmplitude=100;

	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;
	

	// Параметры оси Z
	ch3->InputMode=ENCODER_MODE;
	ch3->OutputMode=DAC_SERVO_MODE;
	ch3->Vel=100002;
	ch3->Accel=8000000;//166667; //100000; //
	ch3->Jerk=8.33335e+09;
	ch3->P=0.43; //1.1; //1.5; //0.6; //1.2; //
	ch3->I=0;//0.001; //0.0025; //0;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0.0067;
	ch3->MaxI=2047;
	ch3->MaxErr=1e+09;
	ch3->MaxOutput=2047;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=3;
	ch3->OutputChan0=3;
	ch3->OutputChan1=3;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x12f;
	ch3->LimitSwitchNegBit=1030;
	ch3->LimitSwitchPosBit=1029;
	ch3->SoftLimitPos=1e+09;
	ch3->SoftLimitNeg=-1e+09;
	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=0.00025;
	ch3->Lead=0;
	ch3->MaxFollowingError=50000;
	ch3->StepperAmplitude=100;

	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=1;
	ch3->iir[2].B1=0;
	ch3->iir[2].B2=0;
	ch3->iir[2].A1=0;
	ch3->iir[2].A2=0;
	
    //сброс системы координат в 0 по осям
    
	//Активация осей в 0
	/*
	FILE *f=fopen("C:\\Project\\Project .KFlop\\data\\Init_Axis.txt","r+");     // открытие файла для сохранения настроек
	fscanf(f,"%f%f%f%f",&setup[0],&setup[1],&setup[2],&setup[3]);	
	fclose(f);
	EnableAxisDest(1,setup[1]);
	EnableAxisDest(0,setup[0]);
	EnableAxisDest(2,setup[2]);
	EnableAxisDest(3,setup[3]);

	
	EnableAxis(0);
	EnableAxis(1);
	EnableAxis(2);
	EnableAxis(3);
		*/
	Zero(1);
    	Zero(0);
   	Zero(2);
   	Zero(3);
	EnableAxisDest(1,ch1->Position);
	EnableAxisDest(0,ch0->Position);
	EnableAxisDest(2,ch2->Position);
	EnableAxisDest(3,ch3->Position);
	
	
    //задание системы координат в KMotionCNC 
    DefineCoordSystem(2,0,3,-1);
	printf("Init complete\n");
	
	EnableRS232Cmds(RS232_BAUD_38400);
	DoRS232Cmds = FALSE; 
	FILE *f=fopen("C:\\Project\\Project .KFlop\\data\\Init_Axis.txt","wt");
	for (;;)  //loop forever
    {
    WaitNextTimeSlice(); //Каждый сервоцикл
		if (!(ch0->Enable &&  ch1->Enable  && ch2->Enable&& ch3->Enable && ReadBit(1031))) { //если все оси активны и есть сигнал E-stop       
			//дизактивация серводрайверов и осей
			SetBit(54);
			EnableAxis(3);
			DefineCoordSystem(-1,-1,3,-1);
			MoveAtVel(3,60000,80000);
			while (!CheckDone(3));
			DisableAxis(3);
			ClearBit(54);
			Delay_sec(0.5);
			ClearBit(57);
			fprintf(f,"%.1f %.1f %.1f %.1f\n",ch0->Position,ch1->Position,ch2->Position,ch3->Position); //запись настроек
			fclose(f);
			ClearBit(48);
			ClearBit(50);
			ClearBit(52);
			ClearBit(58);
			ClearBit(59);
			DisableAxis(1);
			DisableAxis(0);
			DisableAxis(2);
			Delay_sec(0.1);
			printf("tut\n");
			
			break;}
    
		/*if (ch0->Enable &&  ch1->Enable  && ch2->Enable&& ch3->Enable && ReadBit(1031)) { //если все оси активны и есть сигнал E-stop       
			//активация серводрайверов
			SetBit(48);
			SetBit(50);
			SetBit(52);
			SetBit(54);
			}
        else {
			//дизактивация серводрайверов и осей
			ClearBit(48);
			ClearBit(50);
			ClearBit(52);
			ClearBit(58);
			DisableAxis(1);
			DisableAxis(0);
			DisableAxis(2);
			printf("tut\n");
			SetBit(54);
			EnableAxis(3);
			DefineCoordSystem(-1,-1,3,-1);
			MoveRelAtVel(3,30000,80000);
			while (!CheckDone(3));
			DisableAxis(3);
			ClearBit(54);
			
			ClearBit(57);
			break;}*/
     }
	 return 0;
	
}

void ReceiveChar()
{
	// wait for data in buffer
	while ((FPGA(RS232_STATUS) & 1)==0);
	return FPGA(RS232_DATA);
}
This script that runs on the INIT button.
I removed the logs using the following script:

Code: Select all

#include "KMotionDef.h"

main()
{
	int cap;

	
for(;;)
{
    

	printf("Position:    %4.1f, %4.1f\n",ch0->Position,ch1->Position);
	printf("Destination: %4.1f, %4.1f\n",ch0->Dest,ch1->Dest);
	Delay_sec(0.1);
	//printf("%d\n",cap);
	}
}
This is what was displayed in the console before and after the INIT button was turned on.

Code: Select all

Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
[color=#FF0000]Init complete[/color]
Position:     1.0,  0.0
Destination:  0.0,  0.0
Position:     1.0,  0.0
Destination:  0.0,  0.0
Position:     1.0,  0.0
Destination:  0.0,  0.0
Position:    -1.0,  0.0
Destination:  0.0,  0.0
Position:    -2.0,  0.0
Destination:  0.0,  0.0
Position:    -1.0,  0.0
Destination:  0.0,  0.0
Position:     0.0,  0.0
Destination:  0.0,  0.0
Position:     2.0,  0.0
Destination:  0.0,  0.0
Position:     3.0,  0.0
Destination:  0.0,  0.0
Position:    -1.0,  0.0
Destination:  0.0,  0.0
Position:    -4.0,  0.0
Destination:  0.0,  0.0
Position:    -4.0,  0.0
Destination:  0.0,  0.0
Position:     7.0,  0.0
Destination:  0.0,  0.0
Position:     7.0,  0.0
Destination:  0.0,  0.0
Position:    -10.0,  0.0
Destination:  0.0,  0.0
Position:     4.0,  0.0
Destination:  0.0,  0.0
Position:    10.0,  0.0
Destination:  0.0,  0.0
Position:    -15.0,  0.0
Destination:  0.0,  0.0
Position:    17.0,  0.0
Destination:  0.0,  0.0
Position:    -18.0,  0.0
Destination:  0.0,  0.0
Position:    20.0, -1.0
Destination:  0.0,  0.0
Position:    -21.0,  0.0
Destination:  0.0,  0.0
Position:    21.0,  0.0
Destination:  0.0,  0.0
Position:    -19.0,  2.0
Destination:  0.0,  0.0
Position:    12.0,  1.0
Destination:  0.0,  0.0
Position:    -3.0,  1.0
Destination:  0.0,  0.0
Position:    -8.0, -3.0
Destination:  0.0,  0.0
Position:    21.0, -1.0
Destination:  0.0,  0.0
Position:    -23.0,  8.0
Destination:  0.0,  0.0
Position:    29.0, -22.0
Destination:  0.0,  0.0
Position:    21.0, -26.0
Destination:  0.0,  0.0
Position:    47.0, -57.0
Destination:  0.0,  0.0
[color=#0000FF]Position:    485.0, -486.0[/color]
Destination:  0.0,  0.0
Position:    -18.0, 16.0
Destination:  0.0,  0.0
Position:    -3.0,  4.0
Destination:  0.0,  0.0
Position:     0.0,  5.0
Destination:  0.0,  0.0
Position:    -1.0, -1.0
Destination:  0.0,  0.0
Position:     0.0, -2.0
Destination:  0.0,  0.0
Position:     0.0,  0.0
Destination:  0.0,  0.0
Position:     0.0, -4.0
Destination:  0.0,  0.0
Position:     0.0,  3.0
Destination:  0.0,  0.0
Position:     0.0, -1.0
From the operator's side, there was no action.

User avatar
TomKerekes
Posts: 17
Joined: Mon Dec 04, 2017 1:49 am

Re: Duble servo on axis

Post by TomKerekes » Thu Feb 15, 2018 5:10 pm

You didn't tell us what have you tested?

Have you tuned the servos?

Possibly the Slave axis is backwards (try changing sign of Slave Gain)

Plot a move of the Master Axis on the Step Response Screen.
Regards,

Tom Kerekes
Dynomotion, Inc.

Al4nse
Posts: 7
Joined: Wed Jan 24, 2018 9:37 am

Re: Duble servo on axis

Post by Al4nse » Fri Feb 16, 2018 5:54 am

This bug appears only when the EnableAxis command is issued. Servos are set up correctly. I have been working on this machine for half a year now.
The slave is configured correctly. ch1->SlaveGain=-1;
I repeat. This bug appears only when the machine starts. In the process, no bugs are detected.
Thank you.

User avatar
TomKerekes
Posts: 17
Joined: Mon Dec 04, 2017 1:49 am

Re: Duble servo on axis

Post by TomKerekes » Fri Feb 16, 2018 7:50 pm

If the problem only occurs when the system is initialized and without problem afterward then it may have to do with how you power and enable the Amplifiers.

I assume the commands below enable the Amplifiers?

SetBit(48);
SetBit(50);
SetBit(52);
SetBit(54);

It may take some time from when the Amplifiers are enabled until they are operational. If the Axes are enabled at the same time then the servo might ramp the output to a large value with no response trying to make a correction. When the Amplifiers finally become functional there will be a jump.

Try adding a delay after the Amplifiers are enabled before the Axes are enabled. For example code:

SetBit(48);
SetBit(50);
SetBit(52);
SetBit(54);
Delay_sec(1.0);
Regards,

Tom Kerekes
Dynomotion, Inc.

Post Reply