stoops-scanner/PIDController.cpp at master · StaziaPhoenix/stoops-scanner · 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
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#include "PIDController.h"

int PIDController::pid(float err, byte & speedy) {
  float change = err-prev_err;

  if (abs(change)-abs(prev_change) < 0) {
    speedy++;
  } else {
    speedy = 55;
  }

  float pid;
  if (change > threshold) {
    pid = err*kP_t+I + err*kI+(change)*kD_t;
  } else {
    pid = err*kP_l+I + err*kI +(change)*kD_l;
  }

  prev_err = err;
  prev_change = change;
  return clamp(pid);
}

void PIDController::setGo(byte state) {
  go = state;
}

int PIDController::clamp (float pid) {
  if (pid > outMax) {
    return outMax;
  } else if (pid < -outMax) {
    return -outMax;
  } else {
    return pid;
  }
}
void PIDController::incP_t() {
  kP_t += 0.01;
}

void PIDController::incD_t() {
  kD_t += 0.05;
}

void PIDController::decP_t() {
  kP_t -= 0.01;
}

void PIDController::decD_t() {
  kD_t -= 0.05;
}

float PIDController::getP_t() const {
  return kP_t;
}

float PIDController::getD_t() const {
  return kD_t;
}

void PIDController::incP_l() {
  kP_l += 0.01;
}

void PIDController::incD_l() {
  kD_l += 0.01;
}

void PIDController::decP_l() {
  kP_l -= 0.01;
}

void PIDController::decD_l() {
  kD_l -= 0.01;
}

float PIDController::getP_l() const {
  return kP_l;
}

float PIDController::getD_l() const {
  return kD_l;
}

void PIDController::setThreshold(float newVal) {
  threshold = newVal;
}

float PIDController::getThreshold() {
  return threshold;
}