[etherlab-users] Controlling motor drivers

Graeme Foot Graeme.Foot at touchcut.com
Wed Jan 20 22:54:16 CET 2016


Hi,

To get information about what PDO’s can be mapped for a device you will need to look into the in device documentation.

Some devices do not allow reconfiguration so you must match what is already there (cstruct command fine for this).  Some devices allow you to map different PDO’s, but only from preconfigured group options (the EL7031 falls into this category).  And lastly some devices allow you to configure any PDO item in any order as long as you have enough space to do so, however with these devices not all items can be used as PDO’s.

If you have a Beckhoff module you can easily download its documentation from their site using the url: http://www.beckhoff.com/<model_number>/
Eg: http://www.beckhoff.com/EL7031/

[cid:image001.png at 01D15433.5C489160]

Then click on the “Documentation (CHM)” link and it will download the documentation.  Note: being a chm file you will need to open it in Windows from a local drive.  The content will also be blocked until you select “Unblock” from right-click, properties:
[cid:image002.png at 01D15433.5C489160]


Have a look at the “Object description and parameterization” section (in this case it is under: EL70x1 - Stepper Motor Interfaces, Configuration with the TwinCAT System Manager, EL7031, ...)

Have a look at the 0x160x groups.  These are the preconfigured read/write item groups.  The value of each item tells you which PDO item addresses will be used.
Then have a look at the 0x1a0x groups.  These are the preconfigured readonly item groups.

Note: there may be restrictions as to which groups you can use together.  This is generally found in the XML device description files.  These files are what tells TwinCAT what configuration options are valid.  Also note that different module firmware revisions will probably support different options and PDO mappings.  If over time you need to support different firmware revisions you will need to read the modules revision from your app and match the correct configuration.


It’s also a good idea to get the Beckhoff XML device descriptions from: http://download.beckhoff.com/download/Config/EtherCAT/XML_Device_Description/Beckhoff_EtherCAT_XML.zip

You can view them in Internet Explorer, but they are large and really slow.  I generally load them into Notepad++ and delete the irrelevant modules and revisions (without saving) to speed things up and make searching easier.

In the XML file find the “AlternativeSmMapping” nodes.  This tells you what various “modes” the module is designed to be used in and what PDO mapping groups are used.

For my application I wanted “Position control”, so I used the following:
  <AlternativeSmMapping>
    <Name>Position control</Name>
    <Sm No="2">
      <Pdo>#x1601</Pdo>
      <Pdo>#x1602</Pdo>
      <Pdo>#x1603</Pdo>
    </Sm>
    <Sm No="3">
      <Pdo>#x1a01</Pdo>
      <Pdo>#x1a03</Pdo>
    </Sm>
  </AlternativeSmMapping>

But I also didn’t want to use latching, so in my case I left off 0x1601 and 0x1a01.



As to changing moving direction, you just need to change the Position Target in the negative direction.

Eg, if you want to move from position 0 to 20 and then back to 0 you would output the positions (one each scan):

0
0   // accelerate moving forward
1
3
6
10  // decelerate moving forward
14
17
19
20  // pause
20
20
20  // accelerate moving reverse
19
17
14
10  // decelerate moving reverse
6
3
1
0
0
...

Note: If the above scan time is 1000Hz then the above would be accelerating/decelerating at 1000 counts/second.

It is up to you to come up with the trajectory of the motor (ie where you want to go) and the kinematics (ie how fast do you want to go and accelerate / decelerate).  Then for each scan period output the target position.

You can use kinematic equations to figure out a simple kinematic profile (eg from: http://www.physicsclassroom.com/class/1DKin/Lesson-6/Kinematic-Equations).  However you may need to get into some more advanced mathematics if you want to introduce smoothing factors (jerk etc.)



However, if you want to move forward and the motor goes in reverse for your mechanical configuration (and vice-versa) then the motor has a Reverse configuration option (0x8012:09, Invert motor polarity).


Regards,

Graeme Foot
Kinetic Engineering Design Ltd.


From: Paul Mulligan [mailto:mulligan252 at yahoo.ie]
Sent: Thursday, 21 January 2016 3:32 a.m.
To: Graeme Foot
Subject: Re: [etherlab-users] Controlling motor drivers

Hi Graeme,

Thanks alot for that.

I'm completely new to Ethercat. Where can I find the SDO entries with indexes that you mentioned, for any device?
Up until now I have only been using the Ethercat command line tool to retrieve the c-structures relating to the PDOs for the slave device and then copying and pasting this information into my code.

Also, in order to change moving direction of the motor, do I need to change an SDO value ?

Regards,
Paul

On Tuesday, 19 January 2016, 22:55:37, Graeme Foot <Graeme.Foot at touchcut.com<mailto:Graeme.Foot at touchcut.com>> wrote:

Hi,

We also use the EL7041 in production, but have also used an EL7031 for testing.

The default the PDO mapping is for velocity control.  You need to change it to use the position control PDO's.


For the EL7031 the PDO setup I use is:

ec_pdo_entry_info_t EL7031_pdoEntries[] = {
    // 0x1602, stepper control (0)
    {0x7010, 0x01, 1},    // Enable
    {0x7010, 0x02, 1},    // Reset
    {0x7010, 0x03, 1},    // Reduce torque
    {0x0000, 0x00, 5},    //  spacer
    {0x0000, 0x00, 8},    //  spacer

    // 0x1603, stepper pos (5)
    {0x7010, 0x11, 32},  // Target position


    // 0x1a03, stepper status (6)
    {0x6010, 0x01, 1},    // Ready to enable
    {0x6010, 0x02, 1},    // Ready
    {0x6010, 0x03, 1},    // Warning
    {0x6010, 0x04, 1},    // Error
    {0x6010, 0x05, 1},    // Moving positive
    {0x6010, 0x06, 1},    // Moving negative
    {0x6010, 0x07, 1},    // Torque reduced
    {0x0000, 0x00, 1},    //  spacer
    {0x0000, 0x00, 3},    //  spacer
    {0x6010, 0x0c, 1},    // Digital input 1
    {0x6010, 0x0d, 1},    // Digital input 2
    {0x1c32, 0x20, 1},    // Sync error
    {0x0000, 0x00, 1},    //  spacer
    {0x1803, 0x09, 1},    // *** unknown ***

};

ec_pdo_info_t EL7031_pdos[] = {
    {0x1602, 5, EL7031_pdoEntries + 0},
    {0x1603, 1, EL7031_pdoEntries + 5},
    {0x1a03, 14, EL7031_pdoEntries + 6},
};

ec_sync_info_t EL7031_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, EL7031_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, EL7031_pdos + 2, EC_WD_DISABLE},
    {0xff}
};


PDO Item #5 {0x7010, 0x11, 32} is the Target position.


You will want to pre-configure some of the SDO values:

  maxCurrent          0x8010, 0x01
  reducedCurrent      0x8010, 0x02
  nominalVoltage      0x8010, 0x03
  motorCoilResistance 0x8010, 0x04
  motorFullSteps      0x8010, 0x06
  maxSpeedRange      0x8012, 0x05
  reversed            0x8012, 0x09


On initialisation you need to set the motor to use the Position Controller mode of operation (3):

  ecrt_slave_config_sdo8(dev->slaveConfig, 0x8012, 0x01, 3);


You may also want to set up the distributed clock (eg:)

  ecrt_slave_config_dc(dev->slaveConfig, 0x0300, g_app.scanTimeNS, 500000, 0, 0);


You will probably want to set up a motor enable state machine but the guts of it is that you will want to:

- before enabling, if the error status bit (PDO Item #9 {0x6010, 0x04, 1}) is set
  set the faultReset control bit (PDO Item #1 {0x7010, 0x02, 1}) for around 100ms

- ensure the readyToEnable status bit (PDO Item #6 {0x6010, 0x01, 1}) is on
  if there is no error set the enable control bit (PDO Item #0 {0x7010, 0x01, 1})
  wait until the ready status bit (PDO Item #7 {0x6010, 0x02, 1}) is on


Once enabled you need to update the target position (PDO Item #5 {0x7010, 0x11, 32}) every scan cycle with the position you want to go to.  It is up to you to create a motion profile for the motor and then figure out what the target position is at each time increment.


Lastly, if you get the warning or error status bits set you can look up what the error or warning is via the 0xA010 SDO diagnostic bits (0xA010:0x01 - 0xA010:0x11).  Note: these are only available via SDO access, so what I do is have a state machine that looks them up if the warning or error status bits are set.


Hope this helps.


Regards,

Graeme Foot
Kinetic Engineering Design Ltd.



-----Original Message-----
From: etherlab-users [mailto:etherlab-users-bounces at etherlab.org<mailto:etherlab-users-bounces at etherlab.org>] On Behalf Of Thomas Bitsky Jr
Sent: Wednesday, 20 January 2016 3:57 a.m.
To: Paul Mulligan; etherlab-users at etherlab.org<mailto:etherlab-users at etherlab.org>
Subject: Re: [etherlab-users] Controlling motor drivers

I use the EL7041 often. He’s a quick copy/paste of how I make it move to position. The code snippet below writes to EtherLAB through a library I wrote, but the tasks required should be pretty obvious.

Thanks!
============

static int
stateIdle(void* lp)
{
    EL7041StepperInterface* p;
    EL7041_ENTER(p, lp);

    if (p->doMove)
    {
        p->doMove = false;


        if ( !lcxReadPdoBit(p->pdoOffset + IX_READY) )
        {
            PRINT( "EL7041.%s not ready for motion. Cancelling.\n",
                __FUNCTION__ );
            return StateMachineRunning;
        }




        lcxWritePdoUInt16(p->pdoOffset + QW_SETACCEL, p->targetAccel );
        lcxWritePdoUInt16(p->pdoOffset + QW_SETDECEL, p->targetDecel );
        lcxWritePdoUInt16(p->pdoOffset + QW_SETVELOCITY, p->targetVelocity );

        p->fsm = &stateWaitWriteMotionParameters;
        return StateMachineTransition;
    }

}

static int
stateWaitWriteMotionParameters(void* lp) {
    EL7041StepperInterface* p;
    EL7041_ENTER(p, lp);

    p->fsm = &stateWriteStartType;
    return StateMachineTransition;
}


static int
stateWriteStartType(void* lp)
{
    EL7041StepperInterface* p;
    EL7041_ENTER(p, lp);

    lcxWritePdoUInt32(p->pdoOffset + QW_SETTARGETPOSITION, p->setTargetPosition );
    lcxWritePdoUInt32(p->pdoOffset + QW_SETSTARTTYPE, p->setStartType );

    p->ton = TON_ENDTIME(100);

    p->fsm = &stateWaitWriteStartType;
    return StateMachineTransition;
}


static int
stateWaitWriteStartType(void* lp)
{
    EL7041StepperInterface* p;
    EL7041_ENTER(p, lp);

    if (TON_ISDONE(p->ton))
    {
        /*
        * ALERT!
        * This command should start motion on the axis.
        */
        lcxWritePdoBit(p->pdoOffset + QX_CONTROLEXECUTE, true);

        p->ton = TON_ENDTIME(500);
        p->fsm = &stateWaitExecute;
        return StateMachineTransition;
    }

    return StateMachineRunning;
}



Thomas C. Bitsky Jr. | Lead Developer
ADC | automateddesign.com <http://automateddesign.com/>

Follow ADC news and media:
Facebook <https://facebook.com/automateddesigncorp> | Twitter <https://twitter.com/ADCSportsLogic> | YouTube <https://www.youtube.com/user/ADCSportsLogic>






From:  etherlab-users <etherlab-users-bounces at etherlab.org<mailto:etherlab-users-bounces at etherlab.org>> on behalf of Paul Mulligan <mulligan252 at yahoo.ie<mailto:mulligan252 at yahoo.ie>>
Reply-To:  Paul Mulligan <mulligan252 at yahoo.ie<mailto:mulligan252 at yahoo.ie>>
Date:  Tuesday, January 19, 2016 at 7:49 AM
To:  "etherlab-users at etherlab.org<mailto:etherlab-users at etherlab.org>" <etherlab-users at etherlab.org<mailto:etherlab-users at etherlab.org>>
Subject:  [etherlab-users] Controlling motor drivers


Hi,  (Formatting was wrong on previous post)

We bought various Beckoff modules for Digital in , Digital out, Analogue in and Stepper Motor control.

I am able to communicate with with the digital I/O and analogue input slave modules from Ethercat master IGH 1.5.2 without problems, but the stepper  motor controller (EL7031) is more complicated.

Below is a list of the PDO entries for the motor controller retrieved from the Ethercat bus using the command line tool with the cstruct option.

I am only able to set the velocity and then set the enable bit to start the motor running. I have no idea how to command the motor to just turn  a certain amount of steps and change direction etc. I have tried to set a counter value but it doesn't make a difference. Also, if I set the bits to move positive or move negative, it doesn't change direction as I would expect. I can't seem to find any documentation  or examples on how to control motors with this module EL7031. If anyone has any information or knowledge , I would really appreciate your help. Thanks



ec_pdo_entry_info_t slave_6_pdo_entries[] = {
    {0x0000, 0x00, 1}, /* Gap */
    {0x7000, 0x02, 1}, /* Enable latch extern on positive edge */
    {0x7000, 0x03, 1}, /* Set counter */
    {0x7000, 0x04, 1}, /* Enable latch extern on negative edge */
    {0x0000, 0x00, 4}, /* Gap */
    {0x0000, 0x00, 8}, /* Gap */
    {0x7000, 0x11, 16}, /* Set counter value */
    {0x7010, 0x01, 1}, /* Enable */
    {0x7010, 0x02, 1}, /* Reset */
    {0x7010, 0x03, 1}, /* Reduce torque */
    {0x0000, 0x00, 5}, /* Gap */
    {0x0000, 0x00, 8}, /* Gap */
    {0x7010, 0x21, 16}, /* Velocity */
    {0x0000, 0x00, 1}, /* Gap */
    {0x6000, 0x02, 1}, /* Latch extern valid */
    {0x6000, 0x03, 1}, /* Set counter done */
    {0x6000, 0x04, 1}, /* Counter underflow */
    {0x6000, 0x05, 1}, /* Counter overflow */
    {0x0000, 0x00, 3}, /* Gap */
    {0x0000, 0x00, 4}, /* Gap */
    {0x6000, 0x0d, 1}, /* Status of extern latch */
    {0x6000, 0x0e, 1}, /* Sync error */
    {0x0000, 0x00, 1}, /* Gap */
    {0x6000, 0x10, 1}, /* TxPDO Toggle */
    {0x6000, 0x11, 16}, /* Counter value */
    {0x6000, 0x12, 16}, /* Latch value */
    {0x6010, 0x01, 1}, /* Ready to enable */
    {0x6010, 0x02, 1}, /* Ready */
    {0x6010, 0x03, 1}, /* Warning */
    {0x6010, 0x04, 1}, /* Error */
    {0x6010, 0x05, 1}, /* Moving positive */
    {0x6010, 0x06, 1}, /* Moving negative */
    {0x6010, 0x07, 1}, /* Torque reduced */
    {0x0000, 0x00, 1}, /* Gap */
    {0x0000, 0x00, 3}, /* Gap */
    {0x6010, 0x0c, 1}, /* Digital input 1 */
    {0x6010, 0x0d, 1}, /* Digital input 2 */
    {0x6010, 0x0e, 1}, /* Sync error */
    {0x0000, 0x00, 1}, /* Gap */
    {0x6010, 0x10, 1}, /* TxPDO Toggle */ }; _______________________________________________
etherlab-users mailing list

etherlab-users at etherlab.org<mailto:etherlab-users at etherlab.org>

http://lists.etherlab.org/mailman/listinfo/etherlab-users


-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.etherlab.org/pipermail/etherlab-users/attachments/20160120/ffa74d13/attachment-0003.htm>
-------------- next part --------------
A non-text attachment was scrubbed...
Name: image001.png
Type: image/png
Size: 139639 bytes
Desc: image001.png
URL: <http://lists.etherlab.org/pipermail/etherlab-users/attachments/20160120/ffa74d13/attachment-0008.png>
-------------- next part --------------
A non-text attachment was scrubbed...
Name: image002.png
Type: image/png
Size: 11689 bytes
Desc: image002.png
URL: <http://lists.etherlab.org/pipermail/etherlab-users/attachments/20160120/ffa74d13/attachment-0009.png>


More information about the Etherlab-users mailing list