Differences

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

--- line_following [2025/02/20 12:29] – [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 218: / Line 218: @@
 \\ {{ :sec00_4.png?nolink&600 |}}
 \\ The distance data measured by the sensor can be saved as a CSV file and visualized as a graph. This makes it easier to observe changes and helps determine how to adjust the PID gain values effectively. The graph above shows the distance from the wall while performing wall following at a target distance of 10 cm. By analyzing this data, the PID gain values can be adjusted to achieve more stable control.
+   //File -----------------------------------------------------------------------
+  fileName = "wf_pid.csv";
+  result = CreateFile(fileName, 1024, fileHandle); //size 1024 bytes
+  //Overwrite existing file
+  while (result == LDR_FILEEXISTS) { //if the file already exists
+    CloseFile(fileHandle); //close the existing file
+    DeleteFile(fileName); //delete it
+    result = CreateFile(fileName, 1024, fileHandle); //create a new file
+  }
+  //Write column headers
+  fileHeader =  "Iteration, Wall distance[cm]"; //define the header
+  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 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;