void partMover::dis_click(GtkButton *button, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; ipid->disablePid(*joint); iamp->disableAmp(*joint); return; }
void partMover::run_click(GtkButton *button, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; GtkWidget **sliderAry = currentPart->sliderArray; double posJoint; while (!iiencs->getEncoder(*joint, &posJoint)) Time::delay(0.001); iamp->enableAmp(*joint); ipid->enablePid(*joint); gtk_range_set_value ((GtkRange *) (sliderAry[*joint]), posJoint); return; }
void partMover::idle_all(GtkButton *button, partMover* currentPart) { IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; GtkWidget **sliderAry = currentPart->sliderArray; double posJoint; int joint; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); for (joint=0; joint < NUMBER_OF_JOINTS; joint++) { iiencs->getEncoder(joint, &posJoint); iamp->disableAmp(joint); ipid->disablePid(joint); gtk_range_set_value ((GtkRange *) (sliderAry[joint]), posJoint); } return; }
int main(int argc, char *argv[]) { // just list the devices if no argument given if (argc <= 2) { printf("You can call %s like this:\n", argv[0]); printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]); printf("For example:\n"); printf(" %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]); printf("Here are devices listed for your system:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } // get command line options Property options; options.fromCommand(argc, argv); if (!options.check("robot") || !options.check("part")) { printf("Missing either --robot or --part options\n"); return 0; } Network::init(); Time::turboBoost(); std::string name; Value& v = options.find("robot"); Value& part = options.find("part"); Value *val; if (!options.check("device", val)) { options.put("device", "remote_controlboard"); } if (!options.check("local", val)) { name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient"; //sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str()); options.put("local", name.c_str()); } if (!options.check("remote", val)) { name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str()); //sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str()); options.put("remote", name.c_str()); } fprintf(stderr, "%s", options.toString().c_str()); // create a device PolyDriver dd(options); if (!dd.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); Network::fini(); return 1; } IPositionControl *pos; IPositionDirect *posDir; IVelocityControl *vel; IEncoders *enc; IPidControl *pid; IAmplifierControl *amp; IControlLimits *lim; // IControlMode *icm; IControlMode2 *iMode2; ITorqueControl *itorque; IOpenLoopControl *iopenloop; IImpedanceControl *iimp; IInteractionMode *iInteract; bool ok; ok = dd.view(pos); ok &= dd.view(vel); ok &= dd.view(enc); ok &= dd.view(pid); ok &= dd.view(amp); ok &= dd.view(lim); // ok &= dd.view(icm); ok &= dd.view(itorque); ok &= dd.view(iopenloop); ok &= dd.view(iimp); ok &= dd.view(posDir); ok &= dd.view(iMode2); ok &= dd.view(iInteract); if (!ok) { printf("Problems acquiring interfaces\n"); return 1; } pos->getAxes(&jnts); printf("Working with %d axes\n", jnts); double *tmp = new double[jnts]; printf("Device active...\n"); while (dd.isValid()) { std::string s; s.resize(1024); printf("-> "); char c = 0; int i = 0; while (c != '\n') { c = (char)fgetc(stdin); s[i++] = c; } s[i-1] = s[i] = 0; Bottle p; Bottle response; bool ok=false; bool rec=false; p.fromString(s.c_str()); printf("Bottle: %s\n", p.toString().c_str()); switch(p.get(0).asVocab()) { case VOCAB_HELP: printf("\n\n"); printf("Available commands:\n"); printf("-------------------\n\n"); printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str()); printf(" [set] [%s] <int> <float>\n", Vocab::decode(VOCAB_OUTPUT).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_OUTPUT).c_str()); printf(" [get] [%s]\n\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str()); printf(" [set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n", Vocab::decode(VOCAB_CM_POSITION).c_str(), Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(), Vocab::decode(VOCAB_CM_VELOCITY).c_str(), Vocab::decode(VOCAB_CM_MIXED).c_str(), Vocab::decode(VOCAB_CM_TORQUE).c_str(), Vocab::decode(VOCAB_CM_OPENLOOP).c_str(), Vocab::decode(VOCAB_CM_IDLE).c_str(), Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(), Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(), Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str()); printf(" [get] [%s] <int>\n\n", Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str()); printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str()); printf(" [get] [%s] <int> to read the measured torque for a single axis\n", Vocab::decode(VOCAB_TRQ).c_str()); printf(" [get] [%s] to read the measured torque for all axes\n", Vocab::decode(VOCAB_TRQS).c_str()); printf(" [set] [%s] <int> <float> to set the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str()); printf(" [set] [%s] <float list> to set the reference torque for all axes\n", Vocab::decode(VOCAB_REFS).c_str()); printf(" [get] [%s] <int> to read the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str()); printf(" [get] [%s] to read the reference torque for all axes\n\n", Vocab::decode(VOCAB_REFS).c_str()); printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str()); printf(" [set] [%s] <int> <float> <float> \n", Vocab::decode(VOCAB_IMP_PARAM).c_str()); printf(" [set] [%s] <int> <float>\n\n", Vocab::decode(VOCAB_IMP_OFFSET).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_IMP_PARAM).c_str()); printf(" [get] [%s] <int>\n\n", Vocab::decode(VOCAB_IMP_OFFSET).c_str()); printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str()); printf(" [set] [%s]|[%s] <int>\n", Vocab::decode(VOCAB_IM_STIFF).c_str(), Vocab::decode(VOCAB_IM_COMPLIANT).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_INTERACTION_MODE).c_str()); printf(" [get] [%s] \n\n", Vocab::decode(VOCAB_INTERACTION_MODES).c_str()); printf("Standard Interfaces:\n"); printf("type [get] and one of the following:\n"); printf(" [%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str()); printf(" [%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf(" [%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str()); printf(" [%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf(" [%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf(" [%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str()); printf(" [%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf(" [%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str()); printf(" [%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str()); printf(" [%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf(" [%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf(" [%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf(" [%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf(" [%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); printf("\n"); printf("type [set] and one of the following:\n"); printf(" [%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str()); printf(" [%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str()); printf(" [%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf(" [%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf(" [%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); printf(" [%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); printf(" [%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf(" [%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf(" [%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str()); printf(" [%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); printf(" [%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf(" [%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf(" [%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str()); printf(" [%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str()); printf(" [%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str()); printf(" [%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("\n"); printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n"); printf(" - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n"); printf("\n"); break; case VOCAB_QUIT: goto ApplicationCleanQuit; break; case VOCAB_ICONTROLMODE: { handleControlModeMsg(iMode2, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_IMPEDANCE: { handleImpedanceMsg(iimp, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_TORQUE: { handleTorqueMsg(itorque, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_INTERFACE_INTERACTION_MODE: { handleInteractionModeMsg(iInteract, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_GET: switch(p.get(1).asVocab()) { case VOCAB_AXES: { int nj = 0; enc->getAxes(&nj); printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj); } break; case VOCAB_ENCODERS: { enc->getEncoders(tmp); printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); pid->getPid(j, &pd); printf("%s: ", Vocab::decode(VOCAB_PID).c_str()); printf("kp %.2f ", pd.kp); printf("kd %.2f ", pd.kd); printf("ki %.2f ", pd.ki); printf("maxi %.2f ", pd.max_int); printf("maxo %.2f ", pd.max_output); printf("off %.2f ", pd.offset); printf("scale %.2f ", pd.scale); printf("\n"); } break; case VOCAB_PIDS: { Pid *p = new Pid[jnts]; ok = pid->getPids(p); Bottle& b = response.addList(); int i; for (i = 0; i < jnts; i++) { Bottle& c = b.addList(); c.addDouble(p[i].kp); c.addDouble(p[i].kd); c.addDouble(p[i].ki); c.addDouble(p[i].max_int); c.addDouble(p[i].max_output); c.addDouble(p[i].offset); c.addDouble(p[i].scale); } printf("%s\n", b.toString().c_str()); delete[] p; } break; case VOCAB_LIMITS: { double min, max; int j = p.get(2).asInt(); lim->getLimits(j, &min, &max); printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str()); printf("limits: (%.2f %.2f)\n", min, max); } break; case VOCAB_ERRS: { pid->getErrors(tmp); printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUTS: { iopenloop->getOutputs(tmp); printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUT: { int j = p.get(2).asInt(); double v; iopenloop->getOutput(j, &v); printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str()); printf("%.2f ", v); printf("\n"); } break; case VOCAB_REFERENCE: { double ref_pos; int j = p.get(2).asInt(); pid->getReference(j,&ref_pos); printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str()); printf ("%.2f ", ref_pos); printf (")\n"); } break; case VOCAB_REFERENCES: { pid->getReferences(tmp); printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEEDS: { pos->getRefSpeeds(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEED: { double ref_speed; int j = p.get(2).asInt(); pos->getRefSpeed(j,&ref_speed); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf ("%.2f ", ref_speed); printf (")\n"); } break; case VOCAB_REF_ACCELERATION: { double ref_acc; int j = p.get(2).asInt(); pos->getRefAcceleration(j,&ref_acc); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf ("%.2f ", ref_acc); printf (")\n"); } break; case VOCAB_REF_ACCELERATIONS: { pos->getRefAccelerations(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_AMP_CURRENTS: { amp->getCurrents(tmp); printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; } break; case VOCAB_SET: switch(p.get(1).asVocab()) { case VOCAB_POSITION_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref); pos->positionMove(j, ref); } break; case VOCAB_VELOCITY_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref); vel->velocityMove(j, ref); } break; case VOCAB_REF_SPEED: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref); pos->setRefSpeed(j, ref); } break; case VOCAB_REF_ACCELERATION: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref); pos->setRefAcceleration(j, ref); } break; case VOCAB_POSITION_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); pos->positionMove(tmp); } break; case VOCAB_VELOCITY_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); vel->velocityMove(tmp); } break; case VOCAB_REF_SPEEDS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); pos->setRefSpeeds(tmp); } break; case VOCAB_REF_ACCELERATIONS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); pos->setRefAccelerations(tmp); } break; case VOCAB_STOP: { int j = p.get(2).asInt(); printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j); pos->stop(j); } break; case VOCAB_STOPS: { printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); pos->stop(); } break; case VOCAB_ENCODER: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref); enc->setEncoder(j, ref); } break; case VOCAB_ENCODERS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str()); enc->setEncoders(tmp); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); Bottle *l = p.get(3).asList(); if (l==0) { printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n"); } else { int elems=l->size(); if (elems>=3) { pd.kp = l->get(0).asDouble(); pd.kd = l->get(1).asDouble(); pd.ki = l->get(2).asDouble(); if (elems>=7) { pd.max_int = l->get(3).asDouble(); pd.max_output = l->get(4).asDouble(); pd.offset = l->get(5).asDouble(); pd.scale = l->get(6).asDouble(); } printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j); pid->setPid(j, pd); } else { printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n"); } } } break; case VOCAB_DISABLE: { int j = p.get(2).asInt(); printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j); pid->disablePid(j); amp->disableAmp(j); } break; case VOCAB_ENABLE: { int j = p.get(2).asInt(); printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j); amp->enableAmp(j); pid->enablePid(j); } break; case VOCAB_LIMITS: { int j = p.get(2).asInt(); printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j); Bottle *l = p.get(3).asList(); lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble()); } break; case VOCAB_OUTPUT: { int j=p.get(2).asInt(); double v=p.get(3).asDouble(); iopenloop->setRefOutput(j,v); printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v); } break; } break; } /* switch get(0) */ } /* while () */ ApplicationCleanQuit: dd.close(); delete[] tmp; Network::fini(); return 0; }
int main(int argc, char *argv[]) { // just list the devices if no argument given if (argc <= 2) { printf("You can call %s like this:\n", argv[0]); printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]); printf("For example:\n"); printf(" %s --robot icub --part any --remote /controlboard\n", argv[0]); printf("Here are devices listed for your system:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } // get command line options Property options; options.fromCommand(argc, argv); if (!options.check("robot") || !options.check("part")) { printf("Missing either --robot or --part options\n"); return 0; } Network yarp; Time::turboBoost(); char name[1024]; Value& v = options.find("robot"); Value& part = options.find("part"); Value *val; if (!options.check("device", val)) { options.put("device", "remote_controlboard"); } if (!options.check("local", val)) { sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str()); options.put("local", name); } if (!options.check("remote", val)) { sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str()); options.put("remote", name); } fprintf(stderr, "%s", options.toString().c_str()); // create a device PolyDriver dd(options); if (!dd.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 1; } IPositionControl *pos; IVelocityControl *vel; IEncoders *enc; IPidControl *pid; IAmplifierControl *amp; IControlLimits *lim; bool ok; ok = dd.view(pos); ok &= dd.view(vel); ok &= dd.view(enc); ok &= dd.view(pid); ok &= dd.view(amp); ok &= dd.view(lim); if (!ok) { printf("Problems acquiring interfaces\n"); return 1; } int jnts = 0; pos->getAxes(&jnts); printf("Working with %d axes\n", jnts); double *tmp = new double[jnts]; assert (tmp != NULL); printf("Device active...\n"); while (dd.isValid()) { char s[1024]; printf("-> "); char c = 0; int i = 0; while (c != '\n') { c = (char)fgetc(stdin); s[i++] = c; } s[i-1] = s[i] = 0; Bottle p; p.fromString(s); printf("Bottle: %s\n", p.toString().c_str()); switch(p.get(0).asVocab()) { case VOCAB_HELP: printf("\n\n"); printf("Available commands:\n\n"); printf("type [get] and one of the following:\n"); printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str()); printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str()); printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str()); printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); printf("\n"); printf("type [set] and one of the following:\n"); printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str()); printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str()); printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str()); printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str()); printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str()); printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str()); printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("\n"); break; case VOCAB_QUIT: goto ApplicationCleanQuit; break; case VOCAB_GET: switch(p.get(1).asVocab()) { case VOCAB_AXES: { int nj = 0; enc->getAxes(&nj); printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj); } break; case VOCAB_ENCODERS: { enc->getEncoders(tmp); printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); pid->getPid(j, &pd); printf("%s: ", Vocab::decode(VOCAB_PID).c_str()); printf("kp %.2f ", pd.kp); printf("kd %.2f ", pd.kd); printf("ki %.2f ", pd.ki); printf("maxi %.2f ", pd.max_int); printf("maxo %.2f ", pd.max_output); printf("off %.2f ", pd.offset); printf("scale %.2f ", pd.scale); printf("\n"); } break; case VOCAB_LIMITS: { double min, max; int j = p.get(2).asInt(); lim->getLimits(j, &min, &max); printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str()); printf("limits: (%.2f %.2f)\n", min, max); } break; case VOCAB_ERRS: { pid->getErrorLimits(tmp); printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUTS: { pid->getErrors(tmp); printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REFERENCES: { pid->getReferences(tmp); printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEEDS: { pos->getRefSpeeds(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_ACCELERATIONS: { pos->getRefAccelerations(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_AMP_CURRENTS: { amp->getCurrents(tmp); printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; } break; case VOCAB_SET: switch(p.get(1).asVocab()) { case VOCAB_POSITION_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref); pos->positionMove(j, ref); } break; case VOCAB_VELOCITY_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref); vel->velocityMove(j, ref); } break; case VOCAB_REF_SPEED: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref); pos->setRefSpeed(j, ref); } break; case VOCAB_REF_ACCELERATION: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref); pos->setRefAcceleration(j, ref); } break; case VOCAB_POSITION_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); pos->positionMove(tmp); } break; case VOCAB_VELOCITY_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); vel->velocityMove(tmp); } break; case VOCAB_REF_SPEEDS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); pos->setRefSpeeds(tmp); } break; case VOCAB_REF_ACCELERATIONS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); pos->setRefAccelerations(tmp); } break; case VOCAB_STOP: { int j = p.get(2).asInt(); printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str()); pos->stop(j); } break; case VOCAB_STOPS: { printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str()); pos->stop(); } break; case VOCAB_ENCODER: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref); enc->setEncoder(j, ref); } break; case VOCAB_ENCODERS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str()); enc->setEncoders(tmp); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); Bottle *l = p.get(3).asList(); pd.kp = l->get(0).asDouble(); pd.kd = l->get(1).asDouble(); pd.ki = l->get(2).asDouble(); pd.max_int = l->get(3).asDouble(); pd.max_output = l->get(4).asDouble(); pd.offset = l->get(5).asDouble(); pd.scale = l->get(6).asDouble(); printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j); pid->setPid(j, pd); } break; case VOCAB_DISABLE: { int j = p.get(2).asInt(); printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j); pid->disablePid(j); amp->disableAmp(j); } break; case VOCAB_ENABLE: { int j = p.get(2).asInt(); printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j); amp->enableAmp(j); pid->enablePid(j); } break; case VOCAB_LIMITS: { int j = p.get(2).asInt(); printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j); Bottle *l = p.get(3).asList(); lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble()); } break; } break; } /* switch get(0) */ } /* while () */ ApplicationCleanQuit: dd.close(); delete[] tmp; return 0; }
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; }