commit ddccf27917e4df3e18644ff6251a93b1edf56ba5
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Tue Dec 1 14:56:39 2015 +0100

    cleanup

diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
index 2ab06cf..2e87f76 100644
--- a/examples/meka/src/mekajointinterface.cpp
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -204,68 +204,13 @@ points:
 //! NOTE: this should also prefill the min/max positions for this joint
 //! \param the enum id of a joint
 void MekaJointInterface::enable_joint(int e){
-#if 0
-    //FIXME ADD THIS:
-    // enable the amplifier and the pid controller on each joint
-    /*for (i = 0; i < nj; i++) {
-       amp->enableAmp(i);
-       pid->enablePid(i);
-    }*/
-
-
-    //set up smooth motion controller
-    //step1: set up framerate
-    //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
-
-    //step2: set controllertype:
-    //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
-
-    //step3: set pid controller:
-    /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
-        printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
-        dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
-    }*/
-
-    //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
-
-    //check if setting pid controllertype was successfull:
-    /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
-        printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
-        exit(1);
-    }*/
-
-    //fetch min/max:
-   // init_joint(e);
-
-    //ok fine, now enable motor:
-    //printf("> enabling motor %s\n", joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
-#endif
+    //meka does not support this, joints are always enabled
 }
 
 //! shutdown and disable a joint
 //! \param the enum id of a joint
 void MekaJointInterface::disable_joint(int e){
-    /*
-        //first: convert humotion enum to our enum:
-        int motor_id = convert_enum_to_motorid(e);
-        if (motor_id == -1){
-            return; //we are not interested in that data, so we just return here
-        }
-
-        //fetch device:
-        Device *dev = get_device(motor_id);
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        */
+    //meka does not support this, joints are always enabled
 }
 
 void MekaJointInterface::store_min_max(int id, float min, float max){
@@ -295,605 +240,3 @@ void MekaJointInterface::init_joints(){
     store_min_max(ID_LIP_RIGHT_LOWER, -1, 1);
 }
 
-#if 0
-
-//! conversion table for humotion motor ids to our ids:
-//! \param enum from JointInterface::JOINT_ID_ENUM
-//! \return int value of motor id
-int iCubJointInterface::convert_enum_to_motorid(int e){
-    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
-    if(it == enum_id_bimap.right.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
-    }
-    return it->second;
-}
-
-
-//! conversion table for our ids to humotion motor ids:
-//! \param  int value of motor id
-//! \return enum from JointInterface::JOINT_ID_ENUM
-int iCubJointInterface::convert_motorid_to_enum(int id){
-    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
-    if(it == enum_id_bimap.left.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
-    }
-    return it->second;
-}
-
-//! special command to set eyelid angle
-//! \param angle in degrees
-void iCubJointInterface::set_eyelid_angle(double angle){
-    if (emotion_port[0].getOutputCount()>0){
-        //try to set the value based on the upper one
-        //some guesses from the sim: S30 = 0° / S40 = 10°
-        int opening = (25.0 + 0.8*angle);
-        opening = min(48, max(24, opening));
-
-        if (opening == lid_opening_previous){
-            //no update necessary
-            return;
-        }
-
-        lid_angle = angle;
-        lid_opening_previous = opening;
-
-        char buf[20];
-        sprintf(buf, "S%2d", opening);
-
-        //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);
-        Bottle &cmd = emotion_port[0].prepare();
-        cmd.clear();
-        cmd.addString(buf);
-        emotion_port[0].write();
-    }else{
-        printf("> ERROR: no icub emotion output\n");
-        exit(EXIT_FAILURE);
-    }
-}
-
-//! special command to set the eyebrow angle
-//! \param id {0=left, 1=right)
-//! \param angle in degrees
-void iCubJointInterface::set_eyebrow_angle(int id){
-    int port_id;
-    if (id == ICUB_ID_EYES_LEFT_BROW){
-        port_id = 1;
-    }else{
-        port_id = 2;
-    }
-
-    if (emotion_port[port_id].getOutputCount()>0){
-        double angle = target_angle[id];
-        int icub_val = 0;
-
-        //swap rotation direction:
-        if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;
-
-        //convert to icub representation
-        if (angle < -20){
-            icub_val = 1;
-        }else if (angle<10){
-            icub_val = 2;
-        }else if (angle<20){
-            icub_val = 4;
-        }else{
-            icub_val = 8;
-        }
-
-        //make sure to update only on new values:
-        if (icub_val == target_angle_previous[id]){
-                //no updata necessary
-                return;
-        }
-
-        //store actual value:
-        target_angle_previous[id] = icub_val;
-
-
-        string cmd_s;
-        if (id==ICUB_ID_EYES_LEFT_BROW){
-            cmd_s = "L0" + to_string(icub_val);
-        }else{
-            cmd_s = "R0" + to_string(icub_val);
-        }
-
-        printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());
-
-        Bottle &cmd = emotion_port[port_id].prepare();
-        cmd.clear();
-        cmd.addString(cmd_s);
-        emotion_port[port_id].write();
-    }else{
-        printf("> ERROR: no icub emotion output\n");
-        exit(EXIT_FAILURE);
-    }
-}
-
-void iCubJointInterface::run(){
-    iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
-    data_receiver->start();
-}
-
-//! set the target position of a joint
-//! \param enum id of joint
-//! \param float value
-void iCubJointInterface::publish_target_position(int e){
-    //first: convert humotion enum to our enum:
-    int id = convert_enum_to_motorid(e);
-    if (id == -1){
-        return; //we are not interested in that data, so we just return here
-    }
-
-    if (id == ICUB_ID_NECK_PAN){
-        //PAN seems to be swapped
-        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
-    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
-        //icub handles eyes differently, we have to set pan angle + vergence
-        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
-        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
-        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
-
-        store_joint(ICUB_ID_EYES_PAN, pan);
-        store_joint(ICUB_ID_EYES_VERGENCE, vergence);
-    }else{
-        store_joint(id, joint_target[e]);
-    }
-}
-
-
-//! set the target position of a joint
-//! \param id of joint
-//! \param float value of position
-void iCubJointInterface::store_joint(int id, float value){
-    //printf("> set joint %d = %f\n",id,value);
-    target_angle[id] = value;
-    //ipos->positionMove(id, value);
-}
-
-//! execute a move in position mode
-//! \param id of joint
-//! \param angle
-void iCubJointInterface::set_target_in_positionmode(int id, double value){
-    if (id>ICUB_ID_EYES_VERGENCE){
-        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
-        return;
-    }
-#if 0
-    //set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
-
-    //first: calculate necessary speed to reach the given target within the next clock tick:
-    double distance = value - target_angle_previous[id];
-    //make the motion smooth: we want to reach 85% of the target in the next iteration:
-    //calculate speed for that:
-    double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);
-
-    //set up speed for controller:
-    ipos->setRefSpeed(id, speed);
-#endif
-    //execute motion
-    ipos->positionMove(id, value);
-}
-
-//! execute a move in velocity mode
-//! \param id of joint
-//! \param angle
-void iCubJointInterface::set_target_in_velocitymode(int id, double value){
-    //first: calculate necessary speed to reach the given target within the next clock tick:
-    double distance = value - target_angle_previous[id];
-    //make the motion smooth: we want to reach 85% of the target in the next iteration:
-    distance = 0.85 * distance;
-    //calculate speed
-    double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
-    //execute:
-    ivel->velocityMove(id, speed);
-    //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
-
-    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
-}
-
-//! actually execute the scheduled motion commands
-void iCubJointInterface::execute_motion(){
-
-    // set up neck and eye motion commands:
-    if (POSITION_CONTROL){
-        //position control
-        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
-            set_target_in_positionmode(i, target_angle[i]);
-        }
-    }else{
-        //velocity control
-        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
-            set_target_in_velocitymode(i, target_angle[i]);
-        }
-    }
-    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
-
-
-    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
-    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
-
-    //eyebrows are set using a special command as well:
-    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
-    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
-
-    //mouth
-    set_mouth();
-
-
-}
-
-void iCubJointInterface::set_mouth(){
-    //convert from 6DOF mouth displacement to icub leds:
-    int led_value = 0;
-
-    //fetch center opening:
-    double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];
-    bool mouth_open = (center_opening>15.0)?true:false;
-
-    //side of mouth high or low?
-    double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0;
-    double left_avg   = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0;
-    double right_avg  = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0;
-
-    //happy, neutral or sad?
-    double diff_l = center_avg - left_avg;
-    double diff_r = center_avg - right_avg;
-    double diff   = (diff_l+diff_r)/2.0;
-
-    if (diff > 2.0){
-        if (mouth_open){
-            led_value = 0x14;
-        }else{
-            if (diff > 2.6){
-                led_value = 0x0A;
-            }else{
-                led_value = 0x0B;
-            }
-        }
-    }else if (diff < -3.0){
-        if (mouth_open){
-            led_value = 0x06;
-        }else{
-            led_value = 0x18;
-        }
-    }else if (diff < -2.0){
-        if (mouth_open){
-            led_value = 0x04; //0x25;
-        }else{
-            led_value = 0x08;
-        }
-    }else{
-        if (mouth_open){
-            led_value = 0x16;
-        }else{
-            led_value = 0x08;
-        }
-    }
-
-
-    if (led_value == previous_mouth_state){
-        //no update necessary
-        return;
-    }
-
-    previous_mouth_state = led_value;
-
-    //convert to string:
-    char buf[10];
-    sprintf(buf, "M%02X",led_value);
-
-    /*printf("> sending mouth '%s'\n",buf);
-    printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);
-    printf("  mouth         %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);
-    printf("  mouth  open=%3.2f diff=%3.2f\n", center_opening, diff);*/
-
-    //add mouth:
-    Bottle &cmd = emotion_port[3].prepare();
-    cmd.clear();
-    cmd.addString(buf);
-    emotion_port[3].write();
-
-
-    //store joint values which we do not handle on icub here:
-    double timestamp = get_timestamp_ms();
-    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);
-    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);
-    JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);
-    JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);
-    JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);
-    JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);
-
-}
-
-double iCubJointInterface::get_timestamp_ms(){
-    struct timespec spec;
-    clock_gettime(CLOCK_REALTIME, &spec);
-    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;
-}
-
-//! set the current position of a joint
-//! \param id of joint
-//! \param float value of position
-//! \param double timestamp
-void iCubJointInterface::fetch_position(int id, double value, double timestamp){
-    //store joint based on id:
-    switch(id){
-        default:
-            printf("> ERROR: unhandled joint id %d\n",id);
-            return;
-
-        case(100):
-            JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
-            break;
-
-        case(2):
-            //PAN is inverted!
-            JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
-            break;
-
-        case(0):
-            JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);
-            break;
-
-        case(1):
-            JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);
-            break;
-
-        case(3):
-            JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);
-            break;
-
-        //icub handles eyes differently, we have to set pan angle + vergence
-        case(4): {//pan
-            last_pos_eye_pan = value;
-            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
-            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
-
-            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
-            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
-            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
-            break;
-        }
-
-        case(5): { //vergence
-            last_pos_eye_vergence = value;
-            float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
-            float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
-
-            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
-            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
-            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
-            break;
-        }
-    }
-
-
-}
-
-//! set the current speed of a joint
-//! \param enum id of joint
-//! \param float value of speed
-//! \param double timestamp
-void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
-
-    switch(id){
-        default:
-            printf("> ERROR: unhandled joint id %d\n",id);
-            return;
-
-        case(2):
-            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
-            break;
-
-        case(0):
-            JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
-            break;
-
-        case(1):
-            JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
-            break;
-
-        case(3):
-            JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
-            break;
-
-        //icub handles eyes differently, we have to set pan angle + vergence
-        case(4): {//pan
-            last_vel_eye_pan = value;
-            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
-            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
-
-            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
-            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
-            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
-            break;
-        }
-
-        case(5): { //vergence
-            last_vel_eye_pan = value;
-            float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
-            float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
-
-            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
-            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
-            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
-            break;
-        }
-    }
-/*
-    JointInterface::store_incoming_speed(ID_LIP_LEFT_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_LEFT_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_CENTER_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_CENTER_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_RIGHT_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_RIGHT_LOWER, 0.0, timestamp);
-
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_BROW, 0.0, timestamp);
-
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_UPPER,0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/
-    /*
-    //fetch enum id:
-    int e = convert_motorid_to_enum(device->get_device_id());
-    if (e == -1){
-        return; //we are not interested in that data, so we just return here
-    }
-
-    JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/
-
-}
-/*
-//! conversion table for humotion motor ids to our ids:
-//! \param enum from JointInterface::JOINT_ID_ENUM
-//! \return int value of motor id
-int HumotionJointInterface::convert_enum_to_motorid(int e){
-    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
-    if(it == enum_id_bimap.right.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
-    }
-
-    return it->second;
-}
-
-
-//! conversion table for our ids to humotion motor ids:
-//! \param  int value of motor id
-//! \return enum from JointInterface::JOINT_ID_ENUM
-int HumotionJointInterface::convert_motorid_to_enum(int id){
-    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
-    if(it == enum_id_bimap.left.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
-    }
-
-    return it->second;
-}
-*/
-
-//! prepare and enable a joint
-//! NOTE: this should also prefill the min/max positions for this joint
-//! \param the enum id of a joint
-void iCubJointInterface::enable_joint(int e){
-    //FIXME ADD THIS:
-    // enable the amplifier and the pid controller on each joint
-    /*for (i = 0; i < nj; i++) {
-       amp->enableAmp(i);
-       pid->enablePid(i);
-    }*/
-
-
-    //set up smooth motion controller
-    //step1: set up framerate
-    //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
-
-    //step2: set controllertype:
-    //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
-
-    //step3: set pid controller:
-    /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
-        printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
-        dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
-    }*/
-
-    //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
-
-    //check if setting pid controllertype was successfull:
-    /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
-        printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
-        exit(1);
-    }*/
-
-    //fetch min/max:
-   // init_joint(e);
-
-    //ok fine, now enable motor:
-    //printf("> enabling motor %s\n", joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
-
-}
-
-void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
-    double min, max;
-    ilimits->getLimits(id, &min, &max);
-    joint_min[e] = min;
-    joint_max[e] = max;
-}
-
-//! initialise a joint (set up controller mode etc)
-//! \param joint enum
-void iCubJointInterface::init_joints(){
-    store_min_max(ilimits, 0, ID_NECK_TILT);
-    store_min_max(ilimits, 1, ID_NECK_ROLL);
-    store_min_max(ilimits, 2, ID_NECK_PAN);
-    store_min_max(ilimits, 3, ID_EYES_BOTH_UD);
-
-    //icub handles eyes differently, we have to set pan angle + vergence
-    double pan_min, pan_max, vergence_min, vergence_max;
-    ilimits->getLimits(4, &pan_min, &pan_max);
-    ilimits->getLimits(5, &vergence_min, &vergence_max);
-
-    //this is not 100% correct, should be fixed:
-    joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
-    joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;
-    joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR];
-    joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR];
-
-    //eyelids:
-    joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30;
-    joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30;
-    lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];
-
-    //eyebrows:
-    joint_min[ID_EYES_LEFT_BROW] = -50;
-    joint_max[ID_EYES_LEFT_BROW] = 50;
-    joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW];
-    joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW];
-
-    //mouth:
-    joint_min[ID_LIP_CENTER_UPPER] = 5;
-    joint_max[ID_LIP_CENTER_UPPER] = 50;
-    joint_min[ID_LIP_CENTER_LOWER] = 5;
-    joint_max[ID_LIP_CENTER_LOWER] = 50;
-    joint_min[ID_LIP_LEFT_UPPER] = 5;
-    joint_max[ID_LIP_LEFT_UPPER] = 50;
-    joint_min[ID_LIP_LEFT_LOWER] = 5;
-    joint_max[ID_LIP_LEFT_LOWER] = 50;
-    joint_min[ID_LIP_RIGHT_UPPER] = 5;
-    joint_max[ID_LIP_RIGHT_UPPER] = 50;
-    joint_min[ID_LIP_RIGHT_LOWER] = 5;
-    joint_max[ID_LIP_RIGHT_LOWER] = 50;
-
-
-}
-
-//! shutdown and disable a joint
-//! \param the enum id of a joint
-void iCubJointInterface::disable_joint(int e){
-    /*
-        //first: convert humotion enum to our enum:
-        int motor_id = convert_enum_to_motorid(e);
-        if (motor_id == -1){
-            return; //we are not interested in that data, so we just return here
-        }
-
-        //fetch device:
-        Device *dev = get_device(motor_id);
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        */
-}
-#endif
