4.8.2.14. GTIRE_FORCE
This subroutine defines a user - defined Gtire force.
Language type |
Subroutine |
C/C++ |
gtire_force(double time, int tid, int wmid, int rid, int rrmid, int sflag, int jflag, int iflag, double tireforce[6]) |
Variable Name |
Size |
Description |
time |
double |
Current simulation time of RecurDyn/Solver. |
tid |
int |
tire entity id to get the tire parameters |
wmid |
int |
wheel marker id |
rid |
int |
road entity id to get the road parameters |
rrmid |
int |
road reference marker id |
sflag |
int |
solver state (dynamic:1 , static:3) |
jflag |
int |
when RD/Solver evaluates a Jacobian, this flag is set to true. |
iflag |
int |
when RD/Solver initializes arrays, this flag is set to true. |
tireforce[6] |
double |
returned force and torque vector at wheel marker (wmid). |
#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 arrays, 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 arrays
{
// 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);
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);
}