Hi,
This is an example to setup Kollmorgen AKD servo drive in cyclic sync position
mode (CSP). Unfortunately this new example has still a small problem: it
triggers a lot of F439 faults during operation. I assume that my position loop
is not correctly tuned.
Nevertheless, I hope it will help people to understand how to setup the servo
drive with IgH Ethercat Master library.
Note: the previous example, in torque mode, can be see here
http://lists.etherlab.org/pipermail/etherlab-users/2015/002698.html
best regards,
--
Sebastien BLANCHET
/*****************************************************************************
* Example of Kollmorgen AKD servo drive setup with IgH EtherCAT Master library
* that starts the servo drive in cyclic synchronous position mode (csp).
*
* 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 cyclic sync position mode (csp).
*
* 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
*
* WARNING !!!
* this example has still a problem, it triggers faults (F439: Following error
* magnitude fault). I think that my position loop is not correctly tuned.
*
* Author: Sebastien Blanchet
* Date: Mar 09, 2015
* Compile with:
gcc akd_position.c -Wall -I/opt/etherlab/include -lethercat \
-L/opt/etherlab/lib -o akd_position
****************************************************************************/
#include "ecrt.h"
#include <stdio.h>
#include <unistd.h>
#include <sched.h>
#include <sys/mman.h>
#include <errno.h>
#include <string.h>
#include <sys/resource.h>
#include <time.h>
#include <sys/types.h>
/* Prerequises: the servo drive is already configured with AKD Workbench:
DRV.TYPE 2
DRV.CMDSOURCE 1
DRV.OPMODE 2
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 (2*TASK_FREQUENCY) /* clearing error timeout */
#define TARGET_POSITION 0
#define AKD_STS_MASK 0xAFF /* mask to remove manufacturer special bits and target_reached */
#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_position;
unsigned int status_word;
unsigned int act_velocity;
unsigned int act_position;
} offset;
const static ec_pdo_entry_reg_t domain1_regs[] = {
{ AkdSlavePos, Kollmorgen_AKD, 0x6040, 0, &offset.ctrl_word },
{ AkdSlavePos, Kollmorgen_AKD, 0x607A, 0, &offset.target_position },
{ AkdSlavePos, Kollmorgen_AKD, 0x6041, 0, &offset.status_word },
{ AkdSlavePos, Kollmorgen_AKD, 0x606C, 0, &offset.act_velocity },
{ AkdSlavePos, Kollmorgen_AKD, 0x6063, 0, &offset.act_position},
{}
};
/* AKD */
ec_pdo_entry_info_t akd_pdo_entries[] = {
/* RxPdo 0x1600 */
{ 0x6040, 0x00, 16 }, /* DS402 command word */
{ 0x607A, 0x00, 32 }, /* target position */
/* TxPDO 0x1a00 */
{ 0x6041, 0x00, 16 }, /* DS402 status word */
/* the maximum length for PDO is 8 bytes, so create another PDO
for actual velocity and position */
/* TxPDO 0x1a01 */
{ 0x606C, 0x00, 32 }, /* actual velocity, in rpm */
{ 0x6063, 0x00, 32 }, /* actual position */
};
ec_pdo_info_t akd_pdos[] = {
{ 0x1600, 2, akd_pdo_entries + 0 },
{ 0x1a00, 1, akd_pdo_entries + 2 },
{ 0x1a01, 2, akd_pdo_entries + 3 },
};
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, 2, 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 int32_t target_position = TARGET_POSITION;
uint16_t status; /* DS402 status register, without manufacturer bits */
float act_velocity; /* actual velocity in rpm */
int act_position; /* actual position in encoder unit */
/* 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;
act_position = EC_READ_S32(domain1_pd + offset.act_position);
/* 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 ) {
if (timeout_error == TIMEOUT_CLEAR_ERROR) {
printf( "AKD: ERROR, wait for timeout\n" );
}
timeout_error--;
} else {
timeout_error = TIMEOUT_CLEAR_ERROR;
command = AKD_CMD_CLR_ERROR;
printf( "AKD: clear error now\n" );
}
} else {
/* print actual values, once per second */
static time_t prev_second = 0;
time_t now = time(NULL);
if ( now != prev_second ) {
printf( "AKD: actual velocity = %.1f rpm, actual position = %d, target_pos = %d\n",
act_velocity, act_position, target_position );
prev_second = now;
}
}
/* write output */
EC_WRITE_S32( domain1_pd + offset.target_position, target_position );
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, 0x607A0020 ); /* 0x60C1:1/32bits target position*/
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_sdo8( sc_akd, 0x1A00, 0, 1 ); /* set number of PDO entries for 0x1A00 */
ecrt_slave_config_sdo32( sc_akd, 0x1A01, 1, 0x606C0020 ); /* 0x606c:0/32bits, act velocity */
ecrt_slave_config_sdo32( sc_akd, 0x1A01, 2, 0x60630020 ); /* 0x6063:0/32bits, act position */
ecrt_slave_config_sdo8( sc_akd, 0x1A01, 0, 2 ); /* set number of PDO entries for 0x1A01 */
ecrt_slave_config_sdo16( sc_akd, 0x1C13, 1, 0x1A00 ); /* list all TxPdo in 0x1C13:1-4 */
ecrt_slave_config_sdo16( sc_akd, 0x1C13, 2, 0x1A01 ); /* list all TxPdo in 0x1C13:1-4 */
ecrt_slave_config_sdo8( sc_akd, 0x1C13, 0, 2 ); /* set number of TxPDO */
/* set operation mode */
ecrt_slave_config_sdo8( sc_akd, 0x6060, 0, 8 ); /* set cyclic synchronous position mode (csp) */
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_position.c */
_______________________________________________
etherlab-users mailing list
[email protected]
http://lists.etherlab.org/mailman/listinfo/etherlab-users
