humotion

iCubDataReceiver::iCubDataReceiver(int period, IEncoders *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){

    iencs->getEncoders(positions.data());

    double timestamp = get_timestamp_ms();

        icub_jointinterface->fetch_position(i, positions[i], timestamp);

        icub_jointinterface->fetch_speed(i, velocities[i], timestamp);

    icub_jointinterface->fetch_position(100, 0.0, timestamp);

#define POSITION_CONTROL 1

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR,   ID_EYES_LEFT_LR));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR,   ID_EYES_RIGHT_LR));

    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){

        commands=1000000;

        emotion_port[0].write();

        emotion_port[port_id].write();

    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){

#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

    double speed = distance * ((double)MAIN_LOOP_FREQUENCY);

    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);

    emotion_port[3].write();

        case(2):

        case(0):

        case(1):

        case(3):

        case(4): {//pan

        case(5): { //vergence

        case(2):

            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);

        case(0):

        case(1):

        case(3):

        case(4): {//pan

        case(5): { //vergence

/*

    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;

*/

    //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);

    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);

    ilimits->getLimits(4, &pan_min, &pan_max);

    ilimits->getLimits(5, &vergence_min, &vergence_max);

//! 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");

        */

}

    humotion::server::Server *humotion_server = new humotion::server::Server("/icub", "RSB", icub_jointinterface);