From 9c13d2755a25811918fe590202d0758633b6a4b5 Mon Sep 17 00:00:00 2001
From: Nathan Kunicki <nathank@mediamolecule.com>
Date: Tue, 26 Jun 2018 14:56:39 +0100
Subject: [PATCH] Added speed mapping

---
 boosthub.ts |  6 +++---
 hub.ts      | 24 +++++++++++++++++++++++-
 wedo2hub.ts |  2 +-
 3 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/boosthub.ts b/boosthub.ts
index 3a8891b..2714b90 100644
--- a/boosthub.ts
+++ b/boosthub.ts
@@ -102,14 +102,14 @@ export class BoostHub extends Hub {
                 const portObj = this._ports[port];
                 if (time) {
                     portObj.busy = true;
-                    const data = Buffer.from([0x0c, 0x00, 0x81, portObj.value, 0x11, 0x09, 0x00, 0x00, speed, 0x64, 0x7f, 0x03]);
+                    const data = Buffer.from([0x0c, 0x00, 0x81, portObj.value, 0x11, 0x09, 0x00, 0x00, this._mapSpeed(speed), 0x64, 0x7f, 0x03]);
                     data.writeUInt16LE(time > 65535 ? 65535 : time, 6);
                     characteristic.write(data, false);
                     portObj.finished = () => {
                         return resolve();
                     };
                 } else {
-                    const data = Buffer.from([0x0a, 0x00, 0x81, portObj.value, 0x11, 0x01, speed, 0x64, 0x7f, 0x03]);
+                    const data = Buffer.from([0x0a, 0x00, 0x81, portObj.value, 0x11, 0x01, this._mapSpeed(speed), 0x64, 0x7f, 0x03]);
                     characteristic.write(data, false);
                     return resolve();
                 }
@@ -134,7 +134,7 @@ export class BoostHub extends Hub {
                 portObj.busy = true;
                 const data = Buffer.from([0x0e, 0x00, 0x81, portObj.value, 0x11, 0x0b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x7f, 0x03]);
                 data.writeUInt32LE(angle, 6);
-                data.writeInt8(speed, 10);
+                data.writeUInt8(this._mapSpeed(speed), 10);
                 characteristic.write(data, false);
                 portObj.finished = () => {
                     return resolve();
diff --git a/hub.ts b/hub.ts
index 0d060ce..312e49a 100644
--- a/hub.ts
+++ b/hub.ts
@@ -6,6 +6,7 @@ import { Port } from "./port";
 import * as Consts from "./consts";
 
 import Debug = require("debug");
+import { METHODS } from "http";
 const debug = Debug("hub");
 
 
@@ -16,7 +17,8 @@ const debug = Debug("hub");
 export class Hub extends EventEmitter {
 
 
-    public autoSubscribe: boolean;
+    public autoSubscribe: boolean = true;
+    public useSpeedMap: boolean = true;
     public type: Consts.Hubs = Consts.Hubs.UNKNOWN;
     public uuid: string;
 
@@ -225,6 +227,26 @@ export class Hub extends EventEmitter {
     }
 
 
+    protected _mapSpeed (speed: number) {
+        if (!this.useSpeedMap) {
+            return speed;
+        }
+        if (speed >= 1) {
+            if (speed > 100) {
+                speed = 100;
+            }
+            return Math.round((speed - 1) * (97 - 15) / (100 - 1) + 15);
+        } else if (speed <= -1) {
+            if (speed < -100) {
+                speed = -100;
+            }
+            return Math.round((speed - -100) * (245 - 160) / (-1 - -100) + 160);
+        } else {
+            return 0;
+        }
+    }
+
+
     private _getModeForDeviceType (type: Consts.Devices) {
         switch (type) {
             case Consts.Devices.BASIC_MOTOR:
diff --git a/wedo2hub.ts b/wedo2hub.ts
index 1397d3f..91c83e0 100644
--- a/wedo2hub.ts
+++ b/wedo2hub.ts
@@ -107,7 +107,7 @@ export class WeDo2Hub extends Hub {
         return new Promise((resolve, reject) => {
             const characteristic = this._characteristics[Consts.BLECharacteristics.WEDO2_MOTOR_VALUE_WRITE];
             if (characteristic) {
-                characteristic.write(Buffer.from([this._ports[port].value, 0x01, 0x02, speed]), false);
+                characteristic.write(Buffer.from([this._ports[port].value, 0x01, 0x02, this._mapSpeed(speed)]), false);
                 return resolve();
             }
         });