How to use user-defined servo algorithm in power pmac without reporting errors

[aoligei]

Now I want to use the custom servo algorithm of power pmac. After using the ether cat communication motor, After changing the usrcode.c file to compile mode，I added a custom adrc algorithm to usrcode.c.            double adrc(struct MotorData *Mptr)
{
_desvel=(Mptr->DesVel/sh)/1000;  
_actvel=(Mptr->ActVel/sh)/1000;  /
_actpos=(Mptr->ActPos - Mptr->HomePos)/1000;/
_poserror=(Mptr->PosError)/1000;
  //open-loop routine
  if(Mptr->ClosedLoop)
  {
   
    e1 = (_actpos - z1);
    z1 = (3*wo*e1 + z2)*sh + z1;
    z2 = (3*pow(wo,2)*e1 + z3 + b0*_u)*sh + z2;
    z3 = (pow(wo,3)*e1)*sh + z3;

//    e1 = Mptr->ActPos - Mptr->DesPos;
//    z1 = (z2-3*wo*(z1-e1))*sh+z1;
//    z2 = (z3-3*pow(wo,2)*(z1-e1)+b0*_u)*sh+z2;
//    z3 = (-pow(wo,3)*(z1-e1))*sh+z3;
    //PD
//    _kpp = wc*wc;
//    _kdd = 2*xi*wc;
    _u0 = wc*wc*(_poserror) + (2*xi*wc)*(_desvel*switch_control - _actvel);
    _u  = (_u0 - z3 ) / b0;
    if(_u>1)
    {
        _u=1;
    }
    if(_u<-1)
    {
       _u=-1;
    }/
    uout = (_u*32768)/18/sqrt(2)/1.25;
    return uout;
  }
  else 
  { // Open loop mode
  z1=_actpos,z2=0,z3=0,e1=0,_u=0,_u0=0,uout=0;
  return 0.0; // Zero output
  }
}

Then the code was added to usrcode.h    double adrc(struct MotorData *Mptr);
EXPORT_SYMBOL(adrc);

Below are my global definitions.pmh

Sys.WpKey = $AAAAAAAA
    GLOBAL z1=0,z2=0,z3=0,e1=0,_u=0,_u0=0,sh=1/8000,uout=0,_actvel=0,_actpos=0,_poserror=0,_desvel=0;
GLOBAL wc=60,wo=500,xi=1,b0=3,switch_control=1;
    Motor[1].pAmpEnable = ECAT[0].IO[3].Data.a
    Motor[1].AmpEnableBit = 0
    Motor[1].pLimits = 0
    Motor[1].pDac = ECAT[0].IO[0].Data.a
    EncTable[1].pEnc = ECAT[0].IO[4099].Data.a
    EncTable[1].pEnc1 = Sys.pushm
    EncTable[1].ScaleFactor = 1
    EncTable[1].Type = 1
    EncTable[2].Type = 0
    Motor[1].pEnc = EncTable[1].a
    Motor[1].pEnc2 = EncTable[1].a
    Motor[1].pAmpFault = ECAT[0].IO[4102].Data.a
    Motor[1].AmpFaultBit = 3
    Motor[1].AmpFaultLevel = 3
    Motor[1].MaxDac = 28000
    Motor[1].DacShift = 0
    Motor[1].ctrl = UserAlgo.ServoCtrlAddr[1]
    Motor[1].ServoCtrl = 1
    Sys.WpKey = $0

Then I selected the user servo setting as , built and downloaded all the  realtime routines and  the download was successful, but when I enabled the motor, the motor jumped and reported an error Motor Fetal following error set, as shown below, what is going on? Someone can help me? Thanks

Edited April 10 by aoligei

[steve.milici]

The EtherCAT setup should already be completed by the IDE’s System Setup. You should not be making changes in a script header file. Implementation of “manual” EtherCAT setup is not supported other than simple power on modifications, and these should not be in header files. These should be in power on PLC code.

Also note that a user-defined servo algorithm will only be possible for a motor setup in CST (Cyclic Synchronous Torque) mode.

[aoligei]

steve

The operation of adding my servo algorithm is based on "ASSOCIATING MOTORS WITH USER-WRITTEN SERVO AND PHASE ALGORITHMS" in power PMAC IDE and "Selecting a Servo Algorithm" in power PMAC user manual. Which part of the Ethercat settings are you referring to? All my Ethercat configurations are placed in the motor configuration file (global.pmh). I understand that there are no relevant Ethercat settings in the code of my header file. If there are, please point them out.

The user servo algorithm I defined is the position loop P controller. Shouldn't I use it in the drive CSV mode? Doesn't this form a three-loop control system of the position loop in the PMAC + the speed loop and current loop in the driver? How should we understand that the user servo algorithm you mentioned can only be applied in CST mode?

Thanks

[steve.milici]

EtherCAT configurations should not be in a Script header file (.pmh). These should be in a custom configuration file in the “Configuration” folder and downloaded to PMAC before the Project “Build and Download”.

You should follow the step-by-step instructions in the IDE manual for your version of the IDE to set up an EtherCAT motor.

What is your IDE version? What is your PMAC platform and firmware version?

Note that a user servo algorithm can only be applied to a well-tuned EtherCAT motor in CST mode.

[steve.milici]

After further review, you should be able to create a user servo algorithm for an EtherCAT motor in all modes (CSP, CSV or CST). It is your responsibility to make sure the retuned value is the proper type (position, velocity, or torque) and scaled properly for your drive. I suspect the reason you get the amplifier fault is the result of returning a “bad” value in your routine. I notice you have a “return 0.0;” at the end of your routine. This may not be what you desire.

Also, as previously said in a well-constructed EtherCAT IDE project there should be no need to have EtherCAT settings in a global include file.

[aoligei]

Thanks Steve, the return0.0 at the end of the program is the setting when the system is in open loop mode. I would like to ask if this has any impact? And when I used the servo algorithm in your company's lectures and teachings, it also showed that the maximum following error limit was exceeded. I followed what you said and the ether cat setting was not in the .pmh file, but an error still occurred. Can you continue to give me some advice? Thank you so much. Below is the code I used for your company's presentation.Is there anything else I want to continue revising?

usrcode.c

/*For more information see notes.txt in the Documentation folder */
#include "usrcode.h"
#define _PPScriptMode_        // for enum mode, replace this with #define _EnumMode_    

#include "../Include/pp_proj.h"

extern struct SHM        *pshm;  // Pointer to shared memory
extern volatile unsigned *piom;  // Pointer to I/O memory
extern void              *pushm; // Pointer to user memory

void MyPhaseAlg(struct MotorData *Mptr)
{
int PresentEnc, PhaseTableOffset;
float *SineTable;
double DeltaEnc, PhasePos, IqVolts, IaVolts, IbVolts;
PresentEnc = *Mptr->pPhaseEnc; // Read new rotor position
DeltaEnc = (double) (PresentEnc - Mptr->PrevPhaseEnc); // Compute change and convert to floating-point
Mptr->PrevPhaseEnc = PresentEnc; // Store new rotor position for next cycle
PhasePos = Mptr->PhasePos + Mptr->PhasePosSf * DeltaEnc; // Scale change to sine table increments and accumulate
if (PhasePos < 0.0) PhasePos +=2048.0 ; // Negative rollover?
else if (PhasePos >=2048.0 ) PhasePos -=2048.0 ; // Positive rollover?
PhaseTableOffset = (int) PhasePos; // Table entry index
Mptr->PhasePos = PhasePos; // Store in structure for next cycle
IqVolts = Mptr->IqCmd; // Get torque (quadrature) command from servo output
SineTable = Mptr->pVoltSineTable; // Start address of lookup table
IaVolts = IqVolts * SineTable[(PhaseTableOffset + 512) & 2047]; // Compute Phase A command
IbVolts = IqVolts * SineTable[(PhaseTableOffset + Mptr->PhaseOffset + 512) & 2047]; // Compute Phase B command
Mptr->pDac[0] = ((int) (IaVolts * 65536)); // Scale, fix, and output Phase A
Mptr->pDac[1] = ((int) (IbVolts * 65536)); // Scale, fix, and output Phase B
}

double MyServoAlg(struct MotorData *Mptr)
{
double ctrl_effort;
if (Mptr->ClosedLoop) { // Servo active in closed loop?
ctrl_effort = Mptr->Servo.Kp * Mptr->PosError - Mptr->Servo.Kvfb * Mptr->ActVel; // PD terms
Mptr->Servo.Integrator += Mptr->PosError * Mptr->Servo.Ki; // I term
ctrl_effort += Mptr->Servo.Integrator; // Combine
return ctrl_effort; // Return value for automatic handling
}
else { // Open loop mode
Mptr->Servo.Integrator = 0.0; // Clear integrator
return 0.0; //} Zero output

}

void CaptCompISR(void)
{
    unsigned *pUnsigned = pushm;
    *pUnsigned = *pUnsigned + 1;
}

double GetLocal(struct LocalData *Ldata, int m)
{
    return *(Ldata->L + Ldata->Lindex + m);
}

void SetLocal(struct LocalData *Ldata, int m, double value)
{
    *(Ldata->L + Ldata->Lindex + m) = value;
}

double *GetLocalPtr(struct LocalData *Ldata, int m)
{
    return (Ldata->L + Ldata->Lindex + m);
}

double CfromScript(double cfrom_type, double arg2, double arg3, double arg4, double arg5, double arg6, double arg7, struct LocalData *Ldata)
{
    int icfrom_type = (int)cfrom_type;
    double *C, *D, *L, *R, rtn; // C, D, R - only needed if doing Kinematics

    C = GetCVarPtr(Ldata);  // Only needed if doing Kinematics
    D = GetDVarPtr(Ldata);  // Only needed if doing Kinematics
    L = GetLVarPtr(Ldata);  // Only needed if using Ldata or Kinematics
    R = GetRVarPtr(Ldata);  // Only needed if doing Kinematics
    rtn = -1.0;
    return rtn;
}

[steve.milici]

I see now that this is the code from the training presentation and has the proper “return ctrl_effort;” statement. The “return 0.0;” statement would be correct usage for open loop. Note that the full PID user servo code would only apply to an EtherCAT system that is in CST mode (and well-tuned). If in CSV mode you should only be calculating a “velocity” output.

There would be no usage of any commutation in a user phase routine as this is not done in PMAC when using EtherCAT.

You should describe what your needed “end goal” is, then we can provide better comments.

[aoligei]

Thanks Steve
The speech code of your company that I am using now is in CST mode, but an error of exceeding the maximum following error occurred. You said last time that I can create servo algorithms for all modes. I now want to use custom ones in CSP mode. Servo algorithm, I would like to ask if the operation I demonstrated above is correct. Modify it in .c and .h, or my servo algorithm is wrong and causes the motor to report an error.

[aoligei]

No matter how I modify the code I always encounter errors that exceed the maximum following error, even using your company's lecture code, whether in CPT or CST mode, it drives me crazy, even if I write a very simple instruction, and Including position limiting, it still exceeds the maximum following error. I can't even use my motion program. It will cause an amplifier failure error during the tuning process. What should I do? Thanks Steve

[steve.milici]

You must first have a functional and well-tuned EtherCAT motor setup in CST mode without user servo enabled. If the motor is not well-tuned in PMAC this could be the cause.

Post your tuning step-move and parabolic plots to review.

Also you should only use the “standard” user servo code as published in the Power PMAC User’s Manual.

[aoligei]

Thanks Steve
I am currently using the system servo algorithm in CST mode. The motor configuration steps are completely in accordance with the manual and the generated files. However, when I complete the configuration, activate ethercat and click jog, an error exceeding the fatal error limit will still occur. No matter Even if I add a motion program, even the motor cannot return to zero. Is it a hardware problem or a problem with my operation method? This problem will not occur in position mode and speed mode. Thank you for your guidance.

Sys.WpKey = $AAAAAAAA
    Motor[1].pAmpEnable = Slave_1001_CDHD_1001_6040_0_ControlWord.a
    Motor[1].AmpEnableBit = 0
    Motor[1].pLimits = 0
    Motor[1].pDac = Slave_1001_CDHD_1001_6071_0_Targettorque.a
    EncTable[1].pEnc = Slave_1001_CDHD_1001_6064_0_Positionactualvalue.a
    EncTable[1].pEnc1 = Sys.pushm
    EncTable[1].ScaleFactor = 1
    EncTable[1].Type = 1
    EncTable[2].Type = 0
    Motor[1].pEnc = EncTable[1].a
    Motor[1].pEnc2 = EncTable[1].a
    Motor[1].pAmpFault = Slave_1001_CDHD_1001_6041_0_StatusWord.a
    Motor[1].AmpFaultBit = 3
    Motor[1].AmpFaultLevel = 3
    Motor[1].MaxDac = 1000
    Motor[1].DacShift = 16
    Motor[1].ctrl = Sys.servoctrl
    Motor[1].ServoCtrl = 1
    Sys.WpKey = $0
 

[steve.milici]

As I previously said we cannot support manual setup of EtherCAT in script header files. Only the IDE’s created configuration files in the “Configuration” folder (as documented in the IDE manual for the IDE version expressed): ECATConfig.cfg, Eni.xml, Systemsetup.cfg.

Simple power on modifications could be acceptable but these should not be in header files. These should be in power on PLC code with proper “verification” code.

Please answer the previously asked questions:

What is your IDE version?

What is your PMAC platform and firmware version?

To verify you have a correct, functional, and well-tuned EtherCAT setup post your tuning step-move and parabolic plots to review, without enabling the user servo code. To provide useful commentary I will need this information along with answers to the above questions.

Also note that your EtherCAT drive manufacturer’s software must properly set up the current loop in the drive. There may be other “torque” related SDOs that need specific settings per their documentation. Such as:

Object 607Fh: Max profile velocity

Object 6072h: Max torque

Please refer to your drive manufacturer’s documentation for details.