From 99a25e07a748e9ec0b1d71b3ef52858a019218e8 Mon Sep 17 00:00:00 2001
From: Michal Moskal <michal@moskal.me>
Date: Fri, 7 Jul 2017 15:15:36 +0100
Subject: [PATCH] Add some motor support

---
 TODO.md              |   4 +-
 libs/core/output.cpp |  21 +++++++++
 libs/core/output.ts  | 102 +++++++++++++++++++++++++++++++++++++++++++
 libs/core/pxt.json   |   2 +
 libs/core/shims.d.ts |   9 ++++
 5 files changed, 136 insertions(+), 2 deletions(-)
 create mode 100644 libs/core/output.cpp
 create mode 100644 libs/core/output.ts

diff --git a/TODO.md b/TODO.md
index 99d264d6..30c0634b 100644
--- a/TODO.md
+++ b/TODO.md
@@ -1,6 +1,6 @@
 * [x] try unlink ELF file before uploading - didn't work
-* [ ] implement serialPoll
-* [ ] try some motors
+* [x] implement serialPoll
+* [x] try some motors
 
 ## Further down
 * [ ] have some protocol for restarting user app if it's running (flag file somewhere?)
diff --git a/libs/core/output.cpp b/libs/core/output.cpp
new file mode 100644
index 00000000..ee0aea40
--- /dev/null
+++ b/libs/core/output.cpp
@@ -0,0 +1,21 @@
+#include "pxt.h"
+#include "ev3.h"
+
+namespace output {
+
+/**
+ * Create a new zero-initialized buffer.
+ * @param size number of bytes in the buffer
+ */
+//%
+Buffer createBuffer(int size) {
+    return mkBuffer(NULL, size);
+}
+
+extern "C" int WriteToPWMDevice(char *bytes, int num_bytes);
+
+//%
+void writePWM(Buffer buf) {
+    WriteToPWMDevice((char*)buf->data, buf->length);
+}
+}
\ No newline at end of file
diff --git a/libs/core/output.ts b/libs/core/output.ts
new file mode 100644
index 00000000..c5f82039
--- /dev/null
+++ b/libs/core/output.ts
@@ -0,0 +1,102 @@
+enum Output {
+    A = 0x01,
+    B = 0x02,
+    C = 0x04,
+    D = 0x08,
+    ALL = 0x0f
+}
+
+enum OutputType {
+    None = 0,
+    Tacho = 7,
+    MiniTacho = 8,
+}
+
+namespace output {
+    //% shim=output::writePWM
+    function writePWM(buf: Buffer): void { }
+
+    function mkCmd(out: Output, cmd: number, addSize: number) {
+        let b = createBuffer(2 + addSize)
+        b.setNumber(NumberFormat.UInt8LE, 0, cmd)
+        b.setNumber(NumberFormat.UInt8LE, 1, out)
+        return b
+    }
+
+    export function stop(out: Output, useBreak = false) {
+        let b = mkCmd(out, DAL.opOutputStop, 1)
+        b.setNumber(NumberFormat.UInt8LE, 2, useBreak ? 1 : 0)
+        writePWM(b)
+    }
+
+    export function start(out: Output) {
+        let b = mkCmd(out, DAL.opOutputStart, 0)
+        writePWM(b)
+    }
+
+    export function reset(out: Output) {
+        let b = mkCmd(out, DAL.opOutputReset, 0)
+        writePWM(b)
+    }
+
+    export function setSpeed(out: Output, speed: number) {
+        let b = mkCmd(out, DAL.opOutputSpeed, 1)
+        b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-100, 100, speed))
+        writePWM(b)
+    }
+
+    export function setPower(out: Output, power: number) {
+        let b = mkCmd(out, DAL.opOutputPower, 1)
+        b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-100, 100, power))
+        writePWM(b)
+    }
+
+    export function setPolarity(out: Output, polarity: number) {
+        let b = mkCmd(out, DAL.opOutputPolarity, 1)
+        b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-1, 1, polarity))
+        writePWM(b)
+    }
+
+    export interface StepOptions {
+        power?: number;
+        speed?: number; // either speed or power has to be present
+        step1: number;
+        step2: number;
+        step3: number;
+        useSteps?: boolean; // otherwise use milliseconds
+        useBreak?: boolean;
+    }
+
+    export function step(out: Output, opts: StepOptions) {
+        let op = opts.useSteps ? DAL.opOutputStepSpeed : DAL.opOutputTimeSpeed
+        let speed = opts.speed
+        if (speed == null) {
+            speed = opts.power
+            op = opts.useSteps ? DAL.opOutputStepPower : DAL.opOutputTimePower
+            if (speed == null)
+                return
+        }
+        speed = Math.clamp(-100, 100, speed)
+
+        let b = mkCmd(out, op, 15)
+        b.setNumber(NumberFormat.Int8LE, 2, speed)
+        // note that b[3] is padding
+        b.setNumber(NumberFormat.Int32LE, 4 + 4 * 0, opts.step1)
+        b.setNumber(NumberFormat.Int32LE, 4 + 4 * 1, opts.step2)
+        b.setNumber(NumberFormat.Int32LE, 4 + 4 * 2, opts.step3)
+        b.setNumber(NumberFormat.Int8LE, 4 + 4 * 3, opts.useBreak ? 1 : 0)
+        writePWM(b)
+    }
+
+    const types = [0, 0, 0, 0]
+    export function setType(out: Output, type: OutputType) {
+        let b = mkCmd(out, DAL.opOutputSetType, 3)
+        for (let i = 0; i < 4; ++i) {
+            if (out & (1 << i)) {
+                types[i] = type
+            }
+            b.setNumber(NumberFormat.UInt8LE, i + 1, types[i])
+        }
+        writePWM(b)
+    }
+}
\ No newline at end of file
diff --git a/libs/core/pxt.json b/libs/core/pxt.json
index 1f6d1c17..44c36cdf 100644
--- a/libs/core/pxt.json
+++ b/libs/core/pxt.json
@@ -11,6 +11,8 @@
         "buttons.cpp",
         "screen.cpp",
         "screen.ts",
+        "output.cpp",
+        "output.ts",
         "shims.d.ts",
         "enums.d.ts",
         "dal.d.ts",
diff --git a/libs/core/shims.d.ts b/libs/core/shims.d.ts
index 956aa617..e65e473f 100644
--- a/libs/core/shims.d.ts
+++ b/libs/core/shims.d.ts
@@ -133,5 +133,14 @@ declare namespace screen {
     //% shim=screen::setFont
     function setFont(font: ScreenFont): void;
 }
+declare namespace output {
+
+    /**
+     * Create a new zero-initialized buffer.
+     * @param size number of bytes in the buffer
+     */
+    //% shim=output::createBuffer
+    function createBuffer(size: int32): Buffer;
+}
 
 // Auto-generated. Do not edit. Really.