static void mdlStart(SimStruct *S) {
// ssGetPWork(S)[0] = (void *) new counter; // store new C++ object in the pointers vector
ssGetPWork(S)[0] = (void *) new yarp::os::Network();
ssGetPWork(S)[1] = (void *) new yarp::os::BufferedPort<yarp::os::Bottle>();
yarp::os::BufferedPort<yarp::os::Bottle> *yPortIn = (yarp::os::BufferedPort<yarp::os::Bottle> *) ssGetPWork(S)[1];
#define LENGTH 100
char_T buf01[LENGTH];
mxGetString(ssGetSFcnParam(S, 0), buf01, LENGTH);
char_T buf02[LENGTH];
mxGetString(ssGetSFcnParam(S, 1), buf02, LENGTH);
std::string strPortNameSender(buf01);
std::string strPortNameReceiver(buf02);
mexPrintf("opening port: %s\n", strPortNameReceiver.c_str());
yPortIn->open(strPortNameReceiver.c_str());
Sleep(1000);
yarp::os::Network *yNetwork = (yarp::os::Network *) ssGetPWork(S)[0];
if(!yNetwork->connect(strPortNameSender.c_str(), strPortNameReceiver.c_str())){
std::string strMessage = "error connecting ports \"" + strPortNameSender + "\" to \"" + strPortNameReceiver + "\"";
mexWarnMsgTxt(strMessage.c_str());
}
}
static void mdlTerminate(SimStruct *S)
{
// IF YOU FORGET TO DESTROY OBJECTS OR DEALLOCATE MEMORY, MATLAB WILL CRASH.
// Retrieve and destroy C++ object
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
toPort->close();
if(ssGetPWork(S) != NULL){
ssSetPWorkValue(S,0,NULL);
}
Network::fini();
fprintf(stderr,"Everything was closed correctly\n");
}
static void mdlTerminate(SimStruct *S)
{
USING_NAMESPACE_QPOASES
int i;
/* reset global message handler */
MessageHandling_reset( qpOASES_getGlobalMessageHandler() );
for ( i=1; i<5; ++i )
{
if ( ssGetPWork(S)[i] != 0 )
free( ssGetPWork(S)[i] );
}
}
static void mdlTerminate(SimStruct *S)
{
PCONFIG_DATA config;
/* Retrieve and destroy C object */
if( ssGetNumPWork(S) > 0 )
{
config = ssGetPWork(S)[0];
if( config )
{
free(config);
ssGetPWork(S)[0] = 0;
}
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
int_T nRobots = *ssGetIWork(S);
// get Pointers
// accessing inputs
const real_T *position = (const real_T*) ssGetInputPortSignal(S,0);
// const real_T *orientation = (const real_T*) ssGetInputPortSignal(S,1);
geometry_msgs::PointStamped msg;
for (unsigned int i = 0; i < nRobots; ++i) {
GenericPublisher<geometry_msgs::PointStamped>* pub
= (GenericPublisher<geometry_msgs::PointStamped>*)vecPWork[i];
// define send Time.
msg.header.stamp = ros::Time::now();
msg.point.x = position[i*3 + 0];
msg.point.y = position[i*3 + 1];
msg.point.z = position[i*3 + 2];
pub->publish(msg);
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
int_T nRobots = *ssGetIWork(S);
// Preparing Outputs
real_T *position = (real_T*)ssGetOutputPortSignal(S,0);
real_T *quaternion = (real_T*)ssGetOutputPortSignal(S,1);
real_T *time = (real_T*)ssGetOutputPortSignal(S,2);
for (unsigned int i = 0; i < nRobots; ++i) {
GenericSubscriber<geometry_msgs::PoseStamped>* sub =
(GenericSubscriber<geometry_msgs::PoseStamped>*)vecPWork[i];
geometry_msgs::PoseStampedConstPtr lastMsg = sub->getLastMsg();
if (lastMsg){ // copy only if a message was actually received
position[i*3 + 0] = lastMsg->pose.position.x;
position[i*3 + 1] = lastMsg->pose.position.y;
position[i*3 + 2] = lastMsg->pose.position.z;
// w, x, y, z
quaternion[i*4 + 0] = lastMsg->pose.orientation.w;
quaternion[i*4 + 1] = lastMsg->pose.orientation.x;
quaternion[i*4 + 2] = lastMsg->pose.orientation.y;
quaternion[i*4 + 3] = lastMsg->pose.orientation.z;
time[i + 0] = lastMsg->header.stamp.toSec();
}
}
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
O22SnapIoMemMap *Brain;
Brain = (O22SnapIoMemMap *) ssGetPWork(S)[0];
// Variador de Frecuencia
Brain->SetAnaPtValue(16,4.0);
// Valvula Solenoide
Brain->SetAnaPtValue(13,4.0);
// Valvula Motorizada
Brain->SetAnaPtValue(12,4.0);
// Calefactor 1
Brain->SetDigPtState(20,0);
// Calefactor 2
Brain->SetDigPtState(22,0);
// Calefactor 3
Brain->SetDigPtState(21,0);
// Agitador
Brain->SetDigPtState(23,0);
// Valv. Solenoide 1
Brain->SetDigPtState(24,0);
// Valv. Solenoide 2
Brain->SetDigPtState(25,0);
// Luces del Laboratorio
Brain->SetDigPtState(26,0);
Brain->Close();
delete Brain;
}
static void mdlStart(SimStruct *S)
{
SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
// init ROS if not yet done.
initROS(S);
void** vecPWork = ssGetPWork(S);
ros::AsyncSpinner* spinner = new ros::AsyncSpinner(1); // Spinner
spinner->start();
vecPWork[0] = spinner;
ros::NodeHandle nodeHandle(ros::this_node::getName());
// get String
size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
char* topic = (char*)mxMalloc(buflen);
size_t status = mxGetString((ssGetSFcnParam(S, 1)), topic, buflen);
//SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
GenericSubscriber<sensor_msgs::Joy>* sub = new GenericSubscriber<sensor_msgs::Joy>(nodeHandle, std::string(topic), 100);
vecPWork[1] = sub;
//*sub = n.subscribe(std::string(topic), 1, msgCallback);
// free char array
mxFree(topic);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
GenericSubscriber<sensor_msgs::Joy>* sub = (GenericSubscriber<sensor_msgs::Joy>*)vecPWork[1];
// Preparing Outputs
real_T* axes = (real_T*)ssGetOutputPortSignal(S,0);
int32_T* buttons = (int32_T*)ssGetOutputPortSignal(S,1);
real_T *time = (real_T*)ssGetOutputPortSignal(S,2);
sensor_msgs::JoyConstPtr lastMsg = sub->getLastMsg();
if (lastMsg) { // copy only if a message was actually received
// axes
for (unsigned int i=0; i < lastMsg->axes.size() && i < CONSTANT_AXES_SIZE; i++) {
axes[i] = lastMsg->axes[i];
}
// do we have to fill in the rest?
// buttons
for (unsigned int i=0; i < lastMsg->buttons.size() && i < CONSTANT_BUTTONS_SIZE; i++) {
buttons[i] = lastMsg->buttons[i];
}
time[0] = lastMsg->header.stamp.toSec();
}
}
// Function: mdlOutputs =======================================================
// Abstract:
// In this function, you compute the outputs of your S-function
// block.
static void mdlOutputs(SimStruct *S, int_T tid)
{
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
Vector *v = toPort->read(false); // Read from the port. Waits until data arrives.
if (v!=NULL)
{
for (int i = 0; i < SIZE_READING_PORT; i++)
{
real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);
int_T widthPort = ssGetOutputPortWidth(S,i);
if (widthPort == 1)
{
for(int_T j=0; j<widthPort; j++)
if (i < (v->length())){
pY[j] = v->data()[i];
//cout<<v->data()[i]<<" ";
}
else
pY[j] = 0;
}
else
cout << "ERROR: something wrong with port dimensions \n";
}
}
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
Context* context;
context = (Context*)malloc(sizeof(Context));
if (context == NULL)
setErrorStatus(S, "mdlStart: memeory allocation error");
memset(context, 0, sizeof(context));
//save context in SS
ssGetPWork(S)[0] = context;
context->enableSpacemouse = (int)(*((double*)mxGetData(ssGetSFcnParam(S, 0))));
# if defined(RT)
if (context->enableSpacemouse) {
context->tmcd2005.deviceBall.handle= tmcdDeviceBallOpen("/dev/tmcd2005/ball/ser1");
if ( !context->tmcd2005.deviceBall.handle )
setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");
}
else {
context->tmcd2005.device.handle= tmcdDeviceOpen("/dev/tmcd2005/noball/ser1");
if ( !context->tmcd2005.device.handle )
setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");
}
# endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
int_T nRobots = *ssGetIWork(S);
// Preparing Outputs
real_T *force = (real_T*)ssGetOutputPortSignal(S,0);
real_T *time = (real_T*)ssGetOutputPortSignal(S,1);
telekyb_msgs::TKMotorCommandsConstPtr lastMsg;
for (unsigned int i = 0; i < nRobots; ++i) {
GenericSubscriber<telekyb_msgs::TKMotorCommands>* sub
= (GenericSubscriber<telekyb_msgs::TKMotorCommands>*)vecPWork[i];
lastMsg = sub->getLastMsg();
if (lastMsg){ // copy only if a message was actually received
if (lastMsg->force.size() == 4){
for (unsigned int j=0; j<4; j++){
force[i*4 + j] = lastMsg->force[j];
}
} else {
SFUNPRINTF("Wrong message size (%d).\n", (int)lastMsg->force.size());
}
time[i + 0] = lastMsg->header.stamp.toSec();
}
}
}
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
char name[7];
MBX *mbx;
int i;
#ifdef KRTAI
i = mbx_rt_get_adr(TargetLedMbxID, MAX_RTAI_LEDS);
sprintf(name, "%s%d", TargetLedMbxID, i);
rtaiLed[i] = S;
mbx = mbx_rt_mbx_init(name, (MBX_RTAI_LED_SIZE/(sizeof(unsigned int))*(sizeof(unsigned int))));
#else
for (i = 0; i < MAX_RTAI_LEDS; i++) {
sprintf(name, "%s%d", TargetLedMbxID, i);
if (!rt_get_adr(nam2num(name))) break;
}
rtaiLed[i] = S;
if (!(mbx = (MBX *)rt_mbx_init(nam2num(name), (MBX_RTAI_LED_SIZE/(sizeof(unsigned int))*(sizeof(unsigned int)))))) {
printf("Cannot init mailbox\n");
exit(1);
}
#endif
ssGetPWork(S)[0]= (void *)mbx;
#endif
}
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
MBX *mbx;
char name[7];
int i;
int_T *dim = ssGetInputPortDimensions(S,0);
#ifdef KRTAI
i = mbx_rt_get_adr(TargetLogMbxID,MAX_RTAI_LOGS);
sprintf(name, "%s%d", TargetLogMbxID, i);
rtaiLog[i] = S;
mbx = mbx_rt_mbx_init(name, (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float))));
#else
for (i = 0; i < MAX_RTAI_LOGS; i++) {
sprintf(name, "%s%d", TargetLogMbxID, i);
if (!rt_get_adr(nam2num(name))) break;
}
rtaiLog[i] = S;
if (!(mbx = (MBX *)rt_mbx_init(nam2num(name), (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float)))))) {
printf("Cannot init mailbox\n");
exit(1);
}
#endif
ssGetPWork(S)[0] = (void *)mbx;
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
int_T nRobots = *ssGetIWork(S);
// get Pointers
// accessing inputs
const real_T *linVel = (const real_T*) ssGetInputPortSignal(S,0);
const real_T *angVel = (const real_T*) ssGetInputPortSignal(S,1);
geometry_msgs::WrenchStamped msg;
for (unsigned int i = 0; i < nRobots; ++i) {
GenericPublisher<geometry_msgs::WrenchStamped>* pub
= (GenericPublisher<geometry_msgs::WrenchStamped>*)vecPWork[i];
// define send Time.
msg.header.stamp = ros::Time::now();
msg.wrench.force.x = linVel[i*3 + 0];
msg.wrench.force.y = linVel[i*3 + 1];
msg.wrench.force.z = linVel[i*3 + 2];
// w, x, y, z
msg.wrench.torque.x = angVel[i*3 + 0];
msg.wrench.torque.y = angVel[i*3 + 1];
msg.wrench.torque.z = angVel[i*3 + 2];
pub->publish(msg);
}
}
static void mdlStart(SimStruct *S)
{
char *bufSimEng; // SIMULATION_ENGINE
int bufSimEngLen;
// Build the list of parameters that configure the tres::Network
std::vector<std::string> ns_params = readMaskAndBuildConfVector(S);
// Get the type of the adapter, i.e., the concrete implementation of tres::Network
bufSimEngLen = mxGetN( ssGetSFcnParam(S,SIMULATION_ENGINE) )+1;
bufSimEng = new char[bufSimEngLen];
mxGetString(ssGetSFcnParam(S,SIMULATION_ENGINE), bufSimEng, bufSimEngLen);
std::string engine(bufSimEng);
delete bufSimEng;
// Instantiate the concrete representation of tres::Network
std::unique_ptr<tres::Network> ns = Factory<tres::Network>::instance()
.create(engine, ns_params);
// Save the C++ object to the pointers vector
tres::Network *_ns = ns.release();
ssGetPWork(S)[0] = _ns;
// Set the `New pending activations available' flag to false
boolean_T *pendingActivsAvail = (boolean_T*) ssGetDWork(S,0);
pendingActivsAvail[0] = false;
// Get the time resolution (actually, its floating point representation)
std::vector<std::string>::iterator it = ns_params.end()-1;
double time_resolution = atof((*it).c_str());
// Save the time resolution to the real vector workspace
ssGetRWork(S)[0] = time_resolution;
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
// get Objects
void** vecPWork = ssGetPWork(S);
int_T nRobots = *ssGetIWork(S);
// Preparing Outputs
real_T *rpy = (real_T*)ssGetOutputPortSignal(S,0);
real_T *thrust = (real_T*)ssGetOutputPortSignal(S,1);
real_T *estim_mass = (real_T*)ssGetOutputPortSignal(S,2);
real_T *time = (real_T*)ssGetOutputPortSignal(S,3);
telekyb_msgs::TKCommandsConstPtr lastMsg;
for (unsigned int i = 0; i < nRobots; ++i) {
GenericSubscriber<telekyb_msgs::TKCommands>* sub
= (GenericSubscriber<telekyb_msgs::TKCommands>*)vecPWork[i];
lastMsg = sub->getLastMsg();
if (lastMsg){ // copy only if a message was actually received
rpy[i*3 + 0] = lastMsg->roll;
rpy[i*3 + 1] = lastMsg->pitch;
rpy[i*3 + 2] = lastMsg->yaw;
thrust[i + 0] = lastMsg->thrust;
estim_mass[i + 0] = lastMsg->mass;
time[i + 0] = lastMsg->header.stamp.toSec();
}
}
}
static void mdlStart(SimStruct *S)
{
// Notify
SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
// init ROS if not yet done.
initROS(S);
ros::NodeHandle nodeHandle(ros::this_node::getName());
void** vecPWork = ssGetPWork(S);
ros::Time::init();
ros::Time* firstExecTime = new ros::Time(ros::Time::now());
vecPWork[0] = firstExecTime;
ros::Time* lastExecTime = new ros::Time(*firstExecTime);
vecPWork[1] = lastExecTime;
real_T* vecRWork = ssGetRWork(S);
vecRWork[0] = mxGetScalar(ssGetSFcnParam(S, 0)); // Tsim
vecRWork[1] = 0.0; // simulationTime
int_T* vecIWork = ssGetIWork(S);
vecIWork[0] = 1; // initialize step counter
}
// Function: mdlTerminate =================================================
// Abstract:
// In this function, you should perform any actions that are necessary
// at the termination of a simulation. For example, if memory was
// allocated in mdlStart, this is the place to free it.
static void mdlTerminate(SimStruct *S)
{
// Get the C++ object back from the pointers vector
tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]);
// Call its destructor
delete ns;
}
static void mdlTerminate(SimStruct *S)
{
USING_NAMESPACE_QPOASES
int i;
/* reset global message handler */
getGlobalMessageHandler( )->reset( );
if ( ssGetPWork(S)[0] != 0 )
delete ssGetPWork(S)[0];
for ( i=1; i<5; ++i )
{
if ( ssGetPWork(S)[i] != 0 )
free( ssGetPWork(S)[i] );
}
}
static void mdlZeroCrossings(SimStruct *S)
{
// Get the C++ object back from the pointers vector
tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]);
// Get the time resolution back from the real vector workspace
double time_resolution = ssGetRWork(S)[0];
ssGetNonsampledZCs(S)[0] = ns->getTimeOfNextEvent()/(time_resolution) - ssGetT(S);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
int j;
ocp_nlp_dims *nlp_dims = (ocp_nlp_dims *) ssGetPWork(S)[0];
ocp_nlp_in *nlp_in = (ocp_nlp_in *) ssGetPWork(S)[1];
ocp_nlp_out *nlp_out = (ocp_nlp_out *) ssGetPWork(S)[2];
ocp_nlp_solver *nlp_solver = (ocp_nlp_solver *) ssGetPWork(S)[4];
ocp_nlp_constraints_bgh_model **constraints = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints;
ocp_nlp_constraints_bgh_dims **constraints_dims = (ocp_nlp_constraints_bgh_dims **) nlp_dims->constraints;
ocp_nlp_cost_nls_model **cost = (ocp_nlp_cost_nls_model **) nlp_in->cost;
const double *x0 = ssGetInputPortRealSignal(S, 0);
const double *reference = ssGetInputPortRealSignal(S, 1);
// bounds
double lb_0[] = {-10000, -10000, 50, 50, 1.14275, 1.53787};
double ub_0[] = {+10000, +10000, 50, 50, 1.14275, 1.53787};
for (j = 0; j < NUM_STATES; ++j) {
lb_0[NUM_CONTROLS+j] = x0[j];
ub_0[NUM_CONTROLS+j] = x0[j];
}
ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "lb", lb_0);
ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "ub", ub_0);
for (j = 0; j <= NUM_STAGES; ++j)
BLASFEO_DVECEL(&cost[j]->y_ref, 0) = *reference;
int status = ocp_nlp_solve(nlp_solver, nlp_in, nlp_out);
double *u0_opt = ssGetOutputPortRealSignal(S, 0);
double *x1 = ssGetOutputPortRealSignal(S, 1);
double *status_out = ssGetOutputPortRealSignal(S, 2);
double *comp_time = ssGetOutputPortRealSignal(S, 3);
blasfeo_unpack_dvec(NUM_CONTROLS, &nlp_out->ux[0], 0, u0_opt);
blasfeo_unpack_dvec(NUM_STATES, &nlp_out->ux[1], NUM_CONTROLS, x1);
*status_out = (double) status;
*comp_time = nlp_out->total_time;
}
static void mdlTerminate(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWork(S)[0];
#ifdef KRTAI
mbx_rt_mbx_delete(mbx);
#else
rt_mbx_delete(mbx);
#endif
#endif
}
/* Function: mdlStart =======================================================
* Abstract:
* allocate memory for work vectors */
static void mdlStart(SimStruct *S)
{
ssGetPWork(S)[0] = calloc(NSTATES(S)*NSTATES(S), sizeof(double)); /* Mn */
ssGetPWork(S)[1] = calloc(NSTATES(S), sizeof(double)); /* qn */
ssGetPWork(S)[2] = calloc(NSTATES(S), sizeof(double)); /* r */
ssGetPWork(S)[3] = calloc(NSTATES(S), sizeof(double)); /* c */
ssGetPWork(S)[4] = calloc(NSTATES(S), sizeof(double)); /* x */
ssGetPWork(S)[5] = Matrix_Init(NSTATES(S),NSTATES(S)+2,"A"); /* pA */
ssGetPWork(S)[6] = Basis_Init(NSTATES(S)); /* pB */
}
static void mdlStart(SimStruct *S)
{
// ######### YARP INITIALIZATION STUFF ##################
Network::init();
fprintf(stderr,"YARP NETWORK INITIALIZED\n");
if (!Network::checkNetwork() || !Network::initialized()){
ssSetErrorStatus(S,"YARP server wasn't found active!! \n");
return;
}
else
cout<<"YARP is running!!\n"<<endl;
int_T buflen, status;
char *String;
buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! \n");
return;
}
//string port_name = String;
char *port_name = String; //FROM port name
buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! \n");
return;
}
char *toPort_name = String;
// ######## CHECKING INPUT PARAMETERS ############
BufferedPort<Vector> *toPort;
toPort = new BufferedPort<Vector>;
toPort->open(toPort_name);
ConstString toPortName = toPort->getName();
cout<<"[From] Port name will be: "<<port_name<<endl;
cout<<"[To] Port name will be: "<<toPort->getName()<<endl;
ssGetPWork(S)[0] = toPort;
Network::connect(port_name,toPortName);
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
// get Objects
void** vecPWork = ssGetPWork(S);
ros::AsyncSpinner* spinner = (ros::AsyncSpinner*)vecPWork[0];
GenericSubscriber<sensor_msgs::Joy>* sub = (GenericSubscriber<sensor_msgs::Joy>*)vecPWork[1];
// cleanup
delete spinner;
delete sub;
SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid){
#if defined(RT)
TmcdDeviceBallActual* actualBall = 0;
TmcdDeviceActual* actual = 0;
# endif
Context* context = (Context*)(ssGetPWork(S)[0]);
#if defined(RT)
if (context->enableSpacemouse)
{
actualBall = tmcdDeviceBallGetActual(context->tmcd2005.deviceBall.handle);
if ( !actualBall )
ssSetErrorStatus(S,"could not get state from TMCD2005");
// copy events to simulink outputs
*((boolean_T*)ssGetOutputPortSignal(S, 0)) = actualBall->on;
((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actualBall->keys[0];
((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actualBall->keys[1];
((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actualBall->keys[2];
((real_T*)ssGetOutputPortSignal(S, 2))[0] = actualBall->ballData[0];
((real_T*)ssGetOutputPortSignal(S, 2))[1] = actualBall->ballData[1];
((real_T*)ssGetOutputPortSignal(S, 2))[2] = actualBall->ballData[2];
((real_T*)ssGetOutputPortSignal(S, 2))[3] = actualBall->ballData[3];
((real_T*)ssGetOutputPortSignal(S, 2))[4] = actualBall->ballData[4];
((real_T*)ssGetOutputPortSignal(S, 2))[5] = actualBall->ballData[5];
*((boolean_T*)ssGetOutputPortSignal(S, 3)) = actualBall->isBallActive;
}
else
{
actual = tmcdDeviceGetActual(context->tmcd2005.device.handle);
if ( !actual )
ssSetErrorStatus(S,"could not get state from TMCD2005");
// copy events to simulink outputs
*((boolean_T*)ssGetOutputPortSignal(S, 0)) = actual->on;
((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actual->keys[0];
((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actual->keys[1];
((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actual->keys[2];
((boolean_T*)ssGetOutputPortSignal(S, 1))[3] = actual->keys[3];
}
#endif
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S) {
// counter *c = (counter *) ssGetPWork(S)[0];
// delete c;
yarp::os::BufferedPort<yarp::os::Bottle> *yPortIn = (yarp::os::BufferedPort<yarp::os::Bottle> *) ssGetPWork(S)[1];
yPortIn->close();
yarp::os::Network *yNetwork = (yarp::os::Network *) ssGetPWork(S)[0];
yNetwork->fini();
delete yNetwork;
delete yPortIn;
}
/* Function: SimInitializeConditions ========================================
* Abstract:
* In this function, you should initialize the continuous and discrete
* states for your S-function block. The initial states are placed
* in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
* You can also perform any other initialization activities that your
* S-function may require. Note, this routine will be called at the
* start of simulation and if it is present in an enabled subsystem
* configured to reset states, it will be call when the enabled subsystem
* restarts execution to reset the states.
*/
void SimInitializeConditions(SimStruct *S)
{
Sim *sim = (Sim *) ssGetPWork(S)[0]; // retrieve C++ object from the pointers vector and use member functions of the object
try
{
sim->init_cond();
}
catch(Exception e)
{
e.kill(S);
return;
}
}
/* Function: SimOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
void SimOutputs(SimStruct *S, int_T tid)
{
Sim *sim = (Sim *) ssGetPWork(S)[0]; // retrieve C++ object from the pointers vector and use member functions of the object
try
{
sim->output();
}
catch(Exception e)
{
e.kill(S);
return;
}
}
