UNPKG

johnny-five

Version:

The JavaScript Arduino Programming Framework.

164 lines (121 loc) 2.92 kB
if (typeof Map !== "function") { require("/usr/local/lib/node_modules/es6-shim"); } var priv = new Map(); // * (P)roportional // * (I)ntegral // * (D)erivative function PID(opts) { var state = { range: opts.range || [0, 180], direction: opts.direction || PID.FORWARD, sampleTime: 100, lastTime: Date.now() - 100, lastInput: 0, kp: opts.kp || 1, ki: opts.ki || 0.05, kd: opts.kd || 0.25, }; this.point = 0; this.output = 0; priv.set(this, state); this.tune(opts.kp, opts.ki, opts.kd); } PID.FORWARD = 1; PID.REVERSE = -1; PID.prototype = { constructor: PID, get range() { return priv.get(this).range; }, set range(value) { var state = priv.get(this); if (Array.isArray(value)) { state.range[0] = value[0]; state.range[1] = value[1]; } }, get direction() { return priv.get(this).direction; }, set direction(value) { var state = priv.get(this); if (value === PID.FORWARD || value === PID.REVERSE) { state.direction = value; } }, get sampleTime() { return priv.get(this).sampleTime; }, set sampleTime(value) { var state, ratio; if (value > 0) { state = priv.get(this); ratio = value / state.sampleTime; state.ki *= ratio; state.kd /= ratio; state.sampleTime = value; } }, tune: function(kp, ki, kd) { var state, seconds; if (kp < 0 || ki < 0 || kd < 0) { return false; } state = priv.get(this); seconds = state.sampleTime / 1000; state.kp = kp; state.ki = ki * seconds; state.kd = kd / seconds; state.kp *= state.direction; state.ki *= state.direction; state.kd *= state.direction; return true; }, compute: function(input) { var state = priv.get(this); var now = Date.now(); var diffTime = (now - state.lastTime); var min = state.range[0]; var max = state.range[1]; var temp = this.output; var output, error, diffInput; if (diffTime >= state.sampleTime) { state.lastInput = input; state.lastTime = now; error = this.point - input; temp += state.ki * error; temp = Math.max(Math.min(temp, max), min); diffInput = input - state.lastInput; output = (state.kp * error) + temp - (state.kd * diffInput); output = Math.max(Math.min(output, max), min); this.output = output; } return this.output; } }; /* var PITCH_P_VAL = 0.5; var PITCH_I_VAL = 0; var PITCH_D_VAL = 1; var pitch = new PID({ kp: PITCH_P_VAL, ki: PITCH_I_VAL, kd: PITCH_D_VAL, direction: PID.REVERSE, range: [-20, 20] }); // Update to speed value. pitch.point = 30; var last = 10; setInterval(function() { var value = ++last; last = value; console.log( value, pitch.compute(value) ); if (value > 30) { last = 30; } }, 50); */ module.exports = PID; // kp=0.25, ki=0.0009, kd=0.002