Jump to content
OMRON Forums

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


Recommended Posts

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

1d9e938e9ced713d179de1e452a3e59.jpg

Edited by aoligei
Link to comment
Share on other sites

  • aoligei changed the title to How to use user-defined servo algorithm in power pmac without reporting errors

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.

Link to comment
Share on other sites

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

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

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

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

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

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

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
 

Link to comment
Share on other sites

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.

Link to comment
Share on other sites

  • 2 weeks later...

Thanks Steve, I did as you said not to set up Ethercat in the header file and then set up the system servo algorithm in torque mode, but I can only do the tuning steps under open loop testing vs sinusoidal testing, my current loop is tuned It doesn't work and the image is very rubbish. Why is that? In addition, my ide version is 4.4.1.7, the platform is POWER PMAC UMAC, and the firmware version is 2.6.1.0

微信截图_20240508151740.png

微信截图_20240508151809.png

Link to comment
Share on other sites

An EtherCAT drive in torque mode can only accept PID position loop tuning. The current loop is in the drive and must be tuned by the drive manufacturer’s software.

The plots you show might indicate the EtherCAT setup is not complete. You must first have a complete EtherCAT setup in torque mode without the application of any user servo and the PMAC position loop should have minimal – this is provided in the IDE’s motor topology setup screens. Have you followed the step-by-step instructions in the IDE manual for EtherCAT setup: “EtherCAT Network and Motor Setup”. You must complete this setup through the “Basic Tuning” and “Commission” boxes. If you are not able to jog the motor at this point the setup is not fully complete.

Then you can apply a user servo and attempt to tune that. 

Please note that the Forums is not a venue for technical support or PMAC training. Please contact your local Omron office for this. They should be your primary choice. They can “escalate” any issues back to the US domestic group if needed. 
 

Link to comment
Share on other sites

Thanks for the suggestion Steve, after your suggestion I reconfigured the ether cat a bit and didn't use any user servo, and generated a step graph in torque mode that looks pretty good, give it a look, next I Do you want to continue tweaking or can you try using the user servo, thanks Steve_20240514095711.thumb.png.95c080ab36f30ec6e4c1392efc77eeda.png

Link to comment
Share on other sites

Posted (edited)

Normally, when EtherCAT works in CST mode, the driver will complete the current loop control and motor commutation control by itself.

You can place speed loop and position loop control in the controller using user-defined servo algorithms.

But due to the huge delay of the EtherCAT bus, your control performance will be far worse than that of the servo drive working with CSP mode.

 

Edited by MoMo
Link to comment
Share on other sites

I would like to know what issues I need to pay attention to when I want to use the custom servo algorithm code shown in your company's speech video? Now the code using the lecture video always shows that the maximum position error limit is exceeded. Why is this?What you said about moving the position and speed loops up into the controller is too difficult for me, I just want to try out a simple custom servo algorithm, do I need to write the speed and position loops in my own algorithm? ?

/*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;
}
 

Link to comment
Share on other sites

As I previously said, the Omron Forums is not a venue for technical support issues. You should contact your local Omron representatives for technical support. They can “escalate” back to the US domestic group if needed.

Having said that, your previous tuning plots are not very good but would indicate that CST mode is probably set up correctly.

You show a user phase algorithm (for motor commutation). Commutation is not supported in any EtherCAT mode. Only the “user servo algorithm” can be used.

When using CST mode you should be able to use the “very simple PID servo routine” (the “company's speech video” as you call it) documented in the “Power PMAC User’s Manual” in the chapter “Writing C Functions and Programs in Power PMAC” starting on page 792. Specifically see the section “User-Written Servo Routines” starting on page 803. You will need to use the IDE’s tuning to retune your “user servo algorithm” as it will not be the same as the PMAC standard servo algorithm (Sys.ServoCtrl). 

In any other EtherCAT mode you should only use the jog functions from the C-API functions under the “rtpmacapi.h Rt/Gp Library Functions”:
JogPosition()
JogSpeed()
JogTrigger()

I can only comment on the “mechanics” of implementing user servo C-programming not on content as I am not a servo specialist.
 

Link to comment
Share on other sites

Thanks again, Steve. I am just a graduate student. After our laboratory purchased Omron products, technical support came for training. I had not yet been admitted to graduate school, so I did not learn anything. However, when I contacted local technical support, I was charged a fee. That’s why I came to the forum to seek help because of the huge expense. Thank you for your help.

  • Like 1
Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

×
×
  • Create New...