From a02dfc2a3d9a731825f82fcc585b22a8e1a2b929 Mon Sep 17 00:00:00 2001 From: kakukri Date: Wed, 3 Nov 2010 23:42:34 +0000 Subject: [PATCH] Re #150: add gyroscopic forces and non-constant weight force in z direction to simuv2.1 git-svn-id: https://svn.code.sf.net/p/speed-dreams/code/trunk@3087 30fe4595-0a0c-4342-8851-515496e4dcbd Former-commit-id: 5dce93c188260a60ad2ccf37fcd0fa3288dc139d Former-commit-id: 74682c28d6bd08a68c99dc241e3319a7fae862fa --- src/modules/simu/simuv2.1/car.cpp | 5 ++++- src/modules/simu/simuv2.1/steer.cpp | 8 ++++++++ src/modules/simu/simuv2.1/wheel.cpp | 19 +++++++++++++++++++ src/modules/simu/simuv2.1/wheel.h | 2 ++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/modules/simu/simuv2.1/car.cpp b/src/modules/simu/simuv2.1/car.cpp index b82d45c38..3371b012b 100644 --- a/src/modules/simu/simuv2.1/car.cpp +++ b/src/modules/simu/simuv2.1/car.cpp @@ -167,7 +167,7 @@ SimCarUpdateForces(tCar *car) SinTheta = (-car->wheel[FRNT_RGT].zRoad - car->wheel[REAR_RGT].zRoad + car->wheel[FRNT_LFT].zRoad + car->wheel[REAR_LFT].zRoad) / (2.0 * car->wheeltrack); F.F.y = -w * SinTheta; - F.F.z = w; /* not 3D */ + F.F.z = w - (F.F.x*F.F.x + F.F.y*F.F.y)/(2.0*w);/*Taylor-polinom of sqrt(w^2-F.F.x^2-F.F.y^2)*/ F.M.x = F.M.y = F.M.z = 0; /* Wheels */ @@ -179,10 +179,13 @@ SimCarUpdateForces(tCar *car) /* moments */ F.M.x += car->wheel[i].forces.z * car->wheel[i].staticPos.y + car->wheel[i].forces.y * (car->statGC.z + car->wheel[i].rideHeight); + F.M.x += car->wheel[i].torques.x; F.M.y -= car->wheel[i].forces.z * car->wheel[i].staticPos.x + car->wheel[i].forces.x * (car->statGC.z + car->wheel[i].rideHeight); + F.M.y += car->wheel[i].torques.y; F.M.z += -car->wheel[i].forces.x * car->wheel[i].staticPos.y + car->wheel[i].forces.y * car->wheel[i].staticPos.x; + F.M.z += car->wheel[i].torques.z; } /* Aero Drag */ diff --git a/src/modules/simu/simuv2.1/steer.cpp b/src/modules/simu/simuv2.1/steer.cpp index caa2567db..68fa05c8b 100644 --- a/src/modules/simu/simuv2.1/steer.cpp +++ b/src/modules/simu/simuv2.1/steer.cpp @@ -51,10 +51,18 @@ SimSteerUpdate(tCar *car) steer2 = atan2((car->wheelbase * tanSteer) , (car->wheelbase - tanSteer * car->wheeltrack)); if (steer > 0) { + car->wheel[FRNT_RGT].torques.x = + car->wheel->cosax * (steer2 - car->wheel[FRNT_RGT].steer) * car->wheel[FRNT_RGT].prespinVel * car->wheel[FRNT_RGT].I / SimDeltaTime; car->wheel[FRNT_RGT].steer = steer2; + car->wheel[FRNT_LFT].torques.x = + car->wheel->cosax * (steer - car->wheel[FRNT_LFT].steer) * car->wheel[FRNT_LFT].prespinVel * car->wheel[FRNT_LFT].I / SimDeltaTime; car->wheel[FRNT_LFT].steer = steer; } else { + car->wheel[FRNT_RGT].torques.x = + car->wheel->cosax * (steer - car->wheel[FRNT_RGT].steer) * car->wheel[FRNT_RGT].prespinVel * car->wheel[FRNT_RGT].I / SimDeltaTime; car->wheel[FRNT_RGT].steer = steer; + car->wheel[FRNT_LFT].torques.x = + car->wheel->cosax * (-steer2 - car->wheel[FRNT_LFT].steer) * car->wheel[FRNT_LFT].prespinVel * car->wheel[FRNT_LFT].I / SimDeltaTime; car->wheel[FRNT_LFT].steer = -steer2; } } diff --git a/src/modules/simu/simuv2.1/wheel.cpp b/src/modules/simu/simuv2.1/wheel.cpp index be1f12b92..82f333d45 100644 --- a/src/modules/simu/simuv2.1/wheel.cpp +++ b/src/modules/simu/simuv2.1/wheel.cpp @@ -58,6 +58,8 @@ SimWheelConfig(tCar *car, int index) } else { wheel->relPos.ax = wheel->staticPos.ax; } + wheel->cosax = cos(wheel->relPos.ax); + wheel->sinax = sin(wheel->relPos.ax); wheel->lfMin = MIN(0.8f, wheel->lfMin); wheel->lfMax = MAX(1.6f, wheel->lfMax); @@ -103,6 +105,7 @@ SimWheelConfig(tCar *car, int index) wheel->feedBack.Tq = 0.0f; wheel->feedBack.brkTq = 0.0f; wheel->rel_vel = 0.0f; + wheel->torques.x = wheel->torques.y = wheel->torques.z = 0.0f; } @@ -276,9 +279,25 @@ SimWheelUpdateRotation(tCar *car) { int i; tWheel *wheel; + tdble deltan; + tdble cosaz, sinaz; for (i = 0; i < 4; i++) { wheel = &(car->wheel[i]); + /*calculate gyroscopic forces*/ + cosaz = cos(wheel->relPos.az); + sinaz = sin(wheel->relPos.az); + if( (i == 0) || (i == 1) ){ + wheel->torques.y = wheel->torques.x * sinaz; + wheel->torques.x = wheel->torques.x * cosaz; + } else { + wheel->torques.x = wheel->torques.y =0.0; + } + deltan = -(wheel->in.spinVel - wheel->prespinVel) * wheel->I / SimDeltaTime; + wheel->torques.x -= deltan * wheel->cosax *sinaz; + wheel->torques.y += deltan * wheel->cosax *cosaz; + wheel->torques.z = deltan * wheel->sinax; + /*update rotation*/ wheel->spinVel = wheel->in.spinVel; FLOAT_RELAXATION2(wheel->spinVel, wheel->prespinVel, 50.0f); diff --git a/src/modules/simu/simuv2.1/wheel.h b/src/modules/simu/simuv2.1/wheel.h index 889478578..675324209 100644 --- a/src/modules/simu/simuv2.1/wheel.h +++ b/src/modules/simu/simuv2.1/wheel.h @@ -31,6 +31,7 @@ typedef struct /* dynamic */ t3Dd forces; /* forces acting on car */ + t3Dd torques; /* torques acting on car (gyroscopic forces) */ tdble rollRes; /* Rolling Resistance (summed over the car) */ tdble rideHeight; /* height of the bottom of the car */ tdble zRoad; /* z of the road */ @@ -56,6 +57,7 @@ typedef struct /* static */ tPosd staticPos; /* pos relative to the GC (z is suspension travel at rest) */ /* and angles are camber (ax), caster (ay) and toe (az) */ + tdble cosax, sinax;/*cosinus and sinus of relPos.ax*/ tdble rollCenter; tdble weight0; /* initial weight on this wheel */