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;
}
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_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;
}