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