====== PID Line Following Tutorial ======
**Author:** <Arim Baek> Email: <baeka3@unlv.nevada.edu> <!-- replace with your email address -->
\\
**Date:** Last modified on <02/20/25>
\\
**Keywords:** Lego NXT, Line Following, PID control
\\

\\
===== Motivation and Audience =====

This tutorial's motivation is to demonstrate how to operate Domabot for line following. 
\\ Readers of this tutorial assumes the reader has the following background and interests: 

<fc blue>  
  * Have prior experience with C Programming
\\
  * Some basic knowledge on PID control
</fc>
\\

\\
===== Section01 - Motor, Button  =====

Code: {{:sec01_mtr.btn.pdf}}
\\ Video: [[https://youtu.be/kHSxJ3z717k?si=IoEDGVxeIUJ22hrF]]
\\ {{ :sec01_3.png?nolink&400 |}}
\\
**- OnFwd(byte outputs, char pwr);**
\\ You can connect a motor to an NXT port and control its movement. The output specifies the **port** where the motor is connected and **pwr** determines the percentage of power used to drive the motor. The **OnFwd** function moves the Domabot forward, while the **OnRev** function moves the Domabot backward.
\\

**- ButtonPressed(BTN, FALSE);**
\\ The NXT detects presses on the left button, right button, and oranege button(middle button). When a button is pressed, its state changes to True. These buttons are used as conditions in a do-while loop, allowing the loop to exit and proceed to the next step when a button is pressed.
\\

**- sec01_mtr.btn.pdf**
\\ You can observe the movement of the Domabot based on the rotation directions of the left and right motors. When the right motor rotates forward(Fwd) and the left motor rotates in reverse(Rev), the domabot rotates counterclockwise. Conversely, when the right motor rotates in reverse and the left rotates in forward, the doambot rotates clockwise.
\\

\\
===== Section02 - IR Sensor =====

Code: {{ :sec02_irsensor.zip |}}
\\ Video: [[https://youtu.be/GL9U_di7g5U?si=93LE_zrrZpM34HZO]]
\\ {{ :sec02_3.png?nolink&400 |}}
\\
**- IR sensor**
\\ This is an infrared sensor for detecting a black line during line following. The sensor provides values in the range of 0 to 100. Dark objects absorb more light and reflect it less, resulting in lower sensor values. Brighter objects absorb less light and reflect it more, leading to higher sensor values. Using this principle, IR sensor measures the amount of light reflected from an object, with darker objects producing lower sensor values.
\\ 

**- Connect IR sensor to the Domabot**
\\ Since the NXT buttons will need to be pressed frequently during the line following project, it is recommended to design the setup in a way that makes button pressing easy. It doesn’t have to be designed like the reference image, but IR sensor must be positioned to face the ground to detect the line properly.
\\

**- helloIRSensor.pdf**
\\ This is a program that displyas IR sensor values on the NXT monitor. Try measuring both dark and bright objects with the sensor and observe the difference in values.
\\ 

**- helloIRSensor_1.pdf**
\\ This program is based on helloIRSensor.nxc. When IR sensor detects a dark object, it produces a low-pitched sound. When it detects a bright object, it produces a high-pitched sound. A threshold value of 40 is used to distinguish between dark and bright. If a value of the sensor is less than 40, a low-pitched sound is played. If the sensor value is 40 or higher, a high-pitched sound is played.
\\ In terms of frequency:
\\ - Low-pitched sound: 262Hz (C in the 4th octave)
\\ - High-pitched sound: 523Hz (C in the 5th octave)
\\ 

\\
===== Section03 - Line Following with Bang-Bang Control =====

Code: {{ :lfbb_spbs50.pdf |}}
\\ Video: [[https://youtu.be/AJxL-cykbwo?si=ueX9ob5xPxhRUD4B]]
\\ {{ :sec03_5.jpg?nolink&400 |}}
\\ {{ :sec03_4.png?nolink&600 |}}
\\
**- Threshold Value**
\\ The Threshold Value is used to distinguish between white and black. Values which are smaller than the threshold are considered as black and bigger than the threshold are considered as white. Before starting line following, sensor values for both black and white should be measured by moving the Domabot. Using these measured values, the Threshold Value is calculated and assigned.
\\
\\ **(i) Step1: To be prepared**
\\ Position the Domabot on the right edge of the black Line to prepare to measure the sensor values.

  //Step1: Turn left(CCW) calibration - will rotate IR sensor from white to black
  endTime = CurrentTick() + 2000; //2000 msec stopwatch
  OnFwd(OUT_A, speedSlow); 
  OnRev(OUT_C, speedSlow); //results in yawing slowly CCW from white to black
  while(CurrentTick() < endTime) {
    irValue = Sensor(IN_4); //read and update sensor value
    if(irValue > irMax) { 
      irMax = irValue; //update maximum value
    } else if(irValue < irMin) {
      irMin = irValue; //update minimum value
    } //end if-else
  } //end while - and we now have max and min light sensor values

\\ **(ii) Step2: Yaw CCW**
\\ Yaw counterclockwise slowly to measure values. The sensor updates its minimum and maximum values as it moves from the black line to the white area.

  //Step2: Yaw CW to rotate IR sensor from black to white
  endTime = CurrentTick() + 3000; //3000msec stopwatch
  OnFwd(OUT_C, speedSlow); 
  OnRev(OUT_A, speedSlow); //results in yawing slowly CW from white to black
  while(CurrentTick() < endTime) {
    irValue = Sensor(IN_4);
    if(irValue > irMax) { 
      //robot rotates CCW for 3s gathering IR values
      irMax = irValue; //update maximum value
    } else if(irValue < irMin) {
      irMin = irValue; //update minimum value
    } //end if-else
  } //end while
  
\\ **(iii) Step3: Yaw CW and Calculate Threshold Value**
\\ Yaw clockwise slowly to measure values. The final minimum and maximum values are recorded as the sensor moves from the white area, crosses the black line, and reaches the white area again.
The Threshold Value is assigned as the average of the minimum and maximum values.

  //Step3: Calculate and display threshold value until user hits Right Button
  Off(OUT_AC);
  PlaySound(SOUND_UP);
  irThresh = (irMin + irMax)/2; //avg of min and max IR values
  ClearScreen();
  TextOut(0, LCD_LINE1, "Calibration values");
  TextOut(0, LCD_LINE2, FormatNum("irMax = %d", irMax));
  TextOut(0, LCD_LINE3, FormatNum("irMin = %d", irMin));
  TextOut(0, LCD_LINE4, FormatNum("irThresh = %d", irThresh));
  TextOut(0, LCD_LINE6, ">>BTN to proceed");
  
\\ **(iv) Step4: Calculate Threshold Value**
\\ The Domabot, currently detecting the white area, rotates slowly CCW again. Rotation stops when the sensor value becomes equal to or smaller than the Threshold Value. Now it's ready to start line following.

  //Step4: Domabot may have yawed onto white
  endTime = CurrentTick() + 2000; //2000 msec stopwatch
  OnFwd(OUT_A, speedSlow); 
  OnRev(OUT_C, speedSlow);
  while(irValue > irThresh) {
    irValue = Sensor(IN_4); //read sensor value
  } //end whlie
  
\\ **- Bang-Bang Line Following**
\\ If the sensor value is bigger than the threshold value, it means that Doomabot is detecting the white area to the right of the black line. In this case, Domabot should move to the left to be on the black line. Conversly, if the sensor value is smaller than the threshold value, it indicates that Domabot is tilted to the left of the black line. Therefore, it should adjust to the right to be at the right edge of the black line.

   do{
    leftBTNPushed = ButtonPressed(BTNLEFT, FALSE);
    irValue = Sensor(IN_4);
    if(irValue > irThresh) {//when white part
      speed = speedBase;
      OnFwd(OUT_A, speed); //turn left
      Off(OUT_C);
    } else if(irValue <= irThresh) {//when black part and boundary
      speed = speedBase;
      OnFwd(OUT_C, speed); //turn right
      Off(OUT_A);
    } //end else-if
  } while(!leftBTNPushed);
  
\\ 

\\
===== Section04 - Line Following with PID Control =====

Code: {{ :lfbb_pid.pdf |}}
\\ Video: [[https://youtu.be/49UfaxzgJC8?si=HaaPoYRK6gKca7R7]]
\\ {{ :sec04_1.jpg?nolink&400 |}}
\\
\\ **- The problem of Bang-Bang Control**
\\ Bang-Bang Control only provides directions for two cases, which prevents Domabot from driving smoothly. The frequent yawing is also caused by the use of Bang-Bang Control. Lowering the speed reduces the yawing angle but increases its frequency. To address this issue and achieve smoother driving, PID Control was chosen.
\\
\\ **- PID Control**
\\ (i) Proportional Control
\\ Domanot is controlled in proportion to the current error. A higher P gain allows it to reach the target value more quickly, but achieving the exact target value perfectly is difficult.
\\ (ii) Integral Control
\\ Domabot is controlled by considering the accumulated error over time. This enables a faster response, but may cause overshoot.
\\ (iii) Derivative Control
\\ Domabot is controlled based on the rate of change of error over time. It detects sudden changes in error, reducing oscillations and allowing for smoother driving.
\\
\\ **- Line Following with PID Control**
\\ By using the measured error and PID gain values, the error is corrected, enabling smoother and more precise driving. Compared to when Bang-Bang Control was used, Domabot rarely deviates from the black line and does less frequent yawing, resulting in smoother and more stable movement.

\\ **(i) Calculate error**
\\ Refer to this example for the PID gain values, but adjust them to best suit your Domabot.

  // PID gain 
  Kp = 2;
  Ki = 0.001;
  Kd = 25;
  
  //The PID control equation established using the PID gain values  
  lE = irValue – irThresh; // Error value
  lEInt += lE; // Cumulative error
  lEDif = lE – lEPrev; // Difference between current error and previous error
  lECorr = (Kp * lE) + (Ki * lEInt) + (Kd * lEDif); // Correction value

\\ **(ii) PID Control**

  //adjust motor speed
  speedLeft = speedBase - lECorr;
  speedRight = speedBase + lECorr;
    
  //motor
  OnFwd(OUT_A, speedRight);
  OnFwd(OUT_C, speedLeft);
    
  lEPrev = lE;
    
  iteration++;
\\

\\
===== Special Section - Wall Following with PID Control  =====

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;