extern void eoprot_fun_UPDT_mc_joint_config_velocitysetpointtimeout(const EOnv* nv, const eOropdescriptor_t* rd) { eOmeas_time_t *time_ptr = (eOmeas_time_t*)nv->ram; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eo_emsController_SetVelTimeout(jxx, *time_ptr); }
extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_motor_params_t *params_ptr = (eOmc_motor_params_t*)nv->ram; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eo_emsController_SetMotorParams ((uint8_t)jxx, *params_ptr); }
extern void eoprot_fun_UPDT_as_strain_config_signaloncefullscale(const EOnv* nv, const eOropdescriptor_t* rd) { eObool_t *signaloncefullscale = (eObool_t*)rd->data; eOas_strainId_t strainId = (eOas_strainId_t)eoprot_ID2index(rd->id32); //this should be always 0 s_signalGetFullScales(strainId, *signaloncefullscale); }
extern void eoprot_fun_UPDT_mc_joint_cmmnds_interactionmode(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_interactionmode_t* interaction = (eOmc_interactionmode_t*)rd->data; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eo_emsController_SetInteractionModeGroupJoints(jxx, *interaction); }
extern void eoprot_fun_UPDT_mc_joint_config_limitsofjoint(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eOmeas_position_limits_t *limit_ptr = (eOmeas_position_limits_t*)nv->ram; eo_emsController_SetPosMin(jxx, limit_ptr->min); eo_emsController_SetPosMax(jxx, limit_ptr->max); }
extern void eoprot_fun_UPDT_mc_joint_status(const EOnv* nv, const eOropdescriptor_t* rd) { #if defined(ENABLE_DEBUG_CONTROLMODESTATUS) eOmc_joint_status_t *js = (eOmc_joint_status_t*)rd->data; s_debug_monitor_controlmodestatus(eo_nv_GetBRD(nv), eoprot_ID2index(rd->id32), js->basic.controlmodestatus); #endif feat_manage_motioncontrol_data(nvBoardNum2FeatIdBoardNum(eo_nv_GetBRD(nv)), rd->id32, (void *)rd->data); }
extern void eoprot_fun_UPDT_mc_joint_cmmnds_calibration(const EOnv* nv, const eOropdescriptor_t* rd) { eOresult_t res; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eOmc_calibrator_t *calibrator = (eOmc_calibrator_t*)nv->ram; eo_emsController_StartCalibration_type3(jxx, calibrator->params.type3.position, calibrator->params.type3.velocity, calibrator->params.type3.offset); }
extern void eoprot_fun_UPDT_mc_motor_config_maxcurrentofmotor(const EOnv* nv, const eOropdescriptor_t* rd) { eOmeas_current_t *curr_ptr = (eOmeas_current_t*)nv->ram; eOmc_motorId_t mxx = eoprot_ID2index(rd->id32); eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingMotorControl, EO_INIT(.cmdId) ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT }; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); eo_appCanSP_SendCmd2Motor(appCanSP_ptr, (eOmc_motorId_t)mxx, msgCmd, (void*)curr_ptr); }
extern void eoprot_fun_UPDT_mc_joint_config_pidposition(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_PID_t *pid_ptr = (eOmc_PID_t*)nv->ram; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); float rescaler = 1.0f/(float)(1<<pid_ptr->scale); eo_emsController_SetPosPid(jxx, pid_ptr->kp*rescaler, pid_ptr->kd*rescaler, pid_ptr->ki*rescaler, pid_ptr->limitonintegral, pid_ptr->limitonoutput, pid_ptr->offset, pid_ptr->stiction_up_val*rescaler, pid_ptr->stiction_down_val*rescaler); }
extern void eoprot_fun_UPDT_as_strain_config_datarate(const EOnv* nv, const eOropdescriptor_t* rd) { eOas_strainId_t strainId = (eOas_strainId_t)eoprot_ID2index(rd->id32); //this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) ICUBCANPROTO_POL_AS_CMD__SET_CANDATARATE }; uint8_t *straindatarate = (uint8_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); eo_appCanSP_SendCmd2SnrStrain(appCanSP_ptr, strainId, msgCmd, (void*)straindatarate); }
extern void eoprot_fun_UPDT_mc_joint_config_impedance(const EOnv* nv, const eOropdescriptor_t* rd) { // eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); //#if defined(VERIFY_ROP_SETIMPEDANCE) // // if(jxx < MAXJ) // { // rxtools_tx_inrop_t txinrop = { 0xffffffff, 0xffffffff}; // txinrop.txtime = (EOK_uint64dummy == time) ? (0xffffffff) : (time / 1000); // txinrop.txprog = sign; // rxtools_results_t results = {0, 0, 0}; // //reset impedence set point info // eo_dgn_rxchecksepoints.impedence[jxx].deltaprognumber = INT32_MAX; // eo_dgn_rxchecksepoints.impedence[jxx].deltarxtime = UINT32_MAX; // // int32_t ret = rxtools_verify_reception(&status_rop_impedance[jxx], &txinrop, maxtimegap_impedance, &results); // // if(-1 == ret) // { // error: eval retflags // if(rxtools_flag_error_prognum == (rxtools_flag_error_prognum & results.flags)) // { // // to do: an error in rop sequence number. there is a gap of results.deltaprognumber // // eo_dgn_rxchecksepoints.impedence[jxx].deltaprognumber = results.deltaprognumber; // } // if(rxtools_flag_error_rxtime == (rxtools_flag_error_rxtime & results.flags)) // { // // to do: an error in timing: there was more than maxtimegap and it was results.deltarxtime // eo_dgn_rxchecksepoints.impedence[jxx].deltarxtime = results.deltarxtime; // } // // eo_theEMSdgn_Signalerror(eo_theEMSdgn_GetHandle(), eodgn_nvidbdoor_rxcheckSetpoints, 1000); // eo_theEMSdgn_resetSetpoints(eo_theEMSdgn_GetHandle()); // } // } // //#endif eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eOmc_impedance_t *cfg = (eOmc_impedance_t*)nv->ram; eo_emsController_SetImpedance(jxx, cfg->stiffness, cfg->damping, cfg->offset); }
void boardtransceiver_fun_UPDT_mc_joint_cmmnds_interactionmode(const EOnv* nv, const eOropdescriptor_t* rd) { EOnv_hid aNV = {0}; eOnvBRD_t brd = eo_nv_GetBRD(nv); EOnvSet* mynvset = arrayofnvsets[brd]; eOprotIndex_t index = eoprot_ID2index(rd->id32); eOenum08_t* pmode = (eOenum08_t*) rd->data; eOnvID32_t id32status = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, eoprot_tag_mc_joint_status); eo_nvset_NV_Get(mynvset, eok_ipv4addr_localhost, id32status, &aNV); eOmc_joint_status_t jointstatus = {0}; uint16_t size = 0; eOresult_t res = eo_nv_Get(&aNV, eo_nv_strg_volatile, &jointstatus, &size); jointstatus.interactionmodestatus = *pmode; eo_nv_Set(&aNV, &jointstatus, eobool_true, eo_nv_upd_dontdo); }
extern void eoprot_fun_UPDT_mc_joint_config_motionmonitormode(const EOnv* nv, const eOropdescriptor_t* rd) { /*NOTE: this function is equal to mc4 fucntion.*/ // eOresult_t res; eOmc_joint_status_t *jstatus = NULL; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); jstatus = eo_protocolwrapper_GetJointStatus(eo_protocolwrapper_GetHandle(), (eOmc_jointId_t)jxx); if(NULL == jstatus) { return; //error } //#warning marco.accame: better using cast to eOmc_motionmonitormode_t if(eomc_motionmonitormode_dontmonitor == *((eOenum08_t*)nv->ram)) { jstatus->basic.motionmonitorstatus = (eOenum08_t)eomc_motionmonitorstatus_notmonitored; } else { jstatus->basic.motionmonitorstatus = eomc_motionmonitorstatus_setpointnotreachedyet; } }
extern void eoprot_fun_UPDT_as_mais_config_resolution(const EOnv* nv, const eOropdescriptor_t* rd) { eOas_mais_status_t *status = NULL; eOas_maisId_t maisId = (eOas_maisId_t)eoprot_ID2index(rd->id32); //this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) ICUBCANPROTO_POL_AS_CMD__SET_RESOLUTION }; eOas_maisresolution_t *maisresolution = (eOas_maisresolution_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); eo_appCanSP_SendCmd2SnrMais(appCanSP_ptr, maisId, msgCmd, (void*)maisresolution); status = eo_protocolwrapper_GetMaisStatus(eo_protocolwrapper_GetHandle(), maisId); if(NULL == status) { return; //error } s_process_mais_resolution(*maisresolution, status); }
// motor-update extern void eoprot_fun_UPDT_mc_motor_config(const EOnv* nv, const eOropdescriptor_t* rd) { eOresult_t res; eOmc_motorId_t mxx = eoprot_ID2index(rd->id32); eObrd_cantype_t boardType; icubCanProto_velocity_t vel_icubCanProtValue; eOmc_motor_config_t *cfg_ptr = (eOmc_motor_config_t*)nv->ram; eOappTheDB_jointOrMotorCanLocation_t canLoc; eOicubCanProto_msgDestination_t msgdest; eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingMotorControl, EO_INIT(.cmdId) 0 }; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); //Since icub can proto uses encoder ticks like position unit, i need of the converter: from icub to encoder EOappMeasConv* appMeasConv_ptr = eo_emsapplBody_GetMeasuresConverterHandle(eo_emsapplBody_GetHandle()); res = eo_appTheDB_GetMotorCanLocation(eo_appTheDB_GetHandle(), mxx, &canLoc, &boardType); if(eores_OK != res) { return; } //set destination of all messages msgdest.dest = ICUBCANPROTO_MSGDEST_CREATE(canLoc.indexinsidecanboard, canLoc.addr); if(eobrd_cantype_1foc == boardType) { // 1) send current pid msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&cfg_ptr->pidcurrent); // 2) send current pid limits msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PIDLIMITS; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&cfg_ptr->pidcurrent); } // 2) set max velocity vel_icubCanProtValue = eo_appMeasConv_jntVelocity_I2E(appMeasConv_ptr, mxx, cfg_ptr->maxvelocityofmotor); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&vel_icubCanProtValue); // 3) set current limit msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&cfg_ptr->maxcurrentofmotor); } extern void eoprot_fun_UPDT_mc_motor_config_pidcurrent(const EOnv* nv, const eOropdescriptor_t* rd) { eOresult_t res; eOmc_motorId_t mxx = eoprot_ID2index(rd->id32); eOmc_PID_t *pid_ptr = (eOmc_PID_t*)nv->ram; eOappTheDB_jointOrMotorCanLocation_t canLoc; eOicubCanProto_msgDestination_t msgdest; eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingMotorControl, EO_INIT(.cmdId) 0 }; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); res = eo_appTheDB_GetMotorCanLocation(eo_appTheDB_GetHandle(), mxx, &canLoc, NULL); if(eores_OK != res) { return; } //set destination of all messages msgdest.dest = ICUBCANPROTO_MSGDEST_CREATE(canLoc.indexinsidecanboard, canLoc.addr); // send current pid msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)pid_ptr); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PIDLIMITS; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)pid_ptr); } extern void eoprot_fun_UPDT_mc_motor_config_maxvelocityofmotor(const EOnv* nv, const eOropdescriptor_t* rd) { eOmeas_velocity_t *vel_ptr = (eOmeas_velocity_t*)nv->ram; eOmc_motorId_t mxx = eoprot_ID2index(rd->id32); icubCanProto_velocity_t vel_icubCanProtValue; eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingMotorControl, EO_INIT(.cmdId) ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY }; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); /*Since icub can proto uses encoder tacks like position unit, i need of the converter: from icub to encoder*/ EOappMeasConv* appMeasConv_ptr = eo_emsapplBody_GetMeasuresConverterHandle(eo_emsapplBody_GetHandle()); vel_icubCanProtValue = eo_appMeasConv_jntVelocity_I2E(appMeasConv_ptr, mxx, *vel_ptr); eo_appCanSP_SendCmd2Motor(appCanSP_ptr, (eOmc_motorId_t)mxx, msgCmd, (void*)&vel_icubCanProtValue); }
// __ALE__ extern void eoprot_fun_UPDT_mc_joint_inputs_externallymeasuredtorque(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eo_emsController_ReadTorque(jxx, *(eOmeas_torque_t*)nv->ram); }
extern void eoprot_fun_UPDT_as_mais_config(const EOnv* nv, const eOropdescriptor_t* rd) { eOas_mais_status_t *status = NULL; eOsmStatesEMSappl_t currentstate; eOicubCanProto_msgDestination_t msgdest; eOappTheDB_board_canlocation_t canLoc; eOas_maisId_t maisId = (eOas_maisId_t)eoprot_ID2index(rd->id32); // this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) 0 }; eOas_mais_config_t *maiscfg = (eOas_mais_config_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); eo_appTheDB_GetSnsrMaisCanLocation(eo_appTheDB_GetHandle(), maisId, &canLoc); //if pc104 tell me to enable maistx, before to send cmd verify if i'm in RUN state: // if yes ==> ok no problem // if no ==> i'll send cmd when go to RUN state if(eoas_maismode_txdatacontinuously == maiscfg->mode) { //only if the appl is in RUN state enable mais tx eom_emsappl_GetCurrentState(eom_emsappl_GetHandle(), ¤tstate); if(eo_sm_emsappl_STrun == currentstate) { msgdest.dest = ICUBCANPROTO_MSGDEST_CREATE(0, canLoc.addr); msgCmd.cmdId = ICUBCANPROTO_POL_AS_CMD__SET_TXMODE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&(maiscfg->mode)); } } msgCmd.cmdId = ICUBCANPROTO_POL_AS_CMD__SET_CANDATARATE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&(maiscfg->datarate)); msgCmd.cmdId = ICUBCANPROTO_POL_AS_CMD__SET_RESOLUTION; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&(maiscfg->resolution)); status = eo_protocolwrapper_GetMaisStatus(eo_protocolwrapper_GetHandle(), maisId); if(NULL == status) { return; //error } s_process_mais_resolution((eOas_maisresolution_t)maiscfg->resolution, status); } extern void eoprot_fun_UPDT_as_mais_config_mode(const EOnv* nv, const eOropdescriptor_t* rd) { eOsmStatesEMSappl_t currentstate; eOas_maisId_t maisId = (eOas_maisId_t)eoprot_ID2index(rd->id32); //this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) ICUBCANPROTO_POL_AS_CMD__SET_TXMODE }; eOas_maismode_t *maismode = (eOas_maismode_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); //if pc104 tell me to enable maistx, before to send cmd verify if i'm in RUN state: // if yes ==> ok no problem // if no ==> i'll send cmd when go to RUN state if(eoas_maismode_txdatacontinuously == *maismode) { //only if the appl is in RUN state enable mais tx eom_emsappl_GetCurrentState(eom_emsappl_GetHandle(), ¤tstate); if(eo_sm_emsappl_STrun == currentstate) { eo_appCanSP_SendCmd2SnrMais(appCanSP_ptr, maisId, msgCmd, (void*)maismode); } } else { eo_appCanSP_SendCmd2SnrMais(appCanSP_ptr, maisId, msgCmd, (void*)maismode); } }
extern void eoprot_fun_UPDT_mc_joint_cmmnds_controlmode(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_controlmode_command_t *controlmode_ptr = (eOmc_controlmode_command_t*)nv->ram; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); /* eOresult_t res; eOappTheDB_jointOrMotorCanLocation_t canLoc; eOmc_controlmode_command_t controlmode_2foc = eomc_controlmode_cmd_openloop; //eomc_controlmode_cmd_current; eOicubCanProto_msgDestination_t msgdest; eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) eo_icubCanProto_msgCmdClass_pollingMotorBoard, EO_INIT(.cmdId) 0 }; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); // 1) send control mode value to 2foc res = eo_appTheDB_GetJointCanLocation(eo_appTheDB_GetHandle(), jxx, &canLoc, NULL); if(eores_OK != res) { return; } //set destination of all messages msgdest.dest = ICUBCANPROTO_MSGDEST_CREATE(canLoc.indexinboard, canLoc.addr); switch(*controlmode_ptr) { case eomc_controlmode_cmd_switch_everything_off: { msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__DISABLE_PWM_PAD; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, NULL); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__CONTROLLER_IDLE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, NULL); }break; default: //2foc control mode is always current { if (eo_emsController_GetControlMode(jxx) == eomc_controlmode_idle) { eOmc_PID_t pid_open_loop; pid_open_loop.kp = 0x0A00; pid_open_loop.kd = 0; pid_open_loop.ki = 0; msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, &pid_open_loop); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, &controlmode_2foc); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__ENABLE_PWM_PAD; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, NULL); msgCmd.cmdId = ICUBCANPROTO_POL_MC_CMD__CONTROLLER_RUN; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, NULL); } } } */ // 2) set control mode to ems controller eo_emsController_SetControlModeGroupJoints(jxx, (eOmc_controlmode_command_t)(*controlmode_ptr)); }
extern void eoprot_fun_UPDT_mc_joint_cmmnds_stoptrajectory(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eo_emsController_Stop(jxx); }
extern void eoprot_fun_UPDT_mc_joint_cmmnds_setpoint(const EOnv* nv, const eOropdescriptor_t* rd) { eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); eOmc_setpoint_t *setPoint = (eOmc_setpoint_t*)nv->ram; eOmc_joint_config_t *jconfig = NULL; eOmc_joint_status_t *jstatus = NULL; //set monitor status = notreachedyet if monitormode is forever jconfig = eo_protocolwrapper_GetJointConfig(eo_protocolwrapper_GetHandle(), (eOmc_jointId_t)jxx); if(NULL == jconfig) { return; //error } //#warning -> marco.accame: cast to proper type if(eomc_motionmonitormode_forever == jconfig->motionmonitormode) { jstatus = eo_protocolwrapper_GetJointStatus(eo_protocolwrapper_GetHandle(), (eOmc_jointId_t)jxx); if(NULL == jstatus) { return; //error } //#warning --> marco.accame: cast to proper type /* if monitorstatus values setpointreached means this is a new set point, so i need to start to check is set point is reached because i'm in monitormode = forever */ if(eomc_motionmonitorstatus_setpointisreached == jstatus->basic.motionmonitorstatus) { jstatus->basic.motionmonitorstatus = eomc_motionmonitorstatus_setpointnotreachedyet; } } switch (setPoint->type) { case eomc_setpoint_position: { // #if defined(EOMTHEEMSAPPLCFG_USE_EB8) || defined(EOMTHEEMSAPPLCFG_USE_EB6) // verify_pair_t pair; // pair.pos = setPoint->to.position.value; // pair.vel = setPoint->to.position.withvelocity; // callback_of_setpoint_all_joints(pair, jxx); // // //callback_of_setpoint(setPoint->to.position.value, jxx); // //callback_of_setpointV2(setPoint->to.position.value, jxx); // #endif // //#if defined (VERIFY_ROP_SETPOINT_EB5) // if(jxx < MAXJ) // { // rxtools_tx_inrop_t txinrop = { 0xffffffff, 0xffffffff}; // txinrop.txtime = (EOK_uint64dummy == time) ? (0xffffffff) : (time / 1000); // txinrop.txprog = sign; // rxtools_results_t results = {0, 0, 0}; // // eo_dgn_rxchecksepoints.position[jxx].deltaprognumber = INT32_MAX; // eo_dgn_rxchecksepoints.position[jxx].deltarxtime = UINT32_MAX; // // int32_t ret = rxtools_verify_reception(&status_rop_setpositionraw[jxx], &txinrop, maxtimegap_setpositionraw, &results); // // // if(-1 == ret) // { // error: eval retflags // if(rxtools_flag_error_prognum == (rxtools_flag_error_prognum & results.flags)) // { // // to do: an error in rop sequence number. there is a gap of results.deltaprognumber // // eo_dgn_rxchecksepoints.position[jxx].deltaprognumber = results.deltaprognumber; // } // if(rxtools_flag_error_rxtime == (rxtools_flag_error_rxtime & results.flags)) // { // // to do: an error in timing: there was more than maxtimegap and it was results.deltarxtime // eo_dgn_rxchecksepoints.position[jxx].deltarxtime = results.deltarxtime; // } // eo_theEMSdgn_Signalerror(eo_theEMSdgn_GetHandle(), eodgn_nvidbdoor_rxcheckSetpoints, 1000); // eo_theEMSdgn_resetSetpoints(eo_theEMSdgn_GetHandle()); // } // } // //#endif eo_emsController_SetPosRef(jxx, setPoint->to.position.value, setPoint->to.position.withvelocity); } break; case eomc_setpoint_positionraw: { //#if (defined(VERIFY_ROP_SETPOSITIONRAW) & (!defined(VERIFY_ROP_SETPOINT_EB5))) // // if(jxx < MAXJ) // { // rxtools_tx_inrop_t txinrop = { 0xffffffff, 0xffffffff}; // txinrop.txtime = (EOK_uint64dummy == time) ? (0xffffffff) : (time / 1000); // txinrop.txprog = sign; // rxtools_results_t results = {0, 0, 0}; // int32_t ret = rxtools_verify_reception(&status_rop_setpositionraw[jxx], &txinrop, maxtimegap_setpositionraw, &results); // // eo_dgn_rxchecksepoints.position[jxx].deltaprognumber = INT32_MAX; // eo_dgn_rxchecksepoints.position[jxx].deltarxtime = UINT32_MAX; // // // if(-1 == ret) // { // error: eval retflags // if(rxtools_flag_error_prognum == (rxtools_flag_error_prognum & results.flags)) // { // // to do: an error in rop sequence number. there is a gap of results.deltaprognumber // // eo_dgn_rxchecksepoints.position[jxx].deltaprognumber = results.deltaprognumber; // } // if(rxtools_flag_error_rxtime == (rxtools_flag_error_rxtime & results.flags)) // { // // to do: an error in timing: there was more than maxtimegap and it was results.deltarxtime // eo_dgn_rxchecksepoints.position[jxx].deltarxtime = results.deltarxtime; // } // eo_theEMSdgn_Signalerror(eo_theEMSdgn_GetHandle(), eodgn_nvidbdoor_rxcheckSetpoints, 1000); // eo_theEMSdgn_resetSetpoints(eo_theEMSdgn_GetHandle()); // } // } // //#endif eo_emsController_SetPosRaw(jxx, setPoint->to.position.value); } break; case eomc_setpoint_velocity: { eo_emsController_SetVelRef(jxx, setPoint->to.velocity.value, setPoint->to.velocity.withacceleration); } break; case eomc_setpoint_torque: { eo_emsController_SetTrqRef(jxx, setPoint->to.torque.value); } break; case eomc_setpoint_current: { eo_emsController_SetOutput(jxx, setPoint->to.current.value); } break; default: { return; } } }
extern void eoprot_fun_UPDT_as_strain_config(const EOnv* nv, const eOropdescriptor_t* rd) { eOicubCanProto_msgDestination_t msgdest; eOappTheDB_board_canlocation_t canLoc; eOsmStatesEMSappl_t currentstate; eOas_strainId_t strainId = (eOas_strainId_t)eoprot_ID2index(rd->id32); //this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) 0 }; eOas_strain_config_t *straincfg = (eOas_strain_config_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); eo_appTheDB_GetSnsrStrainCanLocation(eo_appTheDB_GetHandle(), strainId, &canLoc); msgdest.dest = ICUBCANPROTO_MSGDEST_CREATE(0, canLoc.addr); // only if the appl is in RUN state enable strain tx eom_emsappl_GetCurrentState(eom_emsappl_GetHandle(), ¤tstate); if(eo_sm_emsappl_STrun == currentstate) { msgCmd.cmdId = ICUBCANPROTO_POL_AS_CMD__SET_TXMODE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&(straincfg->mode)); //s_send_diagnostics(eo_errortype_debug, eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01), 0); } else { //s_send_diagnostics(eo_errortype_error, eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01), 0); } msgCmd.cmdId = ICUBCANPROTO_POL_AS_CMD__SET_CANDATARATE; eo_appCanSP_SendCmd(appCanSP_ptr, (eOcanport_t)canLoc.emscanport, msgdest, msgCmd, (void*)&(straincfg->datarate)); //s_send_diagnostics(eo_errortype_debug, eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01), 1); s_signalGetFullScales(strainId, straincfg->signaloncefullscale); } extern void eoprot_fun_UPDT_as_strain_config_mode(const EOnv* nv, const eOropdescriptor_t* rd) { eOsmStatesEMSappl_t currentstate; eOas_strainId_t strainId = (eOas_strainId_t)eoprot_ID2index(rd->id32); // this should be always 0 eOicubCanProto_msgCommand_t msgCmd = { EO_INIT(.class) icubCanProto_msgCmdClass_pollingAnalogSensor, EO_INIT(.cmdId) ICUBCANPROTO_POL_AS_CMD__SET_TXMODE }; eOas_strainmode_t *strainmode = (eOas_strainmode_t*)rd->data; EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()); if(eoas_strainmode_acquirebutdonttx == *strainmode) { eo_appCanSP_SendCmd2SnrStrain(appCanSP_ptr, strainId, msgCmd, (void*)strainmode); } else //if pc104 configures strain mode to send data { //only if the appl is in RUN state enable mais tx eom_emsappl_GetCurrentState(eom_emsappl_GetHandle(), ¤tstate); if(eo_sm_emsappl_STrun == currentstate) { eo_appCanSP_SendCmd2SnrStrain(appCanSP_ptr, strainId, msgCmd, (void*)strainmode); } } }
extern void eoprot_fun_UPDT_mc_joint_config(const EOnv* nv, const eOropdescriptor_t* rd) { //eOresult_t res; eOmc_jointId_t jxx = eoprot_ID2index(rd->id32); float rescaler_pos; float rescaler_trq; eOmc_joint_status_t *jstatus = NULL; eOmc_joint_config_t *cfg = (eOmc_joint_config_t*)nv->ram; //currently no joint config param must be sent to 2foc board. (for us called 1foc :) ) //(currently no pid velocity is sent to 2foc) // 1) set pid position rescaler_pos = 1.0f/(float)(1<<cfg->pidposition.scale); eo_emsController_SetPosPid(jxx, cfg->pidposition.kp*rescaler_pos, cfg->pidposition.kd*rescaler_pos, cfg->pidposition.ki*rescaler_pos, cfg->pidposition.limitonintegral, cfg->pidposition.limitonoutput, cfg->pidposition.offset, cfg->pidposition.stiction_up_val*rescaler_pos, cfg->pidposition.stiction_down_val*rescaler_pos); // 2) set torque pid rescaler_trq = 1.0f/(float)(1<<cfg->pidtorque.scale); eo_emsController_SetTrqPid(jxx, cfg->pidtorque.kp*rescaler_trq, cfg->pidtorque.kd*rescaler_trq, cfg->pidtorque.ki*rescaler_trq, cfg->pidtorque.limitonintegral, cfg->pidtorque.limitonoutput, cfg->pidtorque.offset, cfg->pidtorque.kff*rescaler_trq, cfg->pidtorque.stiction_up_val*rescaler_trq, cfg->pidtorque.stiction_down_val*rescaler_trq); eo_emsController_SetAbsEncoderSign((uint8_t)jxx, (int32_t)cfg->DEPRECATED_encoderconversionfactor); eo_emsController_SetMotorParams((uint8_t)jxx, cfg->motor_params); eo_emsController_SetTcFilterType((uint8_t)jxx, (uint8_t) cfg->tcfiltertype); // 3) set velocity pid: to be implemented // 4) set min position eo_emsController_SetPosMin(jxx, cfg->limitsofjoint.min); // 5) set max position eo_emsController_SetPosMax(jxx, cfg->limitsofjoint.max); // 6) set vel timeout eo_emsController_SetVelTimeout(jxx, cfg->velocitysetpointtimeout); // 7) set impedance eo_emsController_SetImpedance(jxx, cfg->impedance.stiffness, cfg->impedance.damping, cfg->impedance.offset); // 8) set monitormode status jstatus = eo_protocolwrapper_GetJointStatus(eo_protocolwrapper_GetHandle(), (eOmc_jointId_t)jxx); if(NULL == jstatus) { return; //error } if(eomc_motionmonitormode_dontmonitor == cfg->motionmonitormode) { jstatus->basic.motionmonitorstatus = eomc_motionmonitorstatus_notmonitored; } else { jstatus->basic.motionmonitorstatus = eomc_motionmonitorstatus_setpointnotreachedyet; } }