arm_code/FastPID.cpp at master · UofA-SPEAR/arm_code · 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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
#include "FastPID.h"

#include "common.h"

FastPID::~FastPID() {
}

void FastPID::clear() {
  _last_sp = 0;
  _last_out = 0;
  _sum = 0;
  _last_err = 0;
  _cfg_err = false;
}

bool FastPID::setCoefficients(float kp, float ki, float kd, float hz) {
  _p = floatToParam(kp);
  _i = floatToParam(ki / hz);
  _d = floatToParam(kd * hz);
  return ! _cfg_err;
}

bool FastPID::setOutputConfig(int bits, bool sign) {
  // Set output bits
  if (bits > 16 || bits < 1) {
    setCfgErr();
  }
  else {
    if (bits == 16) {
      _outmax = (0xFFFFULL >> (17 - bits)) * PARAM_MULT;
    }
    else{
      _outmax = (0xFFFFULL >> (16 - bits)) * PARAM_MULT;
    }
    if (sign) {
      _outmin = -((0xFFFFULL >> (17 - bits)) + 1) * PARAM_MULT;
    }
    else {
      _outmin = 0;
    }
  }
  return ! _cfg_err;
}

bool FastPID::setOutputRange(int16_t min, int16_t max)
{
  if (min >= max) {
    setCfgErr();
    return ! _cfg_err;
  }
  _outmin = int64_t(min) * PARAM_MULT;
  _outmax = int64_t(max) * PARAM_MULT;
  return ! _cfg_err;
}

bool FastPID::configure(float kp, float ki, float kd, float hz, int bits, bool sign) {
  clear();
  setCoefficients(kp, ki, kd, hz);
  setOutputConfig(bits, sign);
  return ! _cfg_err;
}

uint32_t FastPID::floatToParam(float in) {
  if (in > PARAM_MAX || in < 0) {
    _cfg_err = true;
    return 0;
  }

  uint32_t param = in * PARAM_MULT;

  if (in != 0 && param == 0) {
    _cfg_err = true;
    return 0;
  }

  return param;
}

int16_t FastPID::step(int16_t sp, int16_t fb) {

  // int16 + int16 = int17
  int32_t err = int32_t(sp) - int32_t(fb);
  int32_t P = 0, I = 0;
  int32_t D = 0;

  if (_p) {
    // uint16 * int16 = int32
    P = int32_t(_p) * int32_t(err);
  }

  if (_i) {
    // int17 * int16 = int33
    _sum += int64_t(err) * int64_t(_i);

    // Limit sum to 32-bit signed value so that it saturates, never overflows.
    if (_sum > INTEG_MAX)
      _sum = INTEG_MAX;
    else if (_sum < INTEG_MIN)
      _sum = INTEG_MIN;

    // int32
    I = _sum;
  }

  if (_d) {
    // (int17 - int16) - (int16 - int16) = int19
    int32_t deriv = (err - _last_err) - int32_t(sp - _last_sp);
    _last_sp = sp;
    _last_err = err;

    // Limit the derivative to 16-bit signed value.
    if (deriv > DERIV_MAX)
      deriv = DERIV_MAX;
    else if (deriv < DERIV_MIN)
      deriv = DERIV_MIN;

    // int16 * int16 = int32
    D = int32_t(_d) * int32_t(deriv);
  }

  // int32 (P) + int32 (I) + int32 (D) = int34
  int64_t out = int64_t(P) + int64_t(I) + int64_t(D);

  // Make the output saturate
  if (out > _outmax)
    out = _outmax;
  else if (out < _outmin)
    out = _outmin;

  // Remove the integer scaling factor.
  int16_t rval = out >> PARAM_SHIFT;

  // Fair rounding.
  if (out & (0x1ULL << (PARAM_SHIFT - 1))) {
    rval++;
  }

  return rval;
}

void FastPID::setCfgErr() {
  _cfg_err = true;
  _p = _i = _d = 0;
}