Hi,
There was a bug in my example, EC_WRITE_S32 must be used (instead of
EC_WRITE_S16) to set the target velocity.
I have attached the new version that works.
regards,
--
Sebastien BLANCHET
On 05/14/2018 05:57 PM, Guido wrote:
Dear Sebastien Blanchet,
I've tried to use your velocity program example for the motion of a servo
motor with an AKD drive and it works but I've got some problems to change the
direction of the motion.
I've tried to search an AKD parameter for the motion direction but I didn't find
anything, I've also tried to change the sign of the target velocity in your
program but it still moving in the same direction..
How did you solve this problem?
Thank you.
Guido
_______________________________________________
etherlab-users mailing list
[email protected]
http://lists.etherlab.org/mailman/listinfo/etherlab-users
/*****************************************************************************
* 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: May 15, 2018
* Compile with:
gcc akd_velocity.c -Wall -I/opt/etherlab/include -lethercat \
-L/opt/etherlab/lib -o akd_velocity
*
* Revision:
* - 2018-05-15: use EC_WRITE_S32 to write target_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_S32( 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
[email protected]
http://lists.etherlab.org/mailman/listinfo/etherlab-users
