Differences

This shows you the differences between two versions of the page.

--- line_following [2025/02/20 12:36] – [Special Section - Wall Following with PID Control] arim
+++ line_following [2025/02/20 12:44] (current) – [Special Section - Wall Following with PID Control] arim
@@ Line 207: / Line 207: @@
 ===== Special Section - Wall Following with PID Control  =====
-Code:
+Code: {{ :wfpid_tst.pdf |}}
 \\ Video: [[https://youtu.be/mo9PJ54JmFA?si=aaC9fRfzXhq-x55-]]
 \\ {{ :sec00_3.png?nolink&400 |}}
@@ Line 244: / Line 244: @@
 \\ **- Wall Following with PID**
+\\ Refer to this example for the PID gain values, but adjust them to best suit your Domabot.
-    //(1A) Check for motor staturation i.e. resulting wCorr forces
+  //Variable initializations ---------------------------------------------------
-    //motor getting > 2*speedBase (if speedBase > 50, this means >100)
+  xWall = 0;  //initialize wall distance to 0
-    if(wCorr > 0 && wCorr > speedBase) {
+  dWall = 10;  //Desired distance from wall [cm]
-      wCorr = speedBase;  //saturated so set correction to speedBase
+  wKp = 1.25;  //Wall P gain e.g. (PID) = [1.5, 0.005, 30.0]
-                          //So Motor C speed min will be 0
+  wKi = 0.001;  //Wall I gain
-    };
+  wKd = 7.5;  //Wall D gain
-    if(wCorr < 0 && wCorr < -speedBase) {
-      wCorr = -speedBase;  //saturated so set correction to -speedBase
+  //(1)Calculate wall-following PID gains
-                           //So Motor C speed min will be 0
+  wE = xWall - dWall;
-    };
+  wEInt +=wE;
-    //(1B) If PID gains all zero, then wCorr = 0 so do bang-bang
+  wEDot = wE - wEPrev;
-    if(wCorr == 0 && xWall < dWall) {
+  wCorr = (wKp * wE) + (wKi * wEInt) + (wKd * wEDot);
-      wCorr = -speedBase;  //Move away from wall: C = basespeed, A=0
-    };
+  //(1A) Check for motor staturation i.e. resulting wCorr forces
-    if(wCorr == 0 && xWall >= dWall) {
+  //motor getting > 2*speedBase (if speedBase > 50, this means >100)
-      wCorr = speedBase;  //Move towards wall: A = basespeed, C=0
+  if(wCorr > 0 && wCorr > speedBase) {
-    };
+    wCorr = speedBase;  //saturated so set correction to speedBase
+                        //So Motor C speed min will be 0
-    //(2)Command motors
+  };
-    speedA = speedBase + wCorr;
+  if(wCorr < 0 && wCorr < -speedBase) {
-    speedC = speedBase - wCorr;
+    wCorr = -speedBase;  //saturated so set correction to -speedBase
-    OnFwd(OUT_C, speedC);
+                         //So Motor C speed min will be 0
-    OnFwd(OUT_A, speedA);
+  };
+  //(1B) If PID gains all zero, then wCorr = 0 so do bang-bang
-    //(3) update wall errors for next derivative calculation
+  if(wCorr == 0 && xWall < dWall) {
-    wEPrev = wE;
+    wCorr = -speedBase;  //Move away from wall: C = basespeed, A=0
+  };
+  if(wCorr == 0 && xWall >= dWall) {
+    wCorr = speedBase;  //Move towards wall: A = basespeed, C=0
+  };
+  //(2)Command motors
+  speedA = speedBase + wCorr;
+  speedC = speedBase - wCorr;
+  OnFwd(OUT_C, speedC);
+  OnFwd(OUT_A, speedA);
+  //(3) update wall errors for next derivative calculation
+  wEPrev = wE;