UNPKG

brickpi-raspberry-watch

Version:
151 lines (122 loc) 4 kB
// module that contains the motor class definition and the motorArray class var util = require('util'); var ee = require('events').EventEmitter; Motor = function(params) { ee.call(this); this.name = params.name; this.decription = params.description; this.port = params.port; this.speed = 0; this.enabled = true; this._encoderValue = null; this._actualSpeed = null; this._before = 0; this._offset = 0; this._callback = null; this._endEncoderValue = null; this._tolerance = 10; } exports.Motor = Motor; util.inherits(Motor, ee); Motor.prototype.getPosition = function() { return (this._encoderValue - this._offset); }; Motor.prototype.resetPosition = function() { this._offset = this._encoderValue; return this; }; Motor.prototype.getActualSpeed = function() { return this._actualSpeed; } Motor.prototype.start = function(speed) { this.speed = parseInt(speed, 10); return this; }; Motor.prototype.stop = function(err) { this._endEncoderValue = null; this.speed = 0; if (this._callback) this._callback(err); this.emit('stop', this); return this; }; Motor.prototype.stopIn = function(amount, callback) { this._callback = callback; var startEncoderValue = this._encoderValue; this._endEncoderValue = this._encoderValue + this.speed*(parseInt(amount, 10))/(Math.abs(this.speed)); // setting min and max speed for PID if (this.speed > 0) { this._maxSpeed = this.speed; this._minSpeed = -1*this.speed; } else { this._maxSpeed = -1*this.speed; this._minSpeed = this.speed; } return this; }; Motor.prototype.moveTo = function(moveToPosition, callback) { this._callback = callback; var startEncoderValue = this._encoderValue; this._endEncoderValue = parseInt(this._offset, 10) + parseInt(moveToPosition, 10); // setting proper direction if (((this._endEncoderValue > startEncoderValue) && (this.speed < 0)) || ((this._endEncoderValue < startEncoderValue) && (this.speed > 0))) { this.speed = -1 * this.speed; } // setting min and max speed for PID if (this.speed > 0) { this._maxSpeed = this.speed; this._minSpeed = -1*this.speed; } else { this._maxSpeed = -1*this.speed; this._minSpeed = this.speed; } return this; }; Motor.prototype._update = function(encoderValue) { this._encoderValue = parseInt(encoderValue, 10); // speed calculations. in ticks/seconds var now = (new Date()).getTime(); var timelapse = now - this._before; this._before = now; if (this._previousEncoderValue !== null) { this._actualSpeed = (this._encoderValue - this._previousEncoderValue)/timelapse*1000; } this._previousEncoderValue = this._encoderValue; ///////// if (this._actualSpeed !== 0) { this.emit('move'); } if (this._endEncoderValue) { var targetSpeed = this._PIDCalculation(this._endEncoderValue, this._encoderValue, timelapse, this._minSpeed, this._maxSpeed); if (((Math.abs(this._endEncoderValue - this._encoderValue) < this._tolerance) || (Math.abs(targetSpeed) < 20))&& (Math.abs(this._actualSpeed) === 0)) { this.stop(); } else { this.speed = targetSpeed.toFixed(0); } } }; Motor.prototype._PIDCalculation = function(setPoint, currentPoint, dt, min, max) { var KP = 0.4; var KD = 0.01; var KI = 0.001; var epsilon = 1; this._pre_error = 0; this._integral = 0; var error; var derivative; var output; error = setPoint - currentPoint; if (Math.abs(error) > epsilon) { this._integral = this._integral + error*dt; } derivative = (error - this._pre_error)/dt; output = KP*error + KI*this._integral + KD*derivative; //Saturation Filter if (output > max) { output = max; } else if (output < min) { output = min; } //Update error this._pre_error = error; return output; }