/* Do the averaving */
int mu_stats( vect_n * torques, vect_n * positions )
{
int n;
int j;
vect_n * torque_vars;
vect_n * position_vars;
int dof;
dof = len_vn(torques);
torque_vars = new_vn(dof);
position_vars = new_vn(dof);
/* Zero the values */
fill_vn(torques,0.0);
fill_vn(positions,0.0);
fill_vn(torque_vars,0.0);
fill_vn(position_vars,0.0);
/* Calculate the average */
for (n = 0; n < NUM_POINTS; n++)
{
for (j=0; j<dof; j++)
{
torques->q[j] += mu_jts[j][n];
positions->q[j] += mu_jps[j][n];
}
}
set_vn(torques, scale_vn( 1.0/NUM_POINTS, torques ));
set_vn(positions, scale_vn( 1.0/NUM_POINTS, positions ));
/* Calculate the variance */
for (n = 0; n < NUM_POINTS; n++)
{
for (j=0; j<dof; j++)
{
torque_vars->q[j] += (mu_jts[j][n] - torques->q[j]) * (mu_jts[j][n] - torques->q[j]);
position_vars->q[j] += (mu_jps[j][n] - positions->q[j]) * (mu_jps[j][n] - positions->q[j]);
}
}
set_vn(torque_vars, scale_vn( 1.0/NUM_POINTS, torque_vars ));
set_vn(position_vars, scale_vn( 1.0/NUM_POINTS, position_vars ));
/* Print positions */
mvprintw(12,0, " Positions:");
mvprintw(13,0, " (std-dev):");
mvprintw(14,0, " Torques:");
mvprintw(15,0, " (std-dev):");
for (j=0; j<dof; j++)
{
mvprintw(12, 13 + 9*j, "% 08.5f ",positions->q[j]);
mvprintw(13, 13 + 9*j, "% 08.5f ",sqrt(position_vars->q[j]));
mvprintw(14, 13 + 9*j, "% 08.5f ",torques->q[j]);
mvprintw(15, 13 + 9*j, "% 08.5f ",sqrt(torque_vars->q[j]));
}
destroy_vn(&torque_vars);
destroy_vn(&position_vars);
return 0;
}
VN * IR_GVN::register_bin_vn(IR_TYPE irt, VN const* v0, VN const* v1)
{
IS_TRUE0(v0 && v1);
if (is_commutative(irt) && (VN_id(v0) > VN_id(v1))) {
return register_bin_vn(irt, v1, v0);
} else if (irt == IR_GT) {
return register_bin_vn(IR_LT, v1, v0);
} else if (irt == IR_GE) {
return register_bin_vn(IR_LE, v1, v0);
}
VEC2 * v0_vec = m_irt_vec.get(irt);
if (v0_vec == NULL) {
v0_vec = new VEC2();
m_vec_lst.append_tail((SVECTOR<VN*>*)v0_vec);
m_irt_vec.set(irt, v0_vec);
}
VEC1 * v1_vec = v0_vec->get(VN_id(v0));
if (v1_vec == NULL) {
v1_vec = new VEC1();
m_vec_lst.append_tail((SVECTOR<VN*>*)v1_vec);
m_vnvec_lst.append_tail(v1_vec);
v0_vec->set(VN_id(v0), v1_vec);
}
VN * res = v1_vec->get(VN_id(v1));
if (res == NULL) {
res = new_vn();
VN_type(res) = VN_OP;
VN_op(res) = irt;
v1_vec->set(VN_id(v1), res);
}
return res;
}
//Compute VN for array according to anonymous domdef.
VN * IR_GVN::comp_array_by_anon_domdef(IR const* arr, VN const* basevn,
VN const* ofstvn, IR const* domdef,
bool & change)
{
IS_TRUE0(IR_type(arr) == IR_ARRAY && m_du->is_may_def(domdef, arr, false));
ARR_VNE2VN * vnexp_map = m_def2arrtab.get(domdef);
UINT dtsz = arr->get_dt_size(m_dm);
VNE_ARR vexp(VN_id(basevn), VN_id(ofstvn), ARR_ofst(arr), dtsz);
/* NOTE:
foo();
array(v1); //s1
goo();
array(v1); //s2
vn of s1 should not same with s2. */
if (vnexp_map == NULL) {
vnexp_map = new ARR_VNE2VN(m_pool, 16); //bsize to be evaluate.
m_def2arrtab.set(domdef, vnexp_map);
}
VN * vn = vnexp_map->get(&vexp);
if (vn == NULL) {
vn = new_vn();
VN_type(vn) = VN_OP;
VN_op(vn) = IR_ARRAY;
vnexp_map->setv((OBJTY)&vexp, vn);
}
m_ir2vn.set(IR_id(arr), vn);
change = true;
return vn;
}
//Compute VN for ild according to anonymous domdef.
VN * IR_GVN::comp_ild_by_anon_domdef(IR const* ild, VN const* mlvn,
IR const* domdef, bool & change)
{
IS_TRUE0(IR_type(ild) == IR_ILD && m_du->is_may_def(domdef, ild, false));
ILD_VNE2VN * vnexp_map = m_def2ildtab.get(domdef);
UINT dtsz = ild->get_dt_size(m_dm);
VNE_ILD vexp(VN_id(mlvn), ILD_ofst(ild), dtsz);
/*
NOTE:
foo();
ild(v1); //s1
goo();
ild(v1); //s2
vn of s1 should not same with s2.
*/
if (vnexp_map == NULL) {
vnexp_map = new ILD_VNE2VN(m_pool, 16); //bsize to be evaluate.
m_def2ildtab.set(domdef, vnexp_map);
}
VN * ildvn = vnexp_map->get(&vexp);
if (ildvn == NULL) {
ildvn = new_vn();
VN_type(ildvn) = VN_OP;
VN_op(ildvn) = IR_ILD;
vnexp_map->setv((OBJTY)&vexp, ildvn);
}
m_ir2vn.set(IR_id(ild), ildvn);
change = true;
return ildvn;
}
VN * IR_GVN::comp_sc_by_anon_domdef(IR const* exp, IR const* domdef,
bool & change)
{
IS_TRUE0((IR_type(exp) == IR_LD || IR_type(exp) == IR_PR) &&
m_du->is_may_def(domdef, exp, false));
SCVNE2VN * vnexp_map = m_def2sctab.get(domdef);
UINT dtsz = exp->get_dt_size(m_dm);
MD const* md = exp->get_exact_ref();
IS_TRUE0(md);
VNE_SC vexp(MD_id(md), exp->get_ofst(), dtsz);
/*
NOTE:
foo();
v1; //s1
goo();
v1; //s2
vn of s1 should not same with s2.
*/
if (vnexp_map == NULL) {
vnexp_map = new SCVNE2VN(m_pool, 16); //bsize to be evaluate.
m_def2sctab.set(domdef, vnexp_map);
}
VN * vn = vnexp_map->get(&vexp);
if (vn == NULL) {
vn = new_vn();
VN_type(vn) = VN_VAR;
vnexp_map->setv((OBJTY)&vexp, vn);
}
m_ir2vn.set(IR_id(exp), vn);
change = true;
return vn;
}
void main()
{
tseg ms;
vect_n *a,*b,*c,*res;
FILE *out;
char buf[250];
cseg t;
int cnt;
btreal s,ts;
ms.vf = 100;
ms.xf = 1000;
ms.tf = 10;
ms.acc = 10;
a = new_vn(4);
b = new_vn(4);
c = new_vn(4);
res = new_vn(4);
const_vn(a,0.0,0.0,0.0,0.0);
const_vn(b,0.0,2.0,0.0,0.0);
const_vn(c,0.0,0.0,0.0,0.0);
ts = init_cseg(&t,0.01,a,b,c);
dump_cseg(&t,stdout);
printf("length: %f ts:%f \n",t.phi);
out = fopen("curve.csv","w");
ts = 10;
s = 0.0;
printf("ds: %f\n",ts/50);
//fprintf(out,"%s\n",sprint_csv_vn(buf,t.x0));
//fprintf(out,"%s\n",sprint_csv_vn(buf,b));
//fprintf(out,"%s\n",sprint_csv_vn(buf,t.xf));
for(cnt = 0;cnt < 51;cnt++){
//set_vn(res,calc_cseg(&t,s));
s+=ts/50;
//fprintf(out,"%s\n",sprint_csv_vn(buf,res));
fprintf(out,"%f, %f\n",s,calc_tseg(&ms,s));
}
fclose(out);
}
VN * IR_GVN::register_md_vn(MD const* md)
{
if (m_md2vn.get_bucket_size() == 0) {
m_md2vn.init(10/*TO reevaluate*/);
}
VN * x = m_md2vn.get(md);
if (x == NULL) {
x = new_vn();
VN_type(x) = VN_VAR;
m_md2vn.set(md, x);
}
return x;
}
VN * IR_GVN::register_int_vn(LONGLONG v)
{
if (v == 0) {
if (m_zero_vn == NULL) {
m_zero_vn = new_vn();
VN_type(m_zero_vn) = VN_INT;
VN_int_val(m_zero_vn) = 0;
}
return m_zero_vn;
}
if (m_ll2vn.get_bucket_size() == 0) {
m_ll2vn.init(10/*TO reevaluate*/);
}
VN * vn = m_ll2vn.get(v);
if (vn != NULL) {
return vn;
}
vn = new_vn();
VN_type(vn) = VN_INT;
VN_int_val(vn) = v;
m_ll2vn.set(v, vn);
return vn;
}
/************************ pararray_vn **************************************/
pararray_vn* new_pavn(int max, int elements)
{
void *vmem;
int cnt;
pararray_vn *pavn;
vmem = btmalloc(sizeof(pararray_vn) + sizeof(pararray*) * elements);
pavn = (pararray_vn*)vmem;
pavn->pa = (pararray**)(vmem + sizeof(pararray_vn));
pavn->elements = elements;
for (cnt = 0; cnt < elements; cnt++)
pavn->pa[cnt] = new_pa(max);
pavn->result = new_vn(elements);
return pavn;
}
VN * IR_GVN::register_str_vn(SYM * v)
{
if (m_str2vn.get_bucket_size() == 0) {
m_str2vn.init(16/*TO reevaluate*/);
}
VN * vn = m_str2vn.get(v);
if (vn != NULL) {
return vn;
}
vn = new_vn();
VN_type(vn) = VN_STR;
VN_str_val(vn) = v;
m_str2vn.set(v, vn);
return vn;
}
VN * IR_GVN::register_fp_vn(double v)
{
if (m_fp2vn.get_bucket_size() == 0) {
m_fp2vn.init(10/*TO reevaluate*/);
}
VN * vn = m_fp2vn.get(v);
if (vn != NULL) {
return vn;
}
vn = new_vn();
VN_type(vn) = VN_FP;
VN_fp_val(vn) = v;
m_fp2vn.set(v, vn);
return vn;
}
void IR_GVN::process_call(IR const* ir, bool & change)
{
for (IR const* p = CALL_param_list(ir); p != NULL; p = IR_next(p)) {
comp_vn(p, change);
}
VN * x = m_ir2vn.get(IR_id(ir));
if (x == NULL) {
x = new_vn();
VN_type(x) = VN_VAR;
change = true;
m_ir2vn.set(IR_id(ir), x);
}
return;
}
VN * IR_GVN::comp_sc(IR const* exp, bool & change)
{
VN * evn = m_ir2vn.get(IR_id(exp));
if (evn != NULL) { return evn; }
evn = comp_mem(exp, change);
if (evn != NULL) { return evn; }
DU_SET const* du = m_du->get_du_c(exp);
IS_TRUE(du, ("This situation should be catched in comp_mem()"));
IS_TRUE0(du->get_elem_count() > 0);
IR const* exp_stmt = const_cast<IR*>(exp)->get_stmt();
IR const* domdef = m_du->find_dom_def(exp, exp_stmt, du, false);
if (domdef == NULL) { return NULL; }
if (domdef->is_stid() && ST_ofst(domdef) != exp->get_ofst()) {
return NULL;
}
if (!domdef->is_stid() && !domdef->is_stpr()) {
return comp_sc_by_anon_domdef(exp, domdef, change);
}
switch (IR_type(exp)) {
case IR_LD:
if (domdef->is_stpr() || (LD_idinfo(exp) != ST_idinfo(domdef))) {
return NULL;
}
break;
case IR_PR:
if (domdef->is_stid() || PR_no(exp) != STPR_no(domdef)) {
return NULL;
}
break;
default: IS_TRUE(0, ("unsupport"));
}
VN * uni_vn = m_ir2vn.get(IR_id(domdef));
if (uni_vn == NULL) {
uni_vn = new_vn();
VN_type(uni_vn) = VN_VAR;
m_ir2vn.set(IR_id(domdef), uni_vn);
}
m_ir2vn.set(IR_id(exp), uni_vn);
change = true;
return uni_vn;
}
VN * IR_GVN::register_uni_vn(IR_TYPE irt, VN const* v0)
{
SVECTOR<VN*> * v1_vec = (SVECTOR<VN*>*)m_irt_vec.get(irt);
if (v1_vec == NULL) {
v1_vec = new SVECTOR<VN*>();
m_vec_lst.append_tail(v1_vec);
m_vnvec_lst.append_tail(v1_vec);
m_irt_vec.set(irt, (SVECTOR<SVECTOR<VN*>*>*)v1_vec);
}
VN * res = v1_vec->get(VN_id(v0));
if (res == NULL) {
res = new_vn();
VN_type(res) = VN_OP;
VN_op(res) = irt;
v1_vec->set(VN_id(v0), res);
}
return res;
}
VN * IR_GVN::register_qua_vn(IR_TYPE irt, VN const* v0, VN const* v1,
VN const* v2, VN const* v3)
{
IS_TRUE0(v0 && v1 && v2 && v3);
IS_TRUE0(is_quad(irt));
VEC4 * v0_vec = (VEC4*)m_irt_vec.get(irt);
if (v0_vec == NULL) {
v0_vec = new VEC4();
m_vec_lst.append_tail((VEC1*)v0_vec);
m_irt_vec.set(irt, (VEC2*)v0_vec);
}
VEC3 * v1_vec = v0_vec->get(VN_id(v0));
if (v1_vec == NULL) {
v1_vec = new VEC3();
m_vec_lst.append_tail((VEC1*)v1_vec);
v0_vec->set(VN_id(v0), v1_vec);
}
VEC2 * v2_vec = v1_vec->get(VN_id(v1));
if (v2_vec == NULL) {
v2_vec = new VEC2();
m_vec_lst.append_tail((VEC1*)v2_vec);
v1_vec->set(VN_id(v1), v2_vec);
}
VEC1 * v3_vec = v2_vec->get(VN_id(v2));
if (v3_vec == NULL) {
v3_vec = new VEC1();
m_vec_lst.append_tail(v3_vec);
m_vnvec_lst.append_tail(v3_vec);
v2_vec->set(VN_id(v2), v3_vec);
}
VN * res = v3_vec->get(VN_id(v3));
if (res == NULL) {
res = new_vn();
VN_type(res) = VN_OP;
VN_op(res) = irt;
v3_vec->set(VN_id(v3), res);
}
return res;
}
/**
Convert a vectray of time/points to a piecewise parabolic function.
Time in the first value;
Time values must be monotonically increasing. Results are otherwize undefined.
*/
pararray_vn* vr2pararray(vectray* vr,btreal acceleration)
{
pararray_vn* pavn;
parabolic pa;
int cnt,idx;
btreal t1,t2,t3,x1,x2,x3;
btreal v1,v2,v3,tacc,t1p2,t2p3,tmax;
btreal tf,a,s0,sf;
btreal sv0,svf,sa0,saf,sa0_last,v1_last,saf_last;
btreal s0_prev,tf_prev;
btreal acc,min_acc;
btreal dt,dx,t_last;
vect_n *p0, *pf; //store first and last points of vr
p0 = new_vn(numelements_vr(vr)); // numelements_vr() returns vr->n (columns)
pf = new_vn(numelements_vr(vr));
set_vn(p0, idx_vr(vr, 0));
set_vn(pf, idx_vr(vr, numrows_vr(vr)-1));
//new pararray of size (Viapoints - 1)*2 + 1
pavn = new_pavn(2*numrows_vr(vr) -1 +5, numelements_vr(vr)-1);
tmax = 0;
// For each column of pavn
for (cnt = 0; cnt < pavn->elements; cnt ++) {
acc = fabs(acceleration);
/* First acceleration segment calcs*/
t1 = getval_vn(idx_vr(vr,0),0);
t2 = getval_vn(idx_vr(vr,1),0);
x1 = getval_vn(idx_vr(vr,0),cnt+1);
x2 = getval_vn(idx_vr(vr,1),cnt+1);
dt = t2-t1;
dx = x2-x1;
v1 = 0.0;
if(dt == 0.0)
syslog(LOG_ERR, "vr2pararray(): about to divide by dt = 0.0");
v2 = dx/dt;
min_acc = 8.0 * fabs(dx) / (3.0 * dt*dt);
if(isnan(min_acc))
syslog(LOG_ERR, "vr2pararray(): min_acc is NaN (bad)");
//Make sure initial acceleration is fast enough
if (acc < min_acc) {
//syslog(LOG_ERR,"vr2pararray: Boosting initial acc (%f) to %f", acc, min_acc);
acc = min_acc;
}
tacc = dt - sqrt(dt*dt - 2*fabs(dx)/acc);
saf = x1 + 0.5*acc*tacc*tacc*Sgn(dx); //Final x
v2 = (x2-saf)/(dt-tacc); //final velocity
if(isnan(v2))
syslog(LOG_ERR, "vr2pararray: v2 is NaN (bad)");
sa0 = x1;
tf_prev = s0sfspftf_pb(&pa, 0.0, sa0, saf, v2, tacc); //acc seg starting at time 0.0
add_bseg_pa(pavn->pa[cnt],&pa);
setval_vn(idx_vr(vr,0),cnt+1,x2-dt*v2);
idx = numrows_vr(vr)-1;
/* Last velocity and acceleration segment calcs */
acc = fabs(acceleration);
t1 = getval_vn(idx_vr(vr,idx-1),0);
t2 = getval_vn(idx_vr(vr,idx),0);
x1 = getval_vn(idx_vr(vr,idx-1),cnt+1);
x2 = getval_vn(idx_vr(vr,idx),cnt+1);
dt = t2-t1;
dx = x2-x1;
v1 = dx/dt;
v2 = 0.0;
min_acc = 8.0*fabs(dx)/(3.0*dt*dt);
//Make sure final acceleration is fast enough
if (acc < min_acc) {
acc = min_acc;
//syslog(LOG_ERR,"vr2pararray: Boosting final acc to %f",acc);
}
tacc = dt - sqrt(dt*dt - 2*fabs(dx)/acc);
sa0_last = x2 - 0.5*acc*tacc*tacc*Sgn(dx); //Final x
v1_last = (sa0_last-x1)/(dt-tacc); //final velocity
saf_last = x2;
t_last = tacc;
setval_vn(idx_vr(vr,idx),cnt+1,x1+dt*v1_last);
/*Internal (remaining) segments calcs*/
acc = fabs(acceleration);
for(idx = 1; idx < numrows_vr(vr)-1; idx++) {
/* Extract info */
t1 = getval_vn(idx_vr(vr,idx-1),0);
t2 = getval_vn(idx_vr(vr,idx),0);
t3 = getval_vn(idx_vr(vr,idx+1),0);
x1 = getval_vn(idx_vr(vr,idx-1),cnt+1);
x2 = getval_vn(idx_vr(vr,idx),cnt+1);
x3 = getval_vn(idx_vr(vr,idx+1),cnt+1);
/* Calc some useful values */
//if ((t2-t1) <= 0.0 || (t3-t2) <= 0.0)
//syslog(LOG_ERR,"vr2pararray: Equal time points and unsortet times are not supported.",tacc,idx);
v1 = (x2-x1)/(t2-t1);
v2 = (x3-x2)/(t3-t2);
t1p2 = (t1 + t2)/2;
t2p3 = (t2 + t3)/2;
/* Shrink acceleration if necessary */
tmax = min_bt(2*(t2-t1p2),2*(t2p3-t2));
//vf = v0 + at => t = (vf-v0)/a
tacc = fabs(v2-v1)/acc;
if (tmax < tacc) {
//Need to increase acceleration to make corner
tacc = tmax;
//syslog(LOG_ERR,"vr2pararray: Forcing acc time decrease to %f at point %d.",tacc,idx);
}
/* Calc : tf_prev & saf carry history from prev loops */
sa0 = x2 - v1*(tacc/2); //acc start pos
tf_prev = s0sfspftf_pb(&pa,tf_prev,saf,sa0,v1,t2-tacc/2); //velocity seg
add_bseg_pa(pavn->pa[cnt],&pa);
saf = x2 + v2*(tacc/2); //acc end pos
tf_prev = s0sfspftf_pb(&pa,tf_prev,sa0,saf,v2,t2+tacc/2); //acc seg
add_bseg_pa(pavn->pa[cnt],&pa);
//usleep(1);
}
v2 = 0.0;
tf_prev = s0sfspftf_pb(&pa,tf_prev,saf,sa0_last,v1_last,t3-t_last); //last velocity seg
add_bseg_pa(pavn->pa[cnt],&pa);
tf_prev = s0sfspftf_pb(&pa,tf_prev,sa0_last,saf_last,v2,t3); //acc seg ending at tf
add_bseg_pa(pavn->pa[cnt],&pa);
}
set_vn(idx_vr(vr,0),p0);
set_vn(idx_vr(vr,numrows_vr(vr)-1),pf);
return pavn;
}
/**
Handles inner (theta) angles of pi. Explodes on angles of 0.0
*/
btreal init_cseg(cseg *csg,btreal rho, vect_n *a, vect_n *b, vect_n *c)
{
int vsz;
local_vn(tmpa,10);
local_vn(tmpb,10);
btreal div,churl;
vsz = len_vn(a);
init_vn(tmpa,vsz);
init_vn(tmpb,vsz);
csg->a = new_vn(vsz);
csg->b = new_vn(vsz);
csg->c = new_vn(vsz);
csg->C = new_vn(vsz);
csg->W = new_vn(vsz);
csg->A = new_vn(vsz);
csg->x0 = new_vn(vsz);
csg->x0p = new_vn(vsz);
csg->xf = new_vn(vsz);
csg->xfp = new_vn(vsz);
csg->last_result = new_vn(vsz);
csg->rho = rho;
set_vn(csg->a,a);
set_vn(csg->b,b);
set_vn(csg->c,c);
set_vn(tmpa,unit_vn(sub_vn(a,b)));
set_vn(csg->x0p,neg_vn(tmpa));
set_vn(tmpb,unit_vn(sub_vn(c,b)));
set_vn(csg->xfp,tmpb);
printf("\n");
print_vn(tmpa);printf("\n");
print_vn(tmpb);printf("\n");
csg->theta = angle_vn(tmpa,tmpb)/2; //Half the angle between the two line segments
csg->phi = pi/2.0 - csg->theta; //The other angle in the triangle (TH #7, pp12)
div = sin(csg->theta)/cos(csg->theta);
csg->ptlen = csg->rho/div;
csg->hlen = sqrt(csg->rho*csg->rho + csg->ptlen*csg->ptlen);
set_vn(csg->x0,add_vn(csg->b,scale_vn(csg->ptlen,tmpa)));
set_vn(csg->xf,add_vn(csg->b,scale_vn(csg->ptlen,tmpb)));
set_vn(csg->C,add_vn(csg->b,scale_vn(csg->hlen,unit_vn(add_vn(scale_vn(0.5,sub_vn(tmpb,tmpa)),tmpa)))));
set_vn(csg->A,unit_vn(sub_vn(csg->xf,csg->x0)));
csg->beta = pi/2 - angle_vn(csg->x0p,csg->A);
return csg->phi*csg->rho*2.0;
}
int do_mode_mu()
{
int err; /* Generic error variable for function calls */
int busCount; /* Number of WAMs defined in the configuration file */
int j;
/* Stuff that's filled in once */
vect_n ** poses;
int num_poses;
int pose;
vect_n * angle_diff;
/* Stuff that's used once for each pose in measure mode */
vect_n * torques_top;
vect_n * positions_top;
vect_n * torques_bottom;
vect_n * positions_bottom;
/* For storing the results from measure mode, one vector per pose */
vect_n ** torques;
vect_n ** positions;
/* For storing the results from computation mode, one vector per joint */
vect_3 ** mus;
/* Make a list of phases for each pose*/
enum {
MU_P_START,
MU_P_TO_TOP,
MU_P_FROM_TOP,
MU_P_MEAS_TOP,
MU_P_TO_BOT,
MU_P_FROM_BOT,
MU_P_MEAS_BOT,
MU_P_DONE
} phase;
char * phasenm[] = {
"START",
"TO_TOP",
"FROM_TOP",
"MEAS_TOP",
"TO_BOT",
"FROM_BOT",
"MEAS_BOT",
"DONE"
};
clear();
mvprintw(0,0,"Starting Gravity Calibration Mode");
/* Ensure the WAM is set up, power cycled, and in the home position */
mvprintw(2,0,"To begin the calibration, follow the following steps:");
mvprintw(3,0," a) Ensure that all WAM power and signal cables are securely fastened.");
mvprintw(4,0," b) Ensure that the WAM is powered on (receiving power).");
mvprintw(5,0," c) Ensure that all E-STOPs are released.");
mvprintw(6,0," d) Place the WAM in Shift+Idle mode.");
mvprintw(7,0," e) Carefully ensure that the WAM is in its home (folded) position.");
mvprintw(8,0,"Press [Enter] to continue.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Initialize system buses */
err = ReadSystemFromConfig("../../wam.conf", &busCount);
if(err) return -1;
/* Spin off the CAN thread */
startDone = 0;
btrt_thread_create(&can_thd,"can",45,(void*)can_thd_function,NULL);
while (!startDone) usleep(10000);
/* Allocate the global variables */
mu_jts = (double **) malloc( wam->dof * sizeof(double *) );
mu_jps = (double **) malloc( wam->dof * sizeof(double *) );
for (j=0; j<wam->dof; j++)
{
mu_jts[j] = (double *) malloc( NUM_POINTS * sizeof(double) );
mu_jps[j] = (double *) malloc( NUM_POINTS * sizeof(double) );
}
/* Spin off the WAM thread */
mu_n = NUM_POINTS;
wam_thd.period = 0.002; /* Control loop period in seconds */
registerWAMcallback(wam, mu_callback);
btrt_thread_create(&wam_thd,"ctrl",90,(void*)WAMControlThread,(void*)wam);
/* Grab poses from the configuration file */
do {
btparser parser;
char key[256];
err = btParseFile(&parser,"cal.conf");
if (err)
{
syslog(LOG_ERR,"Calibration configuration file cal.conf not found.");
printf("Calibration configuration file cal.conf not found.\n");
break;
}
sprintf(key,"calibration-poses-wam%d.poseCount",wam->dof);
err = btParseGetVal(&parser,INT, key, &num_poses);
if (err || num_poses < 4)
{
syslog(LOG_ERR,"Configuration group calibration-poses-wam%d not found,",wam->dof);
syslog(LOG_ERR,"numPoses not present, or numPoses not at least 4.");
printf("Configuration group calibration-poses-wam%d not found,\n",wam->dof);
printf("numPoses not present, or numPoses not at least 4.");
btParseClose(&parser);
break;
}
poses = (vect_n **) btmalloc( num_poses * sizeof(vect_n *) );
for (pose=0; pose<num_poses; pose++)
{
poses[pose] = new_vn(wam->dof);
sprintf(key, "calibration-poses-wam%d.pose[%d]", wam->dof, pose);
err = btParseGetVal(&parser, VECTOR, key, (void*)poses[pose]);
if (err)
{
syslog(LOG_ERR,"Not enough poses found! (%d expected, found only %d)",
num_poses, pose);
printf("Not enough poses found! (%d expected, found only %d)\n",
num_poses, pose);
break;
}
}
btParseClose(&parser);
} while (0);
if (err)
{
/* Free the variables */
for (j=0; j<wam->dof; j++)
{
free(mu_jts[j]);
free(mu_jps[j]);
}
free(mu_jts);
free(mu_jps);
/* Stop threads ... */
wam_thd.done = 1;
usleep(10000);
can_thd.done = 1;
usleep(50000);
/* End ncurses */
endwin();
return 0;
}
angle_diff = new_vn(wam->dof);
fill_vn(angle_diff,ANGLE_DIFF);
torques_top = new_vn(wam->dof);
positions_top = new_vn(wam->dof);
torques_bottom = new_vn(wam->dof);
positions_bottom = new_vn(wam->dof);
torques = (vect_n **) malloc( num_poses * sizeof(vect_n *) );
positions = (vect_n **) malloc( num_poses * sizeof(vect_n *) );
for (pose=0; pose<num_poses; pose++)
{
torques[pose] = new_vn(wam->dof);
positions[pose] = new_vn(wam->dof);
}
mus = (vect_3 **) malloc( wam->dof * sizeof(vect_3 *) );
for (j=0; j<wam->dof; j++)
mus[j] = new_v3();
/* Allow the user to shift-activate */
mvprintw(10,0,"IMPORTANT: Once gravity compensation begins, the WAM will begin");
mvprintw(11,0,"to move to a set of %d predefined poses (defined in cal.conf).",num_poses);
mvprintw(13,0,"DO NOT TOUCH the WAM during the measurement process, or the");
mvprintw(14,0,"calibration computations will be sigificantly wrong, and");
mvprintw(15,0,"any subsequent gravity compensation will fail spectacularly!");
mvprintw(17,0,"Place the WAM in Shift+Activate mode,");
mvprintw(18,0,"and press [Enter] to start.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Start the GUI! */
pose = 0;
phase = MU_P_START;
clear();
mvprintw(1,0,"Note: Press [Control+C] at any time to cancel the calibration.");
mvprintw(2,0,"DO NOT TOUCH the WAM during the calibration process!");
done = 0;
signal(SIGINT, sigint);
while (!done)
{
/* Print current state */
mvprintw(0, 0,"Current Pose: %d of %d. ",pose+1,num_poses);
mvprintw(0,30,"Current Phase: %s ",phasenm[phase]);
/* Print current joint position and torque */
mvprintw(4,0, " Joint:");
mvprintw(5,0, " Position:");
mvprintw(6,0, " Torque:");
for (j=0; j<wam->dof; j++)
{
mvprintw(4, 13 + 9*j, " Joint %d ",j+1);
mvprintw(5, 13 + 9*j, "% 08.5f ",wam->Jpos->q[j]);
mvprintw(6, 13 + 9*j, "% 08.4f ",wam->Jtrq->q[j]);
}
/* Line 9 - Status Updates */
mvprintw(8,0,"Current Status:");
/* Note - lines 12-?? reserved for printing statistics */
mvprintw(11,0,"Recent Statistics:");
refresh();
usleep(5E4);
/* Move through the state machine */
switch (phase)
{
case MU_P_START:
mvprintw(9,3,"Moving to above position ... ");
MoveSetup(wam, 1.0, 1.0);
MoveWAM(wam,add_vn(poses[pose],angle_diff));
phase++;
break;
case MU_P_TO_TOP:
if (!MoveIsDone(wam)) break;
mvprintw(9,3,"Moving to position (from above) ... ");
MoveSetup(wam, 0.05, 0.05);
MoveWAM(wam,poses[pose]);
phase++;
case MU_P_FROM_TOP:
if (!MoveIsDone(wam)) break;
mvprintw(9,3,"Starting a measurement ... ");
mu_n = 0;
phase++;
break;
case MU_P_MEAS_TOP:
if (mu_n < NUM_POINTS) break;
mu_stats( torques_top, positions_top );
mvprintw(9,3,"Moving to below position ... ");
MoveSetup(wam, 1.0, 1.0);
MoveWAM(wam,sub_vn(poses[pose],angle_diff));
phase++;
break;
case MU_P_TO_BOT:
if (!MoveIsDone(wam)) break;
mvprintw(9,3,"Moving to position (from below) ... ");
MoveSetup(wam, 0.05, 0.05);
MoveWAM(wam,poses[pose]);
phase++;
break;
case MU_P_FROM_BOT:
if (!MoveIsDone(wam)) break;
mvprintw(9,3,"Starting a measurement ... ");
mu_n = 0;
phase++;
break;
case MU_P_MEAS_BOT:
if (mu_n < NUM_POINTS) break;
mu_stats( torques_bottom, positions_bottom );
phase++;
break;
case MU_P_DONE:
/* Get the midpoint position and torque ... */
set_vn(torques[pose],scale_vn(0.5,add_vn(torques_top,torques_bottom)));
set_vn(positions[pose],scale_vn(0.5,add_vn(positions_top,positions_bottom)));
pose++;
phase = MU_P_START;
if (pose == num_poses)
done = 1;
break;
}
}
/* Re-fold, print, and exit */
clear();
mvprintw(0,0,"Moving back to the park location ...");
refresh();
MoveSetup(wam, 1.0, 1.0);
MoveWAM(wam,wam->park_location);
while (!MoveIsDone(wam)) usleep(10000);
mvprintw(1,0,"Shift+Idle, and press [Enter] to continue.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Stop threads ... */
wam_thd.done = 1;
usleep(10000);
can_thd.done = 1;
usleep(50000);
/* Free unneeded variables */
for (j=0; j<wam->dof; j++)
{
free(mu_jts[j]);
free(mu_jps[j]);
}
free(mu_jts);
free(mu_jps);
destroy_vn(&angle_diff);
destroy_vn(&torques_top);
destroy_vn(&positions_top);
destroy_vn(&torques_bottom);
destroy_vn(&positions_bottom);
/* End ncurses */
endwin();
if (done == 1)
{
/* Here we have the "Iterative Algorithm"
* described in the Chris Dellin document entitled
* "Newton-Euler First-Moment Gravity Compensation" */
/* Here we have the matrix composed of L matrices (negative) */
matr_mn * nLL;
/* We have a GT matrix and a Y vector for each link */
matr_mn ** GT;
vect_n ** Y;
/* We have a solution vector P for each link */
vect_n ** P;
vect_n * grav_torques;
/* Start calculating ...
* We have vectors of torque and position
* in torques[] and positions[] */
printf("\n");
printf("Calculating ...\n");
/* Make the LL matrix */
nLL = new_mn( 3*num_poses, 3+2*num_poses );
zero_mn(nLL);
for (pose=0; pose<num_poses; pose++)
{
setval_mn(nLL, 3*pose+0, 3+2*pose+0, -1.0 );
setval_mn(nLL, 3*pose+1, 3+2*pose+1, -1.0 );
}
/* Make each link's GT matrix */
/* Make each link's Y torque solution matrix */
Y = (vect_n **) btmalloc( wam->dof * sizeof(vect_n *) );
GT = (matr_mn **) btmalloc( wam->dof * sizeof(matr_mn *) );
for (j=0; j<wam->dof; j++)
{
Y[j] = new_vn( 3*num_poses );
GT[j] = new_mn( 3*num_poses, 3+2*num_poses );
zero_mn(GT[j]);
}
grav_torques = new_vn(wam->dof);
for (pose=0; pose<num_poses; pose++)
{
/* Put the robot in the pose (by position) */
set_vn(wam->robot.q,positions[pose]);
eval_fk_bot(&wam->robot);
get_gravity_torques(&wam->robot,grav_torques);
for (j=0; j<wam->dof; j++)
{
/* GT: Gravity skew matrix */
setval_mn(GT[j], 3*pose+0, 1, -getval_v3(wam->robot.links[j].g,2) );
setval_mn(GT[j], 3*pose+0, 2, getval_v3(wam->robot.links[j].g,1) );
setval_mn(GT[j], 3*pose+1, 0, getval_v3(wam->robot.links[j].g,2) );
setval_mn(GT[j], 3*pose+1, 2, -getval_v3(wam->robot.links[j].g,0) );
setval_mn(GT[j], 3*pose+2, 0, -getval_v3(wam->robot.links[j].g,1) );
setval_mn(GT[j], 3*pose+2, 1, getval_v3(wam->robot.links[j].g,0) );
/* GT: -R*L */
setval_mn(GT[j], 3*pose+0, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,0) );
setval_mn(GT[j], 3*pose+0, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,0) );
setval_mn(GT[j], 3*pose+1, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,1) );
setval_mn(GT[j], 3*pose+1, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,1) );
setval_mn(GT[j], 3*pose+2, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,2) );
setval_mn(GT[j], 3*pose+2, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,2) );
/* Y */
setval_vn(Y[j], 3*pose+0, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,0) );
setval_vn(Y[j], 3*pose+1, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,1) );
setval_vn(Y[j], 3*pose+2, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,2) );
if (j < wam->dof-1)
setval_vn(Y[j], 3*pose+2, getval_vn(Y[j],3*pose+2) - getval_vn(torques[pose],j+1) );
}
}
destroy_vn(&grav_torques);
/* Make a space for each link's P solution vector */
P = (vect_n **) btmalloc( wam->dof * sizeof(vect_n *) );
for (j=0; j<wam->dof; j++)
P[j] = new_vn( 3+2*num_poses );
/* Do the regression for each link */
{
int i;
matr_mn * m2x2;
matr_mn * m2x3;
vect_n * Yi;
vect_n * index;
vect_n * col;
m2x2 = new_mn( 3+2*num_poses, 3+2*num_poses );
m2x3 = new_mn( 3+2*num_poses, 3*num_poses );
Yi = new_vn( 3*num_poses );
index = new_vn( 3+2*num_poses );
col = new_vn( 3+2*num_poses );
for (j=wam->dof-1; j>=0; j--)
{
mul_mn( m2x2, T_mn(GT[j]), GT[j] );
for (i=0; i<3+2*num_poses; i++)
setval_mn(m2x2,i,i, getval_mn(m2x2,i,i) + CALC_LAMBDA );
mul_mn( m2x3, inv_mn(m2x2,index,col), T_mn(GT[j]) );
if (j < wam->dof-1)
matXvec_mn(nLL,P[j+1],Yi);
else
fill_vn(Yi,0.0);
set_vn(Yi, add_vn(Yi,Y[j]) );
matXvec_mn( m2x3, Yi, P[j] );
}
destroy_mn(&m2x2);
destroy_mn(&m2x3);
destroy_vn(&Yi);
destroy_vn(&index);
destroy_vn(&col);
/* Copy the results */
for (j=0; j<wam->dof; j++)
{
setval_v3(mus[j],0, getval_vn(P[j],0) );
setval_v3(mus[j],1, getval_vn(P[j],1) );
setval_v3(mus[j],2, getval_vn(P[j],2) );
}
/* Clean up */
destroy_mn(&nLL);
for (j=0; j<wam->dof; j++)
{
destroy_mn(>[j]);
destroy_vn(&Y[j]);
destroy_vn(&P[j]);
}
btfree((void **)>);
btfree((void **)&Y);
btfree((void **)&P);
}
/* Print results */
printf("\n");
printf("Gravity calibration ended.\n");
printf("\n");
printf("Copy the following lines into your wam.conf file, in the %s{} group.\n",wam->name);
printf("It is usually placed above the safety{} group.\n");
printf("--------\n");
printf(" # Calibrated gravity vectors\n");
printf(" calibrated-gravity{\n");
for (j=0; j<wam->dof; j++)
{
printf(" mu%d = < % .6f, % .6f, % .6f >\n", j+1,
getval_v3(mus[j],0),
getval_v3(mus[j],1),
getval_v3(mus[j],2));
}
printf(" }\n");
printf("--------\n");
{
FILE * logfile;
logfile = fopen("cal-gravity.log","w");
if (logfile)
{
fprintf(logfile," # Calibrated gravity vectors\n");
fprintf(logfile," calibrated-gravity{\n");
for (j=0; j<wam->dof; j++)
fprintf(logfile," mu%d = < % .6f, % .6f, % .6f >\n", j+1,
getval_v3(mus[j],0),
getval_v3(mus[j],1),
getval_v3(mus[j],2));
fprintf(logfile," }\n");
fclose(logfile);
printf("This text has been saved to cal-gravity.log.\n");
}
else
{
syslog(LOG_ERR,"Could not write to cal-gravity.log.");
printf("Error: Could not write to cal-gravity.log.\n");
}
}
printf("\n");
}
/* Free the variables */
for (pose=0; pose<num_poses; pose++)
{
destroy_vn(&torques[pose]);
destroy_vn(&positions[pose]);
}
free(torques);
free(positions);
for (j=0; j<wam->dof; j++)
destroy_vn((vect_n **)&mus[j]);
free(mus);
return 0;
}
int do_mode_zero()
{
int err; /* Generic error variable for function calls */
int busCount; /* Number of WAMs defined in the configuration file */
int i; /* For iterating through the pucks */
/* GUI stuff */
enum {
MODE_TOZERO,
MODE_CANCEL,
MODE_PRINTVALS,
MODE_JSELECT,
MODE_EDIT
} mode;
int joint;
int decplace;
vect_n * jangle;
char newhome[80];
char zeromag[80];
clear();
mvprintw(0,0,"Starting Zero Calibration Mode");
/* Ensure the WAM is set up, power cycled, and in the home position */
mvprintw(2,0,"To begin the calibration, follow the following steps:");
mvprintw(3,0," a) Ensure that all WAM power and signal cables are securely fastened.");
mvprintw(4,0," b) Ensure that the WAM is powered on (receiving power).");
mvprintw(5,0," c) E-STOP the WAM (Important!).");
mvprintw(6,0," d) Release all E-STOPs.");
mvprintw(7,0," e) Place the WAM in Shift+Idle mode.");
mvprintw(8,0," f) Carefully ensure that the WAM is in its home (folded) position.");
mvprintw(9,0,"Press [Enter] to continue.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Initialize system buses */
err = ReadSystemFromConfig("../../wam.conf", &busCount);
if(err) return -1;
/* Spin off the CAN thread */
startDone = 0;
btrt_thread_create(&can_thd,"can",45,(void*)can_thd_function,NULL);
while (!startDone) usleep(10000);
/* Spin off the magenc thread, which also detecs puck versions into mechset */
/* EEK! Why must this be such high priority? */
mz_mechset = (int *) malloc( wam->dof * sizeof(int) );
mz_magvals = (int *)calloc( wam->dof, sizeof(int) );
mz_magvals_set = 0;
btrt_thread_create(&magenc_thd, "mage", 91, (void*)magenc_thd_function, NULL);
/* Spin off the WAM thread */
wam_thd.period = 0.002; /* Control loop period in seconds */
btrt_thread_create(&wam_thd,"ctrl",90,(void*)WAMControlThread,(void*)wam);
/* Allow the user to shift-activate */
mvprintw(11,0,"Place the WAM in Shift+Activate mode,");
mvprintw(12,0,"and press [Enter] to continue.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Hold position */
SetJointSpace(wam);
MoveSetup(wam, 1.0, 1.0);
MoveWAM(wam,wam->Jpos);
/* Start the user interface */
mode = MODE_TOZERO;
joint = 0;
jangle = new_vn(wam->dof);
set_vn(jangle,wam->Jpos);
clear();
done = 0;
signal(SIGINT, sigint);
while (!done)
{
char buf[80];
int j;
int line;
enum btkey key;
/* Display the Zeroing calibration stuff ... */
mz_magvals_set = 1;
mvprintw(0, 0, "Joint Position (rad): %s", sprint_vn(buf, wam->Jpos));
line = 2;
if (mode == MODE_TOZERO)
{
attron(A_BOLD);
mvprintw(line++, 0, "--> ] Move To Current Zero");
attroff(A_BOLD);
}
else
mvprintw(line++, 0, " ] Move To Current Zero");
if (mode == MODE_CANCEL)
{
attron(A_BOLD);
mvprintw(line++, 0, "--> ] Cancel Calibration");
attroff(A_BOLD);
}
else
mvprintw(line++, 0, " ] Cancel Calibration");
if (mode == MODE_PRINTVALS)
{
attron(A_BOLD);
mvprintw(line++, 0, "--> ] Print Calibrated Values and Exit");
attroff(A_BOLD);
}
else
mvprintw(line++, 0, " ] Print Calibrated Values and Exit");
if (mode == MODE_JSELECT)
{
attron(A_BOLD);
mvprintw(line, 0, "--> ] Joint:");
attroff(A_BOLD);
}
else
mvprintw(line, 0, " ] Joint:");
mvprintw(line+1, 5, "-------");
if (mode == MODE_EDIT) attron(A_BOLD);
mvprintw(line+2, 5, " Set:");
if (mode == MODE_EDIT) attroff(A_BOLD);
mvprintw(line+3, 5, "Actual:");
mvprintw(line+5, 5, " Motor:");
mvprintw(line+6, 5, "MagEnc:");
for (j=0; j<wam->dof; j++)
{
if ((mode == MODE_JSELECT || mode == MODE_EDIT) && j==joint)
{
attron(A_BOLD);
mvprintw(line+0, 13 + 9*j, "[Joint %d]",j+1);
attroff(A_BOLD);
}
else
mvprintw(line+0, 13 + 9*j, " Joint %d ",j+1);
/* total with 8, 5 decimal points (+0.12345) */
if ( mode==MODE_EDIT && j==joint )
{
int boldplace;
mvprintw(line+1, 13 + 9*j, " _._____ ",j+1);
mvprintw(line+2, 13 + 9*j, "% 08.5f ",jangle->q[j]);
boldplace = decplace + 1;
if (decplace) boldplace++;
mvprintw(line+1, 13 + 9*j + boldplace,"x");
mvchgat(line+2, 13 + 9*j+boldplace, 1, A_BOLD, 0, NULL );
}
else
{
mvprintw(line+1, 13 + 9*j, " ------- ",j+1);
mvprintw(line+2, 13 + 9*j, "% 08.5f ",jangle->q[j]);
}
mvprintw(line+3, 13 + 9*j, "% 08.5f ",getval_vn(wam->Jpos,j));
mvprintw(line+5, 13 + 9*j, " Motor %d",j+1);
if (mz_mechset[j])
mvprintw(line+6, 13 + 9*j, " %04d",mz_magvals[j]);
else
mvprintw(line+6, 13 + 9*j, " (None)",mz_magvals[j]);
}
refresh();
/* Wait a bit ... */
usleep(5E4);
key = btkey_get();
/* If the user is in menu mode, work the menu ... */
if (mode != MODE_EDIT) switch (key)
{
case BTKEY_UP:
mode--;
if ((signed int)mode < 0) mode = 0;
break;
case BTKEY_DOWN:
mode++;
if (mode > MODE_JSELECT) mode = MODE_JSELECT;
break;
case BTKEY_ENTER:
switch (mode)
{
case MODE_TOZERO:
if (!MoveIsDone(wam)) break;
fill_vn(jangle,0.0);
MoveWAM(wam,jangle);
break;
case MODE_CANCEL:
done = -1;
break;
case MODE_PRINTVALS:
if (!MoveIsDone(wam)) break;
done = 1;
break;
case MODE_JSELECT:
mode = MODE_EDIT;
decplace = 4;
break;
}
break;
default:
if (mode == MODE_JSELECT) switch (key)
{
case BTKEY_LEFT:
joint--;
if (joint < 0) joint = 0;
break;
case BTKEY_RIGHT:
joint++;
if (joint >= wam->dof) joint = wam->dof - 1;
break;
}
break;
}
/* We're in joint edit mode */
else switch (key)
{
case BTKEY_LEFT:
decplace--;
if (decplace < 0) decplace = 0;
break;
case BTKEY_RIGHT:
decplace++;
if (decplace > 5) decplace = 5;
break;
case BTKEY_BACKSPACE:
mode = MODE_JSELECT;
break;
/* Actually do the moves */
case BTKEY_UP:
if (!MoveIsDone(wam)) break;
jangle->q[joint] += pow(10,-decplace);
MoveWAM(wam,jangle);
break;
case BTKEY_DOWN:
if (!MoveIsDone(wam)) break;
jangle->q[joint] -= pow(10,-decplace);
MoveWAM(wam,jangle);
break;
default:
break;
}
}
if (done == 1)
{
vect_n * vec;
/* Save the new home location */
vec = new_vn(wam->dof);
set_vn( vec, sub_vn(wam->park_location,jangle) );
sprint_vn(newhome,vec);
destroy_vn(&vec);
/* Save the zeromag values */
for (i=0; i<wam->dof; i++) if (!mz_mechset[i]) mz_magvals[i] = -1;
sprintf(zeromag,"< %d",mz_magvals[0]);
for (i=1; i<wam->dof; i++)
sprintf(zeromag+strlen(zeromag),", %d",mz_magvals[i]);
sprintf(zeromag+strlen(zeromag)," >");
}
/* Re-fold, print, and exit */
clear();
mvprintw(0,0,"Moving back to the park location ...");
refresh();
MoveWAM(wam,wam->park_location);
while (!MoveIsDone(wam)) usleep(10000);
mvprintw(1,0,"Shift+Idle, and press [Enter] to continue.");
refresh();
while (btkey_get()!=BTKEY_ENTER) usleep(10000);
/* Stop threads ... */
wam_thd.done = 1;
usleep(10000);
can_thd.done = 1;
usleep(50000);
/* Stop ncurses ... */
endwin();
if (done == 1)
{
/* Print the results */
printf("\n");
printf("Zeroing calibration ended.\n");
printf("\n");
for (i=0; i<wam->dof; i++) if (!mz_mechset[i])
{
printf("Note: Some (or all) of your pucks do not support absolute\n");
printf("position measurement, either because they do not use magnetic\n");
printf("encoders, or because they have not been updated to firmware r118.\n");
printf("\n");
break;
}
printf("Copy the following lines into your wam.conf file,\n");
printf("near the top, in the %s{} group.\n",wam->name);
printf("Make sure it replaces the old home = < ... > definition.\n");
printf("--------\n");
printf(" # Calibrated zero values ...\n");
printf(" home = %s\n",newhome);
for (i=0; i<wam->dof; i++) if (mz_mechset[i])
{ printf(" zeromag = %s\n",zeromag); break; }
printf("--------\n");
{
FILE * logfile;
logfile = fopen("cal-zero.log","w");
if (logfile)
{
fprintf(logfile," # Calibrated zero values ...\n");
fprintf(logfile," home = %s\n",newhome);
for (i=0; i<wam->dof; i++) if (mz_mechset[i])
{ fprintf(logfile," zeromag = %s\n",zeromag); break; }
fclose(logfile);
printf("This text has been saved to cal-zero.log.\n");
printf("\n");
}
else
{
syslog(LOG_ERR,"Could not write to cal-zero.log.");
printf("Error: Could not write to cal-zero.log.\n");
printf("\n");
}
}
printf("Note that you must E-Stop (or power-cycle) your WAM\n");
printf("for the calibrated settings to take effect!\n");
printf("\n");
}
return 0;
}
