C/C++ example for tire force user subroutine

#include "stdafx.h"

#include "DllFunc.h"

#include "math.h"

#include <map>

#define PI 3.1415926535897

std::map<int, double> posErrX;

USUB_GTireForce_API void __cdecl gtire_force

(double time, int tid, int wmid, int rid, int rrmid, int sflag, int jflag, int iflag, double tireforce[6])

{

using namespace rd_syscall;

// Parameter Information

// time : Simulation time of RD/Solver. (Input)

// tid : tire entity id to get the tire parameters. (Input)

// wmid : wheel marker id. (Input)

// rid : road entity id to get the road parameters. (Input)

// rrmid : road reference marker id. (Input)

// sflag : solver state (dynamic:1 , static:3). (Input)

// jflag : When RD/Solver evaluates a Jacobian, the flag is true. (Input)

// iflag : When RD/Solver initializes arraies, the flag is true. (Input)

// tireforce: returned force and torque vector at wheel marker (wmid).

// User Statement

int errflg = 0;

char sNameTireFile[256];

int nNameTireFile;

int idTireFileParam;

char sNameRoadFile[256];

int nNameRoadFile;

int idRoadFileParam;

char sNameTable[256];

int nNodes;

int nElem;

int nNameTable;

double* patchRoad;

// DE

double dif1[1];

double dif[1];

double difini[1];

dif1[0] = 0.0;

dif[0] = 0.0;

difini[0] = 0.0;

if (iflag) // when RD/Solver initializes arraies

{

// reads parameter from GTire file

get_gtire_tirefilename(tid, sNameTireFile, &nNameTireFile, &errflg);

load_propfile(sNameTireFile, nNameTireFile, &idTireFileParam, &errflg);

get_propfile_double(idTireFileParam, "DIMENSION", (int)strlen("DIMENSION"), "UNLOADED_RADIUS", (int)strlen("UNLOADED_RADIUS"), &tireRadius[tid], &errflg);

get_propfile_double(idTireFileParam, "CHARACTERISTIC", (int)strlen("CHARACTERISTIC"), "STIFFNESS", (int)strlen("STIFFNESS"), &stiffness[tid], &errflg);

get_propfile_double(idTireFileParam, "MODEL", (int)strlen("MODEL"), "NUM_DIFF_EQ_RD", (int)strlen("NUM_DIFF_EQ_RD"), &numDiff[tid], &errflg);

unload_propfile(idTireFileParam, &errflg);

if (numDiff[tid] != 1)

{

errmes(errflg, "Number of differential equation is not one in the tire file.", 1, "GTireUSUB");

}

// reads parameter from GRoad file

get_gtire_roadfilename(rid, sNameRoadFile, &nNameRoadFile, &errflg);

load_propfile(sNameRoadFile, nNameRoadFile, &idRoadFileParam, &errflg);

get_propfile_tableinfo(idRoadFileParam, "NODES", (int)strlen("NODES"), sNameTable, &nNameTable, &nNodes, &errflg);

patchRoad = new double[nNodes];

get_propfile_table(idRoadFileParam, "NODES", (int)strlen("NODES"), sNameTable, (int)strlen(sNameTable), patchRoad, &errflg);

pos_patch_rm_Z[tid] = patchRoad[3];

for (int i = 0; i < nNodes; i++)

{

char messageString[256];

sprintf_s(messageString, "%f", patchRoad[i]);

printmsg(messageString, strlen(messageString));

}

unload_propfile(idRoadFileParam, &errflg);

// reads user parameter from Tire Entity

double uPara[4];

int nPara;

get_gtire_parameter(tid, uPara, &nPara, &errflg);

if (nPara == 4)

{

pGain[tid] = uPara[0];

iGain[tid] = uPara[1];

dGain[tid] = uPara[2];

controlMarker[tid] = (int)uPara[3];

}

else

{

errmes(errflg, "Number of parameter is not four. [1:P Gain, 2:I Gain, 3:D Gain, 4:Control Marker]", 2, "GTireUSUB");

}

// initialize differential equation

difini[0] = 0.0;

set_gtire_deqini(tid, difini, &errflg);

}

// set marker: tire marker - road marker (reference road marker)

int mk_tm_rm[3];

mk_tm_rm[0] = wmid;

mk_tm_rm[1] = rrmid;

mk_tm_rm[2] = rrmid;

// set marker: tire marker orientation

int mk_tm_ori[1];

mk_tm_ori[0] = wmid;

// set marker: control marker - tire marker (reference road marker)

int mk_cm_tm[3];

mk_cm_tm[0] = controlMarker[tid];

mk_cm_tm[1] = wmid;

mk_cm_tm[2] = rrmid;

// get position for tire, road and control marker

double pos_tm_rm_Z = 0;

double pos_tm_rm_X = 0;

double tm_ori = 0;

double pos_cm_tm_X = 0;

double vel_cm_tm_X = 0;

double* tireXY = new double[2];

double* tireZ = new double[1];

int nStrDataArray = 0; // it's not used in default cmd

char StrData[2560]; // it's not used in default cmd

sysfnc("DZ", mk_tm_rm, 3, &pos_tm_rm_Z, &errflg);

sysfnc("DX", mk_tm_rm, 3, &pos_tm_rm_X, &errflg);

sysfnc("AZ", mk_tm_ori, 1, &tm_ori, &errflg);

sysfnc("DX", mk_cm_tm, 3, &pos_cm_tm_X, &errflg);

sysfnc("VX", mk_cm_tm, 3, &vel_cm_tm_X, &errflg);

tireXY[0] = pos_tm_rm_X;

tireXY[1] = 0;

get_gtire_roaddata(rid, rrmid, 1, tireXY, tireZ, nStrDataArray, StrData, &errflg);

// calculate control force

double controlForceX = 0;

get_gtire_deqvar(tid, dif, &errflg);

controlForceX = pGain[tid] * pos_cm_tm_X + iGain[tid] * dif[0] + dGain[tid] * vel_cm_tm_X;

// calculate contact force

double contactForceZ = 0;

if (pos_tm_rm_Z - tireRadius[tid] - tireZ[0] < 0) // (tire marker - road marker) - (tire radius) - (patch - road marker)

{

contactForceZ = (tireZ[0] - (pos_tm_rm_Z - tireRadius[tid]))*stiffness[tid];

}

// return tire force

tireforce[0] = contactForceZ * sin(tm_ori - PI)*(-1) + controlForceX * cos(tm_ori - PI)*(-1);;

tireforce[1] = contactForceZ * cos(tm_ori - PI) + controlForceX * sin(tm_ori - PI)*(-1);;

tireforce[2] = 0;

tireforce[3] = 0;

tireforce[4] = 0;

tireforce[5] = 0;

// Set diff equation

dif1[0] = pos_cm_tm_X; // posErr Intg X

set_gtire_deqder(tid, dif1, &errflg);

// return custom output

set_gtire_postdata(tid, 1, &(controlForceX), &errflg);

set_gtire_postdata(tid, 2, &(contactForceZ), &errflg);

set_gtire_postdata(tid, 3, &(pos_tm_rm_Z), &errflg);

set_gtire_postdata(tid, 4, &(tm_ori), &errflg);

set_gtire_postdata(tid, 5, &(pos_cm_tm_X), &errflg);

set_gtire_postdata(tid, 6, &(vel_cm_tm_X), &errflg);

set_gtire_postdata(tid, 7, &(dif1[0]), &errflg);

set_gtire_postdata(tid, 8, &(dif[0]), &errflg);

double data9 = 9.0;

double data10 = 10.0;

double data11 = 11.0;

double data12 = 12.0;

double data13 = 13.0;

double data14 = 14.0;

double data15 = 15.0;

double data16 = 16.0;

double data17 = 17.0;

set_gtire_postdata(tid, 9, &(data9), &errflg);

set_gtire_postdata(tid, 10, &(data10), &errflg);

set_gtire_postdata(tid, 11, &(data11), &errflg);

set_gtire_postdata(tid, 12, &(data12), &errflg);

set_gtire_postdata(tid, 13, &(data13), &errflg);

set_gtire_postdata(tid, 14, &(data14), &errflg);

set_gtire_postdata(tid, 15, &(data15), &errflg);

set_gtire_postdata(tid, 16, &(data16), &errflg);