void simu_controller_loop(MBSdataStruct *MBSdata)
{
// controller variables
ControllerStruct *cvs;
#ifndef YARP
cvs = MBSdata->user_IO->cvs;
// controller
controller_inputs(MBSdata);
controller_loop(cvs);
controller_outputs(MBSdata);
// simulation outputs
simu_outputs(MBSdata);
#else //if def YARP
// TODO : ifdef YARP, then change argument of simu_controller_loop to remove MBSdata
getControllerInput_Yarp();
//controller_loop(cvs); // uncomment when getControllerInput_Yarp will be working
writeControllerOutput_Yarp();
#endif
}
void simu_controller_loop(MBSdataStruct *MBSdata)
{
double tsim;
ControllerStruct *cvs;
UserIOStruct *uvs;
uvs = MBSdata->user_IO;
cvs = uvs->cvs;
tsim = MBSdata->tsim;
// controller
if (tsim >= uvs->last_t_ctrl + PERIOD_CTRL - TIME_EPSILON)
{
uvs->last_t_ctrl = tsim;
controller_inputs(MBSdata);
controller_loop(cvs);
controller_outputs(MBSdata);
}
// simulation outputs
simu_outputs(MBSdata);
// objective function
fitness(MBSdata);
// stopping the simulation if needed
stop_simu(MBSdata);
}
// Returns 0 if no problem
int user_initialization(MBSdataStruct *MBSdata, LocalDataStruct *lds)
#endif
{
// inputs of the controller
controller_inputs(MBSdata);
// controller initialization
controller_init(MBSdata->user_IO->cvs);
//init_SEAStruct(1);
#ifdef CMEX
return 0;
#endif
}