Renamed sensors.raw.foo to sensors.foo. Renamed speed and steering to…

… velocity and rotation. Remote control is now a sensor. Manual control moved to inside brains. General cleanup/code-golf.

actions/driveStraight.js

@@ -41,18 +41,18 @@ module.exports = createAction({
       }
     }
 
-    return sensors => {
+    return ({ odometry }) => {
       if(!isGoalSet) {
-        phiDesiredPID.setTarget(sensors.odometry.phi);
+        phiDesiredPID.setTarget(odometry.phi);
         startTime = Date.now();
-        startLocation.x = sensors.odometry.x;
-        startLocation.y = sensors.odometry.y;
+        startLocation.x = odometry.x;
+        startLocation.y = odometry.y;
         isGoalSet = true;
       }
 
       return {
-        speed: speed(sensors.odometry, done),
-        steering: phiDesiredPID.update(sensors.odometry.phi)
+        velocity: speed(odometry, done),
+        rotation: phiDesiredPID.update(odometry.phi)
       };
     }
   }

actions/go-to-goal.js

@@ -1,10 +1,8 @@
 const { createAction } = require('../utils/behaviour-engine');
 const { drivingSpeeds, steeringPid } = require('../config');
-const { distance } = require('../utils')
+const { distance, isWithin } = require('../utils')
 const PID = require('../utils/pid');
 
-const isWithin = (range, a) => b => distance(a, b) < range;
-
 module.exports = createAction({
   name: 'Go to goal',
   action: (goal = ({x:0, y:0}), done) => {
@@ -24,13 +22,13 @@ module.exports = createAction({
 
       if(hasReached(odometry)) {
         done();
-        return { speed: 0, steering: 0 };
+        return { velocity: 0, rotation: 0 };
       }
 
       return {
-        speed: isCloseBy(odometry) ? drivingSpeeds.slow : drivingSpeeds.medium,
-        steering: phiDesiredPID.update(odometry.phi)
-      }
+        velocity: isCloseBy(odometry) ? drivingSpeeds.slow : drivingSpeeds.medium,
+        rotation: phiDesiredPID.update(odometry.phi)
+      };
     };
   }
 });

actions/stop.js

@@ -5,13 +5,10 @@ module.exports = createAction({
   action: ({ emergency }, done) => {
     const startTime = Date.now();
 
-    return sensors => {
-      if(
-        Date.now() - startTime > 500
-        && sensors.raw.ticks.left === 0 && sensors.raw.ticks.right === 0
-      ) done();
+    return ({ ticks: { left, right }}) => {
+      if(Date.now() - startTime > 500 && left === 0 && right === 0) done();
 
-      return { speed: 0, steering: 0, emergency };
+      return { velocity: 0, rotation: 0, emergency };
     };
   }
 });

actions/turn.js

@@ -13,10 +13,10 @@ const turnForTime = (time, direction, done) => {
 
     if(Date.now() - startTime > time) {
       done();
-      return { speed: 0, steering: 0 };
+      return { velocity: 0, rotation: 0 };
     }
 
-    return { speed: 0, steering: dir * steeringSpeed };
+    return { velocity: 0, rotation: dir * steeringSpeed };
   }
 }
 
@@ -27,10 +27,10 @@ const turnByAngle = (by, done) => {
   let isGoalSet = false;
   let target;
 
-  return sensors => {
+  return ({ odometry, ticks }) => {
     if(!isGoalSet) {
-      target = sensors.odometry.phi + by;
-      const error = Math.abs(target - sensors.odometry.phi);
+      target = odometry.phi + by;
+      const error = Math.abs(target - odometry.phi);
 
       phiDesiredPID = new PID(23/error, 0, 0);
       phiDesiredPID.angle = true;
@@ -40,29 +40,28 @@ const turnByAngle = (by, done) => {
     }
 
     if(
-      Math.abs(restrictAngle(target - sensors.odometry.phi)) < 0.1
-      && sensors.raw.ticks.left === 0 && sensors.raw.ticks.right === 0
+      Math.abs(restrictAngle(target - odometry.phi)) < 0.1
+      && ticks.left === 0 && ticks.right === 0
     ) done();
 
     // Leaky integrator checks if we're making progress
     li.leak(1);
-    if(sensors.raw.ticks.left === 0 && sensors.raw.ticks.right === 0) li.add(2);
+    if(ticks.left === 0 && ticks.right === 0) li.add(2);
     if(li.level() > 30) {
       console.log(`TURN: We aren't making progress. Bailing out.`);
       done();
     }
 
-    return {speed: 0, steering: phiDesiredPID.update(sensors.odometry.phi)};
+    return {
+      velocity: 0,
+      rotation: phiDesiredPID.update(odometry.phi)
+    };
   }
 }
 
 module.exports = createAction({
   name: 'Turn',
-  action: ({by, time, direction}, done) => {
-    if(time) {
-      return turnForTime(time, direction, done);
-    } else {
-      return turnByAngle(by, done);
-    }
-  }
+  action: ({ by, time, direction }, done) => (
+    time ? turnForTime(time, direction, done) : turnByAngle(by, done)
+  )
 });

behaviours/avoidObstacle.js

@@ -10,7 +10,7 @@ module.exports = ['left', 'right'].reduce((obj, direction) => ({
   ...obj,
   [behaviourKey(direction)]: createBehavior({
     name: `Avoid ${direction} obstacle`,
-    needsControl: sensors => sensors.raw.obstacleSensors[direction] < obstacleThreshold,
+    needsControl: ({ obstacleSensors }) => obstacleSensors[direction] < obstacleThreshold,
     actions: [
       stop({ emergency: true }),
       driveStraight({ direction: 'reverse', distance: 300 }),

package.json

@@ -22,6 +22,7 @@
     "glamor": "^2.20.40",
     "mousetrap": "^1.6.1",
     "nodemon": "^1.12.1",
+    "point-in-polygon": "^1.0.1",
     "power-off": "^1.1.2",
     "react-scripts": "^1.0.14",
     "reconnecting-websocket": "^3.2.2",

sensors/index.js

@@ -1,28 +1,29 @@
 const odometry = require('./odometry');
+const virtualBumpers = require('./virtual-bumpers');
+const { bus } = require('../utils');
+const remoteCommand = require('./remote-command');
 const { notify } = require('../ui-server');
-const { bus } = require("../utils");
-// const virtualBumpers = require('./virtual-bumpers');
 
 let lastMotorCommand = { left: 0, right: 0 };
 bus.on('motors', mc => lastMotorCommand = mc);
 
 module.exports = sensors => {
-  const processed = {
-    raw: sensors,
-    odometry: odometry({ ticks: sensors.ticks, lastMotorCommand }),
+  const odo = odometry({ ticks: sensors.ticks, lastMotorCommand });
+
+  const sensed = {
+    ...sensors,
+    odometry: odo,
+    virtualBump: virtualBumpers(odo, [
+      {x: -600, y: 600},
+      {x: -600, y: -600},
+      {x: 600, y: -600},
+      {x: 600, y: 600}
+    ]),
+    remoteCommand: remoteCommand(),
     lastMotorCommand
   };
 
-  // processed.virtualBump = virtualBumpers(processed.odometry, [
-  //   {x: -600, y: 600},
-  //   {x: -600, y: -600},
-  //   {x: 600, y: -600},
-  //   {x: 600, y: 600}
-  // ]);
-  //
-  // if(processed.virtualBump === true) console.log('VIRTUAL BUMP');
-
-  notify('sensed', processed);
+  notify('sensed', sensed);
 
-  return processed;
+  return sensed;
 };

streams/manual-control.js → sensors/remote-command.js

@@ -1,5 +1,5 @@
+// Receives commands from remote control, and UI.
 const { bus } = require('../utils');
-const { drivingSpeeds, steeringSpeed } = require('../config');
 const lirc = require('../utils/lirc_receiver');
 
 const remoteKeysToInput = {
@@ -25,17 +25,4 @@ lirc.on('keypress', ({ key }) => {
 
 bus.on('motorCommand', setInput);
 
-module.exports = sensors => {
-  let command = { speed: 0, steering: 0 };
-
-  if(input) {
-    switch(input) {
-      case 'forward': command.speed = drivingSpeeds.medium; break;
-      case 'left': command.steering = steeringSpeed; break;
-      case 'right': command.steering = -steeringSpeed; break;
-      case 'reverse': command.speed = -drivingSpeeds.medium; break;
-    }
-  }
-
-  return command;
-}
+module.exports = () => input;

sensors/virtual-bumpers.js

@@ -1,20 +1,5 @@
-module.exports = (odometry, polygonPoints) => {
-  const distances = polygonPoints.map((currentPoint, index) => {
-    const previousPoint = index === 0 ? polygonPoints[polygonPoints.length - 1] : polygonPoints[index - 1];
+const inside = require('point-in-polygon'); // Isn't npm just awesome?
 
-    if(previousPoint.x === currentPoint.x) {
-      // Vertical line. Distance is difference between x values.
-      return Math.abs(odometry.x - currentPoint.x);
-    } else {
-      // y = mx + c => m = slope, c = y-intercept
-      const slope = (currentPoint.y - previousPoint.y) / (currentPoint.x - previousPoint.x);
-      const yIntercept = currentPoint.y - (slope * currentPoint.x);
-
-      // d = |Ax + By  + C\ / sqrt(A^2 + B^2) where line = Ax + By + C = 0
-      // Converting line from y=mx+c, A = slope, B = -1, C = yIntercept
-      return Math.abs((slope * odometry.x) - odometry.y + yIntercept) / Math.sqrt(Math.pow(slope, 2) + 1);
-    }
-  });
-
-  return distances.some(x => x < 50);
+module.exports = ({ x, y }, polygonPoints) => {
+  return !inside([x,y], polygonPoints.map(({x,y}) => [x,y]));
 }

src/components/HeaderInfo.js

@@ -31,14 +31,14 @@ export default loadConditionally(() => store.sensed, props => (
       {' '}
       <span className={css({
           color: store.sensed && (
-            store.sensed.raw.batteryVoltage > 7 ? 'green' :
-              store.sensed.raw.batteryVoltage < 6.2 ? 'red' : 'orange'
+            store.sensed.batteryVoltage > 7 ? 'green' :
+              store.sensed.batteryVoltage < 6.2 ? 'red' : 'orange'
           )
         })}>
-        {store.sensed && store.sensed.raw.batteryVoltage.toFixed(2)}v
+        {store.sensed && store.sensed.batteryVoltage.toFixed(2)}v
       </span>
       <br />
-      {store.sensed && store.sensed.raw.batteryVoltage < 6.1 && 'Shutdown imminent'}
+      {store.sensed && store.sensed.batteryVoltage < 6.1 && 'Shutdown imminent'}
     </div>
     <div className={styles.enable}>
       Mode: {store.mode === 'manual' ? 'Manual' : 'Auto'}

src/components/Sensors.js

@@ -5,7 +5,7 @@ import store from '../store';
 
 export default loadConditionally(() => store.sensed, props => (
   <Window heading='Other sensors'>
-    Left obstacle: {store.sensed.raw.obstacleSensors.left}<br />
-    Right obstacle: {store.sensed.raw.obstacleSensors.right}
+    Left obstacle: {store.sensed.obstacleSensors.left}<br />
+    Right obstacle: {store.sensed.obstacleSensors.right}
   </Window>
 ));

src/store.js

@@ -4,7 +4,7 @@ const ws = new WebSocket(`ws://${document.location.hostname}:8080/`, 'protocolOn
 
 const sendCommand = (command, args) => {
   ws.send(JSON.stringify({type: 'command', command, args}));
-}
+};
 
 const store = {
   connected: false,
@@ -23,7 +23,7 @@ const addTimeSeriesData = (key, data) => {
   if(!store.timeSeriesData[key]) store.timeSeriesData[key] = [];
   const length = store.timeSeriesData[key].unshift({ data, ts: Date.now() });
   if(length > 1000) store.timeSeriesData[key].pop();
-}
+};
 
 const render = () => { if(store.onChange) store.onChange() };
 
@@ -38,8 +38,8 @@ ws.onmessage = evt => {
   if(event.type === 'notification') {
     if(knownLogs.includes(event.logType)) store[event.logType] = event.data;
     if(event.logType === 'sensed') {
-      addTimeSeriesData('ticksLeft', event.data.raw.ticks.left);
-      addTimeSeriesData('ticksRight', event.data.raw.ticks.right);
+      addTimeSeriesData('ticksLeft', event.data.ticks.left);
+      addTimeSeriesData('ticksRight', event.data.ticks.right);
       addTimeSeriesData('odometry', event.data.odometry);
     }
     if(event.logType === 'motors') {
@@ -49,6 +49,6 @@ ws.onmessage = evt => {
   }
 
   render();
-}
+};
 
 export default store;

streams/brains.js

@@ -1,28 +1,50 @@
 const { bus } = require('../utils');
 const { obj: map } = require('through2-map');
-const { notify } = require('../ui-server');
 const powerOff = require('power-off');
-const manualControl = require('./manual-control');
 const behave = require('../behaviours');
+const { drivingSpeeds, steeringSpeed } = require('../config');
+const createLeakyIntegrator = require('../utils/leaky-integrator');
+const { notify } = require('../ui-server');
 
-let mode = 'manual';
-notify('mode', mode);
+// Mode selection
+let mode;
+const setMode = m => { mode = m; notify('mode', mode); };
 
-bus.on('setMode', value => {
-  mode = value;
-  notify('mode', value);
-});
+bus.on('setMode', setMode);
+setMode('manual');
 
+// Shutdown if needed
 let shutdownInitiated = false;
-const shutdown = () => {
+const li = createLeakyIntegrator();
+const shutdownIfNeeded = batteryVoltage => {
   if(shutdownInitiated) return;
 
-  powerOff(() => {});
-  shutdownInitiated = true;
-}
+  li.leak(1);
+  if(batteryVoltage < 6) li.add(2);
+  if(li.level() > 20) {
+    powerOff(() => {});
+    shutdownInitiated = true;
+  }
+};
+
+// Manual control
+const manualControl = ({ remoteCommand }) => {
+  let command = { velocity: 0, rotation: 0 };
+
+  if(!remoteCommand) return command;
+
+  switch(remoteCommand) {
+    case 'forward': command.velocity = drivingSpeeds.medium; break;
+    case 'left': command.rotation = steeringSpeed; break;
+    case 'right': command.rotation = -steeringSpeed; break;
+    case 'reverse': command.velocity = -drivingSpeeds.medium; break;
+  }
+
+  return command;
+};
 
 module.exports = map(sensors => {
-  if(sensors.raw.batteryVoltage < 6) shutdown();
+  shutdownIfNeeded(sensors.batteryVoltage);
 
   if(mode === 'manual') return manualControl(sensors);
 

streams/hardware.js

@@ -1,4 +1,4 @@
-// This is a duplex stream that pipes out data from the sensors
+  // This is a duplex stream that pipes out data from the sensors
 // every config.clock milliseconds.
 // Motor commands can be piped into this stream, which then get sent
 // to the motors.