Differences

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

--- line_following [2025/02/20 12:31] – [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 220: / Line 220: @@
    //File -----------------------------------------------------------------------
-  fileName = "wf_[125.001.75].csv";
+  fileName = "wf_pid.csv";
-  result = CreateFile(fileName, 1024, fileHandle);
+  result = CreateFile(fileName, 1024, fileHandle); //size 1024 bytes
   //Overwrite existing file
-  while (result == LDR_FILEEXISTS) {
+  while (result == LDR_FILEEXISTS) { //if the file already exists
-    CloseFile(fileHandle);
+    CloseFile(fileHandle); //close the existing file
-    DeleteFile(fileName);
+    DeleteFile(fileName); //delete it
-    result = CreateFile(fileName, 1024, fileHandle);
+    result = CreateFile(fileName, 1024, fileHandle); //create a new file
   }
   //Write column headers
-  fileHeader =  "Iteration, Wall distance[cm]";
+  fileHeader =  "Iteration, Wall distance[cm]"; //define the header
-  WriteLnString(fileHandle, fileHeader, bytesWritten);
+  WriteLnString(fileHandle, fileHeader, bytesWritten); //write the header to the file
   //Convert data to string and write to file
   strIteration = FormatNum("%d", iteration);
   strxWall = FormatNum("%d", xWall);
   text = StrCat(strIteration, ",", strxWall);
   result = WriteLnString(fileHandle, text, bytesWritten);
-  if(result == LDR_EOFEXPECTED) CloseFile(fileHandle);
+  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;