int pmCircle9Init(PmCircle9 * const circ9,
EmcPose const * const start,
EmcPose const * const end,
PmCartesian const * const center,
PmCartesian const * const normal,
int turn)
{
PmCartesian start_xyz, end_xyz;
PmCartesian start_uvw, end_uvw;
PmCartesian start_abc, end_abc;
emcPoseToPmCartesian(start, &start_xyz, &start_abc, &start_uvw);
emcPoseToPmCartesian(end, &end_xyz, &end_abc, &end_uvw);
int xyz_fail = pmCircleInit(&circ9->xyz, &start_xyz, &end_xyz, center, normal, turn);
//Initialize line parts of Circle9
int abc_fail = pmCartLineInit(&circ9->abc, &start_abc, &end_abc);
int uvw_fail = pmCartLineInit(&circ9->uvw, &start_uvw, &end_uvw);
if (xyz_fail || abc_fail || uvw_fail) {
rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Circle9, err codes %d, %d, %d\n",
xyz_fail, abc_fail, uvw_fail);
return TP_ERR_FAIL;
}
return TP_ERR_OK;
}
int tpAddCircle(TP_STRUCT * tp, EmcPose end,
PmCartesian center, PmCartesian normal, int turn, int type,
double vel, double ini_maxvel, double acc, unsigned char enables, char atspeed)
{
TC_STRUCT tc;
PmCircle circle;
PmLine line_uvw, line_abc;
PmPose start_xyz, end_xyz;
PmPose start_uvw, end_uvw;
PmPose start_abc, end_abc;
double helix_z_component; // z of the helix's cylindrical coord system
double helix_length;
PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 };
if (!tp || tp->aborting)
return -1;
start_xyz.tran = tp->goalPos.tran;
end_xyz.tran = end.tran;
start_abc.tran.x = tp->goalPos.a;
start_abc.tran.y = tp->goalPos.b;
start_abc.tran.z = tp->goalPos.c;
end_abc.tran.x = end.a;
end_abc.tran.y = end.b;
end_abc.tran.z = end.c;
start_uvw.tran.x = tp->goalPos.u;
start_uvw.tran.y = tp->goalPos.v;
start_uvw.tran.z = tp->goalPos.w;
end_uvw.tran.x = end.u;
end_uvw.tran.y = end.v;
end_uvw.tran.z = end.w;
start_xyz.rot = identity_quat;
end_xyz.rot = identity_quat;
start_uvw.rot = identity_quat;
end_uvw.rot = identity_quat;
start_abc.rot = identity_quat;
end_abc.rot = identity_quat;
pmCircleInit(&circle, start_xyz, end_xyz, center, normal, turn);
pmLineInit(&line_uvw, start_uvw, end_uvw);
pmLineInit(&line_abc, start_abc, end_abc);
// find helix length
pmCartMag(circle.rHelix, &helix_z_component);
helix_length = pmSqrt(pmSq(circle.angle * circle.radius) +
pmSq(helix_z_component));
tc.sync_accel = 0;
tc.cycle_time = tp->cycleTime;
tc.target = helix_length;
tc.progress = 0.0;
tc.reqvel = vel;
tc.maxaccel = acc;
tc.feed_override = 0.0;
tc.maxvel = ini_maxvel;
tc.id = tp->nextId;
tc.active = 0;
tc.atspeed = atspeed;
tc.currentvel = 0.0;
tc.blending = 0;
tc.blend_vel = 0.0;
tc.vel_at_blend_start = 0.0;
tc.coords.circle.xyz = circle;
tc.coords.circle.uvw = line_uvw;
tc.coords.circle.abc = line_abc;
tc.motion_type = TC_CIRCULAR;
tc.canon_motion_type = type;
tc.blend_with_next = tp->termCond == TC_TERM_COND_BLEND;
tc.tolerance = tp->tolerance;
tc.synchronized = tp->synchronized;
tc.velocity_mode = tp->velocity_mode;
tc.uu_per_rev = tp->uu_per_rev;
tc.enables = enables;
tc.indexrotary = -1;
if (syncdio.anychanged != 0) {
tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling
tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it
} else {
tc.syncdio.anychanged = 0;
}
if (tcqPut(&tp->queue, tc) == -1) {
return -1;
}
tp->goalPos = end;
tp->done = 0;
tp->depth = tcqLen(&tp->queue);
tp->nextId++;
return 0;
}
