motion.c
1 /******************************************************************** 2 * Description: motion.c 3 * Main module initialisation and cleanup routines. 4 * 5 * Author: 6 * License: GPL Version 2 7 * System: Linux 8 * 9 * Copyright (c) 2004 All rights reserved. 10 ********************************************************************/ 11 12 #include <stdarg.h> 13 #include "rtapi.h" /* RTAPI realtime OS API */ 14 #include "rtapi_app.h" /* RTAPI realtime module decls */ 15 #include "rtapi_string.h" /* memset */ 16 #include "hal.h" /* decls for HAL implementation */ 17 #include "motion.h" 18 #include "motion_debug.h" 19 #include "motion_struct.h" 20 #include "mot_priv.h" 21 #include "rtapi_math.h" 22 #include "homing.h" 23 24 // Mark strings for translation, but defer translation to userspace 25 #define _(s) (s) 26 27 /*********************************************************************** 28 * KERNEL MODULE PARAMETERS * 29 ************************************************************************/ 30 31 /* module information */ 32 /* register symbols to be modified by insmod 33 see "Linux Device Drivers", Alessandro Rubini, p. 385 34 (p.42-44 in 2nd edition) */ 35 MODULE_AUTHOR("Matt Shaver/John Kasunich"); 36 MODULE_DESCRIPTION("Motion Controller for EMC"); 37 MODULE_LICENSE("GPL"); 38 39 /* RTAPI shmem key - for comms with higher level user space stuff */ 40 static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */ 41 RTAPI_MP_INT(key, "shared memory key"); 42 static long base_period_nsec = 0; /* fastest thread period */ 43 RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)"); 44 int base_thread_fp = 0; /* default is no floating point in base thread */ 45 RTAPI_MP_INT(base_thread_fp, "floating point in base thread?"); 46 static long servo_period_nsec = 1000000; /* servo thread period */ 47 RTAPI_MP_LONG(servo_period_nsec, "servo thread period (nsecs)"); 48 static long traj_period_nsec = 0; /* trajectory planner period */ 49 RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)"); 50 static int num_spindles = 1; /* default number of spindles is 1 */ 51 RTAPI_MP_INT (num_spindles, "number of spindles"); 52 int motion_num_spindles; 53 static int num_joints = EMCMOT_MAX_JOINTS; /* default number of joints present */ 54 RTAPI_MP_INT(num_joints, "number of joints used in kinematics"); 55 static int num_extrajoints = 0; /* default number of extra joints present */ 56 RTAPI_MP_INT(num_extrajoints, "number of extra joints (not used in kinematics)"); 57 static int num_dio = DEFAULT_DIO; /* default number of motion synched DIO */ 58 RTAPI_MP_INT(num_dio, "number of digital inputs/outputs"); 59 static int num_aio = DEFAULT_AIO; /* default number of motion synched AIO */ 60 RTAPI_MP_INT(num_aio, "number of analog inputs/outputs"); 61 62 static int unlock_joints_mask = 0;/* mask to select joints for unlock pins */ 63 RTAPI_MP_INT(unlock_joints_mask, "mask to select joints for unlock pins"); 64 /*********************************************************************** 65 * GLOBAL VARIABLE DEFINITIONS * 66 ************************************************************************/ 67 68 /* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */ 69 emcmot_hal_data_t *emcmot_hal_data = 0; 70 71 /* pointer to joint data */ 72 emcmot_joint_t *joints = 0; 73 74 /* pointer to axis data */ 75 emcmot_axis_t *axes = 0; 76 77 #ifndef STRUCTS_IN_SHMEM 78 /* allocate array for joint data */ 79 emcmot_joint_t joint_array[EMCMOT_MAX_JOINTS]; 80 /* allocate array for axis data */ 81 emcmot_axis_t axis_array[EMCMOT_MAX_AXIS]; 82 #endif 83 84 /* 85 Principles of communication: 86 87 Data is copied in or out from the various types of comm mechanisms: 88 mbuff mapped memory for Linux/RT-Linux, or OS shared memory for Unixes. 89 90 emcmotStruct is ptr to this memory. 91 92 emcmotCommand points to emcmotStruct->command, 93 emcmotStatus points to emcmotStruct->status, 94 emcmotError points to emcmotStruct->error, and 95 */ 96 emcmot_struct_t *emcmotStruct = 0; 97 /* ptrs to either buffered copies or direct memory for command and status */ 98 struct emcmot_command_t *emcmotCommand = 0; 99 struct emcmot_status_t *emcmotStatus = 0; 100 struct emcmot_config_t *emcmotConfig = 0; 101 struct emcmot_debug_t *emcmotDebug = 0; 102 struct emcmot_error_t *emcmotError = 0; /* unused for RT_FIFO */ 103 104 /*********************************************************************** 105 * LOCAL VARIABLE DECLARATIONS * 106 ************************************************************************/ 107 108 /* RTAPI shmem ID - for comms with higher level user space stuff */ 109 static int emc_shmem_id; /* the shared memory ID */ 110 111 static int mot_comp_id; /* component ID for motion module */ 112 113 /*********************************************************************** 114 * LOCAL FUNCTION PROTOTYPES * 115 ************************************************************************/ 116 117 /* init_hal_io() exports HAL pins and parameters making data from 118 the realtime control module visible and usable by the world 119 */ 120 static int init_hal_io(void); 121 122 /* functions called by init_hal_io() */ 123 124 // halpins for ALL joints (kinematic joints and extra joints): 125 static int export_joint(int num, joint_hal_t * addr); 126 // additional halpins for extrajoints: 127 static int export_extrajoint(int num, extrajoint_hal_t * addr); 128 129 static int export_axis(char c, axis_hal_t * addr); 130 static int export_spindle(int num, spindle_hal_t * addr); 131 132 /* init_comm_buffers() allocates and initializes the command, 133 status, and error buffers used to communicate witht the user 134 space parts of emc. 135 */ 136 static int init_comm_buffers(void); 137 138 /* functions called by init_comm_buffers() */ 139 140 /* init_threads() creates realtime threads, exports functions to 141 do the realtime control, and adds the functions to the threads. 142 */ 143 static int init_threads(void); 144 145 /* functions called by init_threads() */ 146 static int setTrajCycleTime(double secs); 147 static int setServoCycleTime(double secs); 148 149 /*********************************************************************** 150 * PUBLIC FUNCTION CODE * 151 ************************************************************************/ 152 int joint_is_lockable(int joint_num) { 153 return (unlock_joints_mask & (1 << joint_num) ); 154 } 155 156 void switch_to_teleop_mode(void) { 157 int joint_num; 158 emcmot_joint_t *joint; 159 160 if (emcmotConfig->kinType != KINEMATICS_IDENTITY) { 161 if (!checkAllHomed()) { 162 reportError(_("all joints must be homed before going into teleop mode")); 163 return; 164 } 165 } 166 167 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 168 joint = &joints[joint_num]; 169 if (joint != 0) { joint->free_tp.enable = 0; } 170 } 171 172 emcmotDebug->teleoperating = 1; 173 emcmotDebug->coordinating = 0; 174 } 175 176 177 void emcmot_config_change(void) 178 { 179 if (emcmotConfig->head == emcmotConfig->tail) { 180 emcmotConfig->config_num++; 181 emcmotStatus->config_num = emcmotConfig->config_num; 182 emcmotConfig->head++; 183 } 184 } 185 186 void reportError(const char *fmt, ...) 187 { 188 va_list args; 189 190 va_start(args, fmt); 191 emcmotErrorPutfv(emcmotError, fmt, args); 192 va_end(args); 193 } 194 195 #ifndef va_copy 196 #define va_copy(dest, src) ((dest)=(src)) 197 #endif 198 199 static rtapi_msg_handler_t old_handler = NULL; 200 static void emc_message_handler(msg_level_t level, const char *fmt, va_list ap) 201 { 202 va_list apc; 203 va_copy(apc, ap); 204 if(level == RTAPI_MSG_ERR) emcmotErrorPutfv(emcmotError, fmt, apc); 205 if(old_handler) old_handler(level, fmt, ap); 206 va_end(apc); 207 } 208 209 int rtapi_app_main(void) 210 { 211 int retval; 212 213 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() starting...\n"); 214 215 /* connect to the HAL and RTAPI */ 216 mot_comp_id = hal_init("motmod"); 217 if (mot_comp_id < 0) { 218 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: hal_init() failed\n")); 219 return -1; 220 } 221 if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) { 222 rtapi_print_msg(RTAPI_MSG_ERR, 223 _("MOTION: num_joints is %d, must be between 1 and %d\n"), num_joints, EMCMOT_MAX_JOINTS); 224 hal_exit(mot_comp_id); 225 return -1; 226 } 227 228 if (( num_extrajoints < 0 ) || ( num_extrajoints > num_joints )) { 229 rtapi_print_msg(RTAPI_MSG_ERR, 230 _("\nMOTION: num_extrajoints is %d, must be between 0 and %d\n\n"), num_extrajoints, num_joints); 231 hal_exit(mot_comp_id); 232 return -1; 233 } 234 if ( (num_extrajoints > 0) && (kinematicsType() != KINEMATICS_BOTH) ) { 235 rtapi_print_msg(RTAPI_MSG_ERR, _("\nMOTION: nonzero num_extrajoints requires KINEMATICS_BOTH\n\n")); 236 return -1; 237 } 238 if (num_extrajoints > 0) { 239 rtapi_print_msg(RTAPI_MSG_ERR, 240 _("\nMOTION: kinematicjoints=%2d\n extrajoints=%2d\n Total joints=%2d\n\n"), 241 num_joints-num_extrajoints, num_extrajoints, num_joints 242 ); 243 } 244 245 if (( num_spindles < 0 ) || ( num_spindles > EMCMOT_MAX_SPINDLES )) { 246 rtapi_print_msg(RTAPI_MSG_ERR, 247 _("MOTION: num_spindles is %d, must be between 0 and %d\n"), num_spindles, EMCMOT_MAX_SPINDLES); 248 hal_exit(mot_comp_id); 249 return -1; 250 } 251 motion_num_spindles = num_spindles; 252 253 if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) { 254 rtapi_print_msg(RTAPI_MSG_ERR, 255 _("MOTION: num_dio is %d, must be between 1 and %d\n"), num_dio, EMCMOT_MAX_DIO); 256 hal_exit(mot_comp_id); 257 return -1; 258 } 259 260 if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) { 261 rtapi_print_msg(RTAPI_MSG_ERR, 262 _("MOTION: num_aio is %d, must be between 1 and %d\n"), num_aio, EMCMOT_MAX_AIO); 263 hal_exit(mot_comp_id); 264 return -1; 265 } 266 267 /* initialize/export HAL pins and parameters */ 268 retval = init_hal_io(); 269 if (retval != 0) { 270 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_hal_io() failed\n")); 271 hal_exit(mot_comp_id); 272 return -1; 273 } 274 275 /* allocate/initialize user space comm buffers (cmd/status/err) */ 276 retval = init_comm_buffers(); 277 if (retval != 0) { 278 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_comm_buffers() failed\n")); 279 hal_exit(mot_comp_id); 280 return -1; 281 } 282 283 /* set up for realtime execution of code */ 284 retval = init_threads(); 285 if (retval != 0) { 286 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_threads() failed\n")); 287 hal_exit(mot_comp_id); 288 return -1; 289 } 290 291 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() complete\n"); 292 293 hal_ready(mot_comp_id); 294 295 old_handler = rtapi_get_msg_handler(); 296 rtapi_set_msg_handler(emc_message_handler); 297 return 0; 298 } 299 300 void rtapi_app_exit(void) 301 { 302 int retval; 303 304 rtapi_set_msg_handler(old_handler); 305 306 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() started.\n"); 307 308 retval = hal_stop_threads(); 309 if (retval < 0) { 310 rtapi_print_msg(RTAPI_MSG_ERR, 311 _("MOTION: hal_stop_threads() failed, returned %d\n"), retval); 312 } 313 /* free shared memory */ 314 retval = rtapi_shmem_delete(emc_shmem_id, mot_comp_id); 315 if (retval < 0) { 316 rtapi_print_msg(RTAPI_MSG_ERR, 317 _("MOTION: rtapi_shmem_delete() failed, returned %d\n"), retval); 318 } 319 /* disconnect from HAL and RTAPI */ 320 retval = hal_exit(mot_comp_id); 321 if (retval < 0) { 322 rtapi_print_msg(RTAPI_MSG_ERR, 323 _("MOTION: hal_exit() failed, returned %d\n"), retval); 324 } 325 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() finished.\n"); 326 } 327 328 /*********************************************************************** 329 * LOCAL FUNCTION CODE * 330 ************************************************************************/ 331 332 /* init_hal_io() exports HAL pins and parameters making data from 333 the realtime control module visible and usable by the world 334 */ 335 static int init_hal_io(void) 336 { 337 int n, retval; 338 joint_hal_t *joint_data; 339 extrajoint_hal_t *ejoint_data; 340 axis_hal_t *axis_data; 341 342 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n"); 343 344 /* allocate shared memory for machine data */ 345 emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t)); 346 if (emcmot_hal_data == 0) { 347 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: emcmot_hal_data malloc failed\n")); 348 return -1; 349 } 350 351 /* export machine wide hal pins */ 352 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) != 0) goto error; 353 if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) != 0) goto error; 354 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) != 0) goto error; 355 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) != 0) goto error; 356 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->homing_inhibit), mot_comp_id, "motion.homing-inhibit")) != 0) goto error; 357 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->tp_reverse), mot_comp_id, "motion.tp-reverse")) < 0) goto error; 358 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) != 0) goto error; 359 360 /* export motion-synched digital output pins */ 361 /* export motion digital input pins */ 362 for (n = 0; n < num_dio; n++) { 363 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) != 0) goto error; 364 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) != 0) goto error; 365 } 366 367 /* export motion-synched analog output pins */ 368 /* export motion analog input pins */ 369 for (n = 0; n < num_aio; n++) { 370 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) != 0) goto error; 371 if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) != 0) goto error; 372 } 373 374 /* export machine wide hal pins */ 375 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->motion_enabled), mot_comp_id, "motion.motion-enabled")) != 0) goto error; 376 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->in_position), mot_comp_id, "motion.in-position")) != 0) goto error; 377 if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->motion_type), mot_comp_id, "motion.motion-type")) != 0) goto error; 378 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->coord_mode), mot_comp_id, "motion.coord-mode")) != 0) goto error; 379 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->teleop_mode), mot_comp_id, "motion.teleop-mode")) != 0) goto error; 380 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->coord_error), mot_comp_id, "motion.coord-error")) != 0) goto error; 381 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->on_soft_limit), mot_comp_id, "motion.on-soft-limit")) != 0) goto error; 382 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->current_vel), mot_comp_id, "motion.current-vel")) != 0) goto error; 383 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->requested_vel), mot_comp_id, "motion.requested-vel")) != 0) goto error; 384 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->distance_to_go), mot_comp_id, "motion.distance-to-go")) != 0) goto error; 385 if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id, "motion.program-line")) != 0) goto error; 386 387 /* export debug parameters */ 388 /* these can be used to view any internal variable, simply change a line 389 in control.c:output_to_hal() and recompile */ 390 if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_0), mot_comp_id, "motion.debug-bit-0")) != 0) goto error; 391 if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_1), mot_comp_id, "motion.debug-bit-1")) != 0) goto error; 392 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_0), mot_comp_id, "motion.debug-float-0")) != 0) goto error; 393 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_1), mot_comp_id, "motion.debug-float-1")) != 0) goto error; 394 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_2), mot_comp_id, "motion.debug-float-2")) != 0) goto error; 395 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_3), mot_comp_id, "motion.debug-float-3")) != 0) goto error; 396 if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id, "motion.debug-s32-0")) != 0) goto error; 397 if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id, "motion.debug-s32-1")) != 0) goto error; 398 399 // FIXME - debug only, remove later 400 // export HAL parameters for some trajectory planner internal variables 401 // so they can be scoped 402 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_pos_out), mot_comp_id, "traj.pos_out")) != 0) goto error; 403 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_vel_out), mot_comp_id, "traj.vel_out")) != 0) goto error; 404 if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->traj_active_tc), mot_comp_id, "traj.active_tc")) != 0) goto error; 405 406 for ( n = 0 ; n < 4 ; n++ ) { 407 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n)) != 0) goto error; 408 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n)) != 0) goto error; 409 if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n)) != 0) goto error; 410 } 411 // end of exporting trajectory planner internals 412 413 // export timing related HAL pins so they can be scoped and/or connected 414 if ((retval = hal_pin_u32_newf(HAL_OUT, &(emcmot_hal_data->last_period), mot_comp_id, "motion.servo.last-period")) != 0) goto error; 415 #ifdef HAVE_CPU_KHZ 416 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->last_period_ns), mot_comp_id, "motion.servo.last-period-ns")) != 0) goto error; 417 #endif 418 419 // export timing related HAL pins so they can be scoped 420 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id, "motion.tooloffset.x")) != 0) goto error; 421 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id, "motion.tooloffset.y")) != 0) goto error; 422 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id, "motion.tooloffset.z")) != 0) goto error; 423 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id, "motion.tooloffset.a")) != 0) goto error; 424 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id, "motion.tooloffset.b")) != 0) goto error; 425 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id, "motion.tooloffset.c")) != 0) goto error; 426 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id, "motion.tooloffset.u")) != 0) goto error; 427 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id, "motion.tooloffset.v")) != 0) goto error; 428 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id, "motion.tooloffset.w")) != 0) goto error; 429 430 /* initialize machine wide pins and parameters */ 431 *(emcmot_hal_data->adaptive_feed) = 1.0; 432 *(emcmot_hal_data->feed_hold) = 0; 433 *(emcmot_hal_data->feed_inhibit) = 0; 434 *(emcmot_hal_data->homing_inhibit) = 0; 435 436 *(emcmot_hal_data->probe_input) = 0; 437 /* default value of enable is TRUE, so simple machines 438 can leave it disconnected */ 439 *(emcmot_hal_data->enable) = 1; 440 441 /* motion synched dio, init to not enabled */ 442 for (n = 0; n < num_dio; n++) { 443 *(emcmot_hal_data->synch_do[n]) = 0; 444 *(emcmot_hal_data->synch_di[n]) = 0; 445 } 446 447 for (n = 0; n < num_aio; n++) { 448 *(emcmot_hal_data->analog_output[n]) = 0.0; 449 *(emcmot_hal_data->analog_input[n]) = 0.0; 450 } 451 452 /*! \todo FIXME - these don't really need initialized, since they are written 453 with data from the emcmotStatus struct */ 454 *(emcmot_hal_data->motion_enabled) = 0; 455 *(emcmot_hal_data->in_position) = 0; 456 *(emcmot_hal_data->motion_type) = 0; 457 *(emcmot_hal_data->coord_mode) = 0; 458 *(emcmot_hal_data->teleop_mode) = 0; 459 *(emcmot_hal_data->coord_error) = 0; 460 *(emcmot_hal_data->on_soft_limit) = 0; 461 462 /* init debug parameters */ 463 emcmot_hal_data->debug_bit_0 = 0; 464 emcmot_hal_data->debug_bit_1 = 0; 465 emcmot_hal_data->debug_float_0 = 0.0; 466 emcmot_hal_data->debug_float_1 = 0.0; 467 emcmot_hal_data->debug_float_2 = 0.0; 468 emcmot_hal_data->debug_float_3 = 0.0; 469 470 *(emcmot_hal_data->last_period) = 0; 471 472 /* export spindle pins and params */ 473 for (n=0; n < num_spindles; n++) { 474 retval = export_spindle(n, &(emcmot_hal_data->spindle[n])); 475 if (retval != 0){ 476 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: spindle %d pin export failed"), n); 477 return -1; 478 } 479 } 480 481 /* export joint pins and parameters */ 482 for (n = 0; n < num_joints; n++) { 483 joint_data = &(emcmot_hal_data->joint[n]); 484 /* export all vars */ 485 retval = export_joint(n, joint_data); 486 if (retval != 0) { 487 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n); 488 return -1; 489 } 490 *(joint_data->amp_enable) = 0; 491 492 /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now, 493 because it is always supported. */ 494 } 495 496 /* export joint home pins (assigned to motion comp)*/ 497 retval = export_joint_home_pins(num_joints,mot_comp_id); 498 if (retval != 0) { 499 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: export_joint_home_pins failed\n")); 500 return -1; 501 } 502 /* export joint pins and parameters */ 503 for (n = 0; n < num_extrajoints; n++) { 504 ejoint_data = &(emcmot_hal_data->ejoint[n]); 505 retval = export_extrajoint(n + num_joints - num_extrajoints,ejoint_data); 506 if (retval != 0) { 507 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: ejoint %d pin/param export failed\n"), n); 508 return -1; 509 } 510 } 511 512 /* export axis pins and parameters */ 513 for (n = 0; n < EMCMOT_MAX_AXIS; n++) { 514 char c = "xyzabcuvw"[n]; 515 axis_data = &(emcmot_hal_data->axis[n]); 516 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].pos_cmd), 517 mot_comp_id, "axis.%c.pos-cmd", c)) != 0) goto error; 518 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].teleop_vel_cmd), 519 mot_comp_id, "axis.%c.teleop-vel-cmd", c)) != 0) goto error; 520 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].teleop_pos_cmd), 521 mot_comp_id, "axis.%c.teleop-pos-cmd", c)) != 0) goto error; 522 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].teleop_vel_lim), 523 mot_comp_id, "axis.%c.teleop-vel-lim", c)) != 0) goto error; 524 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->axis[n].teleop_tp_enable), 525 mot_comp_id, "axis.%c.teleop-tp-enable",c)) != 0) goto error; 526 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_enable), 527 mot_comp_id, "axis.%c.eoffset-enable", c)) != 0) return retval; 528 if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_clear), 529 mot_comp_id, "axis.%c.eoffset-clear", c)) != 0) return retval; 530 531 if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_counts), 532 mot_comp_id, "axis.%c.eoffset-counts", c)) != 0) return retval; 533 if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->axis[n].eoffset_scale), 534 mot_comp_id, "axis.%c.eoffset-scale", c)) != 0) return retval; 535 536 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].external_offset), 537 mot_comp_id, "axis.%c.eoffset", c)) != 0) return retval; 538 if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->axis[n].external_offset_requested), 539 mot_comp_id, "axis.%c.eoffset-request", c)) != 0) return retval; 540 541 retval = export_axis(c, axis_data); 542 if (retval != 0) { 543 rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: axis %c pin/param export failed\n"), c); 544 return -1; 545 } 546 } 547 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->eoffset_limited), mot_comp_id, 548 "motion.eoffset-limited")) < 0) goto error; 549 if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->eoffset_active), mot_comp_id, 550 "motion.eoffset-active")) < 0) goto error; 551 552 /* Done! */ 553 rtapi_print_msg(RTAPI_MSG_INFO, 554 "MOTION: init_hal_io() complete, %d axes.\n", n); 555 return 0; 556 557 error: 558 return retval; 559 560 } 561 562 static int export_spindle(int num, spindle_hal_t * addr){ 563 int retval, msg; 564 565 msg = rtapi_get_msg_level(); 566 rtapi_set_msg_level(RTAPI_MSG_WARN); 567 568 if ((retval = hal_pin_bit_newf(HAL_IO, &(addr->spindle_index_enable), mot_comp_id, "spindle.%d.index-enable", num)) != 0) return retval; 569 570 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_on), mot_comp_id, "spindle.%d.on", num)) != 0) return retval; 571 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_forward), mot_comp_id, "spindle.%d.forward", num)) != 0) return retval; 572 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_reverse), mot_comp_id, "spindle.%d.reverse", num)) != 0) return retval; 573 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_brake), mot_comp_id, "spindle.%d.brake", num)) != 0) return retval; 574 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_speed_out), mot_comp_id, "spindle.%d.speed-out", num)) != 0) return retval; 575 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_speed_out_abs), mot_comp_id, "spindle.%d.speed-out-abs", num)) != 0) return retval; 576 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_speed_out_rps), mot_comp_id, "spindle.%d.speed-out-rps", num)) != 0) return retval; 577 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_speed_out_rps_abs), mot_comp_id, "spindle.%d.speed-out-rps-abs", num)) != 0) return retval; 578 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_speed_cmd_rps), mot_comp_id, "spindle.%d.speed-cmd-rps", num)) != 0) return retval; 579 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->spindle_inhibit), mot_comp_id, "spindle.%d.inhibit", num)) != 0) return retval; 580 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->spindle_amp_fault), mot_comp_id, "spindle.%d.amp-fault-in", num)) != 0) return retval; 581 *(addr->spindle_inhibit) = 0; 582 583 // spindle orient pins 584 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->spindle_orient_angle), mot_comp_id, "spindle.%d.orient-angle", num)) < 0) return retval; 585 if ((retval = hal_pin_s32_newf(HAL_OUT, &(addr->spindle_orient_mode), mot_comp_id, "spindle.%d.orient-mode", num)) < 0) return retval; 586 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_orient), mot_comp_id, "spindle.%d.orient", num)) < 0) return retval; 587 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->spindle_locked), mot_comp_id, "spindle.%d.locked", num)) < 0) return retval; 588 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->spindle_is_oriented), mot_comp_id, "spindle.%d.is-oriented", num)) < 0) return retval; 589 if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->spindle_orient_fault), mot_comp_id, "spindle.%d.orient-fault", num)) < 0) return retval; 590 *(addr->spindle_orient_angle) = 0.0; 591 *(addr->spindle_orient_mode) = 0; 592 *(addr->spindle_orient) = 0; 593 594 if ((retval = hal_pin_float_newf(HAL_IN, &(addr->spindle_revs), mot_comp_id, "spindle.%d.revs", num)) != 0) return retval; 595 if ((retval = hal_pin_float_newf(HAL_IN, &(addr->spindle_speed_in), mot_comp_id, "spindle.%d.speed-in", num)) != 0) return retval; 596 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->spindle_is_atspeed), mot_comp_id, "spindle.%d.at-speed", num)) != 0) return retval; 597 *(addr->spindle_is_atspeed) = 1; 598 /* restore saved message level */ 599 rtapi_set_msg_level(msg); 600 return 0; 601 } 602 603 static int export_joint(int num, joint_hal_t * addr) 604 { 605 int retval, msg; 606 607 /* This function exports a lot of stuff, which results in a lot of 608 logging if msg_level is at INFO or ALL. So we save the current value 609 of msg_level and restore it later. If you actually need to log this 610 function's actions, change the second line below */ 611 msg = rtapi_get_msg_level(); 612 rtapi_set_msg_level(RTAPI_MSG_WARN); 613 614 /* export joint pins */ 615 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd), mot_comp_id, "joint.%d.coarse-pos-cmd", num)) != 0) return retval; 616 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "joint.%d.pos-cmd", num)) != 0) return retval; 617 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "joint.%d.pos-fb", num)) != 0) return retval; 618 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "joint.%d.motor-pos-cmd", num)) != 0) return retval; 619 if ((retval = hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "joint.%d.motor-pos-fb", num)) != 0) return retval; 620 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num)) != 0) return retval; 621 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "joint.%d.pos-lim-sw-in", num)) != 0) return retval; 622 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num)) != 0) return retval; 623 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num)) != 0) return retval; 624 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "joint.%d.amp-fault-in", num)) != 0) return retval; 625 if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->jjog_counts), mot_comp_id, "joint.%d.jog-counts", num)) != 0) return retval; 626 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->jjog_enable), mot_comp_id, "joint.%d.jog-enable", num)) != 0) return retval; 627 if ((retval = hal_pin_float_newf(HAL_IN, &(addr->jjog_scale), mot_comp_id, "joint.%d.jog-scale", num)) != 0) return retval; 628 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->jjog_vel_mode), mot_comp_id, "joint.%d.jog-vel-mode", num)) != 0) return retval; 629 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "joint.%d.vel-cmd", num)) != 0) return retval; 630 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_acc_cmd), mot_comp_id, "joint.%d.acc-cmd", num)) != 0) return retval; 631 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "joint.%d.backlash-corr", num)) != 0) return retval; 632 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "joint.%d.backlash-filt", num)) != 0) return retval; 633 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "joint.%d.backlash-vel", num)) != 0) return retval; 634 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "joint.%d.f-error", num)) != 0) return retval; 635 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "joint.%d.f-error-lim", num)) != 0) return retval; 636 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "joint.%d.free-pos-cmd", num)) != 0) return retval; 637 if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "joint.%d.free-vel-lim", num)) != 0) return retval; 638 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "joint.%d.free-tp-enable", num)) != 0) return retval; 639 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->kb_jjog_active), mot_comp_id, "joint.%d.kb-jog-active", num)) != 0) return retval; 640 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jjog_active), mot_comp_id, "joint.%d.wheel-jog-active", num)) != 0) return retval; 641 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "joint.%d.in-position", num)) != 0) return retval; 642 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "joint.%d.pos-hard-limit", num)) != 0) return retval; 643 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "joint.%d.neg-hard-limit", num)) != 0) return retval; 644 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "joint.%d.active", num)) != 0) return retval; 645 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num)) != 0) return retval; 646 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "joint.%d.f-errored", num)) != 0) return retval; 647 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "joint.%d.faulted", num)) != 0) return retval; 648 if ((retval = hal_pin_float_newf(HAL_IN,&(addr->jjog_accel_fraction),mot_comp_id,"joint.%d.jog-accel-fraction", num)) != 0) return retval; 649 *addr->jjog_accel_fraction = 1.0; // fraction of accel for wheel jjogs 650 651 if ( joint_is_lockable(num) ) { 652 // these pins may be needed for rotary joints 653 rtapi_print_msg(RTAPI_MSG_WARN,"motion.c: Creating unlock hal pins for joint %d\n",num); 654 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "joint.%d.unlock", num)) != 0) return retval; 655 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "joint.%d.is-unlocked", num)) != 0) return retval; 656 } 657 658 /* restore saved message level */ 659 rtapi_set_msg_level(msg); 660 return 0; 661 } 662 663 static int export_extrajoint(int num, extrajoint_hal_t * addr) 664 { 665 int retval; 666 /* export extrajoint pins */ 667 if ((retval = hal_pin_float_newf(HAL_IN, &(addr->posthome_cmd), mot_comp_id, 668 "joint.%d.posthome-cmd", num)) != 0) return retval; 669 return 0; 670 } 671 672 static int export_axis(char c, axis_hal_t * addr) 673 { 674 int retval, msg; 675 msg = rtapi_get_msg_level(); 676 rtapi_set_msg_level(RTAPI_MSG_WARN); 677 678 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->ajog_enable), mot_comp_id,"axis.%c.jog-enable", c)) != 0) return retval; 679 if ((retval = hal_pin_float_newf(HAL_IN,&(addr->ajog_scale), mot_comp_id,"axis.%c.jog-scale", c)) != 0) return retval; 680 if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->ajog_counts), mot_comp_id,"axis.%c.jog-counts", c)) != 0) return retval; 681 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->ajog_vel_mode), mot_comp_id,"axis.%c.jog-vel-mode", c)) != 0) return retval; 682 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->kb_ajog_active), mot_comp_id,"axis.%c.kb-jog-active", c)) != 0) return retval; 683 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->wheel_ajog_active),mot_comp_id,"axis.%c.wheel-jog-active", c)) != 0) return retval; 684 685 if ((retval = hal_pin_float_newf(HAL_IN,&(addr->ajog_accel_fraction), mot_comp_id,"axis.%c.jog-accel-fraction", c)) != 0) return retval; 686 *addr->ajog_accel_fraction = 1.0; // fraction of accel for wheel ajogs 687 688 rtapi_set_msg_level(msg); 689 return 0; 690 } 691 692 /* init_comm_buffers() allocates and initializes the command, 693 status, and error buffers used to communicate with the user 694 space parts of emc. 695 */ 696 static int init_comm_buffers(void) 697 { 698 int joint_num, axis_num, spindle_num, n; 699 emcmot_joint_t *joint; 700 int retval; 701 702 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() starting...\n"); 703 704 emcmotStruct = 0; 705 emcmotDebug = 0; 706 emcmotStatus = 0; 707 emcmotCommand = 0; 708 emcmotConfig = 0; 709 710 /* allocate and initialize the shared memory structure */ 711 emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t)); 712 if (emc_shmem_id < 0) { 713 rtapi_print_msg(RTAPI_MSG_ERR, 714 "MOTION: rtapi_shmem_new failed, returned %d\n", emc_shmem_id); 715 return -1; 716 } 717 retval = rtapi_shmem_getptr(emc_shmem_id, (void **) &emcmotStruct); 718 if (retval < 0) { 719 rtapi_print_msg(RTAPI_MSG_ERR, 720 "MOTION: rtapi_shmem_getptr failed, returned %d\n", retval); 721 return -1; 722 } 723 724 /* zero shared memory before doing anything else. */ 725 memset(emcmotStruct, 0, sizeof(emcmot_struct_t)); 726 727 /* we'll reference emcmotStruct directly */ 728 emcmotCommand = &emcmotStruct->command; 729 emcmotStatus = &emcmotStruct->status; 730 emcmotConfig = &emcmotStruct->config; 731 emcmotDebug = &emcmotStruct->debug; 732 emcmotError = &emcmotStruct->error; 733 734 /* init error struct */ 735 emcmotErrorInit(emcmotError); 736 737 /* init command struct */ 738 emcmotCommand->head = 0; 739 emcmotCommand->command = 0; 740 emcmotCommand->commandNum = 0; 741 emcmotCommand->tail = 0; 742 743 /* init status struct */ 744 emcmotStatus->head = 0; 745 emcmotStatus->commandEcho = 0; 746 emcmotStatus->commandNumEcho = 0; 747 emcmotStatus->commandStatus = 0; 748 749 /* init more stuff */ 750 emcmotDebug->head = 0; 751 emcmotConfig->head = 0; 752 753 emcmotStatus->motionFlag = 0; 754 SET_MOTION_ERROR_FLAG(0); 755 SET_MOTION_COORD_FLAG(0); 756 SET_MOTION_TELEOP_FLAG(0); 757 emcmotDebug->split = 0; 758 emcmotStatus->heartbeat = 0; 759 760 ALL_JOINTS = num_joints; // emcmotConfig->numJoints from [KINS]JOINTS 761 emcmotConfig->numExtraJoints = num_extrajoints; // from motmod num_extrajoints= 762 emcmotStatus->numExtraJoints = num_extrajoints; 763 764 emcmotConfig->numSpindles = num_spindles; 765 emcmotConfig->numDIO = num_dio; 766 emcmotConfig->numAIO = num_aio; 767 768 ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd); 769 ZERO_EMC_POSE(emcmotStatus->carte_pos_fb); 770 emcmotStatus->vel = 0.0; 771 emcmotConfig->limitVel = 0.0; 772 emcmotStatus->acc = 0.0; 773 emcmotStatus->feed_scale = 1.0; 774 emcmotStatus->rapid_scale = 1.0; 775 emcmotStatus->net_feed_scale = 1.0; 776 /* adaptive feed is off by default, feed override, spindle 777 override, and feed hold are on */ 778 emcmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED; 779 emcmotStatus->enables_queued = emcmotStatus->enables_new; 780 emcmotStatus->id = 0; 781 emcmotStatus->depth = 0; 782 emcmotStatus->activeDepth = 0; 783 emcmotStatus->paused = 0; 784 emcmotStatus->overrideLimitMask = 0; 785 SET_MOTION_INPOS_FLAG(1); 786 SET_MOTION_ENABLE_FLAG(0); 787 /* record the kinematics type of the machine */ 788 emcmotConfig->kinType = kinematicsType(); 789 emcmot_config_change(); 790 791 /* init pointer to joint structs */ 792 #ifdef STRUCTS_IN_SHMEM 793 joints = &(emcmotDebug->joints[0]); 794 axes = &(emcmotDebug->axes[0]); 795 #else 796 joints = &(joint_array[0]); 797 axes = &(axis_array[0]); 798 #endif 799 800 for (spindle_num = 0; spindle_num < EMCMOT_MAX_SPINDLES; spindle_num++){ 801 emcmotStatus->spindle_status[spindle_num].scale = 1.0; 802 emcmotStatus->spindle_status[spindle_num].speed = 0.0; 803 } 804 805 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 806 emcmot_axis_t *axis; 807 axis = &axes[axis_num]; 808 axis->locking_joint = -1; 809 } 810 /* init per-joint stuff */ 811 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 812 /* point to structure for this joint */ 813 joint = &joints[joint_num]; 814 815 /* init the config fields with some "reasonable" defaults" */ 816 joint->type = 0; 817 joint->max_pos_limit = 1.0; 818 joint->min_pos_limit = -1.0; 819 joint->vel_limit = 1.0; 820 joint->acc_limit = 1.0; 821 joint->min_ferror = 0.01; 822 joint->max_ferror = 1.0; 823 joint->backlash = 0.0; 824 825 joint->comp.entries = 0; 826 joint->comp.entry = &(joint->comp.array[0]); 827 /* the compensation code has -DBL_MAX at one end of the table 828 and +DBL_MAX at the other so _all_ commanded positions are 829 guaranteed to be covered by the table */ 830 joint->comp.array[0].nominal = -DBL_MAX; 831 joint->comp.array[0].fwd_trim = 0.0; 832 joint->comp.array[0].rev_trim = 0.0; 833 joint->comp.array[0].fwd_slope = 0.0; 834 joint->comp.array[0].rev_slope = 0.0; 835 for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) { 836 joint->comp.array[n].nominal = DBL_MAX; 837 joint->comp.array[n].fwd_trim = 0.0; 838 joint->comp.array[n].rev_trim = 0.0; 839 joint->comp.array[n].fwd_slope = 0.0; 840 joint->comp.array[n].rev_slope = 0.0; 841 } 842 843 /* init joint flags */ 844 joint->flag = 0; 845 SET_JOINT_INPOS_FLAG(joint, 1); 846 847 /* init status info */ 848 joint->coarse_pos = 0.0; 849 joint->pos_cmd = 0.0; 850 joint->vel_cmd = 0.0; 851 joint->acc_cmd = 0.0; 852 joint->backlash_corr = 0.0; 853 joint->backlash_filt = 0.0; 854 joint->backlash_vel = 0.0; 855 joint->motor_pos_cmd = 0.0; 856 joint->motor_pos_fb = 0.0; 857 joint->pos_fb = 0.0; 858 joint->ferror = 0.0; 859 joint->ferror_limit = joint->min_ferror; 860 joint->ferror_high_mark = 0.0; 861 862 /* init internal info */ 863 cubicInit(&(joint->cubic)); 864 } 865 866 homing_init(); // for all joints 867 868 /*! \todo FIXME-- add emcmotError */ 869 870 emcmotDebug->cur_time = emcmotDebug->last_time = 0.0; 871 emcmotDebug->start_time = etime(); 872 emcmotDebug->running_time = 0.0; 873 874 /* init motion emcmotDebug->coord_tp */ 875 if (-1 == tpCreate(&emcmotDebug->coord_tp, DEFAULT_TC_QUEUE_SIZE, 876 emcmotDebug->queueTcSpace)) { 877 rtapi_print_msg(RTAPI_MSG_ERR, 878 "MOTION: failed to create motion emcmotDebug->coord_tp\n"); 879 return -1; 880 } 881 // tpInit(&emcmotDebug->coord_tp); // tpInit called from tpCreate 882 tpSetCycleTime(&emcmotDebug->coord_tp, emcmotConfig->trajCycleTime); 883 tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); 884 tpSetVmax(&emcmotDebug->coord_tp, emcmotStatus->vel, emcmotStatus->vel); 885 tpSetAmax(&emcmotDebug->coord_tp, emcmotStatus->acc); 886 887 emcmotStatus->tail = 0; 888 889 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n"); 890 return 0; 891 } 892 893 /* init_threads() creates realtime threads, exports functions to 894 do the realtime control, and adds the functions to the threads. 895 */ 896 static int init_threads(void) 897 { 898 double base_period_sec, servo_period_sec; 899 int servo_base_ratio; 900 int retval; 901 902 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() starting...\n"); 903 904 /* if base_period not specified, assume same as servo_period */ 905 if (base_period_nsec == 0) { 906 base_period_nsec = servo_period_nsec; 907 } 908 if (traj_period_nsec == 0) { 909 traj_period_nsec = servo_period_nsec; 910 } 911 /* servo period must be greater or equal to base period */ 912 if (servo_period_nsec < base_period_nsec) { 913 rtapi_print_msg(RTAPI_MSG_ERR, 914 "MOTION: bad servo period %ld nsec\n", servo_period_nsec); 915 return -1; 916 } 917 /* convert desired periods to floating point */ 918 base_period_sec = base_period_nsec * 0.000000001; 919 servo_period_sec = servo_period_nsec * 0.000000001; 920 /* calculate period ratios, round to nearest integer */ 921 servo_base_ratio = (servo_period_sec / base_period_sec) + 0.5; 922 /* revise desired periods to be integer multiples of each other */ 923 servo_period_nsec = base_period_nsec * servo_base_ratio; 924 /* create HAL threads for each period */ 925 /* only create base thread if it is faster than servo thread */ 926 if (servo_base_ratio > 1) { 927 retval = hal_create_thread("base-thread", base_period_nsec, base_thread_fp); 928 if (retval < 0) { 929 rtapi_print_msg(RTAPI_MSG_ERR, 930 "MOTION: failed to create %ld nsec base thread\n", 931 base_period_nsec); 932 return -1; 933 } 934 } 935 retval = hal_create_thread("servo-thread", servo_period_nsec, 1); 936 if (retval < 0) { 937 rtapi_print_msg(RTAPI_MSG_ERR, 938 "MOTION: failed to create %ld nsec servo thread\n", 939 servo_period_nsec); 940 return -1; 941 } 942 /* export realtime functions that do the real work */ 943 retval = hal_export_funct("motion-controller", emcmotController, 0 /* arg 944 */ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id); 945 if (retval < 0) { 946 rtapi_print_msg(RTAPI_MSG_ERR, 947 "MOTION: failed to export controller function\n"); 948 return -1; 949 } 950 retval = hal_export_funct("motion-command-handler", emcmotCommandHandler, 0 /* arg 951 */ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id); 952 if (retval < 0) { 953 rtapi_print_msg(RTAPI_MSG_ERR, 954 "MOTION: failed to export command handler function\n"); 955 return -1; 956 } 957 /*! \todo Another #if 0 */ 958 #if 0 959 /*! \todo FIXME - currently the traj planner is called from the controller */ 960 /* eventually it will be a separate function */ 961 retval = hal_export_funct("motion-traj-planner", emcmotTrajPlanner, 0 /* arg 962 */ , 1 /* uses_fp */ , 963 0 /* reentrant */ , mot_comp_id); 964 if (retval < 0) { 965 rtapi_print_msg(RTAPI_MSG_ERR, 966 "MOTION: failed to export traj planner function\n"); 967 return -1; 968 } 969 #endif 970 971 // if we don't set cycle times based on these guesses, emc doesn't 972 // start up right 973 setServoCycleTime(servo_period_nsec * 1e-9); 974 setTrajCycleTime(traj_period_nsec * 1e-9); 975 976 rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() complete\n"); 977 return 0; 978 } 979 980 void emcmotSetCycleTime(unsigned long nsec ) 981 { 982 int servo_mult; 983 servo_mult = traj_period_nsec / nsec; 984 if(servo_mult < 0) servo_mult = 1; 985 setTrajCycleTime(nsec * 1e-9); 986 setServoCycleTime(nsec * servo_mult * 1e-9); 987 } 988 989 /* call this when setting the trajectory cycle time */ 990 static int setTrajCycleTime(double secs) 991 { 992 static int t; 993 994 rtapi_print_msg(RTAPI_MSG_INFO, 995 "MOTION: setting Traj cycle time to %ld nsecs\n", (long) (secs * 1e9)); 996 997 /* make sure it's not zero */ 998 if (secs <= 0.0) { 999 return -1; 1000 } 1001 1002 emcmot_config_change(); 1003 1004 /* compute the interpolation rate as nearest integer to traj/servo */ 1005 if(emcmotConfig->servoCycleTime) 1006 emcmotConfig->interpolationRate = 1007 (int) (secs / emcmotConfig->servoCycleTime + 0.5); 1008 else 1009 emcmotConfig->interpolationRate = 1; 1010 1011 /* set traj planner */ 1012 tpSetCycleTime(&emcmotDebug->coord_tp, secs); 1013 1014 /* set the free planners, cubic interpolation rate and segment time */ 1015 for (t = 0; t < ALL_JOINTS; t++) { 1016 cubicSetInterpolationRate(&(joints[t].cubic), 1017 emcmotConfig->interpolationRate); 1018 } 1019 1020 /* copy into status out */ 1021 emcmotConfig->trajCycleTime = secs; 1022 1023 return 0; 1024 } 1025 1026 /* call this when setting the servo cycle time */ 1027 static int setServoCycleTime(double secs) 1028 { 1029 static int t; 1030 1031 rtapi_print_msg(RTAPI_MSG_INFO, 1032 "MOTION: setting Servo cycle time to %ld nsecs\n", (long) (secs * 1e9)); 1033 1034 /* make sure it's not zero */ 1035 if (secs <= 0.0) { 1036 return -1; 1037 } 1038 1039 emcmot_config_change(); 1040 1041 /* compute the interpolation rate as nearest integer to traj/servo */ 1042 emcmotConfig->interpolationRate = 1043 (int) (emcmotConfig->trajCycleTime / secs + 0.5); 1044 1045 /* set the cubic interpolation rate and PID cycle time */ 1046 for (t = 0; t < ALL_JOINTS; t++) { 1047 cubicSetInterpolationRate(&(joints[t].cubic), 1048 emcmotConfig->interpolationRate); 1049 cubicSetSegmentTime(&(joints[t].cubic), secs); 1050 } 1051 1052 /* copy into status out */ 1053 emcmotConfig->servoCycleTime = secs; 1054 1055 return 0; 1056 }