From aa6b6daf4593b97e3e40b3dceab55d406c88f992 Mon Sep 17 00:00:00 2001
From: "Matthias L. Jugel" <leo@thinkberg.com>
Date: Sun, 8 Oct 2017 17:26:52 +0200
Subject: [PATCH] hotfix for motor/servo problem
 (https://calliope-mini.myjetbrains.com/youtrack/agiles/82-2/83-5)

---
 libs/core/pins.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/libs/core/pins.cpp b/libs/core/pins.cpp
index 90fac148..60587775 100644
--- a/libs/core/pins.cpp
+++ b/libs/core/pins.cpp
@@ -206,6 +206,17 @@ namespace pins {
         return end - start;
     }
 
+    // TODO FIX THIS IN THE DAL!
+    inline void fixMotorIssue(AnalogPin name) {
+        NRF_TIMER2->SHORTS = TIMER_SHORTS_COMPARE3_CLEAR_Msk;
+        NRF_TIMER2->INTENCLR = TIMER_INTENCLR_COMPARE3_Msk;
+        NRF_TIMER2->PRESCALER = 4;
+        NRF_TIMER2->CC[3] = 20000;
+        NRF_TIMER2->TASKS_START = 1;
+        NRF_TIMER2->EVENTS_COMPARE[3] = 0;
+        PINOP(getDigitalValue());
+    }
+
     /**
      * Writes a value to the servo, controlling the shaft accordingly. On a standard servo, this will set the angle of the shaft (in degrees), moving the shaft to that orientation. On a continuous rotation servo, this will set the speed of the servo (with ``0`` being full-speed in one direction, ``180`` being full speed in the other, and a value near ``90`` being no movement).
      * @param name pin to write to
@@ -215,6 +226,7 @@ namespace pins {
     //% blockId=device_set_servo_pin block="servo write|pin %name|to %value" blockGap=8
     //% parts=microservo trackArgs=0
     void servoWritePin(AnalogPin name, int value) {
+        fixMotorIssue(name);
         PINOP(setServoValue(value));
     }
 
@@ -226,6 +238,7 @@ namespace pins {
     //% help=pins/servo-set-pulse weight=19
     //% blockId=device_set_servo_pulse block="servo set pulse|pin %value|to (µs) %micros"
     void servoSetPulse(AnalogPin name, int micros) {
+        fixMotorIssue(name);
         PINOP(setServoPulseUs(micros));
     }