realtime.c
Go to the documentation of this file.
00001 /*
00002  * This file is part of the omnimod project.
00003  *
00004  * Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
00005  *                    Ingo Kresse <kresse@in.tum.de>
00006  *
00007  * This program is free software; you can redistribute it and/or modify
00008  * it under the terms of the GNU General Public License as published by
00009  * the Free Software Foundation; either version 2 of the License, or
00010  * (at your option) any later version.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program; if not, write to the Free Software
00019  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
00020  */
00021 
00022 #include <errno.h>
00023 #include <signal.h>
00024 #include <stdio.h>
00025 #include <string.h>
00026 #include <sys/resource.h>
00027 #include <sys/time.h>
00028 #include <sys/types.h>
00029 #include <unistd.h>
00030 
00031 #include <pthread.h>
00032 
00033 /****************************************************************************/
00034 
00035 #include <igh_eml/ecrt.h>
00036 
00037 /****************************************************************************/
00038 
00039 #include "realtime.h"  // defines omniread_t, omniwrite_t
00040 
00041 /*****************************************************************************/
00042 
00043 #define FREQUENCY 1000
00044 
00045 /* Optional features */
00046 #define CONFIGURE_PDOS  1
00047 #define EXTERNAL_MEMORY 1
00048 #define SDO_ACCESS      0
00049 
00050 /*****************************************************************************/
00051 
00052 static ec_master_t *master = NULL;
00053 static ec_master_state_t master_state;// = { };
00054 
00055 static ec_domain_t *domain1 = NULL;
00056 static ec_domain_state_t domain1_state;// = { };
00057 
00058 static ec_slave_config_t *sc[4];// = { NULL, NULL, NULL, NULL };
00059 static ec_slave_config_state_t sc_state[4];// = {{}, {}, {}, {}};
00060 
00061 static char prevent_set_position = 0;
00062 
00063 static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
00064 static int exiting = 0;
00065 static pthread_t thread;
00066 static int misses=0;
00067 
00068 /*****************************************************************************/
00069 
00070 /* Process data */
00071 static uint8_t *domain1_pd;     /* Process data memory */
00072 
00073 /* Slave alias, slave position */
00074 #define M0SlavePos 0, 0
00075 #define M1SlavePos 0, 1
00076 #define M2SlavePos 0, 2
00077 #define M3SlavePos 0, 3
00078 
00079 /* Slave vendor ID, slave product code */
00080 #define BM3411 0x0000015a, 0x03010001
00081 
00082 /* Offsets for 'write' PDO entries */
00083 // static unsigned int off_controlword[4];
00084 static unsigned int off_target_velocity[4];
00085 
00086 /* Offsets for 'read' PDO entries */
00087 static unsigned int off_statusword[4];
00088 static unsigned int off_speed_ctrlr_output[4];
00089 static unsigned int off_actual_position[4];
00090 static unsigned int off_actual_velocity[4];
00091 
00092 static unsigned int off_torque_actual_value[4];
00093 static unsigned int off_torque_add_value[4];
00094 static unsigned int off_target_position[4];
00095 static unsigned int off_pos_ctrlr_output[4];
00096 
00097 /* (alias, position), (VID, PID), PDO entry index, PDO entry subindex, pointer, bit position */
00098 static const ec_pdo_entry_reg_t domain1_regs[] = {
00099 
00100         {M0SlavePos, BM3411, 0x4169, 0, &off_target_position[0], 0},
00101         {M1SlavePos, BM3411, 0x4169, 0, &off_target_position[1], 0},
00102         {M2SlavePos, BM3411, 0x4169, 0, &off_target_position[2], 0},
00103         {M3SlavePos, BM3411, 0x4169, 0, &off_target_position[3], 0},
00104 
00105         {M0SlavePos, BM3411, 0x6042, 0, &off_target_velocity[0], 0},
00106         {M1SlavePos, BM3411, 0x6042, 0, &off_target_velocity[1], 0},
00107         {M2SlavePos, BM3411, 0x6042, 0, &off_target_velocity[2], 0},
00108         {M3SlavePos, BM3411, 0x6042, 0, &off_target_velocity[3], 0},
00109 
00110         /* Torque additional set value, P1022 -> 0x43fe */
00111         {M0SlavePos, BM3411, 0x43fe, 0, &off_torque_add_value[0], 0},
00112         {M1SlavePos, BM3411, 0x43fe, 0, &off_torque_add_value[1], 0},
00113         {M2SlavePos, BM3411, 0x43fe, 0, &off_torque_add_value[2], 0},
00114         {M3SlavePos, BM3411, 0x43fe, 0, &off_torque_add_value[3], 0},
00115 
00116 
00117 
00118         {M0SlavePos, BM3411, 0x6041, 0, &off_statusword[0], 0},
00119         {M1SlavePos, BM3411, 0x6041, 0, &off_statusword[1], 0},
00120         {M2SlavePos, BM3411, 0x6041, 0, &off_statusword[2], 0},
00121         {M3SlavePos, BM3411, 0x6041, 0, &off_statusword[3], 0},
00122 
00123         /* P0351 */
00124         {M0SlavePos, BM3411, 0x415F, 0, &off_pos_ctrlr_output[0], 0},
00125         {M1SlavePos, BM3411, 0x415F, 0, &off_pos_ctrlr_output[1], 0},
00126         {M2SlavePos, BM3411, 0x415F, 0, &off_pos_ctrlr_output[2], 0},
00127         {M3SlavePos, BM3411, 0x415F, 0, &off_pos_ctrlr_output[3], 0},
00128 
00129         /* P0362 */
00130         {M0SlavePos, BM3411, 0x416a, 0, &off_actual_position[0], 0},
00131         {M1SlavePos, BM3411, 0x416a, 0, &off_actual_position[1], 0},
00132         {M2SlavePos, BM3411, 0x416a, 0, &off_actual_position[2], 0},
00133         {M3SlavePos, BM3411, 0x416a, 0, &off_actual_position[3], 0},
00134 
00135         /* P0353 */
00136         {M0SlavePos, BM3411, 0x606c, 0, &off_actual_velocity[0], 0},
00137         {M1SlavePos, BM3411, 0x606c, 0, &off_actual_velocity[1], 0},
00138         {M2SlavePos, BM3411, 0x606c, 0, &off_actual_velocity[2], 0},
00139         {M3SlavePos, BM3411, 0x606c, 0, &off_actual_velocity[3], 0},
00140          
00141         /* P0333 -> 0x4000 + hex(333) = 0x414d */
00142         {M0SlavePos, BM3411, 0x414d, 0, &off_torque_actual_value[0], 0},
00143         {M1SlavePos, BM3411, 0x414d, 0, &off_torque_actual_value[1], 0},
00144         {M2SlavePos, BM3411, 0x414d, 0, &off_torque_actual_value[2], 0},
00145         {M3SlavePos, BM3411, 0x414d, 0, &off_torque_actual_value[3], 0},
00146 
00147         {0,0,        0,0,    0,      0, 0,                           0}
00148 };
00149 
00150 static unsigned int counter = 0;
00151 
00152 static int max_v = 100;
00153 
00154 static omniwrite_t tar, tar_buffer;  /* Target velocities */
00155 static omniread_t cur, cur_buffer;    /* Current velocities/torques/positions */
00156 
00157 /*****************************************************************************/
00158 
00159 /* PDO index, subindex, size in bits */
00160 static ec_pdo_entry_info_t foo_pdo_entries[] = {
00161         /* Write */
00162         // {0x6040, 0, 16},     /* control word */
00163         {0x4169, 0, 32},        /* target position */
00164         {0x6042, 0, 16},        /* target velocity */
00165         {0x43fe, 0, 16},        /* torque additional set value */
00166 
00167         /* Read */
00168         {0x6041, 0, 16},        /* statusword */
00169         {0x415F, 0, 16},        /* pos ctrlr output */
00170         {0x416a, 0, 32},        /* actual position unsigned */
00171         {0x606c, 0, 32},        /* actual velocity */
00172         {0x414d, 0, 16},    /* torque actual value */
00173 };
00174 
00175 /* PDO index, #entries, array of entries to map */
00176 static ec_pdo_info_t foo_pdos[] = {
00177         {0x1600, 3, foo_pdo_entries}, 
00178         {0x1a00, 5, foo_pdo_entries + 3}, 
00179 };
00180 
00181 /* Sync manager index, SM direction, #PDOs, arrays with PDOs to assign */
00182 static ec_sync_info_t foo_syncs[] = {
00183         {2 /* SM2 */ , EC_DIR_OUTPUT, 1, foo_pdos, EC_WD_DISABLE},
00184         {3 /* SM3 */ , EC_DIR_INPUT, 1, foo_pdos + 1, EC_WD_DISABLE},
00185         {0xff        , 0,            0, 0, EC_WD_DISABLE}
00186 };
00187 
00188 
00189 /*****************************************************************************/
00190 
00191 
00192 void check_domain1_state(void)
00193 {
00194         ec_domain_state_t ds;
00195 
00196         ecrt_domain_state(domain1, &ds);
00197 
00198         if (ds.working_counter != domain1_state.working_counter)
00199                 printf("Domain1: WC %u.\n", ds.working_counter);
00200         if (ds.wc_state != domain1_state.wc_state)
00201                 printf("Domain1: State %u.\n", ds.wc_state);
00202 
00203         domain1_state = ds;
00204         cur.working_counter = ds.working_counter;
00205         cur.working_counter_state = ds.wc_state;
00206 }
00207 
00208 
00209 /*****************************************************************************/
00210 
00211 
00212 void check_master_state(void)
00213 {
00214         ec_master_state_t ms;
00215 
00216         ecrt_master_state(master, &ms);
00217 
00218         if (ms.slaves_responding != master_state.slaves_responding)
00219                 printf("%u slave(s).\n", ms.slaves_responding);
00220         if (ms.al_states != master_state.al_states)
00221                 printf("AL states: 0x%02X.\n", ms.al_states);
00222         if (ms.link_up != master_state.link_up)
00223                 printf("Link is %s.\n",
00224                        ms.link_up ? "up" : "down");
00225 
00226         master_state = ms;
00227         cur.master_link = ms.link_up;
00228         cur.master_al_states = ms.al_states;
00229         cur.master_slaves_responding = ms.slaves_responding;
00230 
00231 }
00232 
00233 /*****************************************************************************/
00234 
00235 void check_slave_config_states(void)
00236 {
00237         int i;
00238 
00239         ec_slave_config_state_t s;
00240 
00241         for (i = 0; i < 4; i++) {
00242                 ecrt_slave_config_state(sc[i], &s);
00243                 if (s.al_state != sc_state[i].al_state)
00244                         printf("m%d: State 0x%02X.\n", i, s.al_state);
00245                 if (s.online != sc_state[i].online)
00246                         printf("m%d: %s.\n", i,
00247                                s.online ? "online" : "offline");
00248                 if (s.operational != sc_state[i].operational)
00249                         printf("m%d: %soperational.\n", i,
00250                                s.operational ? "" : "Not ");
00251                 sc_state[i] = s;
00252 
00253                 cur.slave_state[i] = s.al_state;
00254                 cur.slave_online[i] = s.online;
00255                 cur.slave_operational[i] = s.operational;       
00256         }
00257 }
00258 
00259 
00260 /*****************************************************************************/
00261 
00262 void cyclic_task()
00263 {
00264         int i;
00265 
00266         /* Receive process data. */
00267         ecrt_master_receive(master);
00268         ecrt_domain_process(domain1);
00269 
00270         /* Check process data state (optional). */
00271         check_domain1_state();
00272 
00273         for (i = 0; i < 4; i++) {
00274                 cur.status[i]                   = EC_READ_U16(domain1_pd + off_statusword[i]);
00275                 cur.speed_ctrlr_output[i]       = EC_READ_S16(domain1_pd + off_speed_ctrlr_output[i]);
00276                 cur.pos_ctrlr_output[i]         = EC_READ_S16(domain1_pd + off_pos_ctrlr_output[i]);
00277 
00278                 cur.position[i]             = EC_READ_U32(domain1_pd + off_actual_position[i]);
00279                 cur.actual_velocity[i]          = EC_READ_S32(domain1_pd + off_actual_velocity[i]);
00280                 cur.torque_actual_value[i]      = EC_READ_S16(domain1_pd + off_torque_actual_value[i]); 
00281         }
00282 
00283 
00284     // TODO: factor out these calls
00285         if (counter) {
00286                 counter--;
00287         } else {                /* Do this at 1 Hz */
00288                 counter = FREQUENCY;
00289 
00290                 /* Check for master state (optional). */
00291                 check_master_state();
00292 
00293                 /* Check for slave configuration state(s) (optional). */
00294                 check_slave_config_states();
00295         }
00296 
00297         if(tar.target_position[0] == 0 && 
00298            tar.target_position[1] == 0 && 
00299            tar.target_position[2] == 0 && 
00300            tar.target_position[3] == 0) { 
00301                 for (i = 0; i < 4; i++) { 
00302                         EC_WRITE_U32(domain1_pd + off_target_position[i], cur.position[i]);
00303                 }
00304                 prevent_set_position = 1; 
00305         } else { 
00306                 prevent_set_position = 0; 
00307         }
00308 
00309         /* Write process data. */
00310         /* Note: You _must_ write something, as this is how the drives sync. */
00311         for (i = 0; i < 4; i++) { 
00312                 if(!prevent_set_position) { 
00313                         EC_WRITE_U32(domain1_pd + off_target_position[i], tar.target_position[i]);
00314                 }
00315                 EC_WRITE_S16(domain1_pd + off_torque_add_value[i], tar.torque_add_set_value[i]);
00316                 EC_WRITE_S16(domain1_pd + off_target_velocity[i], tar.target_velocity[i]);
00317         }
00318 
00319         /* Send process data. */
00320         ecrt_domain_queue(domain1);
00321         ecrt_master_send(master);
00322 
00323         cur.pkg_count = counter;
00324 
00325 }
00326 
00327 
00328 /*****************************************************************************/
00329 
00330 
00331 static void enforce_max_velocities(omniwrite_t *t)
00332 {
00333         int i;
00334 
00335         for (i = 0; i < 4; i++) {
00336                 t->target_velocity[i] =
00337                   (t->target_velocity[i] > max_v) ? max_v : t->target_velocity[i];
00338                 t->target_velocity[i] =
00339                   (t->target_velocity[i] < -max_v) ? -max_v : t->target_velocity[i];
00340         }
00341 }
00342 
00343 
00344 /*****************************************************************************/
00345 
00346 static void stop_motors(void)
00347 {
00348         int i;
00349 
00350         for (i = 0; i < 4; i++)
00351                 EC_WRITE_S16(domain1_pd + off_target_velocity[i], 0);
00352 
00353         /* Send process data. */
00354         ecrt_domain_queue(domain1);
00355         ecrt_master_send(master);
00356 
00357         //usleep(1000);
00358 }
00359 
00360 
00361 static void timespecInc(struct timespec *tick, int nsec)
00362 {
00363   tick->tv_nsec += nsec;
00364   while (tick->tv_nsec >= 1e9)
00365   {
00366     tick->tv_nsec -= 1e9;
00367     tick->tv_sec++;
00368   }
00369 }
00370 
00371 
00372 void* realtimeMain(void* udata)
00373 {
00374   struct timespec tick;
00375   int period = 1e+6; // 1 ms in nanoseconds
00376 
00377   while(!exiting)
00378   {
00379     cyclic_task();
00380 
00381     if(pthread_mutex_trylock(&mutex) == 0)
00382     {
00383       tar = tar_buffer;
00384       cur_buffer = cur;
00385       pthread_mutex_unlock(&mutex);
00386     }
00387 
00388     // Compute end of next period
00389     timespecInc(&tick, period);
00390 
00391     struct timespec before;
00392     clock_gettime(CLOCK_REALTIME, &before);
00393     if ((before.tv_sec + before.tv_nsec/1e9) > (tick.tv_sec + tick.tv_nsec/1e9))
00394     {
00395       // We overran, snap to next "period"
00396       tick.tv_sec = before.tv_sec;
00397       tick.tv_nsec = (before.tv_nsec / period) * period;
00398       timespecInc(&tick, period);
00399 
00400       misses++;
00401     }
00402     // Sleep until end of period
00403     clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
00404 
00405   }
00406 
00407   stop_motors();
00408 
00409   
00410   printf("Releasing master...\n");
00411   ecrt_release_master(master);
00412 
00413   return 0;
00414 }
00415 
00416 
00417 /*****************************************************************************/
00418 
00419 /* Interface functions */
00420 
00421 int start_omni_realtime(int max_vel)
00422 {
00423         int i;
00424 
00425     max_v = max_vel;
00426 
00427         printf("Init omni...\n");
00428 
00429         /* Zero out the target/current data structs, just in case. */
00430         memset(&tar, 0, sizeof(tar));
00431         memset(&cur, 0, sizeof(cur));
00432         cur.magic_version = OMNICOM_MAGIC_VERSION;
00433 
00434         printf("Starting omni....\n");
00435 
00436         if (!(master = ecrt_request_master(0))) {
00437                 printf( "Requesting master 0 failed!\n");
00438                 goto out_return;
00439         }
00440 
00441         printf("Registering domain...\n");
00442         if (!(domain1 = ecrt_master_create_domain(master))) {
00443                 printf( "Domain creation failed!\n");
00444                 goto out_release_master;
00445         }
00446 
00447         for (i = 0; i < 4; i++) {
00448                 /* master, (slave alias, slave position), (VID, PID) */
00449                 if (!(sc[i] = ecrt_master_slave_config(master, 0, i /* M?SlavePos */, BM3411))) {
00450                         printf(
00451                                "Failed to get slave configuration for motor %d.\n", i);
00452                         goto out_release_master;
00453                 }
00454                 printf("Configuring PDOs for motor %d...\n", i);
00455                 /* slave config, sync manager index, index of the PDO to assign */
00456                 if (ecrt_slave_config_pdos(sc[i], EC_END, foo_syncs)) {
00457                         printf( "Failed to configure PDOs for motor %d.\n", i);
00458                         goto out_release_master;
00459                 }
00460         }
00461 
00462         printf("Registering PDO entries...\n");
00463         if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
00464                 printf( "PDO entry registration failed!\n");
00465                 goto out_release_master;
00466         }
00467 
00468         printf("Activating master...\n");
00469         if (ecrt_master_activate(master)) {
00470                 printf( "Failed to activate master!\n");
00471                 goto out_release_master;
00472         }
00473         /* Get internal process data for domain. */
00474         domain1_pd = ecrt_domain_data(domain1);
00475 
00476         printf("Starting cyclic thread.\n");
00477 
00478     pthread_attr_t tattr;
00479     struct sched_param sparam;
00480     sparam.sched_priority = sched_get_priority_max(SCHED_FIFO);
00481     pthread_attr_init(&tattr);
00482     pthread_attr_setschedpolicy(&tattr, SCHED_FIFO);
00483     pthread_attr_setschedparam(&tattr, &sparam);
00484     pthread_attr_setinheritsched (&tattr, PTHREAD_EXPLICIT_SCHED);
00485     
00486     if(pthread_create(&thread, &tattr, &realtimeMain, 0) != 0) {
00487       printf("# ERROR: could not create realtime thread\n");
00488       goto out_release_master;
00489     }
00490 
00491 
00492         printf("Started.\n");
00493 
00494         return 1;
00495 
00496 out_release_master:
00497         printf( "Releasing master...\n");
00498         ecrt_release_master(master);
00499 out_return:
00500         printf( "Failed to load. Aborting.\n");
00501         return 0;
00502 }
00503 
00504 /*****************************************************************************/
00505 
00506 void stop_omni_realtime(void)
00507 {
00508         printf("Stopping...\n");
00509 
00510 
00511         /* Signal a stop the realtime thread */
00512     exiting = 1;
00513     pthread_join(thread, 0);
00514 
00515         /* Now stop all motors. */
00516         //stop_motors();
00517         //usleep(100);
00518 
00519 
00520         printf("Unloading.\n");
00521 }
00522 
00523 
00524 void omni_write_data(struct omniwrite data)
00525 {
00526   pthread_mutex_lock(&mutex);
00527   tar_buffer = data;
00528   enforce_max_velocities(&tar_buffer);
00529   pthread_mutex_unlock(&mutex);
00530 }
00531 
00532 struct omniread omni_read_data()
00533 {
00534   struct omniread data;
00535   pthread_mutex_lock(&mutex);
00536   data = cur_buffer;
00537   pthread_mutex_unlock(&mutex);
00538   return data;
00539 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


omni_ethercat
Author(s): Ingo Kresse
autogenerated on Thu May 23 2013 12:56:49