improve documentation

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;
     }

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 */

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:

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

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: