/*
tpClear() is a "soft init" in the sense that the TP_STRUCT configuration
parameters (cycleTime, vMax, and aMax) are left alone, but the queue is
cleared, and the flags are set to an empty, ready queue. The currentPos
is left alone, and goalPos is set to this position.
This function is intended to put the motion queue in the state it would
be if all queued motions finished at the current position.
*/
int tpClear(TP_STRUCT * tp)
{
tcqInit(&tp->queue);
tp->queueSize = 0;
tp->goalPos = tp->currentPos;
tp->nextId = 0;
tp->execId = 0;
tp->motionType = 0;
tp->termCond = TC_TERM_COND_BLEND;
tp->tolerance = 0.0;
tp->done = 1;
tp->depth = tp->activeDepth = 0;
tp->aborting = 0;
tp->pausing = 0;
tp->vScale = emcmotStatus->net_feed_scale;
tp->synchronized = 0;
tp->velocity_mode = 0;
tp->uu_per_rev = 0.0;
emcmotStatus->spindleSync = 0;
emcmotStatus->current_vel = 0.0;
emcmotStatus->requested_vel = 0.0;
emcmotStatus->distance_to_go = 0.0;
ZERO_EMC_POSE(emcmotStatus->dtg);
return tpClearDIOs();
}
EmcPose tpGetPos(TP_STRUCT * tp)
{
EmcPose retval;
if (0 == tp) {
ZERO_EMC_POSE(retval);
return retval;
}
return tp->currentPos;
}
int tpInit(TP_STRUCT * tp)
{
tp->cycleTime = 0.0;
tp->vLimit = 0.0;
tp->vScale = 1.0;
tp->aMax = 0.0;
tp->vMax = 0.0;
tp->ini_maxvel = 0.0;
tp->wMax = 0.0;
tp->wDotMax = 0.0;
ZERO_EMC_POSE(tp->currentPos);
return tpClear(tp);
}
static enum EMC_RESULT _load_tool_table(const char *filename, struct CANON_TOOL_TABLE toolTable[])
{
FILE *fp;
int i, toolno, pocket, scanned;
double zoffset, diameter;
char buf[CANON_TOOL_ENTRY_LEN];
char comment[CANON_TOOL_ENTRY_LEN];
char path[LINELEN];
snprintf(path, sizeof(path), "%s/%s", USER_HOME_DIR, filename);
MSG("Loading tool table: %s\n", path);
// open tool table file
if (NULL == (fp = fopen(path, "r")))
{
MSG("Warning unable to open tool table %s\n", path);
return EMC_R_ERROR;
}
// clear out tool table
for (i = 1; i < CANON_POCKETS_MAX; i++)
{
toolTable[i].toolno = -1;
ZERO_EMC_POSE(toolTable[i].offset);
toolTable[i].diameter = 0.0;
toolTable[i].frontangle = 0.0;
toolTable[i].backangle = 0.0;
toolTable[i].orientation = 0;
}
while (fgets(buf, sizeof(buf), fp) != NULL)
{
// for nonrandom machines, just read the tools into pockets 1..n
// no matter their tool numbers. NB leave the spindle pocket 0
// unchanged/empty.
if (buf[0] == '#')
continue;
if ((scanned = sscanf(buf, "%d %d %lf %lf %[^\n]", &toolno, &pocket, &zoffset, &diameter, comment)) && (scanned == 4 || scanned == 5))
{
if (pocket < 0 || pocket >= CANON_POCKETS_MAX)
{
BUG("invalid tool number %d, skipping...\n", toolno);
continue;
}
else
{
/* mill tool */
toolTable[pocket].toolno = toolno;
toolTable[pocket].offset.tran.z = zoffset;
toolTable[pocket].diameter = diameter;
// these aren't used on a mill
toolTable[pocket].frontangle = toolTable[pocket].backangle = 0.0;
toolTable[pocket].offset.tran.x = 0.0;
toolTable[pocket].orientation = 0;
}
}
}
return EMC_R_OK;
} /* _load_tool_table() */
/* init_comm_buffers() allocates and initializes the command,
status, and error buffers used to communicate with the user
space parts of emc.
*/
static int init_comm_buffers(void)
{
int joint_num, n;
emcmot_joint_t *joint;
int retval;
rtapi_print_msg(RTAPI_MSG_INFO,
"MOTION: init_comm_buffers() starting...\n");
emcmotStruct = 0;
emcmotDebug = 0;
emcmotStatus = 0;
emcmotCommand = 0;
emcmotConfig = 0;
/* record the kinematics type of the machine */
kinType = kinematicsType();
/* allocate and initialize the shared memory structure */
emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t));
if (emc_shmem_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: rtapi_shmem_new failed, returned %d\n", emc_shmem_id);
return -1;
}
retval = rtapi_shmem_getptr(emc_shmem_id, (void **) &emcmotStruct);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: rtapi_shmem_getptr failed, returned %d\n", retval);
return -1;
}
/* zero shared memory before doing anything else. */
memset(emcmotStruct, 0, sizeof(emcmot_struct_t));
/* we'll reference emcmotStruct directly */
emcmotCommand = &emcmotStruct->command;
emcmotStatus = &emcmotStruct->status;
emcmotConfig = &emcmotStruct->config;
emcmotDebug = &emcmotStruct->debug;
emcmotInternal = &emcmotStruct->internal;
emcmotError = &emcmotStruct->error;
/* init error struct */
emcmotErrorInit(emcmotError);
/* init command struct */
emcmotCommand->head = 0;
emcmotCommand->command = 0;
emcmotCommand->commandNum = 0;
emcmotCommand->tail = 0;
emcmotCommand->spindlesync = 0.0;
/* init status struct */
emcmotStatus->head = 0;
emcmotStatus->commandEcho = 0;
emcmotStatus->commandNumEcho = 0;
emcmotStatus->commandStatus = 0;
/* init more stuff */
emcmotDebug->head = 0;
emcmotConfig->head = 0;
emcmotStatus->motionFlag = 0;
SET_MOTION_ERROR_FLAG(0);
SET_MOTION_COORD_FLAG(0);
SET_MOTION_TELEOP_FLAG(0);
emcmotDebug->split = 0;
emcmotStatus->heartbeat = 0;
emcmotStatus->computeTime = 0.0;
emcmotConfig->numJoints = num_joints;
ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd);
ZERO_EMC_POSE(emcmotStatus->carte_pos_fb);
emcmotStatus->vel = VELOCITY;
emcmotConfig->limitVel = VELOCITY;
emcmotStatus->acc = ACCELERATION;
emcmotStatus->feed_scale = 1.0;
emcmotStatus->spindle_scale = 1.0;
emcmotStatus->net_feed_scale = 1.0;
/* adaptive feed is off by default, feed override, spindle
override, and feed hold are on */
emcmotStatus->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
emcmotStatus->enables_queued = emcmotStatus->enables_new;
emcmotStatus->id = 0;
emcmotStatus->depth = 0;
emcmotStatus->activeDepth = 0;
emcmotStatus->paused = 0;
emcmotStatus->overrideLimitMask = 0;
emcmotStatus->spindle.speed = 0.0;
SET_MOTION_INPOS_FLAG(1);
SET_MOTION_ENABLE_FLAG(0);
emcmotConfig->kinematics_type = kinType;
emcmotDebug->oldPos = emcmotStatus->carte_pos_cmd;
ZERO_EMC_POSE(emcmotDebug->oldVel);
emcmot_config_change();
/* init pointer to joint structs */
#ifdef STRUCTS_IN_SHMEM
joints = &(emcmotDebug->joints[0]);
#else
joints = &(joint_array[0]);
#endif
/* init per-joint stuff */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to structure for this joint */
joint = &joints[joint_num];
/* init the config fields with some "reasonable" defaults" */
joint->type = 0;
joint->max_pos_limit = 1.0;
joint->min_pos_limit = -1.0;
joint->vel_limit = 1.0;
joint->acc_limit = 1.0;
joint->min_ferror = 0.01;
joint->max_ferror = 1.0;
joint->home_search_vel = 0.0;
joint->home_latch_vel = 0.0;
joint->home_final_vel = -1;
joint->home_offset = 0.0;
joint->home = 0.0;
joint->home_flags = 0;
joint->home_sequence = -1;
joint->backlash = 0.0;
joint->comp.entries = 0;
joint->comp.entry = &(joint->comp.array[0]);
/* the compensation code has -DBL_MAX at one end of the table
and +DBL_MAX at the other so _all_ commanded positions are
guaranteed to be covered by the table */
joint->comp.array[0].nominal = -DBL_MAX;
joint->comp.array[0].fwd_trim = 0.0;
joint->comp.array[0].rev_trim = 0.0;
joint->comp.array[0].fwd_slope = 0.0;
joint->comp.array[0].rev_slope = 0.0;
for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
joint->comp.array[n].nominal = DBL_MAX;
joint->comp.array[n].fwd_trim = 0.0;
joint->comp.array[n].rev_trim = 0.0;
joint->comp.array[n].fwd_slope = 0.0;
joint->comp.array[n].rev_slope = 0.0;
}
/* init status info */
joint->flag = 0;
joint->coarse_pos = 0.0;
joint->pos_cmd = 0.0;
joint->vel_cmd = 0.0;
joint->backlash_corr = 0.0;
joint->backlash_filt = 0.0;
joint->backlash_vel = 0.0;
joint->motor_pos_cmd = 0.0;
joint->motor_pos_fb = 0.0;
joint->pos_fb = 0.0;
joint->ferror = 0.0;
joint->ferror_limit = joint->min_ferror;
joint->ferror_high_mark = 0.0;
/* init internal info */
cubicInit(&(joint->cubic));
/* init misc other stuff in joint structure */
joint->big_vel = 10.0 * joint->vel_limit;
joint->home_state = 0;
/* init joint flags (reduntant, since flag = 0 */
SET_JOINT_ENABLE_FLAG(joint, 0);
SET_JOINT_ACTIVE_FLAG(joint, 0);
SET_JOINT_NHL_FLAG(joint, 0);
SET_JOINT_PHL_FLAG(joint, 0);
SET_JOINT_INPOS_FLAG(joint, 1);
SET_JOINT_HOMING_FLAG(joint, 0);
SET_JOINT_HOMED_FLAG(joint, 0);
SET_JOINT_FERROR_FLAG(joint, 0);
SET_JOINT_FAULT_FLAG(joint, 0);
SET_JOINT_ERROR_FLAG(joint, 0);
}
/*! \todo FIXME-- add emcmotError */
emcmotDebug->tMin = 0.0;
emcmotDebug->tMax = 0.0;
emcmotDebug->tAvg = 0.0;
emcmotDebug->sMin = 0.0;
emcmotDebug->sMax = 0.0;
emcmotDebug->sAvg = 0.0;
emcmotDebug->nMin = 0.0;
emcmotDebug->nMax = 0.0;
emcmotDebug->nAvg = 0.0;
emcmotDebug->yMin = 0.0;
emcmotDebug->yMax = 0.0;
emcmotDebug->yAvg = 0.0;
emcmotDebug->fyMin = 0.0;
emcmotDebug->fyMax = 0.0;
emcmotDebug->fyAvg = 0.0;
emcmotDebug->fMin = 0.0;
emcmotDebug->fMax = 0.0;
emcmotDebug->fAvg = 0.0;
emcmotDebug->cur_time = emcmotDebug->last_time = 0.0;
emcmotDebug->start_time = etime();
emcmotDebug->running_time = 0.0;
/* init motion emcmotDebug->queue */
if (-1 == tpCreate(&emcmotDebug->queue, DEFAULT_TC_QUEUE_SIZE,
emcmotDebug->queueTcSpace)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to create motion emcmotDebug->queue\n");
return -1;
}
// tpInit(&emcmotDebug->queue); // tpInit called from tpCreate
tpSetCycleTime(&emcmotDebug->queue, emcmotConfig->trajCycleTime);
tpSetPos(&emcmotDebug->queue, emcmotStatus->carte_pos_cmd);
tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel, emcmotStatus->vel);
tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
emcmotStatus->tail = 0;
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
return 0;
}
