Hi,
This is an example to setup Kollmorgen AKD servo drive in profile velocity mode
with IgH Ethercat Master library.
See also:
torque mode
http://lists.etherlab.org/pipermail/etherlab-users/2015/002698.html
position mode:
http://lists.etherlab.org/pipermail/etherlab-users/2015/002723.html
best regards,
--
Sebastien BLANCHET
/*****************************************************************************
* Example of Kollmorgen AKD servo drive setup with IgH EtherCAT Master library
* that starts the servo drive in profile velocity 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 profile velocity mode.
*
* 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: Mar 18, 2015
* Compile with:
gcc akd_velocity.c -Wall -I/opt/etherlab/include -lethercat \
-L/opt/etherlab/lib -o akd_velocity
****************************************************************************/
#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>
/* Application parameters */
#define TASK_FREQUENCY 1000 /* Hz */
#define TIMEOUT_CLEAR_ERROR (1*TASK_FREQUENCY) /* clearing error timeout */
#define TARGET_VELOCITY 4000 /* target velocity in milli rpm*/
#define PROFILE_VELOCITY 3 /* Operation mode for 0x6060:0*/
#define AKD_STS_MASK 0xAFF /* 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_velocity;
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, 0x60FF, 0, &offset.target_velocity },
{ 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 */
{ 0x60FF, 0x00, 32 }, /* target velocity, milli rpm */
/* TxPDO 0x1a00 */
{ 0x6041, 0x00, 16 }, /* DS402 status word */
{ 0x606C, 0x00, 32 }, /* actual velocity, in milli rpm */
};
ec_pdo_info_t akd_pdos[] = {
{ 0x1600, 2, akd_pdo_entries + 0 },
{ 0x1a00, 2, akd_pdo_entries + 2 },
{ 0x1a01, 1, akd_pdo_entries + 4 },
};
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_velocity = TARGET_VELOCITY; /* target velocity in milli rpm */
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: act velocity = %.1f rpm, cmd = 0x%x, status = 0x%x\n",
act_velocity, command, status);
prev_second = tv.tv_sec;
}
}
/* write output */
EC_WRITE_S16( domain1_pd + offset.target_velocity, target_velocity );
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, 0x60FF0020 ); /* 0x60FF:1/32bits target velocity xxx */
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_sdo16( sc_akd, 0x1C13, 2, 0x1A01 ); /* list all TxPdo in 0x1C13:1-4 xxxx */
ecrt_slave_config_sdo8( sc_akd, 0x1C13, 0, 2 ); /* set number of TxPDO */
/* set operation mode */
ecrt_slave_config_sdo8( sc_akd, 0x6060, 0, PROFILE_VELOCITY );
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;
}
/* end of akd_velocity.c */
_______________________________________________
etherlab-users mailing list
etherlab-users@etherlab.org
http://lists.etherlab.org/mailman/listinfo/etherlab-users
