Hi,
This is a full example for the Kollmorgen AKD servo drive.
It shows how to configure the free PDO mapping and how to setup the DS402 state
machine to start the servo drive in digital torque mode with IgH EtherCAT Master
library.
best regards,
--
Sebastien BLANCHET
/*****************************************************************************
* 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 }, /* DS4020 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
etherlab-users@etherlab.org
http://lists.etherlab.org/mailman/listinfo/etherlab-users
