678 lines
26 KiB
C
678 lines
26 KiB
C
#define _GNU_SOURCE
|
|
#include <errno.h>
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <sys/resource.h>
|
|
#include <sys/time.h>
|
|
#include <sys/types.h>
|
|
#include <time.h>
|
|
#include <unistd.h>
|
|
#include <stdbool.h>
|
|
#include <getopt.h>
|
|
#include <sched.h>
|
|
#include <pthread.h>
|
|
#include <stdarg.h>
|
|
#include "Rockchip_MADHT1505BA1.h"
|
|
|
|
#define POSITION_MAX 2000000000UL
|
|
#define FREQUENCY 1000
|
|
#define CLOCK_TO_USE CLOCK_MONOTONIC
|
|
#define MEASURE_TIMING
|
|
#define TARGET_VELOCITY 1124000 /*target velocity*/
|
|
|
|
#define NSEC_PER_SEC (1000000000L)
|
|
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)
|
|
#define SHIFT_NS (NSEC_PER_SEC / FREQUENCY /4)
|
|
#define STATUS_SERVO_ENABLE_BIT (0x04)
|
|
|
|
#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
|
|
(B).tv_nsec - (A).tv_nsec)
|
|
|
|
#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
|
|
#define SLAVES_NUM_MAX 10
|
|
|
|
// Time statistics
|
|
uint32_t latency_min_ns = 0, latency_max_ns = 0,
|
|
period_min_ns = 0, period_max_ns = 0,
|
|
exec_min_ns = 0, exec_max_ns = 0;
|
|
static int clean_cycle = 0;
|
|
// Time statistics
|
|
|
|
static ec_master_t *master = NULL;
|
|
static ec_master_state_t master_state = {};
|
|
|
|
int debug_mode = 0;
|
|
bool run = true;
|
|
const struct timespec cycletime = {0, PERIOD_NS};
|
|
int slaves_cnt;
|
|
pthread_t thread;
|
|
int cpu_core;
|
|
MADHT1505BA1_object* slaves_group[SLAVES_NUM_MAX];
|
|
|
|
static void printf_debug(const char* fmt, ...){
|
|
if (debug_mode == 1) {
|
|
va_list args;
|
|
va_start(args, fmt);
|
|
vprintf(fmt, args);
|
|
va_end(args);
|
|
}
|
|
}
|
|
|
|
static int thread_bind_cpu(int target_cpu)
|
|
{
|
|
cpu_set_t mask;
|
|
int cpu_num = sysconf(_SC_NPROCESSORS_CONF);
|
|
int i;
|
|
|
|
if (target_cpu >= cpu_num)
|
|
return -1;
|
|
|
|
CPU_ZERO(&mask);
|
|
CPU_SET(target_cpu, &mask);
|
|
|
|
if (pthread_setaffinity_np(pthread_self(), sizeof(mask), &mask) < 0)
|
|
perror("pthread_setaffinity_np");
|
|
|
|
if (pthread_getaffinity_np(pthread_self(), sizeof(mask), &mask) < 0)
|
|
perror("pthread_getaffinity_np");
|
|
|
|
printf("Thread(%ld) bound to cpu:", gettid());
|
|
for (i = 0; i < CPU_SETSIZE; i++) {
|
|
if (CPU_ISSET(i, &mask)) {
|
|
printf(" %d", i);
|
|
break;
|
|
}
|
|
}
|
|
printf("\n");
|
|
|
|
return i >= cpu_num ? -1 : i;
|
|
}
|
|
|
|
static struct timespec timespec_add(struct timespec time1, struct timespec time2)
|
|
{
|
|
struct timespec result;
|
|
|
|
if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
|
|
result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
|
|
result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
|
|
} else {
|
|
result.tv_sec = time1.tv_sec + time2.tv_sec;
|
|
result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
static int domain_regs_fill_in(MADHT1505BA1_object *object) {
|
|
|
|
// for (int i = 0; i < ARRAY_SIZE(code); i++) {
|
|
// object.domain_regs[i] =
|
|
// (ec_pdo_entry_reg_t){object.alias, object.position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, code[i], 0, (uint8_t *)object + sizeof(unsigned int) * i};
|
|
// }
|
|
|
|
object->domain_regs[0] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6040, 0, &(object->control_word)};
|
|
object->domain_regs[1] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6060, 0, &(object->modes_of_operation)};
|
|
object->domain_regs[2] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x607a, 0, &(object->target_position)};
|
|
object->domain_regs[3] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60b8, 0, &(object->touch_probe_function)};
|
|
object->domain_regs[4] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x603f, 0, &(object->error_code)};
|
|
object->domain_regs[5] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6041, 0, &(object->status_word)};
|
|
object->domain_regs[6] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6061, 0, &(object->modes_of_operation_display)};
|
|
object->domain_regs[7] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6064, 0, &(object->position_actual_value)};
|
|
object->domain_regs[8] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60b9, 0, &(object->touch_probe_status)};
|
|
object->domain_regs[9] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60ba, 0, &(object->touch_probe_pos1_pos_value)};
|
|
object->domain_regs[10] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60f4, 0, &(object->following_error_actual_value)};
|
|
object->domain_regs[11] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60fd, 0, &(object->digital_inputs)};
|
|
object->domain_regs[12] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x606c, 0, &(object->current_velocity)};
|
|
object->domain_regs[13] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x60ff, 0, &(object->target_velocity)};
|
|
object->domain_regs[14] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6081, 0, &(object->profile_velocity)};
|
|
object->domain_regs[15] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6082, 0, &(object->end_velocity)};
|
|
object->domain_regs[16] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6083, 0, &(object->profile_acceleration)};
|
|
object->domain_regs[17] = (ec_pdo_entry_reg_t){object->alias, object->position, MADHT1505BA1_vendor, MADHT1505BA1_product_code, 0x6084, 0, &(object->end_deceleration)};
|
|
object->domain_regs[18] = (ec_pdo_entry_reg_t){};
|
|
|
|
object->domain_pd = NULL;
|
|
object->change_pos = false;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void check_master_state(void)
|
|
{
|
|
|
|
ec_master_state_t ms;
|
|
|
|
ecrt_master_state(master, &ms);
|
|
|
|
if (ms.slaves_responding != master_state.slaves_responding) {
|
|
printf_debug("found %u slave(s).\n", ms.slaves_responding);
|
|
}
|
|
if (ms.al_states != master_state.al_states) {
|
|
printf_debug("AL states: 0x%02X.\n", ms.al_states);
|
|
}
|
|
if (ms.link_up != master_state.link_up) {
|
|
printf_debug("Link is %s.\n", ms.link_up ? "up" : "down");
|
|
}
|
|
|
|
master_state = ms;
|
|
}
|
|
|
|
static void check_slave_config_states(MADHT1505BA1_object *object)
|
|
{
|
|
|
|
ec_slave_config_state_t s;
|
|
|
|
ecrt_slave_config_state(object->sc, &s);
|
|
|
|
if (s.al_state != object->sc_state.al_state) {
|
|
printf_debug("slaveDrive %d: State 0x%02X.\n", object->alias, s.al_state);
|
|
}
|
|
if (s.online != object->sc_state.online) {
|
|
printf_debug("slaveDrive %d: %s.\n", object->alias, s.online ? "online" : "offline");
|
|
}
|
|
printf_debug("slaveDrive %d: %s operational.\n", object->alias, s.operational ? "yes" : "Not ");
|
|
|
|
object->sc_state = s;
|
|
}
|
|
|
|
static void check_domain_state(MADHT1505BA1_object *object)
|
|
{
|
|
|
|
ec_domain_state_t ds;
|
|
|
|
ecrt_domain_state(object->domain, &ds);
|
|
|
|
if (ds.working_counter != object->domain_state.working_counter) {
|
|
printf_debug("Domain %d: WC %u.\n", object->alias, ds.working_counter);
|
|
}
|
|
if (ds.wc_state != object->domain_state.wc_state) {
|
|
printf_debug("Domain %d: State %u.\n", object->alias, ds.wc_state);
|
|
}
|
|
|
|
object->domain_state = ds;
|
|
}
|
|
|
|
int MADHT1505BA1_master_init(int bind_core) {
|
|
char* tmp = NULL;
|
|
printf("rockchip MADHT1505BA1 Motor drive program\n");
|
|
|
|
tmp = getenv("RKOCKCHIP_MADHT1505BA1_DEBUG");
|
|
if (tmp == NULL) {
|
|
printf("env RKOCKCHIP_MADHT1505BA1_DEBUG not set\n");
|
|
debug_mode = 0;
|
|
}else if (!strcmp("1", tmp)) {
|
|
debug_mode = 1;
|
|
}
|
|
cpu_core = bind_core;
|
|
master = ecrt_request_master(0);
|
|
if (!master) {
|
|
printf("ecrt_request_master is err\n");
|
|
return -1;
|
|
}
|
|
printf("request_master sucess, check slaves responding %d\n", master_state.slaves_responding);
|
|
check_master_state();
|
|
|
|
return 0;
|
|
}
|
|
|
|
int MADHT1505BA1_slaves_init(MADHT1505BA1_object *object) {
|
|
domain_regs_fill_in(object);
|
|
object->sc = ecrt_master_slave_config(
|
|
master,
|
|
object->alias,
|
|
object->position,
|
|
MADHT1505BA1_vendor,
|
|
MADHT1505BA1_product_code);
|
|
if(!object->sc) {
|
|
printf("Failed to get slave configuration.\n");
|
|
return -1;
|
|
}
|
|
check_slave_config_states(object);
|
|
|
|
object->domain = ecrt_master_create_domain(master);
|
|
if (!object->domain) {
|
|
printf("ecrt_master_create_domain is fail\n");
|
|
return -1;
|
|
}
|
|
printf("Configuring PDOs...\n");
|
|
if (ecrt_slave_config_pdos(object->sc, EC_END, slave_0_syncs)) {
|
|
printf("Failed to configure PDOs.\n");
|
|
return -1;
|
|
}
|
|
// for(int i = 0; i<15; i++) {
|
|
// printf("domain_regs[%d] : alias = %d position = %d index = %x\n",i, object->domain_regs[i].alias, object->domain_regs[i].position, object->domain_regs[i].index);
|
|
// }
|
|
if(ecrt_domain_reg_pdo_entry_list(object->domain, object->domain_regs)) {
|
|
printf("Failed to ecrt_domain_reg_pdo_entry_list.\n");
|
|
return -1;
|
|
};
|
|
|
|
//ecrt_slave_config_dc(object->sc, 0x300, PERIOD_NS, 0, 0, 0);
|
|
return 0;
|
|
}
|
|
|
|
int MADHT1505BA1_master_activate(void) {
|
|
printf("Activating master...\n");
|
|
if (ecrt_master_activate(master)) {
|
|
printf("ecrt_master_activate is fail\n");
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int MADHT1505BA1_slaves_activate(MADHT1505BA1_object *object) {
|
|
printf("activate slaves %d\n", object->alias);
|
|
if (!(object->domain_pd = ecrt_domain_data(object->domain))) {
|
|
printf("ecrt_domain_data is fail\n");
|
|
return -1;
|
|
}else {
|
|
printf("activate slaves %d is success\n", object->alias);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
int MADHT1505BA1_master_deinit(void) {
|
|
run = false;
|
|
ecrt_master_deactivate(master);
|
|
ecrt_release_master(master);
|
|
master = NULL;
|
|
printf("MADHT1505BA1_master_deinit\n");
|
|
}
|
|
|
|
// void *slave_velocity_mode_pthread(void *arg) {
|
|
// struct timespec wakeupTime, time;
|
|
// //MADHT1505BA1_object **object = (MADHT1505BA1_object **)arg;
|
|
// int counter = 0;
|
|
// struct sched_param param;
|
|
// int maxpri, count, i;
|
|
// int curpos = 0;
|
|
|
|
// printf("slave_pthread bind_cpu\n");
|
|
// if(thread_bind_cpu(cpu_core) == -1) {
|
|
// printf("bind cpu core fail\n");
|
|
// }
|
|
|
|
// // The scheduling priority is the highest
|
|
// maxpri = sched_get_priority_max(SCHED_FIFO);
|
|
// if(maxpri == -1) {
|
|
// printf("sched_get_priority_max() failed");
|
|
// }
|
|
|
|
// param.sched_priority = maxpri;
|
|
// if (sched_setscheduler(getpid(), SCHED_FIFO, ¶m) == -1) {
|
|
// perror("sched_setscheduler() failed");
|
|
// }
|
|
// printf("end thread set\n");
|
|
|
|
// // Time statistics
|
|
// struct timespec startTime, endTime, lastStartTime = {};
|
|
// uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0;
|
|
|
|
// period_max_ns = 0;
|
|
// period_min_ns = 0xffffffff;
|
|
// latency_max_ns = 0;
|
|
// latency_min_ns = 0xffffffff;
|
|
// clock_gettime(CLOCK_TO_USE, &lastStartTime);
|
|
// // Time statistics
|
|
|
|
// clock_gettime(CLOCK_TO_USE, &wakeupTime);
|
|
// while(run) {
|
|
|
|
// wakeupTime = timespec_add(wakeupTime, cycletime);
|
|
// clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
|
|
|
|
// // Write application time to master
|
|
// //
|
|
// // It is a good idea to use the target time (not the measured time) as
|
|
// // application time, because it is more stable.
|
|
// //
|
|
// ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
|
|
|
|
// /*Receive process data*/
|
|
// ecrt_master_receive(master);
|
|
// for (i = 0; i < slaves_cnt; i++) {
|
|
// ecrt_domain_process(slaves_group[i]->domain);
|
|
// // check process data state (optional)
|
|
// check_domain_state(slaves_group[i]);
|
|
// }
|
|
// if(counter) {
|
|
// counter--;
|
|
// }else {
|
|
// counter = FREQUENCY;
|
|
// check_master_state();
|
|
// for (i = 0; i < slaves_cnt; i++) {
|
|
// check_slave_config_states(slaves_group[i]);
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x80);
|
|
// slaves_group[i]->status = EC_READ_U16(slaves_group[i]->domain_pd + slaves_group[i]->status_word);
|
|
// slaves_group[i]->opmode = EC_READ_U8(slaves_group[i]->domain_pd + slaves_group[i]->modes_of_operation_display);
|
|
// slaves_group[i]->cur_velocity = EC_READ_S32(slaves_group[i]->domain_pd + slaves_group[i]->current_velocity);
|
|
|
|
// curpos = EC_READ_S32(slaves_group[i]->domain_pd + slaves_group[i]->position_actual_value);
|
|
// // if(curpos < 0) {
|
|
// // curpos = POSITION_MAX - abs(curpos);
|
|
// // }
|
|
// slaves_group[i]->curpos = curpos;
|
|
|
|
// printf_debug("slave %d madht: act velocity = %d ,act position = %d, status = 0x%x, opmode = 0x%x\n",
|
|
// slaves_group[i]->alias, slaves_group[i]->cur_velocity, slaves_group[i]->curpos, slaves_group[i]->status, slaves_group[i]->opmode);
|
|
|
|
// if( (slaves_group[i]->status & 0x004f) == 0x0040) {
|
|
// printf_debug("0x06\n");
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0006);
|
|
// EC_WRITE_U8(slaves_group[i]->domain_pd + slaves_group[i]->modes_of_operation, 9);
|
|
// }
|
|
// else if( (slaves_group[i]->status & 0x006f) == 0x0021) {
|
|
// printf_debug("0x07\n");
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0007);
|
|
// }
|
|
// else if( (slaves_group[i]->status & 0x006f) == 0x0023) {
|
|
// printf_debug("0x0f\n");
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x000f);
|
|
// EC_WRITE_S32(slaves_group[i]->domain_pd + slaves_group[i]->target_velocity, slaves_group[i]->user_velocity);
|
|
// }
|
|
// //operation enabled
|
|
// else if( (slaves_group[i]->status & 0x006f) == 0x0027) {
|
|
// printf_debug("0x1f\n");
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x001f);
|
|
// }
|
|
// if(slaves_group[i]->change_velocity) {
|
|
// printf_debug("change velocity\n");
|
|
// EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0007); // stop slaves
|
|
// slaves_group[i]->change_velocity = false;
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
// clock_gettime(CLOCK_TO_USE, &time);
|
|
// ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time));
|
|
|
|
// ecrt_master_sync_slave_clocks(master);
|
|
// // send process data
|
|
// for (i = 0; i < slaves_cnt; i++) {
|
|
// ecrt_domain_queue(slaves_group[i]->domain);
|
|
// }
|
|
// ecrt_master_send(master);
|
|
|
|
// // Time statistics
|
|
// clock_gettime(CLOCK_TO_USE, &startTime);
|
|
// latency_ns = DIFF_NS(wakeupTime, startTime);
|
|
// period_ns = DIFF_NS(lastStartTime, startTime);
|
|
// if (clean_cycle >= (12 * 60 * 60 * 1000)) { // 12 hour clean
|
|
// clean_cycle = 0;
|
|
// period_max_ns = 0;
|
|
// period_min_ns = 0xffffffff;
|
|
// latency_max_ns = 0;
|
|
// latency_min_ns = 0xffffffff;
|
|
// }
|
|
// if (latency_ns > latency_max_ns) {
|
|
// latency_max_ns = latency_ns;
|
|
// }
|
|
// if (latency_ns < latency_min_ns) {
|
|
// latency_min_ns = latency_ns;
|
|
// }
|
|
// if (period_ns > period_max_ns) {
|
|
// period_max_ns = period_ns;
|
|
// }
|
|
// if (period_ns < period_min_ns) {
|
|
// period_min_ns = period_ns;
|
|
// }
|
|
// clean_cycle++;
|
|
// lastStartTime = startTime;
|
|
// // Time statistics
|
|
// }
|
|
// }
|
|
|
|
void *slave_position_mode_pthread(void *arg) {
|
|
struct timespec wakeupTime, time;
|
|
//MADHT1505BA1_object **object = (MADHT1505BA1_object **)arg;
|
|
int counter = 0;
|
|
struct sched_param param;
|
|
int maxpri, count, i;
|
|
int curpos = 0;
|
|
bool do_flag = false;
|
|
|
|
printf("slave_pthread bind_cpu\n");
|
|
if(thread_bind_cpu(cpu_core) == -1) {
|
|
printf("bind cpu core fail\n");
|
|
}
|
|
|
|
// The scheduling priority is the highest
|
|
maxpri = sched_get_priority_max(SCHED_FIFO);
|
|
if(maxpri == -1) {
|
|
printf("sched_get_priority_max() failed");
|
|
}
|
|
|
|
param.sched_priority = maxpri;
|
|
if (sched_setscheduler(getpid(), SCHED_FIFO, ¶m) == -1) {
|
|
perror("sched_setscheduler() failed");
|
|
}
|
|
printf("end thread set\n");
|
|
|
|
// Time statistics
|
|
struct timespec startTime, endTime, lastStartTime = {};
|
|
uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0;
|
|
|
|
period_max_ns = 0;
|
|
period_min_ns = 0xffffffff;
|
|
latency_max_ns = 0;
|
|
latency_min_ns = 0xffffffff;
|
|
clock_gettime(CLOCK_TO_USE, &lastStartTime);
|
|
// Time statistics
|
|
|
|
clock_gettime(CLOCK_TO_USE, &wakeupTime);
|
|
while(run) {
|
|
|
|
wakeupTime = timespec_add(wakeupTime, cycletime);
|
|
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
|
|
|
|
// Write application time to master
|
|
//
|
|
// It is a good idea to use the target time (not the measured time) as
|
|
// application time, because it is more stable.
|
|
//
|
|
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
|
|
|
|
/*Receive process data*/
|
|
ecrt_master_receive(master);
|
|
for (i = 0; i < slaves_cnt; i++) {
|
|
ecrt_domain_process(slaves_group[i]->domain);
|
|
// check process data state (optional)
|
|
check_domain_state(slaves_group[i]);
|
|
}
|
|
|
|
for (i = 0; i < slaves_cnt; i++) {
|
|
if(slaves_group[i]->change_pos == true) {
|
|
do_flag = true;
|
|
}
|
|
}
|
|
|
|
if(counter && do_flag == false) {
|
|
counter--;
|
|
}else {
|
|
counter = FREQUENCY;
|
|
check_master_state();
|
|
for (i = 0; i < slaves_cnt; i++) {
|
|
check_slave_config_states(slaves_group[i]);
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x80);
|
|
EC_WRITE_U8(slaves_group[i]->domain_pd + slaves_group[i]->modes_of_operation, 1);
|
|
|
|
// EC_WRITE_S32(slaves_group[i]->domain_pd + slaves_group[i]->target_position, slaves_group[i]->user_set_pos);
|
|
// EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->profile_velocity, TARGET_VELOCITY);
|
|
// EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->end_velocity, TARGET_VELOCITY);
|
|
// EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->profile_acceleration, TARGET_VELOCITY);
|
|
// EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->end_deceleration, TARGET_VELOCITY);
|
|
|
|
slaves_group[i]->status = EC_READ_U16(slaves_group[i]->domain_pd + slaves_group[i]->status_word);
|
|
slaves_group[i]->opmode = EC_READ_U8(slaves_group[i]->domain_pd + slaves_group[i]->modes_of_operation_display);
|
|
|
|
curpos = EC_READ_S32(slaves_group[i]->domain_pd + slaves_group[i]->position_actual_value);
|
|
slaves_group[i]->curpos = curpos;
|
|
|
|
printf_debug("slave %d madht: act position = %d, status = 0x%x, opmode = 0x%x\n",
|
|
slaves_group[i]->alias, slaves_group[i]->curpos, slaves_group[i]->status, slaves_group[i]->opmode);
|
|
|
|
if( (slaves_group[i]->status & 0x004f) == 0x0040) {
|
|
printf_debug("0x06\n");
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0006);
|
|
}
|
|
else if( (slaves_group[i]->status & 0x006f) == 0x0021) {
|
|
printf_debug("0x07\n");
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0007);
|
|
}
|
|
else if( (slaves_group[i]->status & 0x006f) == 0x0023) {
|
|
printf_debug("0x0f\n");
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x000f);
|
|
if(slaves_group[i]->change_pos) {
|
|
EC_WRITE_S32(slaves_group[i]->domain_pd + slaves_group[i]->target_position, slaves_group[i]->user_set_pos);
|
|
EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->profile_velocity, 5000000);
|
|
EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->profile_acceleration, 2*500000000);
|
|
EC_WRITE_U32(slaves_group[i]->domain_pd + slaves_group[i]->end_deceleration, 500000000);
|
|
slaves_group[i]->change_pos = false;
|
|
do_flag = false;
|
|
}
|
|
}
|
|
|
|
// //operation enabled
|
|
else if( (slaves_group[i]->status & 0x006f) == 0x0027) {
|
|
printf_debug("0x1f\n");
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x001f);
|
|
// if((EC_READ_U16(slaves_group[i]->domain_pd + slaves_group[i]->status) & (STATUS_SERVO_ENABLE_BIT)) == 0){
|
|
// printf("STATUS_SERVO_ENABLE_BIT\n");
|
|
// continue;
|
|
// }
|
|
}
|
|
if(slaves_group[i]->change_pos) {
|
|
printf_debug("change pos\n");
|
|
EC_WRITE_U16(slaves_group[i]->domain_pd + slaves_group[i]->control_word, 0x0007); // stop slaves
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
clock_gettime(CLOCK_TO_USE, &time);
|
|
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time));
|
|
|
|
ecrt_master_sync_slave_clocks(master);
|
|
// send process data
|
|
for (i = 0; i < slaves_cnt; i++) {
|
|
ecrt_domain_queue(slaves_group[i]->domain);
|
|
}
|
|
ecrt_master_send(master);
|
|
|
|
// Time statistics
|
|
clock_gettime(CLOCK_TO_USE, &startTime);
|
|
latency_ns = DIFF_NS(wakeupTime, startTime);
|
|
period_ns = DIFF_NS(lastStartTime, startTime);
|
|
if (clean_cycle >= (5 * 60 * 1000)) { // 5 min clean
|
|
clean_cycle = 0;
|
|
period_max_ns = 0;
|
|
period_min_ns = 0xffffffff;
|
|
latency_max_ns = 0;
|
|
latency_min_ns = 0xffffffff;
|
|
}
|
|
if (latency_ns > latency_max_ns) {
|
|
latency_max_ns = latency_ns;
|
|
}
|
|
if (latency_ns < latency_min_ns) {
|
|
latency_min_ns = latency_ns;
|
|
}
|
|
if (period_ns > period_max_ns) {
|
|
period_max_ns = period_ns;
|
|
}
|
|
if (period_ns < period_min_ns) {
|
|
period_min_ns = period_ns;
|
|
}
|
|
clean_cycle++;
|
|
lastStartTime = startTime;
|
|
// Time statistics
|
|
}
|
|
}
|
|
|
|
int MADHT1505BA1_slave_start(int cnt, ...) {
|
|
int err;
|
|
int i;
|
|
__builtin_va_list vaptr;
|
|
__builtin_va_start(vaptr, cnt);
|
|
run = true;
|
|
if (cnt > master_state.slaves_responding) {
|
|
printf("The number of slave stations set by the user is greater than the number of identified slave stations \n");
|
|
return -1;
|
|
}
|
|
|
|
slaves_cnt = cnt;
|
|
|
|
for (i = 0; i < cnt; i++) {
|
|
slaves_group[i] = __builtin_va_arg(vaptr, MADHT1505BA1_object *);
|
|
}
|
|
err = pthread_create(&thread, NULL, slave_position_mode_pthread, NULL);
|
|
if(err != 0) {
|
|
printf("MADHT1505BA1_drive_slave: can't create thread\n");
|
|
return -1;
|
|
}else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// int MADHT1505BA1_motor_start(MADHT1505BA1_object *object, int velocity) {
|
|
// uint16_t status;
|
|
// status = EC_READ_U16(object->domain_pd + object->status_word);
|
|
// if(status == 0x1237) {
|
|
// object->change_velocity = true;
|
|
// object->user_velocity = velocity;
|
|
// }else {
|
|
// printf("slave %d not start\n", object->alias);
|
|
// }
|
|
// return 0;
|
|
// }
|
|
|
|
// int MADHT1505BA1_motor_stop(MADHT1505BA1_object *object) {
|
|
// uint16_t status;
|
|
// status = EC_READ_U16(object->domain_pd + object->status_word);
|
|
// if(status == 0x1237) {
|
|
// object->change_velocity = true;
|
|
// object->user_velocity = 0;
|
|
// }else {
|
|
// printf("slave %d not start\n", object->alias);
|
|
// }
|
|
// return 0;
|
|
// }
|
|
|
|
int MADHT1505BA1_check_motor(MADHT1505BA1_object *object) {
|
|
uint16_t status;
|
|
status = EC_READ_U16(object->domain_pd + object->status_word);
|
|
if(status != 0x1237 && status != 0x1637) {
|
|
return -1;
|
|
}else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
uint32_t MADHT1505BA1_time_statistics_latency_min_ns(void) {
|
|
return latency_min_ns;
|
|
}
|
|
|
|
uint32_t MADHT1505BA1_time_statistics_latency_max_ns(void) {
|
|
return latency_max_ns;
|
|
}
|
|
|
|
uint32_t MADHT1505BA1_time_statistics_period_min_ns(void) {
|
|
return period_min_ns;
|
|
}
|
|
|
|
uint32_t MADHT1505BA1_time_statistics_period_max_ns(void) {
|
|
return period_max_ns;
|
|
}
|
|
|
|
int MADHT1505BA1_run_position_acquisition(MADHT1505BA1_object *object) {
|
|
return object->curpos;
|
|
}
|
|
|
|
void MADHT1505BA1_motor_set_position_run(int user_position, MADHT1505BA1_object *object) {
|
|
object->change_pos = true;
|
|
object->user_set_pos = user_position;
|
|
}
|
|
|
|
void MADHT1505BA1_position_reset(MADHT1505BA1_object *object) {
|
|
object->change_pos = true;
|
|
object->user_set_pos = 0;
|
|
} |