var Gpio = require('onoff').Gpio; var NanoTimer = require('nanotimer'); const ECHO = 17, SENSOR_ID_TO_LOG = 0; //Maximum seperation in cms between sensors, ex: 400 for WBO const MAX_SEPERATION = 140; //Object sensed beyond this factor on max_separation will be considered invalid const VALID_DISTANCE_FACTOR = 0.9; //Possible Object Offset Due to Conical Echo const SENSOR_ANGLE_DELTA = (30.0 * Math.PI / 180.0) / 2.0; var NAN_XY = function() { return [50000, 50000] }; //Assuming XY Plane in CM unit with WB Centre at 0,0 //DIR is sensor angle in radians on the +ve XY Plane, ex: 0 or Math.PI for WBO var sensors = [{ PIN: 16, DIR: 0, X: -MAX_SEPERATION / 2.0, Y: -20, objectXY: NAN_XY() }, { PIN: 13, DIR: 0, X: -MAX_SEPERATION / 2.0, Y: 20, objectXY: NAN_XY() }, { PIN: 7, DIR: Math.PI, X: MAX_SEPERATION / 2.0, Y: -20, objectXY: NAN_XY() }, { PIN: 19, DIR: Math.PI, X: MAX_SEPERATION / 2.0, Y: 20, objectXY: NAN_XY() }]; var objectXYs = Array(sensors.length).fill(NAN_XY()); var activeSensorId = undefined, echo; var echoStart, echoEnd, echoDuration, distance, shifter = new NanoTimer(); process.on('SIGINT', function() { if (shifter) shifter.clearInterval(); var sensor; for (var sensorId in sensors) { sensor = sensors[sensorId]; sensor.trigger.unexport(); } echo.unexport(); }); var onWriteSuccess = function(err) { if (err) throw err; //console.log('Pin Write Success', TRIGGER.readSync()); } var processObject = function(distance) { var activeSensor = sensors[activeSensorId]; //if valid distance if (distance > MAX_SEPERATION * VALID_DISTANCE_FACTOR) { activeSensor.objectXY = NAN_XY(); objectXYs[activeSensorId] = NAN_XY(); return; } // console.log('distance', distance); var x = activeSensor.X + Math.cos(activeSensor.DIR) * distance, y = activeSensor.Y + Math.sin(activeSensor.DIR) * distance; //TODO: Consider SENSOR_ANGLE_DELTA in position range activeSensor.objectXY = [x, y]; objectXYs[activeSensorId] = [x.toFixed(1), y.toFixed(1)]; } var onEcho = function(err, value) { // console.log('Echo Changed To ', value); if (value) echoStart = process.hrtime(); else if (echoStart) { echoEnd = process.hrtime(echoStart); echoStart = undefined; echoDuration = echoEnd[1]; distance = (echoDuration * 34029.0) / (2 * 1000.0 * 1000.0 * 1000.0); processObject(distance); if(SENSOR_ID_TO_LOG === activeSensorId || SENSOR_ID_TO_LOG == undefined) console.log('Distance of Object at Sensor ' + (activeSensorId + 1) + ' is ' + Math.floor(distance) + ' cm'); } } var initEcho = function() { echo = new Gpio(ECHO, 'in', 'both'); echo.watch(onEcho); } var initTriggers = function() { var sensor; for (var sensorId in sensors) { sensor = sensors[sensorId]; sensor.trigger = new Gpio(sensor.PIN, 'out'); sensor.echo = ECHO; sensor.trigger.write(0, onWriteSuccess); } } var getDistance = function(triggerPin) { //reset flags and prepare for next sensor echoStart = echoEnd = echoDuration = distance = undefined; var activeSensor = sensors[activeSensorId]; activeSensor.trigger.write(1, function(err) { var triggerTimer = new NanoTimer(); triggerTimer.setTimeout(function() { activeSensor.trigger.write(0, onWriteSuccess) }, '', '10u'); }); } var shiftSensor = function() { //set to last sensor if (activeSensorId === undefined) { activeSensorId = sensors.length - 1; } else { activeSensorId = (activeSensorId + 1) % sensors.length; } getDistance(); } var init = function() { initEcho(); initTriggers(); shifter.setInterval(shiftSensor, '', '70m'); } init();