kind of working driver

make_install_run.sh

@@ -0,0 +1,8 @@
+set -e
+
+make -j4
+sudo make install -j4
+cp ~/torcs-1.3.7/standard_settings/quickrace.xml ~/.torcs/config/raceman/quickrace.xml
+cp ~/torcs-1.3.7/standard_settings/screen.xml ~/.torcs/config/screen.xml
+cp ~/torcs-1.3.7/standard_settings/sound.xml ~/.torcs/config/sound.xml
+torcs

src/drivers/pid_driver/Makefile

@@ -20,7 +20,7 @@
 ROBOT       = pid_driver
 MODULE      = ${ROBOT}.so
 MODULEDIR   = drivers/${ROBOT}
-SOURCES     = ${ROBOT}.cpp
+SOURCES     = ${ROBOT}.cpp driver.cpp opponent.cpp
 
 SHIPDIR     = drivers/${ROBOT}
 SHIP        = ${ROBOT}.xml 155-DTM.rgb logo.rgb

src/drivers/pid_driver/driver.cpp

@@ -0,0 +1,186 @@
+#include "driver.h"
+
+const float Driver::MAX_UNSTUCK_ANGLE = 30.0 / 180.0 * PI; /* [radians] */
+const float Driver::UNSTUCK_TIME_LIMIT = 2.0;              /* [s] */
+const float Driver::MAX_UNSTUCK_SPEED = 5.0;               /* [m/s] */
+const float Driver::MIN_UNSTUCK_DIST = 3.0;                /* [m] */
+const float Driver::G = 9.81;                              /* [m/(s*s)] */
+const float Driver::FULL_ACCEL_MARGIN = 1.0;               /* [m/s] */
+const float Driver::SHIFT = 0.9;                           /* [-] (% of rpmredline) */
+const float Driver::SHIFT_MARGIN = 4.0;                    /* [m/s] */
+
+Driver::Driver(int index) { INDEX = index; }
+
+/* Called for every track change or new race. */
+void Driver::initTrack(tTrack *t, void *carHandle, void **carParmHandle, tSituation *s) {
+    track = t;
+    *carParmHandle = NULL;
+}
+
+/* Start a new race. */
+void Driver::newRace(tCarElt *car, tSituation *s) {
+    this->car = car;
+    CARMASS = GfParmGetNum(car->_carHandle, SECT_CAR, PRM_MASS, NULL, 1000.0);
+    initCa();
+
+    MAX_UNSTUCK_COUNT = int(UNSTUCK_TIME_LIMIT / RCM_MAX_DT_ROBOTS);
+    stuck = 0;
+
+    /* initialize the list of opponents */
+    opponents = new Opponents(s, this);
+    opponent = opponents->getOpponentPtr();
+}
+
+/* Drive during race. */
+void Driver::drive(tSituation *s) {
+    update(s);
+
+    memset(&car->ctrl, 0, sizeof(tCarCtrl));
+
+    if (isStuck()) {
+        car->ctrl.steer = -angle / car->_steerLock;
+        car->ctrl.gear = -1;      // reverse gear
+        car->ctrl.accelCmd = 0.5; // 50% accelerator pedal
+        car->ctrl.brakeCmd = 0.0; // no brakes
+    } else {
+        float steerangle = angle - car->_trkPos.toMiddle / car->_trkPos.seg->width;
+
+        car->ctrl.steer = steerangle / car->_steerLock;
+        car->ctrl.gear = getGear();
+        car->ctrl.brakeCmd = getBrake();
+        if (car->ctrl.brakeCmd == 0.0) {
+            car->ctrl.accelCmd = getAccel();
+        } else {
+            car->ctrl.accelCmd = 0.0;
+        }
+    }
+}
+
+/* Update my private data every timestep */
+void Driver::update(tSituation *s) {
+    trackangle = RtTrackSideTgAngleL(&(car->_trkPos));
+    angle = trackangle - car->_yaw;
+    NORM_PI_PI(angle);
+
+    mass = CARMASS + car->_fuel;
+}
+
+/* Compute the length to the end of the segment */
+float Driver::getDistToSegEnd() {
+    if (car->_trkPos.seg->type == TR_STR) {
+        return car->_trkPos.seg->length - car->_trkPos.toStart;
+    } else {
+        return (car->_trkPos.seg->arc - car->_trkPos.toStart) * car->_trkPos.seg->radius;
+    }
+}
+
+/* Compute gear */
+int Driver::getGear() {
+    if (car->_gear <= 0)
+        return 1;
+    float gr_up = car->_gearRatio[car->_gear + car->_gearOffset];
+    float omega = car->_enginerpmRedLine / gr_up;
+    float wr = car->_wheelRadius(2);
+
+    if (omega * wr * SHIFT < car->_speed_x) {
+        return car->_gear + 1;
+
+    } else {
+        float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1];
+        omega = car->_enginerpmRedLine / gr_down;
+        if (car->_gear > 1 && omega * wr * SHIFT > car->_speed_x + SHIFT_MARGIN) {
+            return car->_gear - 1;
+        }
+    }
+    return car->_gear;
+}
+
+/* Compute fitting acceleration */
+float Driver::getAccel() {
+    float allowedspeed = getAllowedSpeed(car->_trkPos.seg);
+    float gr = car->_gearRatio[car->_gear + car->_gearOffset];
+    float rm = car->_enginerpmRedLine;
+    if (allowedspeed > car->_speed_x + FULL_ACCEL_MARGIN) {
+        return 1.0;
+    } else {
+        return allowedspeed / car->_wheelRadius(REAR_RGT) * gr / rm;
+    }
+}
+
+/* Compute the allowed speed on a segment */
+float Driver::getAllowedSpeed(tTrackSeg *segment) {
+    if (segment->type == TR_STR) {
+        return FLT_MAX;
+    } else {
+        float mu = segment->surface->kFriction;
+        return sqrt((mu * G * segment->radius) / (1.0 - MIN(1.0, segment->radius * CA * mu / mass)));
+    }
+}
+
+/* Set pitstop commands. */
+int Driver::pitCommand(tSituation *s) { return ROB_PIT_IM; /* return immediately */ }
+
+/* End of the current race */
+void Driver::endRace(tSituation *s) {}
+
+float Driver::getBrake() {
+    tTrackSeg *segptr = car->_trkPos.seg;
+    float currentspeedsqr = car->_speed_x * car->_speed_x;
+    float mu = segptr->surface->kFriction;
+    float maxlookaheaddist = currentspeedsqr / (2.0 * mu * G);
+    float lookaheaddist = getDistToSegEnd();
+    float allowedspeed = getAllowedSpeed(segptr);
+    if (allowedspeed < car->_speed_x)
+        return 1.0;
+    segptr = segptr->next;
+    while (lookaheaddist < maxlookaheaddist) {
+        allowedspeed = getAllowedSpeed(segptr);
+        if (allowedspeed < car->_speed_x) {
+            float allowedspeedsqr = allowedspeed * allowedspeed;
+            float brakedist = (currentspeedsqr - allowedspeedsqr) / (2.0 * mu * G);
+            if (brakedist > lookaheaddist) {
+                return 1.0;
+            }
+        }
+        lookaheaddist += segptr->length;
+        segptr = segptr->next;
+    }
+    return 0.0;
+}
+
+/* Check if I'm stuck */
+bool Driver::isStuck() {
+    if (fabs(angle) > MAX_UNSTUCK_ANGLE && car->_speed_x < MAX_UNSTUCK_SPEED &&
+        fabs(car->_trkPos.toMiddle) > MIN_UNSTUCK_DIST) {
+        if (stuck > MAX_UNSTUCK_COUNT && car->_trkPos.toMiddle * angle < 0.0) {
+            return true;
+        } else {
+            stuck++;
+            return false;
+        }
+    } else {
+        stuck = 0;
+        return false;
+    }
+}
+
+/* Compute aerodynamic downforce coefficient CA */
+void Driver::initCa() {
+    char *WheelSect[4] = {SECT_FRNTRGTWHEEL, SECT_FRNTLFTWHEEL, SECT_REARRGTWHEEL, SECT_REARLFTWHEEL};
+    float rearwingarea = GfParmGetNum(car->_carHandle, SECT_REARWING, PRM_WINGAREA, (char *)NULL, 0.0);
+    float rearwingangle = GfParmGetNum(car->_carHandle, SECT_REARWING, PRM_WINGANGLE, (char *)NULL, 0.0);
+    float wingca = 1.23 * rearwingarea * sin(rearwingangle);
+    float cl = GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_FCL, (char *)NULL, 0.0) +
+               GfParmGetNum(car->_carHandle, SECT_AERODYNAMICS, PRM_RCL, (char *)NULL, 0.0);
+    float h = 0.0;
+    int i;
+    for (i = 0; i < 4; i++)
+        h += GfParmGetNum(car->_carHandle, WheelSect[i], PRM_RIDEHEIGHT, (char *)NULL, 0.20);
+    h *= 1.5;
+    h = h * h;
+    h = h * h;
+    h = 2.0 * exp(-3.0 * h);
+    CA = h * cl + 4.0 * wingca;
+}
+
+Driver::~Driver() { delete opponents; }

src/drivers/pid_driver/driver.h

@@ -0,0 +1,84 @@
+#ifndef _DRIVER_H_
+#define _DRIVER_H_
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <car.h>
+#include <raceman.h>
+#include <robot.h>
+#include <robottools.h>
+#include <tgf.h>
+#include <track.h>
+
+#include "opponent.h"
+class Opponents;
+class Opponent;
+
+class Driver {
+  public:
+    Driver(int index);
+    ~Driver();
+    float filterBColl(float brake);
+    Opponents *opponents;
+    Opponent *opponent;
+
+    /* callback functions called from TORCS */
+    void initTrack(tTrack *t, void *carHandle, void **carParmHandle, tSituation *s);
+    void newRace(tCarElt *car, tSituation *s);
+    void drive(tSituation *s);
+    int pitCommand(tSituation *s);
+    void endRace(tSituation *s);
+
+    tCarElt *getCarPtr() { return car; }
+    tTrack *getTrackPtr() { return track; }
+    float getSpeed() { return speed; }
+
+    float getAllowedSpeed(tTrackSeg *segment);
+    float getDistToSegEnd();
+    float getAccel();
+
+    float getBrake();
+    int getGear();
+
+    void initCa();
+
+  private:
+    /* utility functions */
+    bool isStuck();
+    void update(tSituation *s);
+
+    /* per robot global data */
+    int stuck;
+    float trackangle;
+    float angle;
+
+    /* data that should stay constant after first initialization */
+    int MAX_UNSTUCK_COUNT;
+    int INDEX;
+
+    /* class constants */
+    static const float MAX_UNSTUCK_ANGLE;
+    static const float UNSTUCK_TIME_LIMIT;
+    static const float MAX_UNSTUCK_SPEED;
+    static const float MIN_UNSTUCK_DIST;
+    static const float G;
+    static const float FULL_ACCEL_MARGIN;
+    static const float SHIFT;
+    static const float SHIFT_MARGIN;
+
+    /* track variables */
+    tTrack *track;
+
+    tCarElt *car; // Pointer to tCarElt struct.
+
+    float mass;        /* mass of car + fuel */
+    float CARMASS;     /* mass of the car only */
+    float CA;          /* aerodynamic downforce coefficient */
+
+    float speed; /* speed in track direction */
+};
+
+#endif // _DRIVER_H_