static int capella_cm3602_disable(struct capella_cm3602_data *data)
{
int rc = -EIO;
int irq = data->pdata->irq;
IPS("%s\n", __func__);
if (!data->enabled) {
DPS("%s: already disabled\n", __func__);
return 0;
}
disable_irq(irq);
rc = irq_set_irq_wake(irq, 0);
if (rc < 0)
EPS("%s: failed to set irq %d as a non-wake interrupt\n",
__func__, irq);
rc = gpio_direction_output(data->pdata->p_en, 1);
if (rc < 0)
return rc;
data->pdata->power(PS_PWR_ON, 0);
data->enabled = 0;
input_event(data->input_dev, EV_SYN, SYN_CONFIG, 0);
return rc;
}
/* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */
int kinematicsForward(const double * joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double a0, a1, a3;
double x, y, z, a;
DP ("begin\n");
/* convert joint angles to radians for sin() and cos() */
a0 = joint[0] * ( PM_PI / 180 );
a1 = joint[1] * ( PM_PI / 180 );
a3 = joint[3] * ( PM_PI / 180 );
/* convert angles into world coords */
a1 = a1 + a0;
a3 = a3 + a1;
x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3);
y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3);
//TODO: confirm if it should be "(-/+)joint[3]" in real SCARA
//PPD: pitch per degree
z = D1 + D3 - joint[2] - D5 + joint[3]*PPD;
a = a3;
/* calculate inverse kinematics flags */
*iflags = 0;
if (fabs(joint[1]) < SINGU) {
*iflags |= SCARA_SINGULAR;
}
if (joint[1] < 0) { // 是左撇子嗎？
*iflags |= SCARA_LEFTY;
}
world->tran.x = x;
world->tran.y = y;
world->tran.z = z;
world->a = a * 180 / PM_PI;
// world->a = joint[4];
// world->b = joint[5];
#if (TRACE)
{
int i;
for (i=0; i<4; i++) {
DPS("Joint[%d]=%f ", i, joint[i]);
}
}
#endif
DPS("\n");
DPS("x=%f y=%f z=%f a=%f\n",
x, y, z, world->a);
DP ("end\n");
return (0);
}
void size(int p1, int p2)
{
/* old size in p1, new in ps */
yyval = p2;
dprintf(".\tS%d <- \\s%d %d \\s%d; b=%g, h=%g\n",
yyval, ps, p2, p1, ebase[yyval], eht[yyval]);
if (szstack[nszstack] != 0) {
printf(".ds %d %s\\*(%d\\s\\n(%d\n", yyval, ABSPS(ps), p2, 99-nszstack);
} else
printf(".ds %d %s\\*(%d%s\n", yyval, DPS(p1,ps), p2, DPS(ps,p1));
nszstack--;
ps = p1;
}
void fromto(int p1, int p2, int p3)
{
double b, h1, b1, t;
int subps;
yyval = salloc();
lfont[yyval] = rfont[yyval] = 0;
h1 = eht[yyval] = eht[p1];
b1 = ebase[p1];
b = 0;
subps = ps;
ps += deltaps;
nrwid(p1, ps, p1);
printf(".nr %d \\n(%d\n", yyval, p1);
if (p2 > 0) {
nrwid(p2, subps, p2);
printf(".if \\n(%d>\\n(%d .nr %d \\n(%d\n", p2, yyval, yyval, p2);
eht[yyval] += eht[p2];
b = eht[p2];
}
if (p3 > 0) {
nrwid(p3, subps, p3);
printf(".if \\n(%d>\\n(%d .nr %d \\n(%d\n", p3, yyval, yyval, p3);
eht[yyval] += eht[p3];
}
printf(".ds %d ", yyval); /* bottom of middle box */
if (p2 > 0) {
t = eht[p2]-ebase[p2]+b1;
printf("\\v'%gm'\\h'\\n(%du-\\n(%du/2u'%s\\*(%d%s",
REL(t,ps), yyval, p2, DPS(ps,subps), p2, DPS(subps,ps));
printf("\\h'-\\n(%du-\\n(%du/2u'\\v'%gm'\\\n",
yyval, p2, REL(-t,ps));
}
printf("\\h'\\n(%du-\\n(%du/2u'\\*(%d\\h'\\n(%du-\\n(%du/2u'\\\n",
yyval, p1, p1, yyval, p1);
if (p3 >0) {
t = h1-b1+ebase[p3];
printf("\\v'%gm'\\h'-\\n(%du-\\n(%du/2u'%s\\*(%d%s\\h'\\n(%du-\\n(%du/2u'\\v'%gm'\\\n",
REL(-t,ps), yyval, p3, DPS(ps,subps), p3, DPS(subps,ps), yyval, p3, REL(t,ps));
}
printf("\n");
ebase[yyval] = b + b1;
dprintf(".\tS%d <- %d from %d to %d; h=%g b=%g\n",
yyval, p1, p2, p3, eht[yyval], ebase[yyval]);
sfree(p1);
if (p2 > 0)
sfree(p2);
if (p3 > 0)
sfree(p3);
}
static int fat_node_sync(fat_node_t *node)
{
block_t *b;
fat_bs_t *bs;
fat_dentry_t *d;
int rc;
assert(node->dirty);
bs = block_bb_get(node->idx->service_id);
/* Read the block that contains the dentry of interest. */
rc = _fat_block_get(&b, bs, node->idx->service_id, node->idx->pfc,
NULL, (node->idx->pdi * sizeof(fat_dentry_t)) / BPS(bs),
BLOCK_FLAGS_NONE);
if (rc != EOK)
return rc;
d = ((fat_dentry_t *)b->data) + (node->idx->pdi % DPS(bs));
d->firstc = host2uint16_t_le(node->firstc);
if (node->type == FAT_FILE) {
d->size = host2uint32_t_le(node->size);
} else if (node->type == FAT_DIRECTORY) {
d->attr = FAT_ATTR_SUBDIR;
}
/* TODO: update other fields? (e.g time fields) */
b->dirty = true; /* need to sync block */
rc = block_put(b);
return rc;
}
void putout(int p1)
{
double before, after;
extern double BeforeSub, AfterSub;
dprintf(".\tanswer <- S%d, h=%g,b=%g\n",p1, eht[p1], ebase[p1]);
eqnht = eht[p1];
before = eht[p1] - ebase[p1] - BeforeSub; /* leave room for sub or superscript */
after = ebase[p1] - AfterSub;
if (spaceval || before > 0.01 || after > 0.01) {
printf(".ds %d ", p1); /* used to be \\x'0' here: why? */
if (spaceval != NULL)
printf("\\x'0-%s'", spaceval);
else if (before > 0.01)
printf("\\x'0-%gm'", before);
printf("\\*(%d", p1);
if (spaceval == NULL && after > 0.01)
printf("\\x'%gm'", after);
putchar('\n');
}
if (szstack[0] != 0)
printf(".ds %d %s\\*(%d\\s\\n(99\n", p1, DPS(gsize,gsize), p1);
eqnreg = p1;
if (spaceval != NULL) {
free(spaceval);
spaceval = NULL;
}
}
static int capella_cm3602_enable(struct capella_cm3602_data *data)
{
int rc;
int irq = data->pdata->irq;
IPS("%s\n", __func__);
if (data->enabled) {
DPS("%s: already enabled\n", __func__);
return 0;
}
/* dummy report */
input_report_abs(data->input_dev, ABS_DISTANCE, -1);
input_sync(data->input_dev);
data->pdata->power(PS_PWR_ON, 1);
rc = gpio_direction_output(data->pdata->p_en, 0);
msleep(220);
data->enabled = !rc;
if (!rc)
capella_cm3602_report(data);
enable_irq(irq);
rc = irq_set_irq_wake(irq, 1);
if (rc < 0)
EPS("%s: failed to set irq %d as a wake interrupt\n",
__func__, irq);
return rc;
}
int rtapi_app_main(void) {
int res=0;
#if (TRACE!=0)
dptrace = fopen("art_scarakins.log","w");
#endif
DP ("begin\n");
comp_id = hal_init("art_scarakins");
if (comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(*haldata));
if (!haldata) goto error;
if((res = hal_pin_float_new("scarakins.D1", HAL_IO, &(haldata->d1), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D2", HAL_IO, &(haldata->d2), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D3", HAL_IO, &(haldata->d3), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D4", HAL_IO, &(haldata->d4), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D5", HAL_IO, &(haldata->d5), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D6", HAL_IO, &(haldata->d6), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.PPD", HAL_IO, &(haldata->ppd), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.SING", HAL_IO, &(haldata->sing), comp_id)) < 0) goto error;
DPS("D1=%f ", D1);
DPS("D2=%f ", D2);
DPS("D3=%f ", D3);
DPS("D4=%f ", D4);
DPS("D5=%f ", D5);
DPS("D6=%f ", D6);
DPS("PPD=%f ", PPD);
DPS("SINGU=%f ", SINGU);
DPS("\n");
// D1 = DEFAULT_D1;
// D2 = DEFAULT_D2;
// D3 = DEFAULT_D3;
// D4 = DEFAULT_D4;
// D5 = DEFAULT_D5;
// D6 = DEFAULT_D6;
hal_ready(comp_id);
DP ("success\n");
return 0;
error:
hal_exit(comp_id);
#if (TRACE!=0)
fclose(dptrace);
#endif
return res;
}
static long capella_cm3602_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
int val;
/*DPS("%s cmd %d\n", __func__, _IOC_NR(cmd));*/
switch (cmd) {
case CAPELLA_CM3602_IOCTL_ENABLE:
if (get_user(val, (unsigned long __user *)arg))
return -EFAULT;
if (val)
return capella_cm3602_enable(&the_data);
else
return capella_cm3602_disable(&the_data);
break;
case CAPELLA_CM3602_IOCTL_GET_ENABLED:
return put_user(the_data.enabled, (unsigned long __user *)arg);
break;
default:
DPS("%s: invalid cmd %d\n", __func__, _IOC_NR(cmd));
return -EINVAL;
}
}
int kinematicsInverse(const EmcPose *world,
double *joint,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags)
{
double a3;
double q0, q1;
double xt, yt, rsq, cc;
double x, y, z, a;
DP ("begin\n");
if (*iflags & SCARA_SINGULAR) {
DP ("quit when SCARA is at SINGULARITY configuration\n");
return -1;
}
x = world->tran.x;
y = world->tran.y;
z = world->tran.z;
a = world->a;
/* convert degrees to radians */
a3 = a * ( PM_PI / 180 );
/* center of end effector (correct for D6) */
xt = x - D6*cos(a3);
yt = y - D6*sin(a3);
/* horizontal distance (squared) from end effector centerline
to main column centerline */
rsq = xt*xt + yt*yt;
/* joint 1 angle needed to make arm length match sqrt(rsq) */
cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4);
if(cc < -1) cc = -1;
if(cc > 1) cc = 1;
q1 = acos(cc);
if (fabs(q1) < SINGU) {
DP ("quit when SCARA is about to move to SINGULAR position\n");
DP ("x(%f) y(%f) q1(%f) cur_j1(%f)\n", x, y, q1, joint[1]);
return -1;
}
if (*iflags & SCARA_LEFTY) {
q1 = -q1;
}
/* angle to end effector */
q0 = atan2(yt, xt);
/* end effector coords in inner arm coord system */
xt = D2 + D4*cos(q1);
yt = D4*sin(q1);
/* inner arm angle */
q0 = q0 - atan2(yt, xt);
/* q0 and q1 are still in radians. convert them to degrees */
q0 = q0 * (180 / PM_PI);
q1 = q1 * (180 / PM_PI);
joint[0] = q0;
joint[1] = q1;
joint[3] = a - (q0 + q1);
//TODO: confirm if it should be "(-/+)joint[3]" in real SCARA
//ysli: before 2009-09-18, it's (-)joint[3]
//ysli: after 2009-09-18, it's (+)joint[3]
//PPD: pitch per degree
joint[2] = D1 + D3 - D5 - z + joint[3]*PPD;
// joint[4] = world->a;
// joint[5] = world->b;
*fflags = 0;
DPS("x=%f y=%f z=%f a=%f\n",
x, y, z, world->a);
#if (TRACE)
{
int i;
for (i=0; i<4; i++) {
DPS("Joint[%d]=%f ", i, joint[i]);
}
}
#endif
DPS("\n");
DP ("end\n");
return (0);
}
EmcPose tcGetPosReal(TC_STRUCT * tc, int of_endpoint)
{
EmcPose pos;
PmPose xyz;
PmPose abc;
PmPose uvw;
double progress = of_endpoint? tc->target: tc->progress;
#if(TRACE != 0)
static double last_l, last_u,last_x = 0 , last_y = 0, last_z = 0, last_a = 0;
#endif
if (tc->motion_type == TC_RIGIDTAP) {
if(tc->coords.rigidtap.state > REVERSING) {
pmLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz);
} else {
pmLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz);
}
// no rotary move allowed while tapping
abc.tran = tc->coords.rigidtap.abc;
uvw.tran = tc->coords.rigidtap.uvw;
} else if (tc->motion_type == TC_LINEAR) {
if (tc->coords.line.xyz.tmag > 0.) {
// progress is along xyz, so uvw and abc move proportionally in order
// to end at the same time.
pmLinePoint(&tc->coords.line.xyz, progress, &xyz);
pmLinePoint(&tc->coords.line.uvw,
progress * tc->coords.line.uvw.tmag / tc->target,
&uvw);
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else if (tc->coords.line.uvw.tmag > 0.) {
// xyz is not moving
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, progress, &uvw);
// abc moves proportionally in order to end at the same time
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else {
// if all else fails, it's along abc only
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, 0.0, &uvw);
pmLinePoint(&tc->coords.line.abc, progress, &abc);
}
} else if (tc->motion_type == TC_CIRCULAR) {//we have TC_CIRCULAR
// progress is always along the xyz circle. This simplification
// is possible since zero-radius arcs are not allowed by the interp.
pmCirclePoint(&tc->coords.circle.xyz,
progress * tc->coords.circle.xyz.angle / tc->target,
&xyz);
// abc moves proportionally in order to end at the same time as the
// circular xyz move.
pmLinePoint(&tc->coords.circle.abc,
progress * tc->coords.circle.abc.tmag / tc->target,
&abc);
// same for uvw
pmLinePoint(&tc->coords.circle.uvw,
progress * tc->coords.circle.uvw.tmag / tc->target,
&uvw);
} else {
int s, tmp1,i;
double u,*N,R, X, Y, Z, A, B, C, U, V, W, F, D;
double curve_accel;
#if(TRACE != 0)
double delta_l, delta_u, delta_d, delta_x, delta_y, delta_z, delta_a;
#endif
N = tc->nurbs_block.N;
// NL = tc->nurbs_block.NL;
assert(tc->motion_type == TC_NURBS);
u = progress / tc->target;
if (u<1) {
s = nurbs_findspan(tc->nurbs_block.nr_of_ctrl_pts-1, tc->nurbs_block.order - 1,
u, tc->nurbs_block.knots_ptr); //return span index of u_i
nurbs_basisfun(s, u, tc->nurbs_block.order - 1 , tc->nurbs_block.knots_ptr , N); // input: s:knot span index u:u_0 d:B-Spline degree k:Knots
// output: N:basis functions
// refer to bspeval.cc::line(70) of octave
// refer to opennurbs_evaluate_nurbs.cpp::line(985) of openNurbs
// refer to ON_NurbsCurve::Evaluate() for ...
// refer to opennurbs_knot.cpp::ON_NurbsSpanIndex()
// http://www.rhino3d.com/nurbs.htm (What is NURBS?)
// Some modelers that use older algorithms for NURBS
// evaluation require two extra knot values for a total of
// degree+N+1 knots. When Rhino is exporting and importing
// NURBS geometry, it automatically adds and removes these
// two superfluous knots as the situation requires.
tmp1 = s - tc->nurbs_block.order + 1;
assert(tmp1 >= 0);
assert(tmp1 < tc->nurbs_block.nr_of_ctrl_pts);
R = 0.0;
for (i=0; i<=tc->nurbs_block.order -1 ; i++) {
R += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].R;
}
X = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
X += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].X;
}
X = X/R;
xyz.tran.x = X;
Y = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Y += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Y;
}
Y = Y/R;
xyz.tran.y = Y;
Z = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Z += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Z;
}
Z = Z/R;
xyz.tran.z = Z;
A = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
A += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].A;
}
A = A/R;
abc.tran.x = A;
B = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
B += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].B;
}
B = B/R;
abc.tran.y = B;
C = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
C += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].C;
}
C = C/R;
abc.tran.z = C;
U = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
U += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].U;
}
U = U/R;
uvw.tran.x = U;
V = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
V += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].V;
}
V = V/R;
uvw.tran.y = V;
W = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
W += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].W;
}
W = W/R;
uvw.tran.z = W;
F = 0.0;
F = tc->nurbs_block.ctrl_pts_ptr[tmp1].F;
tc->reqvel = F;
D = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
D += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].D;
}
D = D/R;
// compute allowed feed
if(!of_endpoint) {
curve_accel = (tc->cur_vel * tc->cur_vel)/D;
if(curve_accel > tc->maxaccel) {
// modify req_vel
tc->reqvel = pmSqrt((tc->maxaccel * D));
}
}
#if (TRACE != 0)
if(l == 0 && _dt == 0) {
last_l = 0;
last_u = 0;
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = 0;
_dt+=1;
}
delta_l = l - last_l;
last_l = l;
delta_u = u - last_u;
last_u = u;
delta_x = xyz.tran.x - last_x;
delta_y = xyz.tran.y - last_y;
delta_z = xyz.tran.z - last_z;
delta_a = abc.tran.x - last_a;
delta_d = pmSqrt(pmSq(delta_x)+pmSq(delta_y)+pmSq(delta_z));
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = abc.tran.x;
if( delta_d > 0)
{
if(_dt == 1){
/* prepare header for gnuplot */
DPS ("%11s%15s%15s%15s%15s%15s%15s%15s%15s\n",
"#dt", "u", "l","x","y","z","delta_d", "delta_l","a");
}
DPS("%11u%15.10f%15.10f%15.5f%15.5f%15.5f%15.5f%15.5f%15.5f\n",
_dt, u, l,last_x, last_y, last_z, delta_d, delta_l, last_a);
_dt+=1;
}
#endif // (TRACE != 0)
}else {
xyz.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].X;
xyz.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Y;
xyz.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Z;
uvw.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].U;
uvw.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].V;
uvw.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].W;
abc.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].A;
abc.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].B;
abc.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].C;
// R = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].R;
}
}
//DP ("GetEndPoint?(%d) R(%.2f) X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",of_endpoint, R, X, Y, Z, A);
// TODO-eric if R going to show ?
//#if (TRACE != 0)
// if(_dt == 0){
// /* prepare header for gnuplot */
// DPS ("%11s%15s%15s%15s\n", "#dt", "x", "y", "z");
// }
// DPS("%11u%15.5f%15.5f%15.5f\n", _dt, xyz.tran.x, xyz.tran.y, xyz.tran.z);
// _dt+=1;
//#endif // (TRACE != 0)
#if (TRACE != 1)
if( of_endpoint != 1) {
}
#endif
pos.tran = xyz.tran;
pos.a = abc.tran.x;
pos.b = abc.tran.y;
pos.c = abc.tran.z;
pos.u = uvw.tran.x;
pos.v = uvw.tran.y;
pos.w = uvw.tran.z;
// DP ("GetEndPoint?(%d) tc->id %d MotionType %d X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",
// of_endpoint,tc->id,tc->motion_type, pos.tran.x,
// pos.tran.y, pos.tran.z, pos.a);
return pos;
}
void nrwid(int n1, int p, int n2)
{
printf(".nr %d 0\\w'%s\\*(%d'\n", n1, DPS(gsize,p), n2); /* 0 defends against - width */
}
