restructure code, explicit pid controllers

export/lib/libsolid.a

@@ -1 +1 @@
-/home/jonas/tmp/torcs-1.3.7/src/modules/simu/simuv2/SOLID-2.0/src/libsolid.a
+/home/jonas/torcs-1.3.7/src/modules/simu/simuv2/SOLID-2.0/src/libsolid.a

setup_linux.sh

@@ -2,7 +2,6 @@
 
 [ -z "$1" ] && exit 1
 [ ! -d "$1" ] && exit 1
-
 mkdir -p $1/drivers/human 2>/dev/null
 if [ ! -e  $1/drivers/human/car.xml ] || [ drivers/human/car.xml -nt $1/drivers/human/car.xml ]
 then

src/doc/torcsdoc.conf

@@ -4,9 +4,9 @@
 #
 #    file                 : torcsdoc.conf
 #    created              : Mon Jan 31 23:57:03 CET 2000
-#    copyright            : (C) 2000-2014 by Eric Espie, Bernhard Wymann
-#    email                : [email protected]
-#    version              : $Id: torcsdoc.conf.in,v 1.6.2.5 2014/05/20 12:12:45 berniw Exp $
+#    copyright            : (C) 2000-2014 by Eric Espie, Bernhard Wymann                         
+#    email                : [email protected]   
+#    version              : $Id: torcsdoc.conf.in,v 1.6.2.5 2014/05/20 12:12:45 berniw Exp $                                  
 #
 ##############################################################################
 #

src/drivers/pid_driver/Makefile

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

src/drivers/pid_driver/driver.cpp

@@ -1,6 +1,10 @@
 #include "driver.h"
 
+#include <cmath>
 #include <iostream>
+#ifndef M_PI
+#define M_PI (3.14159265358979323846)
+#endif
 
 const float Driver::MAX_UNSTUCK_ANGLE = 30.0 / 180.0 * PI; /* [radians] */
 const float Driver::UNSTUCK_TIME_LIMIT = 2.0;              /* [s] */
@@ -12,8 +16,24 @@ const float Driver::SHIFT = 0.9;                           /* [-] (% of rpmredli
 const float Driver::SHIFT_MARGIN = 4.0;                    /* [m/s] */
 const float Driver::ABS_SLIP = 0.9;                        /* [-] range [0.95..0.3] */
 const float Driver::ABS_MINSPEED = 3.0;                    /* [m/s] */
-
-Driver::Driver(int index) { INDEX = index; }
+const float Y_DIST_TO_MIDDLE = 5.0;
+const float GOAL_POS_Y = 20;
+const float GOAL_POS_X = 0;
+
+Driver::Driver(int index) {
+    float dt = 0.02;
+    float Kp = -0.5;
+    float Ki = -0.2;
+    float Kd = -0.0005;
+    _pidAcc = PidAcc(dt, Kp, Ki, Kd);
+    Kp = -0.3;
+    Ki = -0.1;
+    Kd = -0.005;
+    float G1 = 0.2;
+    float G2 = 0.8;
+    _pidSteer = PidSteer(dt, Kp, Ki, Kd, G1, G2, 12);
+    INDEX = index;
+}
 
 /* Called for every track change or new race. */
 void Driver::initTrack(tTrack *t, void *carHandle, void **carParmHandle, tSituation *s) {
@@ -42,33 +62,70 @@ void Driver::drive(tCarElt *car, tSituation *s) {
     std::cout << "Other cars ----" << std::endl;
     for (int i = 0; i < opponents->getNOpponents(); i++) {
         std::cout << "State " << i << ": " << opponent[i].getState() << std::endl;
-        std::cout << "SpeedX " << i << ": " << getOpponentSpeedDiffX(opponent[i]) << std::endl;
-        std::cout << "SpeedY " << i << ": " << getOpponentSpeedDiffY(opponent[i]) << std::endl;
+        if (opponent[i].getState() & OPP_FRONT) {
+            std::cout << "State " << i << ": "
+                      << "Front" << std::endl;
+        }
+        std::cout << "SpeedY " << i << ": " << getSpeed() << std::endl;
+        std::cout << "SpeedDiffX " << i << ": " << getOpponentSpeedDiffX(opponent[i]) << std::endl;
+        std::cout << "SpeedDiffY " << i << ": " << getOpponentSpeedDiffY(opponent[i]) << std::endl;
         std::cout << "DistanceY " << i << ": " << getOpponentDistanceY(opponent[i]) << std::endl;
         std::cout << "DistanceX " << i << ": " << getOpponentDistanceX(opponent[i]) << std::endl;
     }
 
-    std::cout << "My car ----" << std::endl;
-    std::cout << car->_trkPos.toMiddle << std::endl;
+    std::cout << "AI car ----" << std::endl;
+    std::cout << "To middle: " << car->_trkPos.toMiddle << std::endl;
+    std::cout << "To left: " << car->_trkPos.toLeft << std::endl;
+    std::cout << "To right: " << car->_trkPos.toRight << std::endl;
+    std::cout << angle << std::endl;
 
     memset(&car->ctrl, 0, sizeof(tCarCtrl));
 
-    if (isStuck(car)) {
-        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;
+    // float steerangle = angle - car->_trkPos.toMiddle / car->_trkPos.seg->width;
+    // car->ctrl.steer = steerangle / car->_steerLock;
+    car->ctrl.gear = getGear(car);
+    handleSpeed();
+    handleSteering();
+}
 
-        car->ctrl.steer = steerangle / car->_steerLock;
-        car->ctrl.gear = getGear(car);
-        car->ctrl.brakeCmd = filterABS(getBrake(car));
-        if (car->ctrl.brakeCmd == 0.0) {
-            car->ctrl.accelCmd = getAccel(car);
-        } else {
-            car->ctrl.accelCmd = 0.0;
-        }
+void Driver::handleSteering() {
+    float currentPosX = getOpponentDistanceX(opponent[0]);
+    car->ctrl.steer = _pidSteer.step(GOAL_POS_X, 0.0, currentPosX, angle);
+    if (getSpeed() < 0)
+        car->ctrl.steer *= -1;
+    std::cout << "Steering: " << car->ctrl.steer << std::endl;
+}
+
+// This decides over the current speed
+void Driver::handleSpeed() {
+    // This is for abs
+    car->ctrl.brakeCmd = filterABS(getBrake(car));
+    if (car->ctrl.brakeCmd != 0.0) {
+        car->ctrl.accelCmd = 0.0;
+        return;
+    }
+
+    // Try to keep a certain distance
+    float currentPosY = getOpponentDistanceY(opponent[0]);
+    float maxAcc = getMaxAccel(car);
+    car->ctrl.accelCmd = _pidAcc.step(GOAL_POS_Y, currentPosY, maxAcc);
+    std::cout << "Acceleration: " << car->ctrl.accelCmd << std::endl;
+
+    // Check if car is upside down
+    if (angle > M_PI * 0.5) {
+        car->ctrl.accelCmd *= -1;
+    }
+
+    // Check if you need the reversed gear
+    if (car->ctrl.accelCmd < 0) {
+        car->ctrl.accelCmd *= -1;
+        car->ctrl.gear = -1;
+    }
+
+    // Check if you need to break to control the speed
+    if (car->ctrl.accelCmd < 0 && getOpponentDistanceY(opponent[0]) < GOAL_POS_Y) {
+        car->ctrl.gear = -1;
+        car->ctrl.brakeCmd = 1.0;
     }
 }
 
@@ -86,22 +143,15 @@ void Driver::update(tCarElt *car, tSituation *s) {
 float Driver::getOpponentDistanceX(Opponent o) { return o.getDistanceToMiddle() - car->_trkPos.toMiddle; }
 
 float Driver::getOpponentDistanceY(Opponent o) {
-    // float distance = sqrt(pow(o.getDistance(), 2) - pow(getOpponentDistanceX(o), 2));
-    float distance = o.getDistance() - getOpponentDistanceX(o);
-    // if(distance < 1.0){
-        // Otherwise it would just show nan
-        // return 0;
-    // }
-    return distance;
+    float distance = sqrt(pow(o.getDistance(), 2) - pow(getOpponentDistanceX(o), 2));
+    if (std::isnan(distance))
+        distance = 0;
+    return sgn(o.getDistance()) * distance + Y_DIST_TO_MIDDLE;
 }
 
-float Driver::getOpponentSpeedDiffX(Opponent o) {
-    return car->_speed_y - o.getSpeedY();
-}
+float Driver::getOpponentSpeedDiffX(Opponent o) { return car->_speed_y - o.getSpeedY(); }
 
-float Driver::getOpponentSpeedDiffY(Opponent o) {
-    return car->_speed_x - o.getSpeedX();
-}
+float Driver::getOpponentSpeedDiffY(Opponent o) { return car->_speed_x - o.getSpeedX(); }
 
 /* Antilocking filter for brakes */
 float Driver::filterABS(float brake) {
@@ -149,7 +199,7 @@ int Driver::getGear(tCarElt *car) {
 }
 
 /* Compute fitting acceleration */
-float Driver::getAccel(tCarElt *car) {
+float Driver::getMaxAccel(tCarElt *car) {
     float allowedspeed = getAllowedSpeed(car->_trkPos.seg);
     float gr = car->_gearRatio[car->_gear + car->_gearOffset];
     float rm = car->_enginerpmRedLine;

src/drivers/pid_driver/driver.h

@@ -13,10 +13,17 @@
 #include <tgf.h>
 #include <track.h>
 
+#include "pidAcc.h"
+#include "pidSteer.h"
 #include "opponent.h"
 class Opponents;
 class Opponent;
 
+// Needed to extract sgn
+template <typename T> int sgn(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
 class Driver {
   public:
     Driver(int index);
@@ -38,7 +45,10 @@ class Driver {
 
     float getAllowedSpeed(tTrackSeg *segment);
     float getDistToSegEnd(tCarElt *car);
+    float getMaxAccel(tCarElt *car);
     float getAccel(tCarElt *car);
+    void handleSpeed();
+    void handleSteering();
 
     float getBrake(tCarElt *car);
     int getGear(tCarElt *car);
@@ -54,6 +64,9 @@ class Driver {
     float getOpponentSpeedDiffY(Opponent o);
 
   private:
+    PidAcc _pidAcc;
+    PidSteer _pidSteer;
+
     /* utility functions */
     bool isStuck(tCarElt *car);
     void update(tCarElt *car, tSituation *s);

src/drivers/pid_driver/opponent.cpp

@@ -7,6 +7,8 @@ float Opponent::BACKCOLLDIST = 50.0;   /* [m] distance to check for other cars *
 float Opponent::LENGTH_MARGIN = 0.0;   /* [m] safety margin, was 2.0 */
 float Opponent::SIDE_MARGIN = 1.0;     /* [m] safety margin */
 
+
+
 Opponent::Opponent() {}
 
 /* compute speed component parallel to the track */

src/drivers/pid_driver/pidAcc.cpp

@@ -0,0 +1,51 @@
+#include "pidAcc.h"
+
+#include <iostream>
+#include <cmath>
+
+
+PidAcc::PidAcc(){}
+
+PidAcc::PidAcc( float dt, float Kp, float Kd, float Ki ) :
+    _dt(dt),
+    _Kp(Kp),
+    _Kd(Kd),
+    _Ki(Ki),
+    _preError(0),
+    _integral(0)
+{
+}
+
+
+float PidAcc::step( float setpoint, float currentVal, float maxAcc)
+{
+
+    // Calculate error
+    float error = setpoint - currentVal;
+
+    // Proportional term
+    float Pout = _Kp * error;
+
+    // Integral term
+    _integral += error * _dt;
+    float Iout = _Ki * _integral;
+
+    // Derivative term
+    float derivative = (error - _preError) / _dt;
+    float Dout = _Kd * derivative;
+
+    // Calculate total output
+    float output = Pout + Iout + Dout;
+    // std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl;
+
+    // Restrict to max/min
+    if( output > maxAcc )
+        output = maxAcc;
+    else if( output < -1 * maxAcc )
+        output = -1 * maxAcc;
+
+    // Save error to previous error
+    _preError = error;
+
+    return output;
+}

src/drivers/pid_driver/pidAcc.h

@@ -0,0 +1,16 @@
+#ifndef PID_ACC_H
+#define PID_ACC_H
+
+class PidAcc{
+    public:
+        PidAcc();
+        PidAcc(float dt, float Kp, float Kd, float Ki);
+
+        float step(float setPoint, float currentVal, float maxAcc);
+
+    private:
+        float _dt, _Kp, _Kd, _Ki, _preError, _integral;
+};
+
+
+#endif // PID_ACC_H

src/drivers/pid_driver/pidSteer.cpp

@@ -0,0 +1,44 @@
+#include "pidSteer.h"
+
+#include <cmath>
+#include <iostream>
+
+PidSteer::PidSteer() {}
+
+PidSteer::PidSteer(float dt, float Kp, float Kd, float Ki, float G1, float G2, float max)
+    : _dt(dt), _Kp(Kp), _Kd(Kd), _Ki(Ki), _G1(G1), _G2(G2), _preError(0), _integral(0), _max(max) {}
+
+float PidSteer::step(float setpoint1, float setpoint2, float currentVal1, float currentVal2) {
+
+    // Calculate error
+    float error1 = 1 * (setpoint1 - currentVal1);
+    float error2 = 1 * (setpoint2 - currentVal2);
+    // std::cout << "1: " << error1 << ", 2: " << error2 << std::endl;
+    float error = _G1 * error1 + _G2 * error2;
+
+    // Proportional term
+    float Pout = _Kp * error;
+
+    // Integral term
+    _integral += error * _dt;
+    float Iout = _Ki * _integral;
+
+    // Derivative term
+    float derivative = (error - _preError) / _dt;
+    float Dout = _Kd * derivative;
+
+    // Calculate total output
+    float output = Pout + Iout + Dout;
+    // std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl;
+
+    // Restrict to max/min
+    if (output > _max)
+        output = _max;
+    else if (output < -1 * _max)
+        output = -1 * _max;
+
+    // Save error to previous error
+    _preError = error;
+
+    return output;
+}

src/drivers/pid_driver/pidSteer.h

@@ -0,0 +1,17 @@
+#ifndef PID_STEER_H
+#define PID_STEER_H
+
+class PidSteer {
+  public:
+    PidSteer();
+    // PidSteer(float dt, float Kp, float Kd, float Ki, float max);
+    PidSteer(float dt, float Kp, float Kd, float Ki, float G1, float G2, float max);
+
+    // float step(float setPoint, float currentVal);
+    float step(float setpoint1, float setpoint2, float currentVal1, float currentVal2);
+
+  private:
+    float _dt, _Kp, _Kd, _Ki, _G1, _G2, _preError, _integral, _max;
+};
+
+#endif // PID_STEER_H