//This is the thread that will run with high priority
void* realtimeMain(void* udata)
{
struct timespec tick;
int period = 1e+6; // 1 ms in nanoseconds
while(!exiting)
{
cyclic_task(); // Send and receave from the motor controllers
if(pthread_mutex_trylock(&mutex) == 0)
{
tar = tar_buffer;
cur_buffer = cur;
pthread_mutex_unlock(&mutex);
}
// Compute end of next period
timespecInc(&tick, period);
struct timespec before;
clock_gettime(CLOCK_REALTIME, &before);
if ((before.tv_sec + before.tv_nsec/1e9) > (tick.tv_sec + tick.tv_nsec/1e9))
{
// We overran, snap to next "period"
tick.tv_sec = before.tv_sec;
tick.tv_nsec = (before.tv_nsec / period) * period;
timespecInc(&tick, period);
misses++;
}
// Sleep until end of period
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
}
stop_motors();
return 0;
}
int main(int argc, char **argv)
{
ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
master = ecrt_request_master(0);
if (!master)
return -1;
domain1 = ecrt_master_create_domain(master);
if (!domain1)
return -1;
if (!(sc_ana_in = ecrt_master_slave_config(
master, AnaInSlavePos, Beckhoff_EL3102))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x3102, 2, 2))) {
fprintf(stderr, "Failed to create SDO request.\n");
return -1;
}
ecrt_sdo_request_timeout(sdo, 500); // ms
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3102_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (!(sc = ecrt_master_slave_config(
master, AnaOutSlavePos, Beckhoff_EL4102))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (!(sc = ecrt_master_slave_config(
master, DigOutSlavePos, Beckhoff_EL2032))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
#endif
// Create configuration for bus coupler
sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
if (!sc)
return -1;
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
printf("Starting timer...\n");
tv.it_interval.tv_sec = 0;
tv.it_interval.tv_usec = 1000000 / FREQUENCY;
tv.it_value.tv_sec = 0;
tv.it_value.tv_usec = 1000;
if (setitimer(ITIMER_REAL, &tv, NULL)) {
fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
return 1;
}
printf("Started.\n");
while (1) {
pause();
#if 0
struct timeval t;
gettimeofday(&t, NULL);
printf("%u.%06u\n", t.tv_sec, t.tv_usec);
#endif
while (sig_alarms != user_alarms) {
cyclic_task();
user_alarms++;
}
}
return 0;
}
int main(int argc, char **argv)
{
ec_slave_config_t *sc;
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
perror("mlockall failed");
return -1;
}
master = ecrt_request_master(0);
if (!master)
return -1;
domain_r = ecrt_master_create_domain(master);
if (!domain_r)
return -1;
domain_w = ecrt_master_create_domain(master);
if (!domain_w)
return -1;
if (!(sc = ecrt_master_slave_config(master, pana, panasonic)))
{
fprintf(stderr, "Failed to get slave1 configuration.\n");
return -1;
}
#if SDO_ACCESS
if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8))
{
return -1;
}
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs))
{
fprintf(stderr, "Failed to configure 1st PDOs.\n");
return -1;
}
#endif
if (ecrt_domain_reg_pdo_entry_list(domain_r, domain_r_regs))
{
fprintf(stderr, "1st motor RX_PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domain_w, domain_w_regs))
{
fprintf(stderr, "1st motor TX_PDO entry registration failed!\n");
return -1;
}
ecrt_slave_config_dc(sc,0x0300,500000,0,0,0);
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain_r_pd = ecrt_domain_data(domain_r)))
{
return -1;
}
if (!(domain_w_pd = ecrt_domain_data(domain_w)))
{
return -1;
}
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -20))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
signal( SIGINT , endsignal );
printf("Starting cyclic function.\n");
cyclic_task();
ecrt_release_master(master);
return 0;
}
int main(int argc, char **argv)
{
cmdline(argc, argv);
ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
master = ecrt_request_master(0);
if (!master)
return -1;
domain1 = ecrt_master_create_domain(master);
if (!domain1)
return -1;
if (!(sc_data_in = ecrt_master_slave_config(
master, SomanetPos, SOMANET_ID))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_data_in, 0x6041, 0, 1))) {
fprintf(stderr, "Failed to create SDO request.\n");
return -1;
}
ecrt_sdo_request_timeout(sdo, 500); // ms
if (!(request[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_POS_VALUE, 0, 4))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_POS_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[0], 500); // ms
if (!(request[1] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_VEL_VALUE, 0, 4))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_VEL_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[1], 500); // ms
if (!(request[2] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_TOR_VALUE, 0, 2))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_TOR_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[2], 500); // ms
/* register sdo download request */
if (!(sdo_download_requests[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_MODES, 0, 4))) {
fprintf(stderr, "Failed to create SDO download request for object 0x%4x\n", CAN_OD_MODES);
return -1;
}
ecrt_sdo_request_timeout(sdo_download_requests[0], 500); // ms
/* set the sdoexample to a specific bit muster */
sdoexample = 0x22442244;
#endif
#if CONFIGURE_PDOS
logmsg(1, "Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_data_in, EC_END, slave_0_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (!(sc = ecrt_master_slave_config(
master, SomanetPos, SOMANET_ID))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if 0
if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (!(sc = ecrt_master_slave_config(
master, DigOutSlavePos, Beckhoff_EL2032))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
#endif
#endif
// Create configuration for bus coupler
sc = ecrt_master_slave_config(master, SomanetPos /*BusCouplerPos*/, SOMANET_ID/*Beckhoff_EK1100*/);
if (!sc)
return -1;
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
logmsg(1, "Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
logmsg(1, "Starting timer...\n");
tv.it_interval.tv_sec = 0;
tv.it_interval.tv_usec = 1000000 / FREQUENCY;
tv.it_value.tv_sec = 0;
tv.it_value.tv_usec = 1000;
if (setitimer(ITIMER_REAL, &tv, NULL)) {
fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
return 1;
}
logmsg(0, "Started.\n");
while (1) {
pause();
#if 0
struct timeval t;
gettimeofday(&t, NULL);
printf("%u.%06u\n", (unsigned)t.tv_sec, (unsigned)t.tv_usec);
#endif
while (sig_alarms != user_alarms) {
cyclic_task();
user_alarms++;
}
}
return 0;
}
int main(int argc, char **argv)
{
// ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
master = ecrt_request_master(0);
if (!master)
return -1;
domain_input = ecrt_master_create_domain(master);
domain_output = ecrt_master_create_domain(master);
if (!domain_input || !domain_output)
{
return -1;
}
if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode)))
{
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x6061, 0, 1))) // data size 1 ?
{
fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n");
return -1;
}
//EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF);
ecrt_sdo_request_timeout(sdo, 500); // ms
#endif
#if 1
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, slave_0_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
#endif
if (ecrt_domain_reg_pdo_entry_list(domain_output, domain_output_regs)) {
fprintf(stderr, "Output PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domain_input, domain_input_regs)) {
fprintf(stderr, "Input PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain_output_pd = ecrt_domain_data(domain_output))) {
return -1;
}
if (!(domain_input_pd = ecrt_domain_data(domain_input))) {
return -1;
}
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
printf("Starting timer...\n");
tv.it_interval.tv_sec = 0;
tv.it_interval.tv_usec = 1000000 / FREQUENCY;
tv.it_value.tv_sec = 0;
tv.it_value.tv_usec = 1000;
if (setitimer(ITIMER_REAL, &tv, NULL)) {
fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
return 1;
}
printf("Started.\n");
while (1) {
pause();
#if 0
struct timeval t;
gettimeofday(&t, NULL);
printf("%u.%06u\n", t.tv_sec, t.tv_usec);
#endif
while (sig_alarms != user_alarms) {
cyclic_task();
user_alarms++;
}
}
return 0;
}
int main(int argc, char **argv)
{
ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
uint16_t i;
master = ecrt_request_master(0);
if (!master)
return -1;
domain1 = ecrt_master_create_domain(master);
if (!domain1)
return -1;
printf("Configuring PDOs...\n");
Init_EL100x(&off_dig_in[0], 0, Beckhoff_EL1004, el1004_syncs);
Init_EL100x(&off_dig_in[1], 1, Beckhoff_EL1004, el1004_syncs);
Init_EL100x(&off_dig_out[0], 2, Beckhoff_EL2004, el2004_syncs);
Init_EL100x(&off_dig_out[1], 3, Beckhoff_EL2004, el2004_syncs);
// Create configuration for bus coupler
sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
if (!sc)
return -1;
fprintf(stderr, "EK1100 configured.\n");
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
printf("Starting timer...\n");
tv.it_interval.tv_sec = 0;
tv.it_interval.tv_usec = 1000000 / FREQUENCY;
tv.it_value.tv_sec = 0;
tv.it_value.tv_usec = 1000;
if (setitimer(ITIMER_REAL, &tv, NULL)) {
fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
return 1;
}
printf("Started.\n");
while (1) {
pause();
#if 0
struct timeval t;
gettimeofday(&t, NULL);
printf("%u.%06u\n", t.tv_sec, t.tv_usec);
#endif
while (sig_alarms != user_alarms) {
cyclic_task();
user_alarms++;
}
}
return 0;
}