Differences

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

--- line_following [2025/02/20 12:20] – [Section02 - IR Sensor] 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 |}}
+\\
+**- US sensor**
+\\ Ultrasonic sensor is used to measure the distance to an object using ultrasound. The sensor emits an ultrasonic signal, which strikes the object and then returns to the sensor as a reflection. By measuring the time it takes for the signal to be transmitted and received, the distance to the object can be calculated. The range of the sensor values is from 0 to 255, where a smaller value indicates a closer distance.
+\\
+\\ **- Save sensor data and display is as a graph**
 \\ {{ :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;