bool partMover::sequence_iterator(partMover* currP)
{
  //fprintf(stderr, "calling sequence iterator \n");
  IPositionControl *ipos = currP->pos;
  IEncoders *iiencs = currP->iencs;
  IAmplifierControl *iamp = currP->amp;
  IPidControl *ipid = currP->pid;
  int *SEQUENCE_TMP = currP->SEQUENCE;
  double *TIMING_TMP = currP->TIMING;
  double **STORED_POS_TMP = currP->STORED_POS;
  double **STORED_VEL_TMP = currP->STORED_VEL;
  int *INV_SEQUENCE_TMP = currP->INV_SEQUENCE;
  GtkWidget **sliderAry = currP->sliderArray;
  GtkWidget **sliderVelAry = currP->sliderVelArray;
  GtkWidget *tree_view = currP->treeview;
  guint32* timeout_seqeunce_rate_tmp = currP->timeout_seqeunce_rate;
  guint* timeout_seqeunce_id_tmp = currP->timeout_seqeunce_id;
  int *SEQUENCE_ITERATOR_TMP = currP->SEQUENCE_ITERATOR;

  int j = (*SEQUENCE_ITERATOR_TMP);
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (INV_SEQUENCE_TMP[j]!=-1)
    {
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[j]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[j]]);
      for (int k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[j]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[j]][k]);
	}
      (*SEQUENCE_ITERATOR_TMP)++;
      *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000);
      gtk_timeout_remove(*timeout_seqeunce_id_tmp);
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator, currP);
    }
  else
    {
      //restart the sequence if finished
      *SEQUENCE_ITERATOR_TMP = 0;
      j = 0;
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[j]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[j]]);
      for (int k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[j]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[j]][k]);
	}
      (*SEQUENCE_ITERATOR_TMP)++;
      *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000);
      gtk_timeout_remove(*timeout_seqeunce_id_tmp);
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator, currP);
    }

  return false;
}
void partMover::go_click(GtkButton *button, partMover *currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;

  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
		
  //get the current row index
  int i = get_index_selection(currentPart);
  if (i != -1)
    {
      if (TIMING_TMP[i]>0)
	{
	  ipos->setRefSpeeds(STORED_VEL_TMP[i]);
	  ipos->positionMove(STORED_POS_TMP[i]);
	  for (int k =0; k < NUMBER_OF_JOINTS; k++)
	    {
	      gtk_range_set_value ((GtkRange *) (sliderAry[k]),  STORED_POS_TMP[i][k]);
	      gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[i][k]);
	    }
	}
    }
		
  return;
}
void partMover::sequence_click(GtkButton *button, partMover* currentPart)
{

  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;

  int j;
	
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
	
  int invSequence[NUMBER_OF_STORED];

  for (j = 0; j < NUMBER_OF_STORED; j++)
    invSequence[j] = -1;

  for (j = 0; j < NUMBER_OF_STORED; j++)
    {
      if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED))
	invSequence[SEQUENCE_TMP[j]] = j;
    }
  for (j = 0; j < NUMBER_OF_STORED; j++)
    if (invSequence[j]!=-1)
      {
	if (TIMING_TMP[invSequence[j]] > 0)
	  {
	    ipos->setRefSpeeds(STORED_VEL_TMP[invSequence[j]]);
	    ipos->positionMove(STORED_POS_TMP[invSequence[j]]);
	    for (int k =0; k < NUMBER_OF_JOINTS; k++)
	      {
		gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[invSequence[j]][k]);
		gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[invSequence[j]][k]);
	      }
	    Time::delay(TIMING_TMP[invSequence[j]]);
	  }
      }
    else
      break;
  return;
}
Beispiel #4
0
    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;
    }
Beispiel #5
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 --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;
}
void partMover::sequence_cycle(GtkButton *button,partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  int *INV_SEQUENCE_TMP = currentPart->INV_SEQUENCE;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;
  GtkWidget *tree_view = currentPart->treeview;
  guint32* timeout_seqeunce_rate_tmp = currentPart->timeout_seqeunce_rate;
  guint* timeout_seqeunce_id_tmp = currentPart->timeout_seqeunce_id;
  int *SEQUENCE_ITERATOR_TMP = currentPart->SEQUENCE_ITERATOR;
	
  int j, k;
  *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[0]*1000);
	
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  for (j = 0; j < NUMBER_OF_STORED; j++)
    INV_SEQUENCE_TMP[j] = -1;

  for (j = 0; j < NUMBER_OF_STORED; j++)
    {
      if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED))
	{
	  INV_SEQUENCE_TMP[SEQUENCE_TMP[j]] = j;
	}
    }

  //if possible execute the first movement
  //SEQUENCE_ITERATOR = 0;
  if (INV_SEQUENCE_TMP[0]!=-1 && TIMING_TMP[0] >0 )
    {
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction)sequence_iterator, currentPart);
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[0]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[0]]);
      for (k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[0]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[0]][k]);
	}
      //point the SEQUENCE ITERATOR to the next movement
      *SEQUENCE_ITERATOR_TMP = 1;

      //deactivate all buttons
      for (k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_widget_set_sensitive(sliderVelAry[k], false);
	  gtk_widget_set_sensitive(sliderAry[k], false);
	}

      //fprintf(stderr, "Disabling bottons\n");
      if (currentPart->button1 != NULL)
	gtk_widget_set_sensitive(currentPart->button1, false);
      if (currentPart->button2 != NULL)
	gtk_widget_set_sensitive(currentPart->button2, false);
      if (currentPart->button3 != NULL)
	gtk_widget_set_sensitive(currentPart->button3, false);
      if (currentPart->button4 != NULL)
	gtk_widget_set_sensitive(currentPart->button4, false);
      if (currentPart->button5 != NULL)
	gtk_widget_set_sensitive(currentPart->button5, false);
      if (currentPart->button7 != NULL)
	gtk_widget_set_sensitive(currentPart->button7, false);
      if (currentPart->button8 != NULL)
	gtk_widget_set_sensitive(currentPart->button8, false);

    }
  return;
}
void partMover::fixed_time_move(const double *cmdPositions, double cmdTime, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  ITorqueControl *itrq= currentPart->trq;

  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;
  int NUM_JOINTS;
  ipos->getAxes(&NUM_JOINTS);
  double *cmdVelocities = new double[NUM_JOINTS];
  double *startPositions = new double[NUM_JOINTS];

  while (!iiencs->getEncoders(startPositions))
    Time::delay(0.001);
    //fprintf(stderr, "getEncoders is returning false\n");
    //fprintf(stderr, "Getting the following values for the encoders");
  //for(int k=0; k<NUM_JOINTS; k++)
  //  fprintf(stderr, "%.1f ", startPositions[k]);
  //fprintf(stderr, "\n");  

  int k;
  for(k=0; k<NUM_JOINTS; k++)
    {
      cmdVelocities[k] = 0;
		
      if (fabs(startPositions[k] - cmdPositions[k]) > 0.01)
	cmdVelocities[k] = fabs(startPositions[k] - cmdPositions[k])/cmdTime;
      else
	cmdVelocities[k] = 1.0;
    }

  //fprintf(stderr, "ExplorerThread-> Start pos:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", startPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> Moving arm to:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", cmdPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> with velocity:\n");
  //for(int ii=0; ii < NUM_JOINTS; ii++)
  //  fprintf(stderr, "%.2lf\t", cmdVelocities[ii]);
  //fprintf(stderr, "\n");

  ipos->setRefSpeeds(cmdVelocities);
  ipos->positionMove(cmdPositions);	

  currentPart->sequence_port_stamp.update();
  currentPart->sequence_port.setEnvelope(currentPart->sequence_port_stamp);
  Vector v(NUM_JOINTS,cmdPositions);
  currentPart->sequence_port.write(v);
  delete cmdVelocities;
  delete startPositions;
  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 --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;
}