improved values of pid controllers

setup_linux.sh

@@ -1,7 +1,7 @@
 #! /bin/bash
 
 [ -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/drivers/Makefile

@@ -2,9 +2,9 @@
 #
 #    file                 : Makefile
 #    created              : Sat Mar 18 23:08:21 CET 2000
-#    copyright            : (C) 2000 by Eric Espie                         
-#    email                : [email protected]   
-#    version              : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $                                  
+#    copyright            : (C) 2000 by Eric Espie
+#    email                : [email protected]
+#    version              : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $
 #
 ##############################################################################
 #

src/drivers/pid_driver/driver.cpp

@@ -17,20 +17,20 @@ 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] */
 const float Y_DIST_TO_MIDDLE = 5.0;
-const float GOAL_POS_Y = 20;
+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;
+    float Kp = -0.3;
+    float Ki = -0.3;
+    float Kd = -0.001;
     _pidAcc = PidAcc(dt, Kp, Ki, Kd);
-    Kp = -0.3;
+    Kp = -0.5;
     Ki = -0.1;
     Kd = -0.005;
-    float G1 = 0.2;
-    float G2 = 0.8;
+    float G1 = 0.3;
+    float G2 = 0.7;
     _pidSteer = PidSteer(dt, Kp, Ki, Kd, G1, G2, 12);
     INDEX = index;
 }
@@ -77,7 +77,7 @@ void Driver::drive(tCarElt *car, tSituation *s) {
     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;
+    std::cout << "Angle:" << angle << std::endl;
 
     memset(&car->ctrl, 0, sizeof(tCarCtrl));
 
@@ -112,7 +112,7 @@ void Driver::handleSpeed() {
     std::cout << "Acceleration: " << car->ctrl.accelCmd << std::endl;
 
     // Check if car is upside down
-    if (angle > M_PI * 0.5) {
+    if (std::abs(angle) > M_PI * 0.5) {
         car->ctrl.accelCmd *= -1;
     }
 

src/drivers/pid_driver/pidSteer.cpp

@@ -29,7 +29,7 @@ float PidSteer::step(float setpoint1, float setpoint2, float currentVal1, float
 
     // Calculate total output
     float output = Pout + Iout + Dout;
-    // std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl;
+    std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl;
 
     // Restrict to max/min
     if (output > _max)

src/drivers/test_bot/Makefile

@@ -0,0 +1,29 @@
+##############################################################################
+#
+#    file                 : Makefile
+#    created              : Do 2. Nov 08:49:38 CET 2017
+#    copyright            : (C) 2002 Jonas Natzer, Michael Heinrich
+#
+##############################################################################
+#
+#   This program is free software; you can redistribute it and/or modify
+#   it under the terms of the GNU General Public License as published by
+#   the Free Software Foundation; either version 2 of the License, or
+#   (at your option) any later version.
+#
+##############################################################################
+
+ROBOT       = test_bot
+MODULE      = ${ROBOT}.so
+MODULEDIR   = drivers/${ROBOT}
+SOURCES     = ${ROBOT}.cpp optimal_line.cpp Trajectory.cpp TrackData.cpp
+
+SHIPDIR     = drivers/${ROBOT}
+SHIP        = ${ROBOT}.xml 155-DTM.rgb logo.rgb
+SHIPSUBDIRS =
+
+PKGSUBDIRS  = ${SHIPSUBDIRS}
+src-robots-test_bot_PKGFILES = $(shell find * -maxdepth 0 -type f -print)
+src-robots-test_bot_PKGDIR   = ${PACKAGE}-${VERSION}/$(subst ${TORCS_BASE},,$(shell pwd))
+
+include ${MAKE_DEFAULT}

src/drivers/test_bot/TrackData.cpp

@@ -0,0 +1,103 @@
+// -*- Mode: c++ -*-
+// copyright (c) 2006 by Christos Dimitrakakis <[email protected]>
+/***************************************************************************
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ ***************************************************************************/
+#include "TrackData.h"
+#include <tgf.h>
+
+TrackData::TrackData()
+{
+    mid.x = mid.y = mid.z = 0.0f;
+    width_l = width_r = 10.0;
+    angle = 0.0;
+    step = 5.0;
+}
+
+void TrackData::setWidth(float width)
+{
+    width_l = width_r = width / 2.0f;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setLeftWidth(float width)
+{
+    width_l = width;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setRightWidth(float width)
+{
+    width_r = width;
+    assert(width_r > -width_l);
+}
+
+void TrackData::setStep(float step)
+{
+    assert(step > 0.0f);
+    this->step = step;
+}
+
+void TrackData::AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r)
+{
+    int N = 1 + (int) floor(length / step);
+    float s = length / (float) N;
+    float d_width_l = (end_width_l - width_l) / (float) N;
+    float d_width_r = (end_width_r - width_r) / (float) N;
+    float hpi = PI / 2.0f;
+    for (int i = 0; i < N; ++i)
+    {
+        mid.x += s * sin(angle);
+        mid.y += s * cos(angle);
+        Point left(mid.x + width_l * sin(angle - hpi),
+                   mid.y + width_l * cos(angle - hpi),
+                   mid.z);
+        Point right(mid.x + width_r * sin(angle + hpi),
+                    mid.y + width_r * cos(angle + hpi),
+                    mid.z);
+        segments.Add(Segment(left, right));
+        width_l += d_width_l;
+        width_r += d_width_r;
+    }
+
+    width_l = end_width_l;
+    width_r = end_width_r;
+}
+
+/// arc in radians
+
+void TrackData::AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r)
+{
+    arc = arc * PI / 180.0f;
+    float length = fabs(arc) * radius;
+    int N = 1 + (int) floor(length / step);
+    float s = length / (float) N;
+    float d_width_l = (end_width_l - width_l) / (float) N;
+    float d_width_r = (end_width_r - width_r) / (float) N;
+    float d_angle = arc / (float) N;
+    float start_angle = angle;
+    float hpi = (float) (PI / 2.0);
+    for (int i = 0; i < N; ++i)
+    {
+        mid.x += s * sin(angle);
+        mid.y += s * cos(angle);
+        Point left(mid.x + width_l * sin(angle - hpi),
+                   mid.y + width_l * cos(angle - hpi),
+                   mid.z);
+        Point right(mid.x + width_r * sin(angle + hpi),
+                    mid.y + width_r * cos(angle + hpi),
+                    mid.z);
+        segments.Add(Segment(left, right));
+        angle += d_angle;
+        width_l += d_width_l;
+        width_r += d_width_r;
+    }
+    width_l = end_width_l;
+    width_r = end_width_r;
+    angle = start_angle + arc;
+}