Merge pull request #5 from Memristor-Robotics/wip-optimisation

Obstacle detection optimisations

config/big.json

@@ -12,6 +12,78 @@
       }
     },
 
+    "ServoCollectorStopper": {
+      "@class": "drivers/dynamixel/DynamixelDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "id": 6,
+      "cid": 2000
+    },
+
+    "ServoCollectorTrackLeft": {
+      "@class": "drivers/dynamixel/DynamixelDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "id": 4,
+      "cid": 2000
+    },
+
+    "ServoCollectorHandLeft": {
+      "@class": "drivers/dynamixel/DynamixelDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "id": 2,
+      "cid": 2000
+    },
+
+    "ServoCollectorTrackRight": {
+      "@class": "drivers/dynamixel/DynamixelDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "id": 9,
+      "cid": 2000
+    },
+
+    "ServoCollectorHandRight": {
+      "@class": "drivers/dynamixel/DynamixelDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "id": 8,
+      "cid": 2000
+    },
+
+    "CollectorBigTrack": {
+      "@class": "drivers/hbridge/HBridgeDriver",
+      "@load": true,
+      "@dependencies": {
+        "communicator": "CanDriver"
+      },
+      "cid": 1101
+    },
+
+    "LunarCollector": {
+      "@class": "drivers/lunarcollector/LunarCollectorDriver",
+      "@load": true,
+      "@dependencies": {
+        "leftTrack": "ServoCollectorTrackLeft",
+        "rightTrack": "ServoCollectorTrackRight",
+        "leftHand": "ServoCollectorHandLeft",
+        "rightHand": "ServoCollectorHandRight",
+        "bigTrack": "CollectorBigTrack"
+      }
+    },
+
+
     "InfraredAuto": {
       "@class": "drivers/infrared/InfraredDriver",
       "@load": false,

package.json

@@ -28,7 +28,7 @@
     "nconf": "^0.8.4",
     "require-new": "^1.1.0",
     "simplify-js": "^1.2.1",
-    "sinon": "^1.17.6",
+    "sinon": "^2.1.0",
     "socketcan": "^2.1.3",
     "termios": "^0.1.3",
     "usage": "^0.7.1"

src/drivers/dynamixel/DynamixelDriver.experiment.js

@@ -6,25 +6,38 @@ global.Mep = require('../../Mep');
 const AX12 = require('./DynamixelDriver');
 const CAN = require('../can/CanDriver');
 
-let ax2 = new AX12('test', {
+let ican = new CAN('CANTest', {});
+
+let ax1 = new AX12('test', {
     id: 2,
     cid: 2000,
-    _communicator: new CAN('CANTest', {})
+    _communicator: ican
 });
 
-
-let ax1 = new AX12('test', {
-    id: 1,
+let ax2 = new AX12('test', {
+    id: 4,
     cid: 2000,
-    _communicator: new CAN('CANTest', {})
+    _communicator: ican
 });
 
 
-//ax1.setLED(false);
-ax1.setSpeed(200);
+
+//ax1.setLED(true);
+//ax1.setSpeed(1000, false);
+
+ax1.getPosition()
+    .then((pos) => { console.log(pos); })
+    .catch(() => { console.log('fail'); });
+ax1.go(500).then(() => {
+    console.log('success');
+}).catch(() => {
+    console.log('fail');
+});
+//ax2.setSpeed(500);
+//ax2.setPosition(1000);
 
 function move(pos) {
-    ax1.setSpeed(100);
+    ax1.setSpeed(200);
     pos = Math.abs(pos);
     ax1.go(pos, {timeout: 2000}).then(() => {
         move((pos + 50) % 200 + 1);
@@ -34,9 +47,10 @@ function move(pos) {
         move((pos + 50) % 200 + 1);
     });
 }
-console.log('start');
+//move();
+//console.log('start');
 //ax1.go(1).then(() => { console.log('asdasdasd'); });
-move(100);
+//move(100);
 //ax2.setId(2);
 
 /*

src/drivers/dynamixel/DynamixelDriver.js

@@ -1,10 +1,12 @@
 'use strict';
 
 /** @namespace drivers.dynamixel */
+const TaskError = Mep.require('strategy/TaskError');
 
-const TAG = 'Dynamixel';
 
 
+const TAG = 'DynamixelDriver';
+
 /**
  * Communicates with dynamixel servos (AX12 & RX24).
  * NOTE: This class doesn't send start bytes & checksum, please make custom `communicator`
@@ -66,7 +68,8 @@ class DynamixelDriver {
     constructor(name, config) {
         this.config = Object.assign({
             id: 0xFE,
-            type: 'AX'  // AX or RX
+            maxPosition: 1023,
+            minPosition: 0
         }, config);
         this.name = name;
 
@@ -88,51 +91,46 @@ class DynamixelDriver {
     }
 
     _onDataReceived(data) {
-        // Length === 1 means there is an error in communication (UART, Servo <--> AVR)
-        if (data.length === 1) {
-            switch(data.readInt8(0)) {
-                case 0x03:
-                    Mep.Log.error(TAG, this.name, 'RX Timeout error');
-                    break;
-
-                case 0x02:
-                    Mep.Log.error(TAG, this.name, 'RX Data corrupted');
-                    break;
-
-                case 0x04:
-                    Mep.Log.error(TAG, this.name, 'TX transfer failed');
-                    break;
-
-                case 0x05:
-                    Mep.Log.error(TAG, this.name, 'TX transfer timeout');
-                    break;
-
-                default:
-                    Mep.Log.error(TAG, this.name, 'Unhandled error', data);
-                    break;
+        if (data.readUInt8(0) === (this.config.id | 0)) {
+
+            // Length === 1 means there is an error in communication (UART, Servo <--> AVR)
+            if (data.length === 2) {
+                switch (data.readInt8(1)) {
+                    case 0x03:
+                        Mep.Log.error(TAG, this.name, 'RX Timeout error');
+                        break;
+
+                    case 0x02:
+                        Mep.Log.error(TAG, this.name, 'RX Data corrupted');
+                        break;
+
+                    case 0x04:
+                        Mep.Log.error(TAG, this.name, 'TX transfer failed');
+                        break;
+
+                    case 0x05:
+                        Mep.Log.error(TAG, this.name, 'TX transfer timeout');
+                        break;
+
+                    default:
+                        Mep.Log.error(TAG, this.name, 'Unhandled error', data);
+                        break;
+                }
+                return;
             }
-            return;
-        }
 
-        if (data.readUInt8(0) === (this.config.id | 0)) {
             if (this.uniqueDataReceivedCallback !== null) {
                 this.uniqueDataReceivedCallback(data);
             }
         }
     }
 
-    init(callback) {
-        callback();
-        // Check status
-    }
-
     getTemperature() {
         return this._read(DynamixelDriver.AX_PRESENT_TEMPERATURE);
     }
 
     async getVoltage() {
-        let val = await this._read(DynamixelDriver.AX_PRESENT_VOLTAGE);
-        return val / 10;
+        return await this._read(DynamixelDriver.AX_PRESENT_VOLTAGE);
     }
 
     getLoad() {
@@ -148,8 +146,7 @@ class DynamixelDriver {
     }
 
     async getPosition() {
-        let position = await this._read(DynamixelDriver.AX_PRESENT_POSITION_L, true);
-        return ((position * (300 / 1023)) | 0);
+        return await this._read(DynamixelDriver.AX_PRESENT_POSITION_L, true);
     }
 
     getSpeed() {
@@ -167,8 +164,8 @@ class DynamixelDriver {
      */
     go(position, config) {
         let c = Object.assign({
-            pollingPeriod: 40,
-            tolerance: 3,
+            pollingPeriod: 150,
+            tolerance: 20,
             timeout: 3000,
             firmwareImplementation: false
         }, config);
@@ -181,7 +178,7 @@ class DynamixelDriver {
             // Apply time out
             setTimeout(() => {
                 timeout = true;
-                reject();
+                reject(new TaskError(TAG, 'timeout', 'Dynamixel cannot reach position in time'));
             }, c.timeout);
 
             if (c.firmwareImplementation === true) {
@@ -204,7 +201,7 @@ class DynamixelDriver {
                                     checkPosition();
                                 }
                             }
-                        });
+                        }).catch(checkPosition);
                     }, c.pollingPeriod);
                 };
                 checkPosition();
@@ -254,28 +251,31 @@ class DynamixelDriver {
     }
 
     setPosition(position) {
-        if (position > 300 || position < 1) {
+        if (position > this.config.maxPosition || position < this.config.minPosition) {
             throw Error(TAG, this.name, 'Position out of range!');
         }
-
-        this._writeWord(DynamixelDriver.AX_GOAL_POSITION_L, (position * (1023 / 300)) | 0);
+        this._writeWord(DynamixelDriver.AX_GOAL_POSITION_L, position | 0);
     }
 
-    setSpeed(speed) {
+    setSpeed(speed, inverse = false) {
         if (speed > 1023 || speed < 0) {
             Mep.Log.error(TAG, this.name, 'Speed out of range!');
             return;
         }
 
+        if (inverse === true) {
+            speed = (1 << 10) | speed;
+        }
+
         this._writeWord(DynamixelDriver.AX_GOAL_SPEED_L, speed | 0);
     }
 
     setCWAngleLimit(angle) {
-        this._writeWord(DynamixelDriver.AX_CW_ANGLE_LIMIT_L, (angle * (1023 / 300)) | 0);
+        this._writeWord(DynamixelDriver.AX_CW_ANGLE_LIMIT_L, angle | 0);
     }
 
     setCCWAngleLimit(angle) {
-        this._writeWord(DynamixelDriver.AX_CCW_ANGLE_LIMIT_L, (angle * (1023 / 300)) | 0);
+        this._writeWord(DynamixelDriver.AX_CCW_ANGLE_LIMIT_L, angle | 0);
     }
 
     setLED(on) {
@@ -319,11 +319,12 @@ class DynamixelDriver {
 
         return new Promise((resolve, reject) => {
             if (ax.config.id === 0xFE) {
-                Mep.Log.error(TAG, this.name, 'Cannot use broadcast ID for reading');
-                reject();
+                reject(new TaskError(TAG, 'broadcast', 'Cannot use broadcast ID for reading'));
                 return;
             }
 
+
+
             ax.uniqueDataReceivedCallback = (data) => {
                 // Catch error code
                 if (data.readUInt8(2) !== 0x00) {

src/drivers/hbridge/HBridgeDriver.experiment.js

@@ -0,0 +1,27 @@
+global.Mep = require('../../Mep');
+const HBridge = require('./HBridgeDriver');
+const CAN = require('../can/CanDriver');
+
+let can = new CAN('CANTest', {});
+
+
+
+let hbridge = new HBridge('TestHBridge', {
+    cid: 1101,
+    _communicator: can
+});
+
+
+hbridge.start(100);
+
+setTimeout(() => {
+    hbridge.stop();
+}, 1000);
+
+setTimeout(() => {
+    hbridge.start(100, true);
+}, 2000);
+
+setTimeout(() => {
+    hbridge.stop();
+}, 3000);