forked from andrew/ar-drone-gps
-
Notifications
You must be signed in to change notification settings - Fork 0
/
PID.js
46 lines (38 loc) · 1.05 KB
/
PID.js
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
// from https://github.com/eschnou/ardrone-autonomy/blob/401d7c920af34457a1ebf88e82004c517f5d6a24/lib/PID.js
module.exports = PID;
function PID(kp, ki, kd) {
this.configure(kp, ki, kd);
this.reset();
}
PID.prototype.configure = function(kp,ki,kd) {
this._kp = kp;
this._ki = ki;
this._kd = kd;
}
PID.prototype.reset = function() {
this._last_time = 0;
this._last_error = Infinity;
this._error_sum = 0;
}
PID.prototype.getCommand = function(e) {
// Compute dt in seconds
var time = Date.now();
var dt = (time - this._last_time) / 1000
var de = 0;
if (this._last_time != 0) {
// Compute de (error derivation)
if (this._last_error < Infinity) {
de = (e - this._last_error) / dt;
}
// Integrate error
this._error_sum += e * dt;
}
// Update our trackers
this._last_time = time;
this._last_error = e;
// Compute commands
var command = this._kp * e
+ this._ki * this._error_sum
+ this._kd * de;
return command;
}