void idlingCoupledJoints(const int i, const bool sw)
{
IControlMode *imod;
driver->view(imod);
PidData &pid=pids[i];
for (size_t j=0; j<pid.idling_joints.size(); j++)
imod->setControlMode(pid.idling_joints[j],
sw?VOCAB_CM_IDLE:VOCAB_CM_POSITION);
}
void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
IPositionControl *ipos = currentPart->pos;
IPidControl *ipid = currentPart->pid;
IPositionDirect *iDir = currentPart->iDir;
GtkWidget **sliderVel = currentPart->sliderVelArray;
double val = gtk_range_get_value(range);
double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]);
IControlMode *iCtrl = currentPart->ctrlmode2;
int mode;
iCtrl->getControlMode(*joint, &mode);
if (!POS_UPDATE[*joint])
{
if( ( mode == VOCAB_CM_POSITION) || (mode == VOCAB_CM_MIXED) )
{
ipos->setRefSpeed(*joint, valVel);
ipos->positionMove(*joint, val);
}
else if( ( mode == VOCAB_CM_IMPEDANCE_POS))
{
fprintf(stderr, " using old 'impedance_position' mode, this control mode is deprecated!");
ipos->setRefSpeed(*joint, valVel);
ipos->positionMove(*joint, val);
}
else if ( mode == VOCAB_CM_POSITION_DIRECT)
{
if (position_direct_enabled)
{
iDir->setPosition(*joint, val);
}
else
{
fprintf(stderr, "You cannot send direct position commands without using --direct option!");
}
}
else
{
fprintf(stderr, "Joint not in position nor positionDirect so cannot send references");
}
}
return;
}
void partMover::sliderVel_release(GtkRange *range, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IPositionDirect *iDir = currentPart->iDir;
GtkWidget **sliderAry = currentPart->sliderArray;
IControlMode *iCtrl = currentPart->ctrlmode2;
int mode;
double val = gtk_range_get_value(range);
double posit = gtk_range_get_value((GtkRange *) sliderAry[*joint]);
ipos->setRefSpeed(*joint, val);
iCtrl->getControlMode(*joint, &mode);
ipos->positionMove(*joint, posit);
return;
}
void _send(const ActionItem *x)
{
if (!connected)
{
cerr<<"Error: not connected to control board skipping"<<endl;
return;
}
int size=x->getCmd().size();
int offset=x->getOffset();
double time=x->getTime();
int nJoints=0;
enc->getAxes(&nJoints);
if ((offset+size)>nJoints)
{
cerr<<"Error: detected possible overflow, skipping"<<endl;
cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
return;
}
Vector disp(size);
if (time==0)
{
return;
}
for (size_t i=0; i<disp.length(); i++)
{
double q;
if (!enc->getEncoder(offset+i,&q))
{
cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
return;
}
disp[i]=x->getCmd()[i]-q;
if (disp[i]<0.0)
disp[i]=-disp[i];
}
// don't blend together the two "for"
// since we have to enforce the modes on the whole
// prior to commanding the joints
std::vector<int> joints;
std::vector<int> modes;
std::vector<double> speeds;
std::vector<double> positions;
for (size_t i=0; i<disp.length(); i++)
{
joints.push_back(offset+i);
speeds.push_back(disp[i]/time);
modes.push_back(VOCAB_CM_POSITION);
positions.push_back(x->getCmd()[i]);
}
mode->setControlModes(disp.length(), joints.data(), modes.data());
yarp::os::Time::delay(0.01); // give time to update control modes value
mode->getControlModes(disp.length(), joints.data(), modes.data());
for (size_t i=0; i<disp.length(); i++)
{
if(modes[i] != VOCAB_CM_POSITION)
{
yError() << "Joint " << i << " not in position mode";
}
}
pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
pos->positionMove(disp.length(), joints.data(), positions.data());
cout << "Script port: " << x->getCmd().toString() << endl;
}
bool partMover::entry_update(partMover *currentPart)
{
GdkColor color_black;
GdkColor color_grey;
GdkColor color_yellow;
GdkColor color_green;
GdkColor color_green_blue;
GdkColor color_dark_green;
GdkColor color_red;
GdkColor color_fault_red;
GdkColor color_pink;
GdkColor color_indaco;
GdkColor color_white;
GdkColor color_blue;
color_pink.red=219*255;
color_pink.green=166*255;
color_pink.blue=171*255;
color_fault_red.red=255*255;
color_fault_red.green=10*255;
color_fault_red.blue=10*255;
color_black.red=10*255;
color_black.green=10*255;
color_black.blue=10*255;
color_red.red=255*255;
color_red.green=100*255;
color_red.blue=100*255;
color_grey.red=220*255;
color_grey.green=220*255;
color_grey.blue=220*255;
color_white.red=250*255;
color_white.green=250*255;
color_white.blue=250*255;
color_green.red=149*255;
color_green.green=221*255;
color_green.blue=186*255;
color_dark_green.red=(149-30)*255;
color_dark_green.green=(221-30)*255;
color_dark_green.blue=(186-30)*255;
color_blue.red=150*255;
color_blue.green=190*255;
color_blue.blue=255*255;
color_green_blue.red=(149+150)/2*255;
color_green_blue.green=(221+190)/2*255;
color_green_blue.blue=(186+255)/2*255;
color_indaco.red=220*255;
color_indaco.green=190*255;
color_indaco.blue=220*255;
color_yellow.red=249*255;
color_yellow.green=236*255;
color_yellow.blue=141*255;
GdkColor* pColor= &color_grey;
static int slowSwitcher = 0;
IControlMode *ictrl = currentPart->ctrlmode2;
IInteractionMode *iint = currentPart->iinteract;
IPositionControl *ipos = currentPart->pos;
IVelocityControl *ivel = currentPart->iVel;
IPositionDirect *iDir = currentPart->iDir;
IEncoders *iiencs = currentPart->iencs;
ITorqueControl *itrq = currentPart->trq;
IAmplifierControl *iamp = currentPart->amp;
GtkEntry * *pos_entry = (GtkEntry **) currentPart->currPosArray;
GtkEntry **trq_entry = (GtkEntry **) currentPart->currTrqArray;
GtkEntry **speed_entry = (GtkEntry **) currentPart->currSpeedArray;
GtkEntry **inEntry = (GtkEntry **) currentPart->inPosArray;
GtkWidget **colorback = (GtkWidget **) currentPart->frameColorBack;
GtkWidget **sliderAry = currentPart->sliderArray;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
char buffer[40] = {'i', 'n', 'i', 't'};
char frame_title [255];
double positions[MAX_NUMBER_OF_JOINTS];
double torques[MAX_NUMBER_OF_JOINTS];
double speeds[MAX_NUMBER_OF_JOINTS];
double max_torques[MAX_NUMBER_OF_JOINTS];
double min_torques[MAX_NUMBER_OF_JOINTS];
static int controlModes[MAX_NUMBER_OF_JOINTS];
static int controlModesOld[MAX_NUMBER_OF_JOINTS];
static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS];
static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS];
int k;
int NUMBER_OF_JOINTS=0;
bool done = false;
bool ret = false;
ipos->getAxes(&NUMBER_OF_JOINTS);
if (NUMBER_OF_JOINTS == 0)
{
fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
Time::delay(0.1);
pColor=&color_grey;
strcpy(frame_title,"DISCONNECTED");
for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
{
if (currentPart->framesArray[k]!=0)
{
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
}
return true;
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
max_torques[k]=0;
min_torques[k]=0;
torques[k]=0;
}
if (!iiencs->getEncoders(positions))
return true;
itrq->getTorques(torques);
iiencs->getEncoderSpeeds(speeds);
//update all joints positions
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
sprintf(buffer, "%.1f", positions[k]);
gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer);
sprintf(buffer, "%.3f", torques[k]);
gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer);
sprintf(buffer, "%.1f", speeds[k]);
gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer);
}
//update all joint sliders
for (k = 0; k < NUMBER_OF_JOINTS; k++)
if(POS_UPDATE[k])
gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]);
// *** update the checkMotionDone box section ***
// (only one at a time in order to save badwidth)
k = slowSwitcher%NUMBER_OF_JOINTS;
slowSwitcher++;
#if DEBUG_GUI
gtk_entry_set_text((GtkEntry*) inEntry[k], "off");
#else
ipos->checkMotionDone(k, &done);
if (!done)
gtk_entry_set_text((GtkEntry*) inEntry[k], " ");
else
gtk_entry_set_text((GtkEntry*) inEntry[k], "@");
#endif
// *** update the controlMode section ***
// the new icubinterface does not increase the bandwidth consumption
// ret = true; useless guys!
ret=ictrl->getControlModes(controlModes);
if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
ret=iint->getInteractionModes(interactionModes);
if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" );
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
controlModesOld[k]=controlModes[k];
sprintf(frame_title,"Joint %d ",k );
switch (controlModes[k])
{
case VOCAB_CM_IDLE:
pColor=&color_yellow;
strcat(frame_title," (IDLE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION:
pColor=&color_green;
strcat(frame_title," (POSITION)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION_DIRECT:
pColor=&color_dark_green;
strcat(frame_title," (POSITION_DIRECT)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_MIXED:
pColor=&color_green_blue;
strcat(frame_title," (MIXED_MODE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_VELOCITY:
pColor=&color_blue;
strcat(frame_title," (VELOCITY)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_TORQUE:
pColor=&color_pink;
strcat(frame_title," (TORQUE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_POS:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE POS)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_VEL:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE VEL)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_OPENLOOP:
pColor=&color_white;
strcat(frame_title," (OPENLOOP)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_HW_FAULT:
pColor=&color_fault_red;
strcat(frame_title," (HARDWARE_FAULT)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"---");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CALIBRATING:
pColor=&color_grey;
strcat(frame_title," (CALIBRATING)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CALIB_DONE:
pColor=&color_grey;
strcat(frame_title," (CALIB DONE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_NOT_CONFIGURED:
pColor=&color_grey;
strcat(frame_title," (NOT CONFIGURED)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CONFIGURED:
pColor=&color_grey;
strcat(frame_title," (CONFIGURED)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
default:
case VOCAB_CM_UNKNOWN:
pColor=&color_grey;
strcat(frame_title," (UNKNOWN)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
}
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && interactionModes[k] == interactionModesOld[k]) continue;
interactionModesOld[k]=interactionModes[k];
switch (interactionModes[k])
{
case VOCAB_IM_STIFF:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_green);
break;
case VOCAB_IM_COMPLIANT:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_fault_red);
break;
default:
case VOCAB_CM_UNKNOWN:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_white);
break;
}
}
currentPart->first_time =false;
return true;
}
bool tune(const int i)
{
PidData &pid=pids[i];
Property pGeneral;
pGeneral.put("joint",i);
string sGeneral="(general ";
sGeneral+=pGeneral.toString();
sGeneral+=')';
Bottle bGeneral,bPlantEstimation,bStictionEstimation;
bGeneral.fromString(sGeneral);
bPlantEstimation.fromString("(plant_estimation (Ts 0.01) (Q 1.0) (R 1.0) (P0 100000.0) (tau 1.0) (K 1.0) (max_pwm 800.0))");
bStictionEstimation.fromString("(stiction_estimation (Ts 0.01) (T 2.0) (vel_thres 5.0) (e_thres 1.0) (gamma (10.0 10.0)) (stiction (0.0 0.0)))");
Bottle bConf=bGeneral;
bConf.append(bPlantEstimation);
bConf.append(bStictionEstimation);
Property pOptions(bConf.toString().c_str());
OnlineCompensatorDesign designer;
if (!designer.configure(*driver,pOptions))
{
yError("designer configuration failed!");
return false;
}
idlingCoupledJoints(i,true);
Property pPlantEstimation;
pPlantEstimation.put("max_time",20.0);
pPlantEstimation.put("switch_timeout",2.0);
designer.startPlantEstimation(pPlantEstimation);
yInfo("Estimating plant for joint %d: max duration = %g seconds",
i,pPlantEstimation.find("max_time").asDouble());
double t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
Property pResults;
designer.getResults(pResults);
double tau=pResults.find("tau_mean").asDouble();
double K=pResults.find("K_mean").asDouble();
yInfo("plant = %g/s * 1/(1+s*%g)",K,tau);
Property pControllerRequirements,pController;
pControllerRequirements.put("tau",tau);
pControllerRequirements.put("K",K);
pControllerRequirements.put("f_c",0.75);
if (i!=15)
{
pControllerRequirements.put("T_dr",1.0);
pControllerRequirements.put("type","PI");
}
else
pControllerRequirements.put("type","P");
designer.tuneController(pControllerRequirements,pController);
yInfo("tuning results: %s",pController.toString().c_str());
double Kp=pController.find("Kp").asDouble();
double Ki=pController.find("Ki").asDouble();
pid.scale=4.0;
int scale=(int)pid.scale; int shift=1<<scale;
double fwKp=floor(Kp*pid.encs_ratio*shift);
double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0);
pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp;
pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi;
pid.Kd=0.0;
yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale);
Property pStictionEstimation;
pStictionEstimation.put("max_time",60.0);
pStictionEstimation.put("Kp",Kp);
pStictionEstimation.put("Ki",0.0);
pStictionEstimation.put("Kd",0.0);
designer.startStictionEstimation(pStictionEstimation);
yInfo("Estimating stiction for joint %d: max duration = %g seconds",
i,pStictionEstimation.find("max_time").asDouble());
t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
designer.getResults(pResults);
pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble());
pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble());
yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down);
IControlMode *imod;
IPositionControl *ipos;
IEncoders *ienc;
driver->view(imod);
driver->view(ipos);
driver->view(ienc);
imod->setControlMode(i,VOCAB_CM_POSITION);
ipos->setRefSpeed(i,50.0);
ipos->positionMove(i,0.0);
yInfo("Driving the joint back to rest... ");
t0=Time::now();
while (Time::now()-t0<5.0)
{
double enc;
ienc->getEncoder(i,&enc);
if (fabs(enc)<1.0)
break;
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
Time::delay(0.2);
}
yInfo("done!");
idlingCoupledJoints(i,false);
return true;
}
bool partMover::entry_update(partMover *currentPart)
{
GdkColor color_grey;
GdkColor color_yellow;
GdkColor color_green;
GdkColor color_red;
GdkColor color_pink;
GdkColor color_indaco;
GdkColor color_white;
GdkColor color_blue;
color_pink.red=219*255;
color_pink.green=166*255;
color_pink.blue=171*255;
color_red.red=255*255;
color_red.green=100*255;
color_red.blue=100*255;
color_grey.red=220*255;
color_grey.green=220*255;
color_grey.blue=220*255;
color_white.red=250*255;
color_white.green=250*255;
color_white.blue=250*255;
color_green.red=149*255;
color_green.green=221*255;
color_green.blue=186*255;
color_blue.red=150*255;
color_blue.green=190*255;
color_blue.blue=255*255;
color_indaco.red=220*255;
color_indaco.green=190*255;
color_indaco.blue=220*255;
color_yellow.red=249*255;
color_yellow.green=236*255;
color_yellow.blue=141*255;
GdkColor* pColor= &color_grey;
static int slowSwitcher = 0;
IControlMode *ictrl = currentPart->ctrlmode;
IPositionControl *ipos = currentPart->pos;
IVelocityControl *ivel = currentPart->iVel;
IEncoders *iiencs = currentPart->iencs;
ITorqueControl *itrq = currentPart->trq;
IAmplifierControl *iamp = currentPart->amp;
GtkEntry * *pos_entry = (GtkEntry **) currentPart->currPosArray;
GtkEntry **trq_entry = (GtkEntry **) currentPart->currTrqArray;
GtkEntry **speed_entry = (GtkEntry **) currentPart->currSpeedArray;
GtkEntry **inEntry = (GtkEntry **) currentPart->inPosArray;
GtkWidget **colorback = (GtkWidget **) currentPart->frameColorBack;
GtkWidget **sliderAry = currentPart->sliderArray;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
char buffer[40] = {'i', 'n', 'i', 't'};
char frame_title [255];
double positions[MAX_NUMBER_OF_JOINTS];
double torques[MAX_NUMBER_OF_JOINTS];
double speeds[MAX_NUMBER_OF_JOINTS];
double max_torques[MAX_NUMBER_OF_JOINTS];
double min_torques[MAX_NUMBER_OF_JOINTS];
static int controlModes[MAX_NUMBER_OF_JOINTS];
static int controlModesOld[MAX_NUMBER_OF_JOINTS];
int k;
int NUMBER_OF_JOINTS=0;
bool done = false;
bool ret = false;
ipos->getAxes(&NUMBER_OF_JOINTS);
if (NUMBER_OF_JOINTS == 0)
{
fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
Time::delay(0.1);
pColor=&color_grey;
strcpy(frame_title,"DISCONNECTED");
for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
{
if (currentPart->framesArray[k]!=0)
{
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
}
return true;
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
max_torques[k]=0;
min_torques[k]=0;
torques[k]=0;
}
if (!iiencs->getEncoders(positions))
return true;
itrq->getTorques(torques);
iiencs->getEncoderSpeeds(speeds);
//update all joints positions
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
sprintf(buffer, "%.1f", positions[k]);
gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer);
sprintf(buffer, "%.3f", torques[k]);
gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer);
sprintf(buffer, "%.1f", speeds[k]);
gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer);
}
//update all joint sliders
for (k = 0; k < NUMBER_OF_JOINTS; k++)
if(POS_UPDATE[k])
gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]);
// *** update the checkMotionDone box section ***
// (only one at a time in order to save badwidth)
k = slowSwitcher%NUMBER_OF_JOINTS;
slowSwitcher++;
#if DEBUG_GUI
gtk_entry_set_text((GtkEntry*) inEntry[k], "off");
#else
ipos->checkMotionDone(k, &done);
if (!done)
gtk_entry_set_text((GtkEntry*) inEntry[k], " ");
else
gtk_entry_set_text((GtkEntry*) inEntry[k], "@");
#endif
// *** update the controlMode section ***
// the new icubinterface does not increase the bandwidth consumption
// ret = true; useless guys!
ret=ictrl->getControlModes(controlModes);
if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
controlModesOld[k]=controlModes[k];
sprintf(frame_title,"Joint %d ",k );
switch (controlModes[k])
{
case VOCAB_CM_IDLE:
pColor=&color_yellow;
strcat(frame_title," (IDLE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION:
pColor=&color_green;
strcat(frame_title," (POSITION)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_VELOCITY:
pColor=&color_blue;
strcat(frame_title," (VELOCITY)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_TORQUE:
pColor=&color_pink;
strcat(frame_title," (TORQUE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_POS:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE POS)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_VEL:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE VEL)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_OPENLOOP:
pColor=&color_white;
strcat(frame_title," (OPENLOOP)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
default:
case VOCAB_CM_UNKNOWN:
pColor=&color_grey;
//strcat(frame_title," (UNKNOWN)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
}
// pColor=&color_blue;
// gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
int curr_amp_status=0;
int amp_status[60]; //fix this!!!
for (int i=0; i<60; i++) amp_status[i]=0; //fix this!!!
iamp->getAmpStatus(amp_status); //fix this!!!
curr_amp_status=amp_status[k]; //fix this!!!
#if 0
if ((amp_status[k] & 0xFF)!=0)
{
//fprintf(stderr, "FAULT DETECTED: %x\n", curr_amp_status);
//pColor=&color_red;
//strcat(frame_title," (FAULT)");
//gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
//gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
#endif
}
currentPart->first_time =false;
return true;
}
