Hacktoberfest-2021/pid_position_controller.ino at master · ArduinoSriLankaCommunity/Hacktoberfest-2021 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

int En = 9;
int Pp = 10;
int Np = 11;

void setup() {
  Serial.begin(115200);

}

void loop() {
  double s = map(analogRead(A0), 0, 1023, 0, 255);
  pidAlgo(s);
}

double prevErr = 0;
double totErr = 0;
int kp = 5;
int kd = 10;
float ki = 0.01;

void pidAlgo(double sY) {

  double sp = -1 * map(analogRead(A1), 0, 1023, 0, 90); //Get the set point from the potentiometer
  double error = sp - sY; //obtain the error from substracting

  double Pterm = error * kp;
  double Dterm = (error - prevErr) * kd;
  double Iterm = (double)ki * totErr;

  double pidVal =  Pterm + Dterm + Iterm ;

  int mspeed = (int)pidVal;
  drM(-mspeed);

  prevErr = error;
  totErr += error;
  Serial.print(sp);
  Serial.print(",");
  Serial.print(sY);
  Serial.print(",");
  Serial.println(pidVal);
}

void drM(int m1) {
  if (m1 >= 255) {
    m1 = 255;
  }
  if (m1 <= -255) {
    m1 = -255;
  }
  if (m1 < 0) {
    m1 = -1 * m1;
    digitalWrite(Pp, LOW); digitalWrite(Np, HIGH);
    analogWrite(En, m1);
  } else {
    digitalWrite(Pp, HIGH); digitalWrite(Np, LOW);
    analogWrite(En, m1);
  }
}