Skip to content

Commit

Permalink
fix minor issues
Browse files Browse the repository at this point in the history
  • Loading branch information
CodingJonas committed Dec 21, 2017
1 parent feea8e5 commit 77dca78
Show file tree
Hide file tree
Showing 3 changed files with 124 additions and 35 deletions.
7 changes: 4 additions & 3 deletions make_install_run.sh
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,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
# cp ~/torcs/torcs-1.3.7/standard_settings/quickrace.xml ~/.torcs/config/raceman/quickrace.xml
cp ~/torcs/torcs-1.3.7/standard_settings/quickrace_ros.xml ~/.torcs/config/raceman/quickrace.xml
cp ~/torcs/torcs-1.3.7/standard_settings/screen.xml ~/.torcs/config/screen.xml
cp ~/torcs/torcs-1.3.7/standard_settings/sound.xml ~/.torcs/config/sound.xml
torcs
66 changes: 34 additions & 32 deletions src/drivers/pid_driver/driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,8 @@ const float Driver::ABS_SLIP = 0.9; /* [-] range [0.95..0
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_X = -1;
const float MIN_DIST_X = 4;
const float GOAL_POS_X = 0;
const float MIN_DIST = 4;

Driver::Driver(int index) {
float dt = 0.02;
Expand Down Expand Up @@ -60,30 +60,23 @@ void Driver::newRace(tCarElt *car, tSituation *s) {
/* Drive during race. */
void Driver::drive(tCarElt *car, tSituation *s) {
update(car, s);
std::cout << "Other cars ----" << std::endl;
// std::cout << "Other cars ----" << std::endl;
for (int i = 0; i < opponents->getNOpponents(); i++) {
std::cout << "State " << i << ": " << opponent[i].getState() << 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 << "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 << "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:" << angle << 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:" << angle << std::endl;

memset(&car->ctrl, 0, sizeof(tCarCtrl));

// float steerangle = angle - car->_trkPos.toMiddle / car->_trkPos.seg->width;
// car->ctrl.steer = steerangle / car->_steerLock;
car->ctrl.gear = getGear(car);
handleSpeed();
handleSteering();
Expand All @@ -92,32 +85,33 @@ void Driver::drive(tCarElt *car, tSituation *s) {
void Driver::handleSteering() {
float currentPosX = getOpponentDistanceX(opponent[0]);
float goalX = getGoalPosX();
std::cout << "goalX: " << goalX << std::endl;
// std::cout << "goalX: " << goalX << std::endl;
car->ctrl.steer = _pidSteer.step(goalX, 0.0, currentPosX, angle);
if (getSpeed() < 0)
car->ctrl.steer *= -1;
std::cout << "Steering: " << car->ctrl.steer << std::endl;
// std::cout << "Steering: " << car->ctrl.steer << std::endl;
}

float Driver::getGoalPosX() {
float dist = GOAL_POS_X;
// Only add a safety margin during overtake, if the goal position is closer than the safety distance
if (std::abs(GOAL_POS_X) < MIN_DIST_X) {
dist += sgn(GOAL_POS_X) * MIN_DIST_X *
std::pow((MIN_DIST_X + Y_DIST_TO_MIDDLE) / getOpponentDistanceY(opponent[0]), 2);
if (std::abs(GOAL_POS_X) < MIN_DIST) {
dist += sgn(GOAL_POS_X) * MIN_DIST *
std::pow((MIN_DIST + Y_DIST_TO_MIDDLE) / getOpponentDistanceY(opponent[0]), 2);

// Check if the distance becomes too large
if (std::abs(dist) > std::abs(MIN_DIST_X)) {
dist = sgn(GOAL_POS_X) * MIN_DIST_X;
if (std::abs(dist) > std::abs(MIN_DIST)) {
dist = sgn(GOAL_POS_X) * MIN_DIST;
}
}
// Always overtake on the side of the front car, that directs towards the center of the street
// if (car->_trkPos.toLeft < car->_trkPos.toRight &&
// std::abs(getOpponentSpeedDiffY(opponent[0])) < std::abs(GOAL_POS_Y) * 0.75) {
// dist *= sgn(dist);
// } else {
// dist *= -1 * sgn(dist);
// }
if (getOpponentDistanceX(opponent[0]) + car->_trkPos.toMiddle > 0) {
dist *= sgn(dist);
// std::cout << "overtake right" << std::endl;
} else {
dist *= -1 * sgn(dist);
// std::cout << "overtake left" << std::endl;
}
return dist;
}

Expand All @@ -134,7 +128,7 @@ void Driver::handleSpeed() {
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;
// std::cout << "Acceleration: " << car->ctrl.accelCmd << std::endl;

// Check if car is upside down
if (std::abs(angle) > M_PI * 0.5) {
Expand All @@ -152,6 +146,14 @@ void Driver::handleSpeed() {
car->ctrl.gear = -1;
car->ctrl.brakeCmd = 1.0;
}

// If you are too close to the front car and centered behind it, brake
if (getOpponentDistanceY(opponent[0]) > 0 && getOpponentDistanceY(opponent[0]) < Y_DIST_TO_MIDDLE * 1.25 &&
std::abs(getOpponentDistanceX(opponent[0])) < 1) {
// std::cout << "BREAK" << std::endl;
car->ctrl.accelCmd = 0.0;
car->ctrl.brakeCmd = 1.0;
}
}

/* Update my private data every timestep */
Expand Down
86 changes: 86 additions & 0 deletions standard_settings/quickrace_ros.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE params SYSTEM "params.dtd">


<params name="Quick Race">
<section name="Header">
<attstr name="name" val="Quick Race"/>
<attstr name="description" val="Quick Race"/>
<attnum name="priority" val="10"/>
<attstr name="menu image" val="data/img/splash-qr.png"/>
</section>

<section name="Tracks">
<attnum name="maximum number" val="1"/>
<section name="1">
<attstr name="name" val="forza"/>
<attstr name="category" val="road"/>
</section>

</section>

<section name="Races">
<section name="1">
<attstr name="name" val="Quick Race"/>
</section>

</section>

<section name="Quick Race">
<attnum name="distance" unit="km" val="0"/>
<attstr name="type" val="race"/>
<attstr name="starting order" val="drivers list"/>
<attstr name="restart" val="yes"/>
<attnum name="laps" val="3"/>
<section name="Starting Grid">
<attnum name="rows" val="2"/>
<attnum name="distance to start" val="25"/>
<attnum name="distance between columns" val="20"/>
<attnum name="offset within a column" val="10"/>
<attnum name="initial speed" val="0"/>
<attnum name="initial height" val="0.2"/>
</section>

</section>

<section name="Drivers">
<attnum name="maximum number" val="40"/>
<attnum name="focused idx" val="0"/>
<attstr name="focused module" val="scr_server"/>
<section name="1">
<attnum name="idx" val="0"/>
<attstr name="module" val="scr_server"/>
</section>

<section name="2">
<attnum name="idx" val="1"/>
<attstr name="module" val="pid_driver"/>
</section>

</section>

<section name="Configuration">
<attnum name="current configuration" val="4"/>
<section name="1">
<attstr name="type" val="track select"/>
</section>

<section name="2">
<attstr name="type" val="drivers select"/>
</section>

<section name="3">
<attstr name="type" val="race config"/>
<attstr name="race" val="Quick Race"/>
<section name="Options">
<section name="1">
<attstr name="type" val="race length"/>
</section>

</section>

</section>

</section>

</params>

0 comments on commit 77dca78

Please sign in to comment.