diff --git a/setup_linux.sh b/setup_linux.sh index baab8da..824b37e 100755 --- a/setup_linux.sh +++ b/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 diff --git a/src/drivers/Makefile b/src/drivers/Makefile index a17cc23..834239b 100644 --- a/src/drivers/Makefile +++ b/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 : torcs@free.fr -# version : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $ +# copyright : (C) 2000 by Eric Espie +# email : torcs@free.fr +# version : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $ # ############################################################################## # diff --git a/src/drivers/pid_driver/driver.cpp b/src/drivers/pid_driver/driver.cpp index a1f91bc..af25807 100644 --- a/src/drivers/pid_driver/driver.cpp +++ b/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; } diff --git a/src/drivers/pid_driver/pidSteer.cpp b/src/drivers/pid_driver/pidSteer.cpp index 13ceb4f..3a36100 100644 --- a/src/drivers/pid_driver/pidSteer.cpp +++ b/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) diff --git a/src/drivers/test_bot/155-DTM.rgb b/src/drivers/test_bot/155-DTM.rgb new file mode 100644 index 0000000..a670020 Binary files /dev/null and b/src/drivers/test_bot/155-DTM.rgb differ diff --git a/src/drivers/test_bot/Makefile b/src/drivers/test_bot/Makefile new file mode 100644 index 0000000..2f89cd6 --- /dev/null +++ b/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} diff --git a/src/drivers/test_bot/TrackData.cpp b/src/drivers/test_bot/TrackData.cpp new file mode 100644 index 0000000..dacddcf --- /dev/null +++ b/src/drivers/test_bot/TrackData.cpp @@ -0,0 +1,103 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * 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 + +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; +} diff --git a/src/drivers/test_bot/TrackData.h b/src/drivers/test_bot/TrackData.h new file mode 100644 index 0000000..042bc47 --- /dev/null +++ b/src/drivers/test_bot/TrackData.h @@ -0,0 +1,203 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * 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. * + * * + ***************************************************************************/ + +#ifndef TRACKDATA_H +#define TRACKDATA_H + +#include +#include +#include +#include +#include +#include + +class Point +{ +protected: + float _length; +public: + float x; + float y; + float z; + + Point() + { + _length = -1.0f; + } + + Point(float x, float y, float z = 0.0) + { + this->x = x; + this->y = y; + this->z = z; + _length = -1.0f; + } + + float Length() + { + if (_length < 0) + { + _length = sqrt(x * x + y * y + z * z); + } + return _length; + } + + void Normalise() + { + float s = 1.0f / Length(); + x *= s; + y *= s; + z *= s; + _length = 1.0f; + } + + Point& operator=(const Point& rhs) + { + x = rhs.x; + y = rhs.y; + z = rhs.z; + return *this; + } + + Point& operator-=(const Point& rhs) + { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + return *this; + } + + Point& operator+=(const Point& rhs) + { + x += rhs.x; + y += rhs.y; + z += rhs.z; + return *this; + } + + Point operator+(const Point& rhs) + { + Point lhs; + lhs.x = x + rhs.x; + lhs.y = y + rhs.y; + lhs.z = z + rhs.z; + return lhs; + } + + Point operator-(const Point& rhs) + { + Point lhs; + lhs.x = x - rhs.x; + lhs.y = y - rhs.y; + lhs.z = z - rhs.z; + return lhs; + } + + Point& operator*=(const float& rhs) + { + x *= rhs; + y *= rhs; + z *= rhs; + return *this; + } + + Point operator*(const float& rhs) + { + Point lhs; + lhs.x = x*rhs; + lhs.y = y*rhs; + lhs.z = z*rhs; + return lhs; + } + + Point& operator/=(const float& rhs) + { + x /= rhs; + y /= rhs; + z /= rhs; + return *this; + } + + Point operator/(const float& rhs) + { + Point lhs; + lhs.x = x / rhs; + lhs.y = y / rhs; + lhs.z = z / rhs; + return lhs; + } + +}; + +class Segment +{ +public: + Point left; + Point right; + + Segment(Point left, Point right) + { + this->left = left; + this->right = right; + } +}; + +class SegmentList +{ +protected: + std::vector segments; +public: + + void Add(Segment segment) + { + segments.push_back(segment); + } + + Segment& operator[](int i) + { + return segments[i]; + } + + int size() + { + return segments.size(); + } + + void PrintSegments() + { + int N = segments.size(); + for (int i = 0; i < N; ++i) + { + printf("%f %f %f %f\n", + segments[i].left.x, segments[i].left.y, + segments[i].right.x, segments[i].right.y); + } + } +}; + +class TrackData +{ + float width_l; + float width_r; + float angle; + float step; + Point mid; +public: + TrackData(); + void setWidth(float width); + void setLeftWidth(float width); + void setRightWidth(float width); + void setStep(float step); + void AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r); + void AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r); +}; + +#endif diff --git a/src/drivers/test_bot/Trajectory.cpp b/src/drivers/test_bot/Trajectory.cpp new file mode 100644 index 0000000..7d635c9 --- /dev/null +++ b/src/drivers/test_bot/Trajectory.cpp @@ -0,0 +1,317 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * 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 +#include +#include +#include +#include +#include +#include "Trajectory.h" +#include + + + +/// Return a point + +Point Trajectory::GetPoint(Segment& s, float w) { + float v = 1.0f - w; + return Point(w * s.left.x + v * s.right.x, + w * s.left.y + v * s.right.y, + w * s.left.z + v * s.right.z); +} + +#define EXP_COST +#undef DBG_OPTIMISE +/// Optimise a track trajectory + +void Trajectory::Optimise(SegmentList track, int max_iter, float alpha, const char* fname, bool reset) { + int N = track.size(); + clock_t start_time = clock(); + int min_iter = max_iter / 2; // minimum number of iterations to do + float time_limit = 2.0f; // if more than min_iter have been done, exit when time elapsed is larger than the time limit + float beta = 0.75f; // amount to reduce alpha to when it seems to be too large + w.resize(N); + dw.resize(N); + dw2.resize(N); + indices.resize(N); + accel.resize(N); + + // initialise vectors + int i; + for (i = 0; i < N; ++i) { + if (reset) { + w[i] = 0.5f; + } + dw2[i] = 1.0f; + indices[i] = i; + } + + + // Shuffle thoroughly +#if 1 + srand(12358); + for (i = 0; i < N - 1; ++i) { + int z = rand() % (N - i); + int tmp = indices[i]; + indices[i] = indices[z + i]; + indices[z + i] = tmp; + } +#endif + + //float prevC = 0.0f; + float Z = 10.0f; + float lambda = 0.9f; + float delta_C = 0.0f; + float prev_dCdw2 = 0.0f; + + for (int iter = 0; iter < max_iter; iter++) { + + float C = 0.0f; + float P = 0.0f; + float dCdw2 = 0.0f; + float EdCdw = 0.0f; + + float direction = 0.0; + for (int j = 0; j < N - 1; ++j) { + int i = indices[j]; //rand()%(N-3) + 3; + int i_p3 = i - 3; + if (i_p3 < 0) i_p3 += N; + int i_p2 = i - 2; + if (i_p2 < 0) i_p2 += N; + int i_p1 = i - 1; + if (i_p1 < 0) i_p1 += N; + //int i_n3 = (i + 3)%N; + int i_n2 = (i + 2) % N; + int i_n1 = (i + 1) % N; + //Segment s_prv3 = track[i_p3]; + //Segment s_prv2 = track[i_p2]; + //Segment s_prv = track[i_p1]; + Segment s_cur = track[i]; + //Segment s_nxt = track[i_n1]; + //Segment s_nxt2 = track[i_n2]; + //Point prv3 = GetPoint(track[i_p3], w[i_p3]); + Point prv2 = GetPoint(track[i_p2], w[i_p2]); + Point prv = GetPoint(track[i_p1], w[i_p1]); + Point cur = GetPoint(track[i], w[i]); + Point nxt = GetPoint(track[i_n1], w[i_n1]); + Point nxt2 = GetPoint(track[i_n2], w[i_n2]); + //Point u_prv2 = prv2 - prv3; + Point u_prv = prv - prv2; + Point u_cur = cur - prv; + Point u_nxt = nxt - cur; + Point u_nxt2 = nxt2 - nxt; + u_prv.Normalise(); + u_cur.Normalise(); + u_nxt.Normalise(); + u_nxt2.Normalise(); + //float l_prv2 = (prv2 - prv3).Length(); + float l_prv = (prv - prv2).Length(); + float l_cur = (cur - prv).Length(); + float l_nxt = (nxt - cur).Length(); +#if 1 + Point a_prv = (u_cur - u_prv) / l_prv; + Point a_cur = (u_nxt - u_cur) / l_cur; + Point a_nxt = (u_nxt2 - u_nxt) / l_nxt; +#else + Point a_prv = (u_prv - u_prv2) / l_prv2; + Point a_cur = (u_cur - u_prv) / l_prv; + Point a_nxt = (u_nxt - u_cur) / l_cur; +#endif + + float current_cost = a_prv.Length() * a_prv.Length() + + a_cur.Length() * a_cur.Length() + + a_nxt.Length() * a_nxt.Length(); + + //accel[i] = +a_nxt.Length(); + accel[i] = (a_prv.Length() + a_cur.Length() + a_nxt.Length()) / 3.0f; + C += current_cost; + + float dCdw = 0.0; + + if (1) { + // Done only for a_cur, ignoring other costs. + { + Point lr = s_cur.left - s_cur.right; + Point d = cur - prv; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_cur.x * a_cur.x + a_cur.y * a_cur.y); + dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += tmp * a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw += a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + { + Point lr = s_cur.left - s_cur.right; + Point d = nxt - cur; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_cur.x * a_cur.x + a_cur.y * a_cur.y); + dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += tmp * a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw += a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + + if (1) { + { + Point lr = s_cur.left - s_cur.right; + Point d = nxt - cur; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_nxt.x * a_nxt.x + a_nxt.y * a_nxt.y); + dCdw -= tmp * a_nxt.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= tmp * a_nxt.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw -= a_nxt.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= a_nxt.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + + if (1) { + { + Point lr = s_cur.left - s_cur.right; + Point d = cur - prv; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_prv.x * a_prv.x + a_prv.y * a_prv.y); + dCdw -= tmp * a_prv.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= tmp * a_prv.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw -= a_prv.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= a_prv.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + float K = 10.0; + float penalty = 0.0; //K*(0.5f - w[i])*(exp(fabs(0.5-w[i]))-1); + if (1) { + float b = 0.1f; + if (w[i] < b) { + penalty += K * (b - w[i]); + } else if (w[i] > 1.0 - b) { + penalty += K * ((1.0 - b) - w[i]); + } + } + P += K * penalty*penalty; + dCdw += K*penalty; + dw2[i] = lambda * dw2[i] + (1.0 - lambda) * dCdw*dCdw; + direction += dCdw * dw[i]; + float delta = dCdw / (dw2[i] + 1.0); + dw[i] = delta; + w[i] += alpha * delta; + + if (1) { + float b = 0.0; + if (w[i] < b) { + w[i] = b; + } else if (w[i] > 1.0 - b) { + w[i] = 1.0 - b; + } + } + + dCdw2 += dCdw*dCdw; + EdCdw += delta / (float) N; + } // indices + + + if (direction < 0) { + alpha *= beta; +#ifdef DBG_OPTIMISE + fprintf(stderr, "# Reducing alpha to %f\n", alpha); +#endif + } + Z = (dCdw2); + if (Z < 0.01) { + Z = 0.01f; + } + + + + bool early_exit = false; + delta_C = 0.9 * delta_C + 0.1 * fabs(EdCdw - prev_dCdw2); + prev_dCdw2 = EdCdw; + + if (delta_C < 0.001f) { + early_exit = true; + } + + if (iter % 100 == 0) { + clock_t current_time = clock(); + float elapsed_time = (float) (current_time - start_time) / (float) CLOCKS_PER_SEC; + if (elapsed_time > time_limit) { + early_exit = true; + } + +#ifdef DBG_OPTIMISE + fprintf(stderr, "%d %f %f %f %f %f %f\n", + iter, + C / (float) N, + P / (float) N, dCdw2, EdCdw, delta_C, elapsed_time); +#endif + } + + if (iter > min_iter && early_exit) { +#ifdef DBG_OPTIMISE + fprintf(stderr, "# Time to break\n"); + fflush(stderr); +#endif + break; + } + //prevC = C; + } + + + +} + + +#if 0 +// example + +int main(int argc, char** argv) { + if (argc != 3) { + fprintf(stderr, "usage: optimise_road iterations learning_rate\n"); + exit(-1); + } + int iter = atoi(argv[1]); + float alpha = atof(argv[2]); + TrackData track_data; + SegmentList track; + + track_data.setStep(10.0f); + + float width_l = 9.0f; + float width_r = 9.0f; + track_data.setWidth(19.0f); + + track_data.AddStraight(track, 50.0, width_l, width_r); //1 + track_data.AddCurve(track, 90.0, 100, width_l, width_r); //2 + + Optimise(track, iter, alpha); + + return 0; +} +#endif diff --git a/src/drivers/test_bot/Trajectory.h b/src/drivers/test_bot/Trajectory.h new file mode 100644 index 0000000..d34f78c --- /dev/null +++ b/src/drivers/test_bot/Trajectory.h @@ -0,0 +1,47 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * 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. * + * * + ***************************************************************************/ + +#ifndef TRAJECTORY_H +#define TRAJECTORY_H + +#include +#include +#include +#include +#include +#include +#include "TrackData.h" + +class Trajectory +{ +public: + std::vector w; ///< parameters + + std::vector dw; ///< parameter steps + + std::vector dw2; ///< parameter gradients + + std::vector accel; ///< maximum acceleration + + std::vector indices; ///< data indices + + static Point GetPoint(Segment& s, float w); + + void Optimise( + SegmentList track, + int max_iter, + float alpha, + const char* fname, + bool reset = true + ); +}; + +#endif diff --git a/src/drivers/test_bot/build_bot b/src/drivers/test_bot/build_bot new file mode 100755 index 0000000..f2e99d4 --- /dev/null +++ b/src/drivers/test_bot/build_bot @@ -0,0 +1,9 @@ +#! /bin/sh + +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib +export TORCS_BASE=/home/michaelheinrich/plib/torcs/torcs-1.3.7 +export MAKE_DEFAULT=$TORCS_BASE/Make-default.mk + +make clean +make +make install diff --git a/src/drivers/test_bot/logo.rgb b/src/drivers/test_bot/logo.rgb new file mode 100644 index 0000000..036f1e2 Binary files /dev/null and b/src/drivers/test_bot/logo.rgb differ diff --git a/src/drivers/test_bot/make_clean b/src/drivers/test_bot/make_clean new file mode 100755 index 0000000..8f60c91 --- /dev/null +++ b/src/drivers/test_bot/make_clean @@ -0,0 +1,7 @@ +#! /bin/sh + +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib +export TORCS_BASE=/home/michaelheinrich/plib/torcs/torcs-1.3.7 +export MAKE_DEFAULT=$TORCS_BASE/Make-default.mk + +make clean diff --git a/src/drivers/test_bot/optimal_line.cpp b/src/drivers/test_bot/optimal_line.cpp new file mode 100644 index 0000000..58ae954 --- /dev/null +++ b/src/drivers/test_bot/optimal_line.cpp @@ -0,0 +1,1091 @@ +#ifdef _WIN32 +#include +#endif + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "optimal_line.h" +#include "Trajectory.h" + +/** + * Get the displaced position of the spot on the line + */ +v2t +LineSegment::pos(float offset) +{ + v2t p = (1 - optimalSpot - offset) * left + (optimalSpot + offset) * right; + return p; +} + +/** + * Get the position of the spot on the line + */ +v2t +LineSegment::pos() +{ + v2t p = (1 - optimalSpot) * left + (optimalSpot) * right; + return p; +} + +/** + * Access segments with wrapping indices. Indices smaller zero and greater + * than the segment count are wrapped around to access neighbors in circular + * track. + */ +LineSegment * +TrackModel::getSegment(int index) +{ + int ind = index; + int size = segments.size(); + if (size == 0) + { + return NULL; + } + + while (ind < 0) + { + ind += size; + } + + + while (ind >= size) + { + ind -= size; + } + + return &segments[ind]; +} + +/** + * + */ +void +TrackModel::initialize(tTrack* track, float maxBreakG, float maxSideG, float suctionParam) +{ + // creal old data first + segments.clear(); + segmentIndices.clear(); + + this->suctionParam = suctionParam; + + // check if we have valid new data + if (track == NULL) + { + return; + } + + std::cout << "optimal_line.cpp: Version 0.07\n"; + + tTrackSeg *first = track->seg; + tTrackSeg *current = first; + + for (int i = 0; i < track->nseg; i++) // limit maximum segments + { + if (current == NULL) + { + break; // error + } + + // initialize LineSegment from track data + + LineSegment seg; + + seg.left.x = current->vertex[0].x; + seg.left.y = current->vertex[0].y; + + seg.right.x = current->vertex[1].x; + seg.right.y = current->vertex[1].y; + + seg.optimalSpot = 0.5; + + seg.type = current->type; + seg.friction = current->surface->kFriction; + + segments.push_back(seg); + + segmentIndices[current] = i; + + /* + std::cout << "optimal_line.h: " + << "i=" << i + << ", lx=" << seg.left.x + << ", ly=" << seg.left.y + << ", rx=" << seg.right.x + << ", ry=" << seg.right.y + << "\n"; + */ + + + // iterate linked list of segments + tTrackSeg *next = current->next; + + current = next; + } + + /* + costOptimization(); + filterLine(); + filterLine(); + filterLine(); + clipLineToLimits(); + //optimizeLine(); + */ + + getTorcsTrajectory(track); + + clipLineToLimits(); + filterLine(); + + // fix problems at finish line + filterFinishLine(); + + + fillRadii(); + + + filterRadii(); + fixFinishRadii(); + fillPointLimits(maxSideG); + propagateRationalLimit(maxBreakG); + + dumpDataToFile("OptimalLineData.csv"); + dumpTrackToFile(track, "TrackGeom.csv"); + + //printRadii(); + return; +} + +const float weightsUnnorm[7] = {1, 1.5, 2, 2.1, 2, 1.5, 1}; +const float weightNorm = 1.0 / (weightsUnnorm[0] + + weightsUnnorm[1] + + weightsUnnorm[2] + + weightsUnnorm[3] + + weightsUnnorm[4] + + weightsUnnorm[5] + + weightsUnnorm[6]); + +const float weights[7] = { + weightsUnnorm[0] * weightNorm, + weightsUnnorm[1] * weightNorm, + weightsUnnorm[2] * weightNorm, + weightsUnnorm[3] * weightNorm, + weightsUnnorm[4] * weightNorm, + weightsUnnorm[5] * weightNorm, + weightsUnnorm[6] * weightNorm +}; + +float +TrackModel::filterSegment(int index) +{ + float vn3 = getSegment(index - 3)->optimalSpot; + float vn2 = getSegment(index - 2)->optimalSpot; + float vn1 = getSegment(index - 1)->optimalSpot; + float v = getSegment(index)->optimalSpot; + float vp1 = getSegment(index + 1)->optimalSpot; + float vp2 = getSegment(index + 2)->optimalSpot; + float vp3 = getSegment(index + 3)->optimalSpot; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + return s; +} + +float +TrackModel::filterSegmentSpatial(int index) +{ + LineSegment *seg = getSegment(index); + + v2t vn3 = getSegment(index - 3)->pos(); + v2t vn2 = getSegment(index - 2)->pos(); + v2t vn1 = getSegment(index - 1)->pos(); + v2t v = seg->pos(); + v2t vp1 = getSegment(index + 1)->pos(); + v2t vp2 = getSegment(index + 2)->pos(); + v2t vp3 = getSegment(index + 3)->pos(); + + v2t p = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + float toL = (seg->left - p).len(); + float toR = (seg->right - p).len(); + + float w = (seg->left - seg->right).len(); + + float r = -(toL * toL - toR * toR - w * w) / (2 * w); + float l = w - r; + + float s = l / w; + + return s; +} + +void TrackModel::filterSegments(int startInd, int stopInd) +{ + int n = stopInd - startInd; + + float tmpOpt[n]; + + for (int i = 0; i < n; i++) + { + tmpOpt[i] = filterSegmentSpatial(i + startInd); + //tmpOpt[i] = filterSegment(i + startInd); + } + + for (int i = 0; i < n; i++) + { + getSegment(i + startInd)->optimalSpot = tmpOpt[i]; + } +} + +/** + * There is a problem with the optimization convergence on the start and finish + * line. We compensate that by overriding all the values on the start and finish + * straight with 0.5 and then filter the coefficients. + */ +void TrackModel::filterFinishLine() +{ + int start = -1; + int end = 1; + + // scan backwards + for (size_t i = 0; i < segments.size(); i++) + { + int index = -i; + LineSegment *seg = getSegment(index); + + if (seg->type != TR_STR) + { + start = i + 1; + break; + } + } + + + // scan forwards + for (size_t i = 0; i < segments.size(); i++) + { + int index = i; + LineSegment *seg = getSegment(index); + + if (seg->type != TR_STR) + { + start = i - 1; + break; + } + } + + for (int i = start + 1; i <= end - 1; i++) + { + getSegment(i)->optimalSpot = .5; + } + + filterSegments(start, end); + filterSegments(start, end); + filterSegments(start, end); + filterSegments(start, end); +} + +void +TrackModel::filterLine() +{ + float tmpOpt[segments.size()]; + + + for (size_t i = 0; i < segments.size(); i++) + { + float vn3 = getSegment(i - 3)->optimalSpot; + float vn2 = getSegment(i - 2)->optimalSpot; + float vn1 = getSegment(i - 1)->optimalSpot; + float v = getSegment(i)->optimalSpot; + float vp1 = getSegment(i + 1)->optimalSpot; + float vp2 = getSegment(i + 2)->optimalSpot; + float vp3 = getSegment(i + 3)->optimalSpot; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + tmpOpt[i] = s; + + } + + for (size_t i = 0; i < segments.size(); i++) + { + getSegment(i)->optimalSpot = tmpOpt[i]; + } +} + +void +TrackModel::filterRadii() +{ + float tmpOpt[segments.size()]; + + float weights[7] = {1, 1.5, 2, 2.1, 2, 1.5, 1}; + + float sum = weights[0] + + weights[1] + + weights[2] + + weights[3] + + weights[4] + + weights[5] + + weights[6]; + + sum = 1.0 / sum; + + weights[0] = weights[0] * sum; + weights[1] = weights[1] * sum; + weights[2] = weights[2] * sum; + weights[3] = weights[3] * sum; + weights[4] = weights[4] * sum; + weights[5] = weights[5] * sum; + weights[6] = weights[6] * sum; + + for (size_t i = 0; i < segments.size(); i++) + { + float vn3 = getSegment(i - 3)->radiusOfCurve; + float vn2 = getSegment(i - 2)->radiusOfCurve; + float vn1 = getSegment(i - 1)->radiusOfCurve; + float v = getSegment(i)->radiusOfCurve; + float vp1 = getSegment(i + 1)->radiusOfCurve; + float vp2 = getSegment(i + 2)->radiusOfCurve; + float vp3 = getSegment(i + 3)->radiusOfCurve; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + tmpOpt[i] = s; + + } + + for (size_t i = 0; i < segments.size(); i++) + { + getSegment(i)->radiusOfCurve = tmpOpt[i]; + } +} + +void +TrackModel::fixFinishRadii() +{ + for (int i = -10; i < 10; i++) + { + float radius = getSegment(i)->radiusOfCurve; + if (radius < 1000) + { + getSegment(i)->radiusOfCurve = 1000; + } + } +} + +void +TrackModel::getTorcsTrajectory(tTrack *track) +{ + + const int N = track->nseg; + + const int iterations = 10 * N; + + float avgSegLen = track->length / N; + + std::cout << "nSegs=" << N << ", its=" << iterations << ", len=" << + track->length << ", avgSegLen=" << avgSegLen << "\n"; + + Trajectory trajectory; + SegmentList segment_list; + + tTrackSeg* seg = track->seg; + seg = track->seg; + + float lengthLimit = 10; + float length = lengthLimit; + + int indices[N]; + int id = 0; + + + for (int i = 0; i < N; i++, seg = seg->next) + { + length += seg->length; + indices[i] = id; + + if (length >= lengthLimit) + { + Point left(seg->vertex[TR_SL].x, seg->vertex[TR_SL].y); + Point right(seg->vertex[TR_SR].x, seg->vertex[TR_SR].y); + segment_list.Add(Segment(left, right)); + id++; + length = 0; + } + + } + + trajectory.Optimise(segment_list, iterations, 0.02f, "/tmp/result"); + + /** + * position in the index array, where the segment started + */ + int segmentStartIndS = 0; + + /** + * segment index of the current position + */ + int lastSegmentIndexT = 0; + + for (int aS = 0; aS < N; aS++) + { + int curIndT = indices[aS]; + int nextSegmentStartS = aS; + + if (curIndT > lastSegmentIndexT) + { + lastSegmentIndexT = curIndT; + segmentStartIndS = aS; + } + for (; nextSegmentStartS <= N; nextSegmentStartS++) + { + int indT = indices[nextSegmentStartS % N]; + if (indT > curIndT) + { + break; + } + } + + float distance = nextSegmentStartS - segmentStartIndS; + + // modulo for negative number + while (distance < 0) + { + distance += N; + } + + float relative = ((float) (aS - segmentStartIndS)) / distance; + + relative = relative > 1 ? 1 : relative; + relative = relative < 0 ? 0 : relative; + + int nextIndT = (curIndT + 1) % N; + + float cW = 1 - trajectory.w[curIndT]; + float nW = 1 - trajectory.w[nextIndT]; + + float weighted = (1.0f - relative) * cW + relative * nW; + + /* + std::cout << "seg#" << std::setw(4) << aS << std::fixed << + ": segmentStartInd=" << segmentStartIndS << + ", nextSegmentIndex=" << nextSegmentStartS << + ", cW=" << cW << + ", nW=" << nW << + ", relative=" << relative << + ", weighted=" << weighted << "\n"; + + */ + + getSegment(aS)->optimalSpot = weighted; + } + + /* + for (int i = 0; i < N; i++, seg = seg->next) + { + getSegment(i)->optimalSpot = 1 - trajectory.w[i]; + } + */ + + //clipLineToLimits(); + //filterLine(); + //filterLine(); +} + +float +TrackModel::getRadius(int i) +{ + // get segment centers at current state. + // out of bounds indexing is handled by getSegment. + + v2t a = getSegment(i - 1)->pos(); + v2t p = getSegment(i)->pos(); + v2t b = getSegment(i + 1)->pos(); + // find perpendicular bisectors to calculate radius + v2t apC = 0.5f * a + 0.5f * p; + v2t pbC = 0.5f * b + 0.5f * p; + v2t apD = p - a; + v2t pbD = b - p; + + /* + -90 degree rotation matrix + 0 -1 + 1 0 + */ + + v2t acD; + acD.x = -apD.y; + acD.y = apD.x; + + v2t bcD; + bcD.x = -pbD.y; + bcD.y = pbD.x; + + straight2t la(apC, acD); + straight2t lb(pbC, bcD); + + v2t center = la.intersect(lb); + + float dx = p.x - center.x; + float dy = p.y - center.y; + + float r = sqrt(dx * dx + dy * dy); + + // check for extremely huge values or bad values + + if (r > 1000000 || isnan(r) || isinf(r)) + { + r = 1000000; + } + + return r; +} + +/** + * Update the radii for all segments + */ +void +TrackModel::fillRadii() +{ + // 0 - look for first low radius + // 1 - look for second low radius + // 2 - look for high radius + int state = 0; + float lastRadius = getRadius(-1); + float suppressRadius = lastRadius; + + for (size_t i = 0; i < segments.size(); i++) + { + float r = getRadius(i); + bool suppress = false; + + switch (state) + { + case 0: + if (r < 0.5 * lastRadius) + { + state = 1; + } + else + { + suppressRadius = r; + } + break; + + case 1: + if (r < 2 * lastRadius) + { + state = 2; + } + else if (r > 2 * lastRadius) + { + getSegment(i - 1)->radiusOfCurve = suppressRadius; + state = 0; + + /* + std::cout << "segment " << (i - 1) << " suppressed from " + << r << " to " << suppressRadius << "\n"; + + */ + } + else + { + state = 0; + suppressRadius = r; + } + break; + + case 2: + if (r > 2 * lastRadius) + { + suppress = true; + } + state = 0; + break; + } + + LineSegment *seg = getSegment(i); + + seg->radiusOfCurve = r; + seg->state = state; + + if (suppress) + { + + std::cout << "segment " << (i - 1) << " suppressed from " + << getSegment(i - 1)->radiusOfCurve << " to " + << suppressRadius << "\n"; + + std::cout << "segment " << (i - 2) << " suppressed from " + << getSegment(i - 2)->radiusOfCurve << " to " + << suppressRadius << "\n"; + + + getSegment(i - 1)->radiusOfCurve = suppressRadius; + getSegment(i - 2)->radiusOfCurve = suppressRadius; + + + suppressRadius = r; + } + + lastRadius = r; + } +} + +void +TrackModel::printRadii() +{ + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float r = seg->radiusOfCurve; + float max = seg->maximumSpeedAtPoint; + float rat = seg->rationalSpeedLimit; + int state = seg->state; + + std::cout << "segment " << std::setw(4) << i << std::fixed << + ": optimalSpot=" << getSegment(i)->optimalSpot << + ", r=" << r << ", maxSpd=" << max << ", ratSpd=" << rat + << ", state=" << state + << "\n"; + } +} + +float +TrackModel::calculateLengthCost() +{ + float lengthCost = 0; + + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *start = getSegment(i); + LineSegment *end = getSegment(i + 1); + + float len = (end->pos() - start->pos()).len(); + + lengthCost += len; + } + + return lengthCost; +} + +float +TrackModel::calculateCost() +{ + /* + float cost = 0; + + for(size_t i = 0; i < segments.size(); i++) + { + float r = getRadius(i); + + cost += 1.0 / (r * r); + } + */ + + return /*cost + */calculateLengthCost(); +} + +float +TrackModel::directedDev(int index) +{ + float oldCost = calculateCost(); + + LineSegment *seg = getSegment(index); + + float old = seg->optimalSpot; + + seg->optimalSpot += 0.001f; + + seg->optimalSpot = seg->optimalSpot > 0.95f ? 0.95f : seg->optimalSpot; + seg->optimalSpot = seg->optimalSpot < 0.05f ? 0.05f : seg->optimalSpot; + + float newCost = calculateCost(); + + seg->optimalSpot = old; + + float dev = (newCost - oldCost) / 0.001f; + + return dev; +} + +void +TrackModel::costOptimizationStep(float amount) +{ + + float tmpOpt[segments.size()]; + + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float grad_i = directedDev(i); + tmpOpt[i] = seg->optimalSpot - amount * grad_i; + } + + for (size_t i = 0; i < segments.size(); i++) + { + segments[i].optimalSpot = tmpOpt[i]; + } + + clipLineToLimits(); +} + +void +TrackModel::costOptimization() +{ + float cost = calculateCost(); + for (int pass = 0; pass < 10; pass++) + { + + costOptimizationStep(0.1); + + float costBefore = cost; + cost = calculateCost(); + float diff = -(cost - costBefore); + + filterLine(); + + std::cout << "optimization pass " << pass << + " cost=" << cost << + ", improvement=" << diff << "\n"; + } +} + +void +TrackModel::clipLineToLimits() +{ + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + + float sp = seg->optimalSpot; + sp = sp > .9 ? .9 : sp; + sp = sp < .1 ? .1 : sp; + + seg->optimalSpot = sp; + } +} + +void +TrackModel::fillPointLimits(float maxG) +{ + float maxA = 9.81 * maxG; + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float radius = seg->radiusOfCurve; + float friction = seg->friction; + + float maxV = sqrt(friction * maxA * radius); + + + float suction = suctionParam * maxV * maxV; + + maxV = sqrt(friction * (maxA + suction) * radius); + + if (maxV < 5) + { + maxV = 5; + } + + seg->maximumSpeedAtPoint = maxV; + seg->rationalSpeedLimit = maxV; + } +} + +void +TrackModel::propagateRationalLimit(float maxG) +{ + float maxA = 9.81 * maxG; + for (size_t i = 0; i < 2 * segments.size(); i++) + { + LineSegment *seg = getSegment(-i); + float plim = seg->rationalSpeedLimit; + + LineSegment *nseg = getSegment(-i + 1); + float neighbourLim = nseg->rationalSpeedLimit; + + + // calculate limit assuming maximimum deceleration + // over straight segment + + float len = (seg->pos() - nseg->pos()).len(); + + float v0 = neighbourLim; + + float friction = seg->friction; + float r = seg->radiusOfCurve; + float suction = suctionParam * v0 * v0; + float aLim = friction * (maxA + suction); + + // maximum acceleration is the sum of lateral + // acceleration and brake acceleration + float aBrake = sqrt(aLim * aLim - ((v0 * v0) / r)); + + float x = -len; + float a0 = -aBrake; + //float a0 = -maxA * seg->friction; + + float v_x = sqrt(v0 * v0 + 2 * a0 * x); + + if (v_x < plim) + { + plim = v_x; + } + + seg->rationalSpeedLimit = plim; + } +} + +float +TrackModel::getOffsetFromCenter(tTrkLocPos *p) +{ + float relPos = 0; + + switch (p->seg->type) + { + case TR_STR: + relPos = p->toStart / p->seg->length; + break; + case TR_LFT: + case TR_RGT: + relPos = p->toStart / p->seg->arc; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + if (startSeg == NULL || endSeg == NULL) + { + return 0; + } + + + + float startWidth = p->seg->startWidth; + float endWidth = p->seg->endWidth; + + float startOpt = startSeg->optimalSpot; + float endOpt = endSeg->optimalSpot; + + float sC = 1 - relPos; + float eC = relPos; + + float width = sC * startWidth + eC * endWidth; + float opt = sC * startOpt + eC * endOpt; + + float off = width * (opt - 0.5); + + return off; + + //return p->seg->width; + //return (startSeg->left - startSeg->right).len(); + //return p->seg->startWidth; +} + +float +TrackModel::getTangentAngle(tTrkLocPos *p) +{ + float baseAngle = 0; + + switch (p->seg->type) + { + case TR_STR: + baseAngle = p->seg->angle[TR_ZS]; + break; + case TR_RGT: + baseAngle = p->seg->angle[TR_ZS] - p->toStart; + break; + case TR_LFT: + baseAngle = p->seg->angle[TR_ZS] + p->toStart; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + float len = (endSeg->pos() - startSeg->pos()).len(); + + float lrStart = startSeg->optimalSpot * (startSeg->right - startSeg->left).len(); + float lrEnd = endSeg->optimalSpot * (endSeg->right - endSeg->left).len(); + + float tanAngle = -(lrEnd - lrStart) / len; + float displAngle = atan(tanAngle); + + return baseAngle + displAngle; +} + +float +TrackModel::getMaximumSpeed(tTrkLocPos *p) +{ + float relPos = 0; + + switch (p->seg->type) + { + case TR_STR: + relPos = p->toStart / p->seg->length; + break; + case TR_LFT: + case TR_RGT: + relPos = p->toStart / p->seg->arc; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + if (startSeg == NULL || endSeg == NULL) + { + return 50; + } + + float startLimit = startSeg->rationalSpeedLimit; + float endLimit = endSeg->rationalSpeedLimit; + + float sC = 1 - relPos; + float eC = relPos; + + float maxV = sC * startLimit + eC * endLimit; + + if (maxV < 5) + { + maxV = 5; + } + + return maxV; +} + +void +TrackModel::dumpTrackToFile(tTrack* track, const char* path) +{ + FILE *f = fopen(path, "w"); + + if (f == 0x00) + { + return; + } + + tTrackSeg *seg = track->seg; + + // memory leak + std::cout << "dump track data to " << canonicalize_file_name(path) << "\n"; + fputs("startLX, startLY, startRX, startRY, endLX, endLY, endRX, endRY\n", f); + + for (int i = 0; i < track->nseg && seg != NULL; i++) + { + t3Dd sl = seg->vertex[TR_SL]; + t3Dd sr = seg->vertex[TR_SR]; + t3Dd el = seg->vertex[TR_EL]; + t3Dd er = seg->vertex[TR_ER]; + + fprintf( + f, + "%f, %f, %f, %f, %f, %f, %f, %f\n", + sl.x, sl.y, sr.x, sr.y, el.x, el.y, er.x, er.y + ); + + seg = seg->next; + } + + fclose(f); +} + +void +TrackModel::dumpDataToFile(const char* path) +{ + FILE *f = fopen(path, "w"); + + if (f == 0x00) + { + return; + } + + // memory leak + std::cout << "dump line data to " << canonicalize_file_name(path) << "\n"; + fputs("x, y, trackPos, pointLim, rationalLim\n", f); + + float trackPos = 0; + for(size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + LineSegment *nseg = getSegment(i + 1); + + v2t pos = seg->pos(); + float segLng = (nseg->pos() - pos).len(); + float x = pos.x; + float y = pos.y; + float pL = seg->maximumSpeedAtPoint; + float rL = seg->rationalSpeedLimit; + + fprintf( + f, + "%f, %f, %f, %f, %f\n", + x, y, trackPos, pL, rL + ); + + trackPos += segLng; + } + + fclose(f); +} + + + + + + + + + + + + + + + + + + + + diff --git a/src/drivers/test_bot/optimal_line.h b/src/drivers/test_bot/optimal_line.h new file mode 100644 index 0000000..c452e1a --- /dev/null +++ b/src/drivers/test_bot/optimal_line.h @@ -0,0 +1,142 @@ +#ifdef _WIN32 +#include +#endif + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Encapsulation of the data we keep for a track segment. + * We think of it as a straight line to get the approximation of + * the optimal line. + */ +class LineSegment +{ +public: + v2t left; // Leftmost point of the track + v2t right; // Rightmost point of the track + + float optimalSpot; // (1 - optimalSpot) * left + optimalSpot * right + // is the position that is currently believed to + // be optimal + + float radiusOfCurve; + float maximumSpeedAtPoint; + float rationalSpeedLimit; + + float friction; + + /** + * TR_RGT 1 + * TR_LFT 2 + * TR_STR 3 + */ + int type; + + int state; + + v2t pos(float offset); + v2t pos(); +}; + +/** + * Minimal model of track on which we will do the optimizations + */ +class TrackModel +{ +public: + std::vector segments; + + std::map segmentIndices; + + void initialize(tTrack* track, float maxBreakG, float maxSideG, float suctionParam); + + void getTorcsTrajectory(tTrack *track); + + float getRadius(int index); + + float calculateLengthCost(); + + float calculateCost(); + + void costOptimizationStep(float amount); + + void costOptimization(); + + float filterSegment(int index); + float filterSegmentSpatial(int index); + + void filterLine(); + void filterFinishLine(); + + void filterSegments(int startInd, int stopInd); + + void filterRadii(); + + void fixFinishRadii(); + + void printRadii(); + + float directedDev(int index); + + void fillRadii(); + + void fillPointLimits(float maxG); + void propagateRationalLimit(float maxG); + + void clipLineToLimits(); + + LineSegment *getSegment(int index); + + float getOffsetFromCenter(tTrkLocPos *p); + + float getTangentAngle(tTrkLocPos *p); + + float getMaximumSpeed(tTrkLocPos *p); + + void dumpDataToFile(const char *path); + void dumpTrackToFile(tTrack* track, const char *path); + +private: + float suctionParam; +}; + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/drivers/test_bot/test_bot.cpp b/src/drivers/test_bot/test_bot.cpp new file mode 100644 index 0000000..7e176b4 --- /dev/null +++ b/src/drivers/test_bot/test_bot.cpp @@ -0,0 +1,577 @@ +/*************************************************************************** + + file : test_bot.cpp + created : Do 2. Nov 08:49:38 CET 2017 + copyright : (C) 2017 Michael Heinrich, Jonas Natzer + + ***************************************************************************/ + +/*************************************************************************** + * * + * 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. * + * * + ***************************************************************************/ + +#ifdef _WIN32 +#include +#endif + + +#include "test_bot.h" + + +static void initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation *s); +static void newrace(int index, tCarElt* car, tSituation *s); +static void drive(int index, tCarElt* car, tSituation *s); +static void endrace(int index, tCarElt *car, tSituation *s); +static void shutdown(int index); +static int InitFuncPt(int index, void *pt); + +static tBot bots[NBBOTS]; + +/* + * Module entry point + */ +extern "C" int +test_bot(tModInfo *modInfo) +{ + memset(modInfo, 0, NBBOTS * sizeof (tModInfo)); + + for (int i = 0; i < NBBOTS; i++) + { + modInfo->name = strdup("Hemic"); /* name of the module (short) */ + modInfo->desc = strdup("2017 TU München research project by " + "Michael Heinrich and Jonas Natzer"); /* description of the module (can be long) */ + modInfo->fctInit = InitFuncPt; /* init function */ + modInfo->gfId = ROB_IDENT; /* supported framework version */ + modInfo->index = i + 1; + } + + return 0; +} + +/* Module interface initialization. */ +static int +InitFuncPt(int index, void *pt) +{ + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return 0; + } + + tRobotItf *itf = (tRobotItf *) pt; + + itf->rbNewTrack = initTrack; /* Give the robot the track view called */ + /* for every track change or new race */ + itf->rbNewRace = newrace; /* Start a new race */ + itf->rbDrive = drive; /* Drive during race */ + itf->rbPitCmd = NULL; + itf->rbEndRace = endrace; /* End of the current race */ + itf->rbShutdown = shutdown; /* Called before the module is unloaded */ + itf->index = index; /* Index used if multiple interfaces */ + return 0; +} + +/* Called for every track change or new race. */ +static void +initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation *s) +{ + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + // track, base, blab, suctionParam + bot->trackModel.initialize( + track, + MAX_BREAK_G, // max Break G + MAX_LATERAL_G, // max lateral G + SUCTION_G_PER_M_SS // additional G per m/s (through suction) + ); + *carParmHandle = NULL; +} + +/* Start a new race. */ +static void +newrace(int index, tCarElt* car, tSituation *s) +{ + std::cout << "test_bot.cpp: Version 0.07\n"; + + + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + + bot->lastManeuver = 0; + bot->lastMoveStep = 0; + bot->currentStep = 0; + bot->remainingBackwardSteps = 0; + bot->hasLaunched = false; + bot->slipDist = 0; + bot->throttlingI = THROTTLING_I_INIT; +} + +/* Drive during race. */ + +/* Drive during race. */ +static void +drive(int index, tCarElt* car, tSituation *s) +{ + memset(&car->ctrl, 0, sizeof (tCarCtrl)); + + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + + float angle; + const float SC = 1.0; + + tCarElt *closestOponent = 0x00; + float closestOpDist = 100; + int ncars = s->raceInfo.ncars; + + tdble cX, cY, oX, oY; + v2t opponentDir; + + RtTrackLocal2Global(&(car->_trkPos), &cX, &cY, 0); + + for (int i = 0; i < ncars; i++) + { + tCarElt *op = s->cars[i]; + + if (op == car) + { + continue; + } + + if (op != 0x00) + { + RtTrackLocal2Global(&(op->_trkPos), &oX, &oY, 1); + v2t dif(oX - cX, oY - cY); + float len = dif.len(); + + + + if (len < closestOpDist) + { + closestOpDist = len; + closestOponent = op; + opponentDir = dif; + } + } + } + + float maneuver = (1 - MANEUVER_INNOVATION) * bot->lastManeuver; + float emergency = 0; + float threatSpeed = 0; + + if (closestOponent != 0x00 && closestOpDist < COLLISION_WARNING_DIST) + { + float ownPos = car->_trkPos.toLeft; + float otherPos = closestOponent->_trkPos.toLeft; + + float diff = ownPos - otherPos; + float dir = diff; + dir = dir >= 0 ? 1 : dir; + dir = dir < 0 ? -1 : dir; + + bool isBehind = car->race.pos < closestOponent->race.pos; + bool isCollisionImmenent = (closestOpDist < COLLISION_IMMINENT_DIST) + && !isBehind; + + float intrusion = dir * (COLLISION_WARNING_DIST - closestOpDist) + / (COLLISION_WARNING_DIST - COLLISION_IMMINENT_DIST); + + // avoid car that intrudes into our comfort radius + maneuver += MANEUVER_INNOVATION * COLLISION_AVOID_GAIN * intrusion; + + float dirX = opponentDir.x; + float dirY = opponentDir.y; + + v2t dirV = opponentDir; + dirV.normalize(); + + v2t dv; + + tPosd vCar = car->pub.DynGCg.vel; + tPosd vOpp = car->pub.DynGCg.vel; + + dv.x = vOpp.x - vCar.x; + dv.y = vOpp.y - vCar.y; + + threatSpeed = -(dv.x * dirV.x + dv.y * dirV.y); + + if (threatSpeed > 0.5 * EMERGENCY_BRAKE_DV) + { + emergency = (2 * (threatSpeed / EMERGENCY_BRAKE_DV)) - 1; + + emergency = emergency > 1 ? 1 : emergency; + emergency = emergency < 0 ? 0 : emergency; + } + + emergency = isCollisionImmenent ? 1.0 : 0.0; + + /* + std::cout << "COLLISION_WARNING: intrusion=" << intrusion << + ", maneuver=" << maneuver << + ", obstructManeuver=" << isBehind + << "\n"; + */ + } + + angle = bot->trackModel.getTangentAngle(&(car->_trkPos)) - car->_yaw; + NORM_PI_PI(angle); // put the angle back in the range from -PI to PI + + float offset = bot->trackModel.getOffsetFromCenter(&(car->_trkPos)) + maneuver; + + // half of width minus security margin + float wh = 0.20 * (car->_trkPos.seg->startWidth + car->_trkPos.seg->endWidth); + + offset = offset > wh ? wh : offset; + offset = offset < -wh ? -wh : offset; + + float correctiveAngle = -(SC * (car->_trkPos.toMiddle + offset)) / car->_trkPos.seg->width; + + float corrLim = 0.2; + + bool offRoad = offset > wh || offset < -wh; + + if (offRoad) + { + corrLim = 1; + } + + + if (correctiveAngle > corrLim) + { + correctiveAngle = corrLim; + } + + if (correctiveAngle < -corrLim) + { + correctiveAngle = -corrLim; + } + + angle += correctiveAngle; + + + float speed = car->pub.speed; + float speedLim = bot->trackModel.getMaximumSpeed(&(car->_trkPos)); + + float relPosition = (car->race.pos - 1) / (float) (ncars - 1); + + float difPos = -(NOMINAL_REL_POSITION - relPosition); + + bot->throttlingI += THROTTLING_I * difPos; + float throttlingP = THROTTLING_P * difPos; + + throttlingP = throttlingP > THROTTLING_P_LIM ? THROTTLING_P_LIM : throttlingP; + throttlingP = throttlingP < -THROTTLING_P_LIM ? -THROTTLING_P_LIM : throttlingP; + + float throttling = throttlingP + bot->throttlingI; + + throttling = throttling > MAX_THROTTLING ? MAX_THROTTLING : throttling; + throttling = throttling < MIN_THROTTLING ? MIN_THROTTLING : throttling; + + speedLim *= throttling; + + + // set up the values to return + car->ctrl.steer = angle / car->_steerLock; + + float dv = speed - speedLim; + + if (dv < -2) + { + car->ctrl.accelCmd = 1.0; // 100% accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < -1) + { + // dv between -2 and -1 + float cmd = -(dv + 1); + + car->ctrl.accelCmd = cmd; // linear accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < 0) + { + // dv between -1 and 0 + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < 1) + { + // dv between 0 and 1 + float cmd = dv; + + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = cmd; // linear brakes + } + else + { + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = 1; // 100% brakes + } + + float maxRpm = car->_enginerpmRedLine * .9f; + float minRpm = maxRpm * .6; + + int gear = car->_gear; + + if (car->_enginerpm > maxRpm) + { + gear++; + } + else if (car->_enginerpm < minRpm) + { + gear--; + } + + if (gear > 6) + { + gear = 6; + } + + if (gear < 1) + { + gear = 1; + } + + float slip = + car->priv.wheel[0].slipAccel + + car->priv.wheel[1].slipAccel + + car->priv.wheel[2].slipAccel + + car->priv.wheel[3].slipAccel; + + slip *= 0.25; + + float skid = car->priv.skid[0] + car->priv.skid[1] + + car->priv.skid[2] + car->priv.skid[3]; + + skid *= 0.25; + + /* + std::cout << + "skid=" << skid << + ", gear=" << gear << + ", slip=" << slip << + "\n"; + */ + + + if (slip < -10) + { + car->ctrl.accelCmd = 0; + bot->slipDist -= slip; + } + else if (slip < -1) + { + float aSlip = -slip; + float rel = (aSlip - 1.0) / (10.0 - 1.0); + // smooth reduction of throttle on starting slip + car->ctrl.accelCmd = rel * 0.0f + (1.0f - rel) * 1.0; + bot->slipDist -= slip; + } + else + { + // AIMD algorithm estimates severity of slip event + bot->slipDist *= 0.9; + } + + if (bot->slipDist > 10) + { + // Disable engine if slip event gets out of control + car->ctrl.accelCmd = 0; + } + + car->ctrl.gear = gear; + + + bool moves = speed > 1; + + if (moves) + { + bot->lastMoveStep = bot->currentStep; + bot->hasLaunched = true; + } + else if (bot->currentStep - bot->lastMoveStep > 20 && bot->hasLaunched) + { + bot->remainingBackwardSteps = UNSTUCKING_STEPS; + } + + if (bot->remainingBackwardSteps > 0) + { + car->ctrl.gear = -1; + car->ctrl.accelCmd = .3; + car->ctrl.brakeCmd = 0; + car->ctrl.steer = -100 * angle / car->_steerLock; + + bot->remainingBackwardSteps--; + } + + if (!bot->hasLaunched) + { + car->ctrl.gear = 1; + car->ctrl.accelCmd = .6; + car->ctrl.brakeCmd = 0; + } + + car->ctrl.accelCmd *= throttling; + + float accCmd = emergency * 0.0 + (1 - emergency) * car->ctrl.accelCmd; + float brakeCmd = (1 - emergency) * car->ctrl.brakeCmd + emergency * 1.0; + + + car->ctrl.accelCmd = accCmd; + car->ctrl.brakeCmd = brakeCmd; + + std::time_t t = std::time(NULL); + std::time_t dt = t - bot->lastDebugOutTime; + + if (true || dt >= 1) + { + bot->lastDebugOutTime = t; + + //std::ofstream logfile; + //logfile.open ("~/test_bot.log", std::ofstream::out | std::ofstream::app); + + std::cout << std::showpos << std::setprecision(3) << std::fixed + << "off=" << std::setw(3) << offset + << std::noshowpos + << ", acc=" << std::setw(3) << car->ctrl.accelCmd + << ", brk=" << std::setw(3) << car->ctrl.brakeCmd + //<< ", spd=" << std::setw(3) << speed + << ", spdLim=" << std::setw(3) << speedLim + << ", thr=" << std::setw(3) << throttling + << ", thrP=" << std::setw(3) << throttlingP + << ", thrI=" << std::setw(3) << bot->throttlingI + << ", relPos=" << std::setw(3) << relPosition + << ", threatSpd=" << std::setw(3) << threatSpeed + << ", emergency=" << std::setw(3) << emergency + + //<< ", friction=" << car->_trkPos.seg->surface->kFriction + << "\n"; + + + bot->lastManeuver = maneuver; + + + // statistic capture + + float ownCt = car->race.curTime; + float othCt = 0; + float othBL = 0; + + bot->otherTime = 0; + + for (int i = 0; i < s->raceInfo.ncars; i++) + { + tCarElt *c = s->cars[i]; + + if (c == car) + { + continue; + } + + othCt = c->race.curTime; + othBL = c->race.timeBehindLeader; + } + + if(ownCt == 0) + { + bot->otherTime = othCt; + bot->ownTime = othCt + car->race.timeBehindLeader; + } + else + { + bot->ownTime = ownCt; + bot->otherTime = ownCt + othBL; + } + + //logfile.close(); + } + + + bot->currentStep++; + /* + car->ctrl.accelCmd = 0.3; // 30% accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + */ +} + +inline bool exists_file(const std::string& name) +{ + if (FILE * file = fopen(name.c_str(), "r")) + { + fclose(file); + return true; + } + else + { + return false; + } +} + +/* End of the current race */ +static void +endrace(int index, tCarElt *car, tSituation *s) +{ + std::cout << "endrace called\n"; + +} + +/* Called before the module is unloaded */ +static void +shutdown(int index) +{ + std::cout << "shutdown called\n"; + + std::ostringstream oss; + + oss << "stats_lim_" << NOMINAL_REL_POSITION << ".csv"; + + std::string path = oss.str(); + + bool existed = exists_file(path); + + FILE *f = fopen(path.c_str(), "a"); + + if (f == 0x00) + { + std::cout << "Couldn't open stats file " << path << "\n"; + return; + } + + if (!existed) + { + fputs("ownTime, otherTime\n", f); + } + + + + fprintf( + f, + "%f, %f\n", + bots[0].ownTime, bots[0].otherTime + ); + + fclose(f); +} + + + diff --git a/src/drivers/test_bot/test_bot.def b/src/drivers/test_bot/test_bot.def new file mode 100644 index 0000000..3852ca2 --- /dev/null +++ b/src/drivers/test_bot/test_bot.def @@ -0,0 +1,19 @@ +; +; file : test_bot.def +; 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. +; + +LIBRARY test_bot + +SECTIONS .data READ WRITE + +EXPORTS + test_bot + diff --git a/src/drivers/test_bot/test_bot.dsp b/src/drivers/test_bot/test_bot.dsp new file mode 100644 index 0000000..1c12aa2 --- /dev/null +++ b/src/drivers/test_bot/test_bot.dsp @@ -0,0 +1,238 @@ +# Microsoft Developer Studio Project File - Name="test_bot" - Package Owner=<4> + +# Microsoft Developer Studio Generated Build File, Format Version 6.00 + +# ** DO NOT EDIT ** + + + +# TARGTYPE "Win32 (x86) Dynamic-Link Library" 0x0102 + + + +CFG=test_bot - Win32 Debug + +!MESSAGE This is not a valid makefile. To build this project using NMAKE, + +!MESSAGE use the Export Makefile command and run + +!MESSAGE + +!MESSAGE NMAKE /f "test_bot.mak". + +!MESSAGE + +!MESSAGE You can specify a configuration when running NMAKE + +!MESSAGE by defining the macro CFG on the command line. For example: + +!MESSAGE + +!MESSAGE NMAKE /f "test_bot.mak" CFG="test_bot - Win32 Debug" + +!MESSAGE + +!MESSAGE Possible choices for configuration are: + +!MESSAGE + +!MESSAGE "test_bot - Win32 Release" (based on "Win32 (x86) Dynamic-Link Library") + +!MESSAGE "test_bot - Win32 Debug" (based on "Win32 (x86) Dynamic-Link Library") + +!MESSAGE + + + +# Begin Project + +# PROP AllowPerConfigDependencies 0 + +# PROP Scc_ProjName "" + +# PROP Scc_LocalPath "" + +CPP=cl.exe + +MTL=midl.exe + +RSC=rc.exe + + + +!IF "$(CFG)" == "test_bot - Win32 Release" + + + +# PROP BASE Use_MFC 0 + +# PROP BASE Use_Debug_Libraries 0 + +# PROP BASE Output_Dir "Release" + +# PROP BASE Intermediate_Dir "Release" + +# PROP BASE Target_Dir "" + +# PROP Use_MFC 0 + +# PROP Use_Debug_Libraries 0 + +# PROP Output_Dir "Release" + +# PROP Intermediate_Dir "Release" + +# PROP Ignore_Export_Lib 1 + +# PROP Target_Dir "" + +# ADD BASE CPP /nologo /MT /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /c + +# ADD CPP /nologo /G5 /W3 /GX /O2 /I "../../../export/include" /I "../../windows/include" /D "WIN32" /D "NDEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /c + +# ADD BASE MTL /nologo /D "NDEBUG" /mktyplib203 /win32 + +# ADD MTL /nologo /D "NDEBUG" /mktyplib203 /win32 + +# ADD BASE RSC /l 0x40c /d "NDEBUG" + +# ADD RSC /l 0x40c /d "NDEBUG" + +BSC32=bscmake.exe + +# ADD BASE BSC32 /nologo + +# ADD BSC32 /nologo + +LINK32=link.exe + +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /dll /machine:I386 + +# ADD LINK32 tgf.lib robottools.lib sg.lib ul.lib /nologo /dll /map /machine:I386 /nodefaultlib:"LIBCD" /libpath:"../../../export/lib" /libpath:"../../windows/lib" + +# Begin Special Build Tool + +WkspDir=. + +TargetDir=.\Release + +SOURCE="$(InputPath)" + +PostBuild_Cmds=copy $(TargetDir)\*.dll $(WkspDir)\runtime\drivers\test_bot + +# End Special Build Tool + + + +!ELSEIF "$(CFG)" == "test_bot - Win32 Debug" + + + +# PROP BASE Use_MFC 0 + +# PROP BASE Use_Debug_Libraries 1 + +# PROP BASE Output_Dir "Debug" + +# PROP BASE Intermediate_Dir "Debug" + +# PROP BASE Target_Dir "" + +# PROP Use_MFC 0 + +# PROP Use_Debug_Libraries 1 + +# PROP Output_Dir "Debug" + +# PROP Intermediate_Dir "Debug" + +# PROP Ignore_Export_Lib 1 + +# PROP Target_Dir "" + +# ADD BASE CPP /nologo /MTd /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /GZ /c + +# ADD CPP /nologo /G5 /W3 /Gm /GX /ZI /Od /I "../../../export/include" /I "../../windows/include" /D "WIN32" /D "_DEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /GZ /c + +# ADD BASE MTL /nologo /D "_DEBUG" /mktyplib203 /win32 + +# ADD MTL /nologo /D "_DEBUG" /mktyplib203 /win32 + +# ADD BASE RSC /l 0x40c /d "_DEBUG" + +# ADD RSC /l 0x40c /d "_DEBUG" + +BSC32=bscmake.exe + +# ADD BASE BSC32 /nologo + +# ADD BSC32 /nologo + +LINK32=link.exe + +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /dll /debug /machine:I386 /pdbtype:sept + +# ADD LINK32 robottools.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib sg.lib ul.lib /nologo /dll /map /debug /machine:I386 /pdbtype:sept /libpath:"../../../export/libd" /libpath:"../../windows/lib" + +# Begin Special Build Tool + +WkspDir=. + +TargetDir=.\Debug + +SOURCE="$(InputPath)" + +PostBuild_Cmds=copy $(TargetDir)\*.dll $(WkspDir)\runtimed\drivers\test_bot + +# End Special Build Tool + + + +!ENDIF + + + +# Begin Target + + + +# Name "test_bot - Win32 Release" + +# Name "test_bot - Win32 Debug" + +# Begin Group "Source Files" + + + +# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" + +# Begin Source File + + + +SOURCE=.\test_bot.cpp + +# End Source File + +# Begin Source File + + + +SOURCE=.\test_bot.def + +# End Source File + +# End Group + +# Begin Group "Header Files" + + + +# PROP Default_Filter "h;hpp;hxx;hm;inl" + +# End Group + +# End Target + +# End Project + diff --git a/src/drivers/test_bot/test_bot.h b/src/drivers/test_bot/test_bot.h new file mode 100644 index 0000000..5b2edd6 --- /dev/null +++ b/src/drivers/test_bot/test_bot.h @@ -0,0 +1,91 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: test_bot.h + * Author: michaelheinrich + * + * Created on 26. November 2017, 23:30 + */ + +#ifndef TEST_BOT_H +#define TEST_BOT_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "optimal_line.h" + + +#define MAX_BREAK_G 1.60 +#define MAX_LATERAL_G 1.60 +#define SUCTION_G_PER_M_SS (0.5 / 1000.0) + + +#define COLLISION_WARNING_DIST 10 +#define COLLISION_AVOID_GAIN 5 + +/** + * If the closest car is closer than COLLISION_IMMINENT_DIST and + * has a lower race position, issue a brake maneuver + * + */ +#define COLLISION_IMMINENT_DIST 5 +#define EMERGENCY_BRAKE_DV 1 + +#define UNSTUCKING_STEPS 90 +#define MANEUVER_INNOVATION 0.1 + +#define THROTTLING_I_INIT 0.8 +#define THROTTLING_P 1.0 +#define THROTTLING_I 0.0003 + +#define THROTTLING_P_LIM 0.1 + +#define MAX_THROTTLING 1.0 +#define MIN_THROTTLING 0.3 +//#define NOMINAL_REL_POSITION 0.5 +#define NOMINAL_REL_POSITION 0.5 + +#define NBBOTS 1 + +typedef struct Bot +{ + TrackModel trackModel; + float lastManeuver; + std::time_t lastDebugOutTime; + int lastMoveStep; + int currentStep; + int remainingBackwardSteps; + bool hasLaunched; + + float throttlingI; + float slipDist; + + float ownTime; + float otherTime; + +} tBot; + +#endif /* TEST_BOT_H */ + diff --git a/src/drivers/test_bot/test_bot.xml b/src/drivers/test_bot/test_bot.xml new file mode 100644 index 0000000..849c7e0 --- /dev/null +++ b/src/drivers/test_bot/test_bot.xml @@ -0,0 +1,35 @@ + + + + + + + + + + +
+
+
+ + + + + + + + + + + + +
+
+
+ diff --git a/src/libs/txml/gennmtab/gennmtab b/src/libs/txml/gennmtab/gennmtab new file mode 100755 index 0000000..caf9ab1 Binary files /dev/null and b/src/libs/txml/gennmtab/gennmtab differ