tdble CalculateTorque (tEngine* engine, tdble rads)
{
double StartTimeStamp = RtTimeStamp(); // Profiling test
tEngineCurve *curve = &(engine->curve);
tdble Tq = curve->data[0].Tq;
tdble Tmax = curve->data[0].Tq;
tdble Tmin = curve->data[0].Tq * 0.5f;
tdble rpm_max = curve->data[0].rads;
tdble rpm_min = -1.0;
tdble alpha = 0.0;
for (int i = 0; i < curve->nbPts; i++) {
if (rads > curve->data[i].rads) {
Tmin = curve->data[i].Tq;
rpm_min = curve->data[i].rads;
if (i<engine->curve.nbPts) {
Tmax = curve->data[i+1].Tq;
rpm_max = curve->data[i+1].rads;
}
}
}
alpha = (rads - rpm_min) / (rpm_max - rpm_min);
Tq = (float)((1.0-alpha) * Tmin + alpha * Tmax);
SimTicks += RtDuration(StartTimeStamp); // Profiling test
return Tq;
}
tdble CalculateTorque3 (tEngine* engine, tdble rads)
{
double StartTimeStamp = RtTimeStamp(); // Profiling test
tEngineCurve *curve = &(engine->curve);
int i = engine->lastInterval;
tdble rpm_min = curve->data[i].rads;
tdble rpm_max = curve->data[i+1].rads;
if (rads > rpm_max) {
if (i < curve->nbPts) {
rpm_min = rpm_max;
engine->lastInterval = ++i;
rpm_max = curve->data[i+1].rads;
}
}
else if (rads < rpm_min) {
if (i > 0) {
rpm_max = rpm_min;
engine->lastInterval = --i;
rpm_min = curve->data[i].rads;
}
}
tdble Tq;
// In case of large changes of rads
if ((rads < rpm_min) || (rads > rpm_max))
Tq = CalculateTorque2 (engine, rads);
else {
tdble alpha = (rads - rpm_min) / (rpm_max - rpm_min);
Tq = (float)((1.0-alpha) * curve->data[i].Tq + alpha * curve->data[i+1].Tq);
}
SimTicks2 += RtDuration(StartTimeStamp); // Profiling test
return Tq;
}
// Get Duration [msec]
// ATTENTION:
// With Linux the duration has to be smaller than 60 sec!
double RtDuration(double StartTimeStamp)
{
return RtTimeStamp() - StartTimeStamp;
}
// Achtung: Diese Prozedur wird sehr häufig und schnell nacheinander
// aufgerufen. Deshalb geben wir hier in der Regel keine Debug-Texte aus!
// Zur Fehlersuche kann das aber mal sinnvoll sein.
//--------------------------------------------------------------------------*
static void Drive(int Index, tCarElt* Car, tSituation *S)
{
//printf("main drive\n");
//LogSimplix.debug("#>>> TDriver::Drive\n");
if (cInstances[Index-IndexOffset].cRobot->CurrSimTime < S->currentTime)
// if (cInstances[Index-IndexOffset].cRobot->CurrSimTime + 0.03 < S->currentTime)
{
//LogSimplix.debug("#Drive\n");
double StartTimeStamp = RtTimeStamp();
cInstances[Index-IndexOffset].cRobot->CurrSimTime = // Update current time
S->currentTime;
cInstances[Index-IndexOffset].cRobot->Update(Car,S); // Update info about opp.
if (cInstances[Index-IndexOffset].cRobot->IsStuck()){ // Check if we are stuck
cInstances[Index-IndexOffset].cRobot->Unstuck(); // Unstuck
}
else{ // or
cInstances[Index-IndexOffset].cRobot->Drive(); // Drive
}
double Duration = RtDuration(StartTimeStamp);
if (cInstances[Index-IndexOffset].cTickCount > 0) // Collect used time
{
if (Duration > 1.0)
cInstances[Index-IndexOffset].cLongSteps++;
if (Duration > 2.0)
cInstances[Index-IndexOffset].cCriticalSteps++;
if (cInstances[Index-IndexOffset].cMinTicks > Duration)
cInstances[Index-IndexOffset].cMinTicks = Duration;
if (cInstances[Index-IndexOffset].cMaxTicks < Duration)
cInstances[Index-IndexOffset].cMaxTicks = Duration;
}
cInstances[Index-IndexOffset].cTickCount++;
cInstances[Index-IndexOffset].cTicks += Duration;
}
else
{
//LogSimplix.debug("#DriveLast\n");
cInstances[Index-IndexOffset].cUnusedCount++;
cInstances[Index-IndexOffset].cRobot->DriveLast(); // Use last drive commands
}
//LogSimplix.debug("#<<< TDriver::Drive\n");
/*
memset((void *)&car->ctrl, 0, sizeof(tCarCtrl));
float angle;
const float SC = 1.0;
//same direction
//angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
//reverse direction
angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
//debug
//printf("angle1 => %f\n",angle);
//printf("yaw => %f\n", car->_yaw);
NORM_PI_PI(angle);
//same direction
//angle -= SC*car->_trkPos.toMiddle/car->_trkPos.seg->width;
//reverse direction
angle -= SC*car->_trkPos.toMiddle/car->_trkPos.seg->width;
//debug
//printf("angle3 => %f\n",angle);
//set up values to return
car->ctrl.steer = angle/car->_steerLock;
car->ctrl.gear = 1;
car->ctrl.accelCmd = 0.1;
car->ctrl.brakeCmd = 0.0;
*/
/*
* add the driving code here to modify the
* car->_steerCmd
* car->_accelCmd
* car->_brakeCmd
* car->_gearCmd
* car->_clutchCmd
*/
}