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