Duble servo on axis

[Al4nse]

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

[TomKerekes]

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?

[Al4nse]

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.

[TomKerekes]

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.

[Al4nse]

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.

[TomKerekes]

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);

[Denis#]

ABOUT! What a familiar problem!
I will say that I have one servo per axis. But this trouble is familiar to me! We fought against it for half a year, but we could not overcome it.
Recently I filled in the latest firmware 434k and this problem seems to be gone.
It used to be like this:
When the drives are turned on (power is supplied), the Z axis starts to move spontaneously. I note that the retention of the servo drives was adjusted. I also note that when measuring the outputs on the kanalog board, there was a low-voltage signal for this output. There was no voltage at the other outputs. During the loading of the computer, the axis was raised by a certain amount. When the INIT button was pressed, similar phenomena occurred. The milling machine was turned into a plate compactor weighing several tons. This was not always repeated. Before changing the firmware, the machine periodically started to launch the X-axis into resonance. We tried to power the servo amplifiers not immediately upon power-up, but a little later. Sometimes it helped, sometimes not.
With the new firmware it happens a little differently: we feed the servo motors, a familiar sound appears when the axis was traveling before. Now her movement is imperceptible, I admit that he is not. Next, turn on the computer, after about a second hear a click (the motors are in position), before this click was either much later, or by pressing the INIT button.
At the moment this problem seems to be gone. Or fell asleep, I do not know.
I apologize if not very clearly written. English is not my native language, I'm translating Google translator.

[TomKerekes]

Hi Denis#,

Sorry I don't understand much of that. But you should keep the Drives completely disabled on power up and have the Init program enable them. Also if you set a small Max Following Error then if the axis moves improperly for any reason it should disable the axis. You might read this:

http://www.dynomotion.com/wiki/index.ph ... _up_Issues