Differences

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

--- line_following [2025/02/20 12:34] – [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 241: / Line 241: @@
   result = WriteLnString(fileHandle, text, bytesWritten);
   if(result == LDR_EOFEXPECTED) CloseFile(fileHandle); //if EOF(End of File) error occurs, close the file
+\\
+\\ **- Wall Following with PID**
+\\ Refer to this example for the PID gain values, but adjust them to best suit your Domabot.
+  //Variable initializations ---------------------------------------------------
+  xWall = 0;  //initialize wall distance to 0
+  dWall = 10;  //Desired distance from wall [cm]
+  wKp = 1.25;  //Wall P gain e.g. (PID) = [1.5, 0.005, 30.0]
+  wKi = 0.001;  //Wall I gain
+  wKd = 7.5;  //Wall D gain
+  //(1)Calculate wall-following PID gains
+  wE = xWall - dWall;
+  wEInt +=wE;
+  wEDot = wE - wEPrev;
+  wCorr = (wKp * wE) + (wKi * wEInt) + (wKd * wEDot);
+  //(1A) Check for motor staturation i.e. resulting wCorr forces
+  //motor getting > 2*speedBase (if speedBase > 50, this means >100)
+  if(wCorr > 0 && wCorr > speedBase) {
+    wCorr = speedBase;  //saturated so set correction to speedBase
+                        //So Motor C speed min will be 0
+  };
+  if(wCorr < 0 && wCorr < -speedBase) {
+    wCorr = -speedBase;  //saturated so set correction to -speedBase
+                         //So Motor C speed min will be 0
+  };
+  //(1B) If PID gains all zero, then wCorr = 0 so do bang-bang
+  if(wCorr == 0 && xWall < dWall) {
+    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;