Differences

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

--- drone-technolgy:pid-controller [2022/05/17 15:25] – ilgarrasulov001
+++ drone-technolgy:pid-controller [2023/01/05 14:38] (current) – external edit 127.0.0.1
@@ Line 74: / Line 74: @@
 {{:drone-technolgy:arm_radian_scheme.png|}}
+<file c arduino_code.ino>
+// importing libraries
+#include "MPU9250.h" // sensor library
+#include "math.h"  // math operations
+//define pins
+#define MOTOR D3   // pin for motor control
+#define PIN_POT  A0  //pin for potentiometer value reading
+MPU9250 mpu;  // sensor instance
+// initial control values
+float kp=6.8;
+float ki=0.1;
+float kd=1.8;
+float multiplier=1; // multiplier variable is used to magnitude the P I D values at the same time by the same factor
+float error;
+float ki_error_range=10;
+float desired_roll=38.0;
+float pError=0.0;
+float current_roll=0.0;
+float PID_p, PID_i, PID_d, PID_total;
+// time parameters for setting the frequency of reading sensor values
+int period = 50; // milliseconds
+float tme;
+// serial input value
+String serialInput;
+void setup() {
+  Serial.begin(115200);
+  Wire.begin();
+  // connection to MPU sensor
+  if (!mpu.setup(0x68)) {  // change to your own address
+        while (1) {
+            Serial.println("MPU connection failed. Please check your connection with `connection_check` example. Trying to reconnect...");
+//            delay(5000);
+          if (mpu.setup(0x68)){
+            break;
+          }
+        }
+    }
+  // motor and potentiometer to output and input
+  pinMode(MOTOR, OUTPUT);
+  pinMode(PIN_POT, INPUT);
+  // set desired roll to the value, read from potentiometer
+  set_desired_roll();
+  Serial.println("Setup finished");
+  tme=millis();
+}
+void set_desired_roll(){
+  // -55 lowest, 20 max, 0 center, total 75
+  // read potentiometer value, range is [1024-10]
+  int rot_1024= analogRead(PIN_POT);
+  // convert to 75 units system
+  int rot_75 = 75*(1024 - rot_1024)/1014;
+  // set desired roll
+  desired_roll=rot_75-55;
+  }
+void loop() {
+  // set desired yaw in accordance to the last read from potentiometer
+  set_desired_roll();
+  // read input from serial monitor
+  // format: <variable>=<float value>
+  // example: kp=1.5
+  if (Serial.available()> 0){ // check if there is an input
+      serialInput = Serial.readString(); //read input as a string
+      int index = serialInput.indexOf('='); // find index of =
+      String variable = serialInput.substring(0,index); // find the first part of substring, meaning the variable name
+      float value = serialInput.substring(index+1, serialInput.length()).toFloat(); // find the second part of substring, meaning the variable value and convert it to float
+    // check variable name and assign the value to the corresponding variable
+    if (variable=="kp"){
+        kp=value;
+    }
+    else if (variable=="ki"){
+        ki=value;
+    }
+    else if (variable=="kd"){
+        kd=value;
+    }
+    else if (variable=="kier"){
+        ki_error_range=value;
+    }
+    }
+    // check the sensor data
+  if (mpu.update()) {
+        if (millis() > tme + period) { // if more than period seconds passed since last read
+            tme=millis(); // set tme variable to current time in milliseconds
+            // read current roll angle
+             current_roll=mpu.getRoll();
+             // error calculation
+            error=desired_roll-current_roll;
+            // P calculation
+            PID_p = kp * multiplier* error;
+            // I calculation
+            // I component starts to accumulate and hence to affect the PID total only if it
+            // is in range of ki error range
+            if(abs(error) < ki_error_range){
+            PID_i = PID_i + (ki *multiplier* error);
+            } else { // else it is set to zero
+              PID_i=0;
+           }
+            // D calculation
+            // pError is previous value of error
+            PID_d = kd*multiplier*((error - pError)/(period));
+            // Total PID calculation
+            PID_total = PID_p + PID_i + PID_d;
+            // trim the PID value if it is outside of [0-255] range
+            if (PID_total > 255){
+              PID_total =255;
+            }
+            if (PID_total < 0){
+              PID_total =0;
+            }
+            // print PID and other variables' values
+            print_pid();
+            // send final PID value to motor
+            analogWrite(MOTOR,PID_total);
+            // set pError value to current error value
+            pError = error;
+    }
+}
+}
+// print variable values to Serial Monitor
+void print_pid() {
+    Serial.print("Current Roll: ");
+    Serial.println(current_roll, 2);
+    Serial.print("Desired Roll: ");
+    Serial.println(desired_roll, 2);
+    Serial.print("Absolute error: ");
+    Serial.println(abs(error), 2);
+    Serial.print("KP ki ki_error_range kd: ");
+    Serial.print(kp);
+    Serial.print(" ");
+    Serial.print(ki);
+    Serial.print(" ");
+    Serial.print(ki_error_range);
+    Serial.print(" ");
+    Serial.println(kd);
+    Serial.print("PID_Total, P, I, D: ");
+    Serial.print(PID_total, 2);
+    Serial.print(", ");
+    Serial.print(PID_p, 2);
+    Serial.print(", ");
+    Serial.print(PID_i, 2);
+    Serial.print(", ");
+    Serial.println(PID_d, 2);
+}
+// not used
+// sending values to PC
+// may be useful in future
+void sendToPC(int* data)
+{
+  byte* byteData = (byte*)(data);
+  Serial.write(byteData, 2);
+}
+void sendToPC(float* data)
+{
+  byte* byteData = (byte*)(data);
+  Serial.write(byteData, 4);
+}
+</file>
 Link to the code in GitHub repository
@@ Line 94: / Line 288: @@
 The commonly accepted way of tuning is following:
-First you start changing Kp coefficient, and Ki Kd are set to 0. P value is proportional to error, and this leads to the oscilation of the system. To decrease the oscilations we can decrease Kp. If we want to react faster to changes, we need to take into account also the speed of changes, or Derivative controller D. Remember its formula
+First you start changing Kp coefficient, and Ki Kd are set to 0. P value is proportional to error, and this leads to the oscilation of the system. To decrease the oscilations we can decrease Kp. If we want to react faster to changes, we need to take into account also the speed of changes, or Derivative controller D. Remember its formula: <br>
+D = Kd*de/dt
+$$ D=K_{d}*\frac{\mathrm{d}e(t)}{\mathrm{d}t} $$
+Here, the smaller the period between measurements or the bigger the change in error, the bigger will be the final D component.
+And finally, you can tune the I component. It affects the final PID value only when error is within the given range relative to desired yaw value. For example, if the error is too small for proportional P controller and the error didn't change since the last measurement, the PID will be zero without I controller.
+To change kp, ki and kd in our experiment without changing the code, you can enter: <br>
+   * "kp=4" for P coefficient value
+   * "ki=2" for I coefficient value
+   * "kd=1" for D coefficient value
+   * "kier=20" for I controller range value
+Here the values should be adapted for your device.
 {{:drone-technolgy:serialmonitor_input.png|}}
+For more examples of PID controller see nice videos on Youtube in Resources section.
 ==== Resources ====
-  * [[https://en.wikipedia.org/wiki/PID_controller]]
+  * PID controller on Wikipedia [[https://en.wikipedia.org/wiki/PID_controller]]
-  * [[https://github.com/eligosoftware/pid_arm_control]]
+  * PID controller code [[https://github.com/eligosoftware/pid_arm_control]]
-  * [[https://www.youtube.com/watch?v=IB1Ir4oCP5k&ab_channel=RealPars]]
+  * PID Tuning [[https://www.youtube.com/watch?v=IB1Ir4oCP5k&ab_channel=RealPars]]
+  * PID Tuning [[https://www.youtube.com/watch?v=JFTJ2SS4xyA&t=612s&ab_channel=Electronoobs]]