Hi,
I do not have Elmo servo drives, but I have Kollmorgen AKD servo drives running
with official IgH EtherLab EtherCAT master on Linux kernel 3.14 PREEMPT-RT
My control loop runs at 500Hz (but I have tested that it works also at 1 KHz)
I have noticed that in the DS402 state machine, the status word does not change
immediately when you send a new control word. Therefore, I have to check the
status word before sending a new control word.
It makes the control loop slightly more complex, but it works.
See my example for AKD servo drive in attachment
Regards
--
Sebastien BLANCHET
On 2/5/20 3:22 PM, BUSSIERES Vincent wrote:
Hello,
I’m using etherlab master with *Gavin **unofficial patch for kernel 4.9 *to
command an Elmo servodrive using CiA DSP 402 protocol.
I mapped useful PDOs to command the servodrive. These PDO include controlword
and statusword objects below:
The master cycle loop period is 1 ms.
Reading object seems to be OK, I can read successfully encoder position
(Position Actual Value).
But when I change controlword value, it doesn’t seem to be any effect. The state
in state machine below doesn’t change (statusword doesn’t change).
However when I change controlword value using sdo download in ethercat command
lines tool, statusword immediately change. It seems to be a problem with writing
PDOs.
When I increase master cycle loop period to 100ms, controlword updates seem to
be take into account after several send commands.
You can see in attachement a part of my source code in C++ / Qt. Init method for
PDO mapping and a while loop for send/receive PDO.
Best regards
*Vincent BUSSIERES*
*Responsable Technique Logiciel*
**
1572337113342
/24 Avenue de Pasleck/
/16400 LA COURONNE/
/-----------------------------------/
/www.hemeria-group.com
/*****************************************************************************
* Example of Kollmorgen AKD servo drive setup with IgH EtherCAT Master library
* that starts the AKD servo drive in digital torque mode
*
* Licence: the same than IgH EtherCAT Master library
*
* ABSTRACT:
*
* This small program shows how to configure a Kollmorgen AKD servo drive
* with IgH EtherCAT Master library. It illustrates the flexible PDO mapping
* and the DS402 command register programming in order to start the servo drive
* in digital torque mode, with a small constant torque.
*
* Because the Kollmorgen AKD servo drive follows the DS402 standard, several
* sections of this example may apply also to other servo drive models.
*
* The code is based on a previous example from IgH EtherCAT Master
*
* Author: Sebastien BLANCHET
* Date: February 16, 2015
* Compile with:
gcc akd_torque.c -Wall -I/opt/etherlab/include -lethercat \
-L/opt/etherlab/lib -o akd_torque
****************************************************************************/
#include "ecrt.h"
#include <stdio.h>
#include <unistd.h>
#include <sched.h>
#include <sys/mman.h>
/* Prerequises: the servo drive is already configured with AKD Workbench:
DRV.TYPE 2
DRV.CMDSOURCE 1
DRV.OPMODE 0
FBUS.PARAM04 1
FBUS.PLLSAMPLEPERIOD 4
Note: To run this example at a lower frequency (for example 100 Hz), disable
fieldbus supervision (FBUS.PARAM04 0) to avoid permanent F125 fault.
*/
/* Application parameters */
#define TASK_FREQUENCY 1000 /* Hz */
#define TIMEOUT_CLEAR_ERROR (1*TASK_FREQUENCY) /* clearing error timeout */
#define TARGET_TORQUE 100 /* in 1/1000 of nominal torque */
#define AKD_STS_MASK 0xEFF /* mask to remove manufacturer special bits */
#define AKD_STS_SWION_DIS 0x250 /* switched on disabled */
#define AKD_STS_RDY_SWION 0x231 /* ready to switch on */
#define AKD_STS_SWION_ENA 0x233 /* switched on enabled */
#define AKD_STS_ERROR 0x218 /* error */
#define AKD_CMD_ENA_QSTOP 0x00 /* enable quick stop */
#define AKD_CMD_DIS_QSTOP 0x06 /* disable quick stop */
#define AKD_CMD_ENA_SWION 0x07 /* enable switched on */
#define AKD_CMD_ENA_OP 0x0F /* enable operation */
#define AKD_CMD_CLR_ERROR 0x80 /* clear error */
/* EtherCAT */
static ec_master_t *master = NULL;
static ec_domain_t *domain1 = NULL;
static uint8_t *domain1_pd = NULL; /* process data */
static ec_slave_config_t *sc_akd = NULL; /* AKD slave configuration */
#define AkdSlavePos 51,0 /* EtherCAT address on the bus */
#define Kollmorgen_AKD 0x0000006a, 0x00414b44 /* vendor_id, product_id */
/* PDO entries offsets */
static struct {
unsigned int ctrl_word;
unsigned int target_torque;
unsigned int status_word;
unsigned int act_velocity;
} offset;
const static ec_pdo_entry_reg_t domain1_regs[] = {
{ AkdSlavePos, Kollmorgen_AKD, 0x6040, 0, &offset.ctrl_word },
{ AkdSlavePos, Kollmorgen_AKD, 0x6071, 0, &offset.target_torque },
{ AkdSlavePos, Kollmorgen_AKD, 0x6041, 0, &offset.status_word },
{ AkdSlavePos, Kollmorgen_AKD, 0x606C, 0, &offset.act_velocity },
{}
};
/* AKD */
ec_pdo_entry_info_t akd_pdo_entries[] = {
/* RxPdo 0x1600 */
{ 0x6040, 0x00, 16 }, /* DS402 command word */
{ 0x6071, 0x00, 16 }, /* target torque, in 1/1000 of nominal torque */
/* TxPDO 0x1a00 */
{ 0x6041, 0x00, 16 }, /* DS402 status word */
{ 0x606C, 0x00, 32 }, /* actual velocity, in rpm */
};
ec_pdo_info_t akd_pdos[] = {
{ 0x1600, 2, akd_pdo_entries + 0 },
{ 0x1a00, 2, akd_pdo_entries + 2 },
};
ec_sync_info_t akd_syncs[] = {
{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE },
{ 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },
{ 2, EC_DIR_OUTPUT, 1, akd_pdos + 0, EC_WD_DISABLE },
{ 3, EC_DIR_INPUT, 1, akd_pdos + 1, EC_WD_DISABLE },
{ 0xFF }
};
/****************************************************************************/
void cyclic_task()
{
static unsigned int timeout_error = 0;
static uint16_t command = AKD_CMD_ENA_QSTOP;
static int16_t torque = TARGET_TORQUE;
uint16_t status; /* DS402 status register, without manufacturer bits */
float act_velocity; /* actual velocity in rpm */
/* receive process data */
ecrt_master_receive(master);
ecrt_domain_process(domain1);
/* read inputs */
status = EC_READ_U16(domain1_pd + offset.status_word) & AKD_STS_MASK;
act_velocity = EC_READ_S32(domain1_pd + offset.act_velocity)/1000.0;
/* DS402 CANopen over EtherCAT status machine */
if (status == AKD_STS_SWION_DIS && command != AKD_CMD_DIS_QSTOP) {
printf( "AKD: disable quick stop\n" );
command = AKD_CMD_DIS_QSTOP;
} else if (status == AKD_STS_RDY_SWION && command != AKD_CMD_ENA_SWION ) {
printf("AKD: enable switch on\n" );
command = AKD_CMD_ENA_SWION;
} else if ( status == AKD_STS_SWION_ENA && command != AKD_CMD_ENA_OP ) {
printf("AKD: start operation\n" );
command = AKD_CMD_ENA_OP;
} else if ( status == AKD_STS_ERROR && command != AKD_CMD_CLR_ERROR ) {
if ( timeout_error ) {
timeout_error--;
printf( "AKD: ERROR, wait for timeout %d\n", timeout_error );
} else {
timeout_error = TIMEOUT_CLEAR_ERROR;
command = AKD_CMD_CLR_ERROR;
printf( "AKD: clear error now\n" );
}
} else {
/* print actual velocity, once per second */
static time_t prev_second = 0;
struct timeval tv;
gettimeofday( &tv, 0 );
if ( tv.tv_sec != prev_second ) {
printf( "AKD: actual velocity = %.1f rpm\n", act_velocity );
prev_second = tv.tv_sec;
}
}
/* write output */
EC_WRITE_S16( domain1_pd + offset.target_torque, torque );
EC_WRITE_U16( domain1_pd + offset.ctrl_word, command );
/* send process data */
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
/****************************************************************************/
int main(int argc, char **argv)
{
static const int enable_realtime = 1;
if (enable_realtime) {
struct sched_param param;
param.sched_priority = 49;
if ( sched_setscheduler( 0, SCHED_FIFO, ¶m ) == -1) {
perror("sched_setscheduler failed");
}
/* Lock memory */
if( mlockall( MCL_CURRENT|MCL_FUTURE ) == -1) {
perror("mlockall failed");
}
}
master = ecrt_request_master( 0 );
if ( !master ) {
exit( EXIT_FAILURE );
}
domain1 = ecrt_master_create_domain( master );
if ( !domain1 ) {
exit( EXIT_FAILURE );
}
if ( !(sc_akd = ecrt_master_slave_config( master, AkdSlavePos, Kollmorgen_AKD)) ) {
fprintf(stderr, "Failed to get slave configuration for AKD.\n");
exit( EXIT_FAILURE );
}
/* Configure AKD flexible PDO */
printf("Configuring AKD with flexible PDO...\n");
/* Clear RxPdo */
ecrt_slave_config_sdo8( sc_akd, 0x1C12, 0, 0 ); /* clear sm pdo 0x1c12 */
ecrt_slave_config_sdo8( sc_akd, 0x1600, 0, 0 ); /* clear RxPdo 0x1600 */
ecrt_slave_config_sdo8( sc_akd, 0x1601, 0, 0 ); /* clear RxPdo 0x1601 */
ecrt_slave_config_sdo8( sc_akd, 0x1602, 0, 0 ); /* clear RxPdo 0x1602 */
ecrt_slave_config_sdo8( sc_akd, 0x1603, 0, 0 ); /* clear RxPdo 0x1603 */
/* Define RxPdo */
ecrt_slave_config_sdo32( sc_akd, 0x1600, 1, 0x60400010 ); /* 0x6040:0/16bits, control word */
ecrt_slave_config_sdo32( sc_akd, 0x1600, 2, 0x60710010 ); /* 0x6071:0/16bits target torque*/
ecrt_slave_config_sdo8( sc_akd, 0x1600, 0, 2 ); /* set number of PDO entries for 0x1600 */
ecrt_slave_config_sdo16( sc_akd, 0x1C12, 1, 0x1600 ); /* list all RxPdo in 0x1C12:1-4 */
ecrt_slave_config_sdo8( sc_akd, 0x1C12, 0, 1 ); /* set number of RxPDO */
/* Clear TxPdo */
ecrt_slave_config_sdo8( sc_akd, 0x1C13, 0, 0 ); /* clear sm pdo 0x1c13 */
ecrt_slave_config_sdo8( sc_akd, 0x1A00, 0, 0 ); /* clear TxPdo 0x1A00 */
ecrt_slave_config_sdo8( sc_akd, 0x1A01, 0, 0 ); /* clear TxPdo 0x1A01 */
ecrt_slave_config_sdo8( sc_akd, 0x1A02, 0, 0 ); /* clear TxPdo 0x1A02 */
ecrt_slave_config_sdo8( sc_akd, 0x1A03, 0, 0 ); /* clear TxPdo 0x1A03 */
/* Define TxPdo */
ecrt_slave_config_sdo32( sc_akd, 0x1A00, 1, 0x60410010 ); /* 0x6041:0/16bits, status word */
ecrt_slave_config_sdo32( sc_akd, 0x1A00, 2, 0x606C0020 ); /* 0x606c:0/32bits, act velocity */
ecrt_slave_config_sdo8( sc_akd, 0x1A00, 0, 2 ); /* set number of PDO entries for 0x1A00 */
ecrt_slave_config_sdo16( sc_akd, 0x1C13, 1, 0x1A00 ); /* list all TxPdo in 0x1C13:1-4 */
ecrt_slave_config_sdo8( sc_akd, 0x1C13, 0, 1 ); /* set number of TxPDO */
printf("Configuring PDOs...\n");
if ( ecrt_slave_config_pdos( sc_akd, EC_END, akd_syncs ) ) {
fprintf( stderr, "Failed to configure AKD PDOs.\n" );
exit( EXIT_FAILURE );
}
if ( ecrt_domain_reg_pdo_entry_list( domain1, domain1_regs ) ) {
fprintf( stderr, "PDO entry registration failed!\n" );
exit( EXIT_FAILURE );
}
printf("Activating master...\n");
if ( ecrt_master_activate( master ) ) {
exit( EXIT_FAILURE );
}
if ( !(domain1_pd = ecrt_domain_data( domain1 )) ) {
exit( EXIT_FAILURE );
}
printf("Started.\n");
while (1) {
usleep( 1000000 / TASK_FREQUENCY );
cyclic_task();
}
return EXIT_SUCCESS;
}
/*
# For reference, this is my AKD configuration for this test:
# These lines are generated by the DRV.DIFVAR command
# which lists all parameters that differ from their default value.
DRV.ACC 500.008 (10000.170)
DRV.CMDSOURCE 1 (0)
DRV.DEC 500.008 (10000.170)
FB1.CALTHRESH 5587.500 (117.000)
FB1.LASTIDENTIFIED 30 (41)
FBUS.SAMPLEPERIOD 4 (32)
GEAR.ACCMAX 10000.170 (240000008.192)
GEAR.DECMAX 10000.170 (240000008.192)
IL.KP 274.468 (50.000)
IL.LIMITN -5.000 (-9.000)
IL.LIMITP 5.000 (9.000)
IP.MODE 1 (0)
LOAD.INERTIA 1.200 (0.000)
MOTOR.IDMAX 0.447 (1.271)
MOTOR.TBRAKETO 30000 (-1)
REGEN.REXT 33 (330)
REGEN.TEXT 16.500 (100.000)
REGEN.TYPE -1 (0)
REGEN.WATTEXT 100 (1000)
*/
/* end of akd_torque.c */
_______________________________________________
etherlab-users mailing list
[email protected]
http://lists.etherlab.org/mailman/listinfo/etherlab-users
