From f6b6c57e4e797038cebe8122bb2e3454789efbb6 Mon Sep 17 00:00:00 2001 From: Jonas Natzer Date: Wed, 24 Jan 2018 11:30:57 +0100 Subject: [PATCH] improve documentation --- src/drivers/pid_driver/driver.cpp | 40 +++++++++++++++-------------- src/drivers/pid_driver/driver.h | 10 ++++++++ src/drivers/pid_driver/pidAcc.h | 1 + src/drivers/pid_driver/pidSteer.cpp | 1 - src/drivers/pid_driver/pidSteer.h | 4 +-- 5 files changed, 34 insertions(+), 22 deletions(-) diff --git a/src/drivers/pid_driver/driver.cpp b/src/drivers/pid_driver/driver.cpp index dd0bb2e..da3d457 100644 --- a/src/drivers/pid_driver/driver.cpp +++ b/src/drivers/pid_driver/driver.cpp @@ -16,17 +16,20 @@ 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] */ -const float Y_DIST_TO_MIDDLE = 5.0; -const float GOAL_POS_Y = -20; -const float GOAL_POS_X = 0; -const float MIN_DIST = 4; +const float Y_DIST_TO_MIDDLE = 5.0; // Input distance is to the front of the car, this value adjusts the + // distance to the middle of the reference car +const float GOAL_POS_Y = -20; // Goal distance to the reference car in y direction +const float GOAL_POS_X = 0; // Goal distance to the reference car in x direction +const float MIN_DIST = 4; // Safety margin the car needs to have in x direction during overtakes Driver::Driver(int index) { + // Values for the PID controller for acceleration float dt = 0.02; float Kp = -0.3; float Kd = -0.2; float Ki = -0.001; _pidAcc = PidAcc(dt, Kp, Kd, Ki); + // Values for the PID controller for steering Kp = -0.1; Kd = -1.0; Ki = -0.005; @@ -62,12 +65,13 @@ void Driver::drive(tCarElt *car, tSituation *s) { update(car, s); // std::cout << "Other cars ----" << std::endl; // for (int i = 0; i < opponents->getNOpponents(); i++) { - // 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 << getOpponentDistanceX(opponent[i]) << ", " << getOpponentDistanceY(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 << getOpponentDistanceX(opponent[i]) << ", " << getOpponentDistanceY(opponent[i]) << + // std::endl; // } // std::cout << "AI car ----" << std::endl; @@ -83,16 +87,17 @@ void Driver::drive(tCarElt *car, tSituation *s) { handleSteering(); } +// Compute the current steering handle void Driver::handleSteering() { float currentPosX = getOpponentDistanceX(opponent[0]); float goalX = getGoalPosX(); - // 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; } +// Compute current goal position, usually this should be GOAL_POS_X, but when the driver comes to close to the +// reference car, it adds a safety margin 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 @@ -108,15 +113,13 @@ float Driver::getGoalPosX() { // Always overtake on the side of the front car, that directs towards the center of the street 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; } -// This decides over the current speed +// Handle different situations and adjust speed to each void Driver::handleSpeed() { // This is for abs car->ctrl.brakeCmd = filterABS(getBrake(car)); @@ -129,9 +132,8 @@ 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; - // Check if car is upside down + // Check if car is facing backwards if (std::abs(angle) > M_PI * 0.5) { car->ctrl.accelCmd *= -1; } @@ -149,9 +151,9 @@ void Driver::handleSpeed() { } // 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 && + 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; } diff --git a/src/drivers/pid_driver/driver.h b/src/drivers/pid_driver/driver.h index 6d9fac1..bdc7007 100644 --- a/src/drivers/pid_driver/driver.h +++ b/src/drivers/pid_driver/driver.h @@ -44,12 +44,19 @@ class Driver { tTrack *getTrackPtr() { return track; } float getSpeed() { return speed; } + // Each track segment allows a different maximum speed, e.g. curves float getAllowedSpeed(tTrackSeg *segment); + // Get Distance to the end of the current segment. Needed to evaluate driver float getDistToSegEnd(tCarElt *car); + // Get the maximal allowed acceleration for the current situation float getMaxAccel(tCarElt *car); + // Wrapper function for the acceleration PID controller float getAccel(tCarElt *car); + // Set best acceleration for the current situation void handleSpeed(); + // Set steering for the current situation void handleSteering(); + // Calculates an updated x distance to the reference car including a safety margin float getGoalPosX(); float getBrake(tCarElt *car); @@ -60,13 +67,16 @@ class Driver { void initCa(); void initCw(); + // Helper functions for distance and speed measurements float getOpponentDistanceX(Opponent o); float getOpponentDistanceY(Opponent o); float getOpponentSpeedDiffX(Opponent o); float getOpponentSpeedDiffY(Opponent o); private: + // The PID controller for acceleration PidAcc _pidAcc; + // The PID controller for steering PidSteer _pidSteer; /* utility functions */ diff --git a/src/drivers/pid_driver/pidAcc.h b/src/drivers/pid_driver/pidAcc.h index bc5f9d8..caa0b7b 100644 --- a/src/drivers/pid_driver/pidAcc.h +++ b/src/drivers/pid_driver/pidAcc.h @@ -6,6 +6,7 @@ class PidAcc{ PidAcc(); PidAcc(float dt, float Kp, float Kd, float Ki); + // Calculate new acceleration value float step(float setPoint, float currentVal, float maxAcc); private: diff --git a/src/drivers/pid_driver/pidSteer.cpp b/src/drivers/pid_driver/pidSteer.cpp index 13ceb4f..3d1e114 100644 --- a/src/drivers/pid_driver/pidSteer.cpp +++ b/src/drivers/pid_driver/pidSteer.cpp @@ -13,7 +13,6 @@ float PidSteer::step(float setpoint1, float setpoint2, float currentVal1, float // 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 diff --git a/src/drivers/pid_driver/pidSteer.h b/src/drivers/pid_driver/pidSteer.h index 8cf39ca..331acb9 100644 --- a/src/drivers/pid_driver/pidSteer.h +++ b/src/drivers/pid_driver/pidSteer.h @@ -4,10 +4,10 @@ class PidSteer { public: PidSteer(); - // PidSteer(float dt, float Kp, float Kd, float Ki, float max); + // G1 and G2 are they weights for the 2 set points PidSteer(float dt, float Kp, float Kd, float Ki, float G1, float G2, float max); - // float step(float setPoint, float currentVal); + // Calculate new steering value float step(float setpoint1, float setpoint2, float currentVal1, float currentVal2); private: