1 /***************************************************************************
2
3 file : berniw.cpp
4 created : Mon Apr 17 13:51:00 CET 2000
5 copyright : (C) 2000-2006 by Bernhard Wymann
6 email : berniw@bluewin.ch
7 version : $Id: inferno.cpp,v 1.20.2.2 2013/08/05 17:22:44 berniw Exp $
8
9 ***************************************************************************/
10
11 /***************************************************************************
12 * *
13 * This program is free software; you can redistribute it and/or modify *
14 * it under the terms of the GNU General Public License as published by *
15 * the Free Software Foundation; either version 2 of the License, or *
16 * (at your option) any later version. *
17 * *
18 ***************************************************************************/
19
20
21 #include "berniw.h"
22
23
24 // Function prototypes.
25 static void initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation * situation);
26 static void drive(int index, tCarElt* car, tSituation *situation);
27 static void newRace(int index, tCarElt* car, tSituation *situation);
28 static int InitFuncPt(int index, void *pt);
29 static int pitcmd(int index, tCarElt* car, tSituation *s);
30 static void shutdown(int index);
31 float getClutch(MyCar* myc, tCarElt* car);
32
33 static const char* botname[BOTS] = {
34 "inferno 1", "inferno 2", "inferno 3", "inferno 4", "inferno 5",
35 "inferno 6", "inferno 7", "inferno 8", "inferno 9", "inferno 10"
36 };
37
38 static const char* botdesc[BOTS] = {
39 "inferno 1", "inferno 2", "inferno 3", "inferno 4", "inferno 5",
40 "inferno 6", "inferno 7", "inferno 8", "inferno 9", "inferno 10"
41 };
42
43 // Module entry point.
inferno(tModInfo * modInfo)44 extern "C" int inferno(tModInfo *modInfo)
45 {
46 for (int i = 0; i < BOTS; i++) {
47 modInfo[i].name = strdup(botname[i]); // Name of the module (short).
48 modInfo[i].desc = strdup(botdesc[i]); // Description of the module (can be long).
49 modInfo[i].fctInit = InitFuncPt; // Init function.
50 modInfo[i].gfId = ROB_IDENT; // Supported framework version.
51 modInfo[i].index = i+1;
52 }
53 return 0;
54 }
55
56
57 // Initialize function (callback) pointers for torcs.
InitFuncPt(int index,void * pt)58 static int InitFuncPt(int index, void *pt)
59 {
60 tRobotItf *itf = (tRobotItf *)pt;
61
62 itf->rbNewTrack = initTrack; // Init new track.
63 itf->rbNewRace = newRace; // Init new race.
64 itf->rbDrive = drive; // Drive during race.
65 itf->rbShutdown = shutdown; // Called for cleanup per driver.
66 itf->rbPitCmd = pitcmd; // Pit command.
67 itf->index = index;
68 return 0;
69 }
70
71
72 static MyCar* mycar[BOTS] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
73 static OtherCar* ocar = NULL;
74 static TrackDesc* myTrackDesc = NULL;
75 static double currenttime;
76 static const tdble waitToTurn = 1.0; // How long should i wait till i try to turn backwards.
77
78
79 // Release resources when the module gets unloaded.
shutdown(int index)80 static void shutdown(int index) {
81 int i = index - 1;
82 if (mycar[i] != NULL) {
83 delete mycar[i];
84 mycar[i] = NULL;
85 }
86 if (myTrackDesc != NULL) {
87 delete myTrackDesc;
88 myTrackDesc = NULL;
89 }
90 if (ocar != NULL) {
91 delete [] ocar;
92 ocar = NULL;
93 }
94 }
95
96
97 // Initialize track data, called for every selected driver.
initTrack(int index,tTrack * track,void * carHandle,void ** carParmHandle,tSituation * situation)98 static void initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation * situation)
99 {
100 if ((myTrackDesc != NULL) && (myTrackDesc->getTorcsTrack() != track)) {
101 delete myTrackDesc;
102 myTrackDesc = NULL;
103 }
104 if (myTrackDesc == NULL) {
105 myTrackDesc = new TrackDesc(track);
106 }
107
108 char buffer[BUFSIZE];
109 char* trackname = strrchr(track->filename, '/') + 1;
110
111 switch (situation->_raceType) {
112 case RM_TYPE_PRACTICE:
113 snprintf(buffer, BUFSIZE, "drivers/inferno/%d/practice/%s", index, trackname);
114 break;
115 case RM_TYPE_QUALIF:
116 snprintf(buffer, BUFSIZE, "drivers/inferno/%d/qualifying/%s", index, trackname);
117 break;
118 case RM_TYPE_RACE:
119 snprintf(buffer, BUFSIZE, "drivers/inferno/%d/race/%s", index, trackname);
120 break;
121 default:
122 break;
123 }
124
125 *carParmHandle = GfParmReadFile(buffer, GFPARM_RMODE_STD);
126 if (*carParmHandle == NULL) {
127 snprintf(buffer, BUFSIZE, "drivers/inferno/%d/default.xml", index);
128 *carParmHandle = GfParmReadFile(buffer, GFPARM_RMODE_STD);
129 }
130
131 // Load and set parameters.
132 float fuel = GfParmGetNum(*carParmHandle, BERNIW_SECT_PRIV, BERNIW_ATT_FUELPERLAP,
133 (char*)NULL, track->length*MyCar::MAX_FUEL_PER_METER);
134 //printf("fuelperlap: %f\n", fuel);
135
136 float fuelmargin = (situation->_raceType == RM_TYPE_RACE) ? 1.0 : 0.0;
137
138 fuel *= (situation->_totLaps + fuelmargin);
139 GfParmSetNum(*carParmHandle, SECT_CAR, PRM_FUEL, (char*)NULL, MIN(fuel, 100.0));
140 }
141
142
143 // Initialize driver for the race, called for every selected driver.
newRace(int index,tCarElt * car,tSituation * situation)144 static void newRace(int index, tCarElt* car, tSituation *situation)
145 {
146 if (ocar != NULL) {
147 delete [] ocar;
148 }
149 ocar = new OtherCar[situation->_ncars];
150 for (int i = 0; i < situation->_ncars; i++) {
151 ocar[i].init(myTrackDesc, situation->cars[i], situation);
152 }
153
154 if (mycar[index-1] != NULL) {
155 delete mycar[index-1];
156 }
157 mycar[index-1] = new MyCar(myTrackDesc, car, situation);
158
159 currenttime = situation->currentTime;
160 }
161
162
163 // Controls the car.
drive(int index,tCarElt * car,tSituation * situation)164 static void drive(int index, tCarElt* car, tSituation *situation)
165 {
166 tdble angle;
167 tdble brake;
168 tdble b1; // Brake value in case we are to fast HERE and NOW.
169 tdble b2; // Brake value for some brake point in front of us.
170 tdble b3; // Brake value for control (avoid loosing control).
171 tdble b4; // Brake value for avoiding high angle of attack.
172 tdble b5; // Brake for the pit;
173 tdble steer, targetAngle, shiftaccel;
174
175 MyCar* myc = mycar[index-1];
176 Pathfinder* mpf = myc->getPathfinderPtr();
177
178 b1 = b2 = b3 = b4 = b5 = 0.0;
179 shiftaccel = 0.0;
180
181 // Update some values needed.
182 myc->update(myTrackDesc, car, situation);
183
184 // Decide how we want to drive, choose a behaviour.
185 if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) {
186 myc->loadBehaviour(myc->NORMAL);
187 } else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) {
188 myc->loadBehaviour(myc->CAREFUL);
189 } else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) {
190 myc->loadBehaviour(myc->SLOW);
191 }
192
193 // Update the other cars just once.
194 if (currenttime != situation->currentTime) {
195 currenttime = situation->currentTime;
196 for (int i = 0; i < situation->_ncars; i++) ocar[i].update();
197 }
198
199 // Startmode.
200 if (myc->trtime < 5.0 && myc->bmode != myc->START) {
201 myc->loadBehaviour(myc->START);
202 myc->startmode = true;
203 }
204 if (myc->startmode && myc->trtime > 5.0) {
205 myc->startmode = false;
206 myc->loadBehaviour(myc->NORMAL);
207 }
208
209 // Compute path according to the situation.
210 mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar);
211
212 // Clear ctrl structure with zeros and set the current gear.
213 memset(&car->ctrl, 0, sizeof(tCarCtrl));
214 car->_gearCmd = car->_gear;
215
216 // Uncommenting the following line causes pitstop on every lap.
217 //if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId());
218
219 // Compute fuel consumption.
220 if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) {
221 if (car->race.laps > 0) {
222 myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel));
223 }
224 myc->lastfuel = car->priv.fuel;
225 myc->lastpitfuel = 0.0;
226 myc->fuelchecked = true;
227 } else if (myc->getCurrentSegId() > 5) {
228 myc->fuelchecked = false;
229 }
230
231 // Decide if we need a pit stop.
232 if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE ||
233 (car->priv.fuel < (myc->fuelperlap*(1.0+myc->FUEL_SAFETY_MARGIN)) &&
234 car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap)))
235 {
236 mpf->setPitStop(true, myc->getCurrentSegId());
237 }
238
239 if (mpf->getPitStop()) {
240 car->_raceCmd = RM_CMD_PIT_ASKED;
241 }
242
243 // Steer toward the next target point.
244 targetAngle = atan2(myc->dynpath->getLoc(myc->destpathsegid)->y - car->_pos_Y, myc->dynpath->getLoc(myc->destpathsegid)->x - car->_pos_X);
245 targetAngle -= car->_yaw;
246 NORM_PI_PI(targetAngle);
247 steer = targetAngle / car->_steerLock;
248
249 // Steer P (proportional) controller. We add a steer correction proportional to the distance error
250 // to the path.
251 // Steer angle has usual meaning, therefore + is to left (CCW) and - to right (CW).
252 // derror sign is + to right and - to left.
253 if (!mpf->getPitStop()) {
254 steer = steer + MIN(myc->STEER_P_CONTROLLER_MAX, myc->derror*myc->STEER_P_CONTROLLER_GAIN)*myc->getErrorSgn();
255 if (fabs(steer) > 1.0) {
256 steer/=fabs(steer);
257 }
258 } else {
259 // Check if we are almost in the pit to set brake to the max to avoid overrun.
260 tdble dl, dw;
261 RtDistToPit(car, myTrackDesc->getTorcsTrack(), &dl, &dw);
262 if (dl < 1.0f) {
263 b5 = 1.0f;
264 }
265 }
266
267 // Try to control angular velocity with a D (differential) controller.
268 double omega = myc->getSpeed()/myc->dynpath->getRadius(myc->currentpathsegid);
269 steer += myc->STEER_D_CONTROLLER_GAIN*(omega - myc->getCarPtr()->_yaw_rate);
270
271
272 // Brakes.
273 tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION);
274 tdble brakespeed, brakedist;
275 tdble lookahead = 0.0;
276 int i = myc->getCurrentSegId();
277 brake = 0.0;
278
279 while (lookahead < brakecoeff * myc->getSpeedSqr()) {
280 lookahead += myc->dynpath->getLength(i);
281 brakespeed = myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(i);
282 if (brakespeed > 0.0) {
283 tdble gm, qb, qs;
284 gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha();
285 qs = myc->dynpath->getSpeedsqr(i);
286
287 brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw)));
288
289 if (brakedist > lookahead - myc->getWheelTrack()) {
290 qb = brakespeed*brakecoeff/brakedist;
291 if (qb > b2) {
292 b2 = qb;
293 }
294 }
295 }
296 i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg();
297 }
298
299 if (myc->getSpeedSqr() > myc->dynpath->getSpeedsqr(myc->currentpathsegid)) {
300 b1 = (myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(myc->currentpathsegid)) / (myc->getSpeedSqr());
301 }
302
303 // Try to avoid flying.
304 if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) {
305 b4 = 1.0;
306 }
307
308 // Check if we are on the way.
309 if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) {
310 if (myc->derror > myc->PATHERR) {
311 vec2d *cd = myc->getDir();
312 vec2d *pd = myc->dynpath->getDir(myc->currentpathsegid);
313 float z = cd->x*pd->y - cd->y*pd->x;
314 // If the car points away from the path brake.
315 if (z*myc->getErrorSgn() >= 0.0) {
316 targetAngle = atan2(myc->dynpath->getDir(myc->currentpathsegid)->y, myc->dynpath->getDir(myc->currentpathsegid)->x);
317 targetAngle -= car->_yaw;
318 NORM_PI_PI(targetAngle);
319 double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos())));
320 b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder;
321 }
322 }
323 }
324
325 // Anti wheel locking and brake code.
326 if (b1 > b2) brake = b1; else brake = b2;
327 if (brake < b3) brake = b3;
328 if (brake < b4) {
329 brake = MIN(1.0, b4);
330 tdble abs_mean;
331 abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed();
332 abs_mean /= 2.0;
333 brake = brake * abs_mean;
334 } else {
335 brake = MIN(1.0, brake);
336 tdble abs_min = 1.0;
337 for (int i = 0; i < 4; i++) {
338 tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed();
339 if (slip < abs_min) abs_min = slip;
340 }
341 brake = brake * abs_min;
342 }
343
344 // Reduce brake value to the approximate normal force available on the wheels.
345 float weight = myc->mass*G;
346 float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED;
347 float force = weight + myc->ca*myc->getSpeedSqr();
348 brake = brake*MIN(1.0, force/maxForce);
349 if (b5 > 0.0f) {
350 brake = b5;
351 }
352
353 // Gear changing.
354 if (myc->tr_mode == 0) {
355 if (car->_gear <= 0) {
356 car->_gearCmd = 1;
357 } else {
358 float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
359 float omega = car->_enginerpmRedLine/gr_up;
360 float wr = car->_wheelRadius(2);
361
362 if (omega*wr*myc->SHIFT < car->_speed_x) {
363 car->_gearCmd++;
364 } else {
365 float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
366 omega = car->_enginerpmRedLine/gr_down;
367 if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) {
368 car->_gearCmd--;
369 }
370 }
371 }
372 }
373
374
375 // Acceleration / brake execution.
376 tdble cerror, cerrorh;
377 cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y);
378 if (cerrorh > myc->TURNSPEED) {
379 cerror = fabs(car->_speed_x)/cerrorh;
380 } else {
381 cerror = 1.0;
382 }
383
384 if (myc->tr_mode == 0) {
385 if (brake > 0.0) {
386 myc->accel = 0.0;
387 car->_accelCmd = myc->accel;
388 car->_brakeCmd = brake*cerror;
389 } else {
390 if (myc->getSpeedSqr() < myc->dynpath->getSpeedsqr(myc->getCurrentSegId())) {
391 if (myc->accel < myc->ACCELLIMIT) {
392 myc->accel += myc->ACCELINC;
393 }
394 car->_accelCmd = myc->accel/cerror;
395 } else {
396 if (myc->accel > 0.0) {
397 myc->accel -= myc->ACCELINC;
398 }
399 // TODO: shiftaccel always 0 at the moment...
400 car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror);
401 }
402 tdble slipspeed = myc->querySlipSpeed(car);
403 if (slipspeed > myc->TCL_SLIP) {
404 car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE);
405 }
406 }
407 }
408
409
410 // Check if we are stuck, try to get unstuck.
411 tdble bx = myc->getDir()->x, by = myc->getDir()->y;
412 tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y;
413 tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by));
414
415 if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
416 myc->turnaround += situation->deltaTime;
417 } else myc->turnaround = 0.0;
418 if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) {
419 if (myc->tr_mode == 0) {
420 myc->tr_mode = 1;
421 }
422 if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) {
423 car->_accelCmd = 0.0;
424 if (myc->tr_mode == 1) {
425 car->_gearCmd--;
426 }
427 car->_brakeCmd = 1.0;
428 } else {
429 myc->tr_mode = 2;
430 if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) {
431 angle = queryAngleToTrack(car);
432 car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0;
433 car->_brakeCmd = 0.0;
434
435 if (myc->accel < 1.0) {
436 myc->accel += myc->ACCELINC;
437 }
438 car->_accelCmd = myc->accel;
439 tdble slipspeed = myc->querySlipSpeed(car);
440 if (slipspeed < -myc->TCL_SLIP) {
441 car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE);
442 }
443 } else {
444 if (myc->getSpeed() < 1.0) {
445 myc->turnaround = 0;
446 myc->tr_mode = 0;
447 myc->loadBehaviour(myc->START);
448 myc->startmode = true;
449 myc->trtime = 0.0;
450 }
451 car->_brakeCmd = 1.0;
452 car->_steerCmd = 0.0;
453 car->_accelCmd = 0.0;
454 }
455 }
456 }
457
458 if (myc->tr_mode == 0) car->_steerCmd = steer;
459 car->_clutchCmd = getClutch(myc, car);
460 }
461
462
463 // Pitstop callback.
pitcmd(int index,tCarElt * car,tSituation * s)464 static int pitcmd(int index, tCarElt* car, tSituation *s)
465 {
466 MyCar* myc = mycar[index-1];
467 Pathfinder* mpf = myc->getPathfinderPtr();
468
469 float fullracedist = (myTrackDesc->getTorcsTrack()->length*s->_totLaps);
470 float remaininglaps = (fullracedist - car->_distRaced)/myTrackDesc->getTorcsTrack()->length;
471
472 car->_pitFuel = MAX(MIN(myc->fuelperlap*(remaininglaps+myc->FUEL_SAFETY_MARGIN) - car->_fuel, car->_tank - car->_fuel), 0.0);
473 myc->lastpitfuel = MAX(car->_pitFuel, 0.0);
474 car->_pitRepair = car->_dammage;
475 mpf->setPitStop(false, myc->getCurrentSegId());
476 myc->loadBehaviour(myc->START);
477 myc->startmode = true;
478 myc->trtime = 0.0;
479
480 return ROB_PIT_IM; // Return immediately.
481 }
482
483
484 // Compute the clutch value.
485 // Does not work great when braking to 0 and accelerating again...
486 // TODO: Improve.
getClutch(MyCar * myc,tCarElt * car)487 float getClutch(MyCar* myc, tCarElt* car)
488 {
489 if (car->_gear > 1) {
490 myc->clutchtime = 0.0;
491 return 0.0;
492 } else {
493 float drpm = car->_enginerpm - car->_enginerpmRedLine/2.0;
494 myc->clutchtime = MIN(myc->CLUTCH_FULL_MAX_TIME, myc->clutchtime);
495 float clutcht = (myc->CLUTCH_FULL_MAX_TIME - myc->clutchtime)/myc->CLUTCH_FULL_MAX_TIME;
496 if (car->_gear == 1 && car->_accelCmd > 0.0) {
497 myc->clutchtime += (float) RCM_MAX_DT_ROBOTS;
498 }
499
500 //printf("ct: %f, car->_gear: %d, car->_gearCmd: %d, drpm: %f\n", myc->clutchtime, car->_gear, car->_gearCmd, drpm);
501 if (drpm > 0) {
502 float speedr;
503 if (car->_gearCmd == 1) {
504 // Compute corresponding speed to engine rpm.
505 float omega = car->_enginerpmRedLine/car->_gearRatio[car->_gear + car->_gearOffset];
506 float wr = car->_wheelRadius(2);
507 speedr = (myc->CLUTCH_SPEED + MAX(0.0, car->_speed_x))/fabs(wr*omega);
508 float clutchr = MAX(0.0, (1.0 - speedr*2.0*drpm/car->_enginerpmRedLine));
509 return MIN(clutcht, clutchr);
510 } else {
511 // For the reverse gear.
512 myc->clutchtime = 0.0;
513 return 0.0;
514 }
515 } else {
516 return clutcht;
517 }
518 }
519 }
520