diff --git a/BoneCentral/Brain/AI/AlignToAngle.js b/BoneCentral/Brain/AI/AlignToAngle.js new file mode 100644 index 0000000..eb30e8e --- /dev/null +++ b/BoneCentral/Brain/AI/AlignToAngle.js @@ -0,0 +1,87 @@ +var $ = require('../Utilities').Promises +var wait = require('../Utilities').Wait; +var vector = require('../Utilities/vector.js'); +var ctools = require('../Utilities/compassTools.js'); +var PID = require('../Utilities/pid.js'); +var Poseidon = require('../app.js'); + +var upInterval, + time = 1.0, + pid_sp = 0; +var pid = new PID(); +var imu = Poseidon.HardwareFactory.createImuSensor(); +var power = Poseidon.HardwareFactory.createPowerManager(); +var engine = Poseidon.HardwareFactory.createThrustController(); +var start = $.Deferred(); +var finish = $.Deferred(); +const DELTA = 150; +const TRANSFORM = 180.0; + +pid.setDelta(DELTA); +pid.setBounds(-TRANSFORM, TRANSFORM); +pid.calibrate(0.1, 0.5, 0.01); + + + +start +.then(function() { + console.log("- System Online -"); + power.turnOnEscs(); + power.turnOnImu(); +}) +.then(function() { + upInterval = setInterval(function(){ + update(); + killOutOfTime(); + }, DELTA); +}); + +finish +.then(function() { + console.log("- Shutting Down -"); + power.turnOffImu(); + power.turnOffEscs(); + power.exit() + .then(process.exit); +}) + +power.connect() +.then(function(){ + console.log("Ready? GO!"); + start.resolve(); +}); + + + +function update() { + var waitHeading = $.Deferred(); + var waitAccel = $.Deferred(); + + imu.getHeading().done(function(data) { + waitHeading.resolve(data); + }); + imu.getAcceleration().done(function(data){ + waitAccel.resolve(data); + }); + + $.WhenAll(waitHeading, waitAccel) + .done(function(cdata,adata) { + console.log(cdata); + console.log(adata); + var compass = vector.v3(cData.X,cData.Y,cData.Z); + var accel = vector.v3(aData.X,aData.Y,aData.Z); + + var pid_pv = ctools.degrees(compass, accel); + var adj = pid.update(pid_sp, pid_pv) / TRANSFORM; + console.log("Adjust: " + (adj*100).toFixed(2) + "%"); + engine.yaw(adj); + }); +} + +function killOutOfTime() { + time = time - (DELTA/1000.0); + if(time <= 0) { + clearInterval(upInterval); + finish.resolve(); + } +} diff --git a/BoneCentral/Brain/CppInterface/Factory.js b/BoneCentral/Brain/CppInterface/Factory.js index 2727448..f07ccf2 100644 --- a/BoneCentral/Brain/CppInterface/Factory.js +++ b/BoneCentral/Brain/CppInterface/Factory.js @@ -30,7 +30,7 @@ module.exports = (function() { }; Factory.prototype.createPowerManager = function () { - return new PowerManager(dispatcherSocket.Input); + return new PowerManager(this.dispatcherSocket.Input, this.dispatcherSocket.Events); }; Factory.prototype.createThrustController = function () { diff --git a/BoneCentral/Brain/CppInterface/PowerManager.js b/BoneCentral/Brain/CppInterface/PowerManager.js index d10e7e6..fd9d71b 100644 --- a/BoneCentral/Brain/CppInterface/PowerManager.js +++ b/BoneCentral/Brain/CppInterface/PowerManager.js @@ -1,7 +1,8 @@ module.exports = (function(){ - function PowerManager(cmdOut) { + function PowerManager(cmdOut, events) { this._cmdOut = cmdOut; + this._events = events; } PowerManager.prototype.turnOnEscs = function(){ @@ -19,9 +20,14 @@ module.exports = (function(){ PowerManager.prototype.turnOffImu = function () { this._cmdOut.write("turnOffImuSensor\n"); }; + + PowerManager.prototype.connect = function() { + return this._events.OnConnect; + } PowerManager.prototype.exit = function () { this._cmdOut.write("exit\n"); + return this._events.OnExit; }; return PowerManager; diff --git a/BoneCentral/Brain/Sockets/index.js b/BoneCentral/Brain/Sockets/index.js index 4882c3a..a2da922 100644 --- a/BoneCentral/Brain/Sockets/index.js +++ b/BoneCentral/Brain/Sockets/index.js @@ -3,15 +3,31 @@ */ var Streams = require('stream'); -var Net = require('net'); +var Net = require('net'); +var $ = require('../Utilities').Promises module.exports.createSocket = function (port) { - var input = new Streams.PassThrough(); - var output = new Streams.PassThrough(); + var input = new Streams.PassThrough(); + var output = new Streams.PassThrough(); + var onExit = $.Deferred(); + var onConnect = $.Deferred(); + Net.createServer(function (socket) { _handleClient(socket, input, output); + socket.on("close", function() { + onExit.resolve(); + }); + onConnect.resolve(); }).listen(port); - return {Input: input, Output: output}; + + return { + Input: input, + Output: output, + Events: { + OnConnect: onConnect.promise(), + OnExit: onExit.promise() + } + }; }; var _handleClient = function (socket, input, output) {