From 2cd2950496887af3de352edf44a9d3b020833ed6 Mon Sep 17 00:00:00 2001
From: Peli de Halleux <pelikhan@users.noreply.github.com>
Date: Thu, 10 Oct 2019 07:44:53 -0700
Subject: [PATCH] polarity in synched motor (#945)

* account for polarity

* more comments

* handle dual motor in runtime

* invert steer

* don't use firmware polarity
---
 libs/core/output.ts    | 16 +++++++++++-----
 sim/state/motornode.ts | 13 +------------
 sim/state/output.ts    |  6 +-----
 3 files changed, 13 insertions(+), 22 deletions(-)

diff --git a/libs/core/output.ts b/libs/core/output.ts
index 6710a1c7..46760a3a 100644
--- a/libs/core/output.ts
+++ b/libs/core/output.ts
@@ -159,6 +159,7 @@ namespace motors {
         private _accelerationTime: number;
         private _decelerationSteps: number;
         private _decelerationTime: number;
+        private _inverted: boolean;
 
         protected static output_types: number[] = [0x7, 0x7, 0x7, 0x7];
 
@@ -176,6 +177,7 @@ namespace motors {
             this._accelerationTime = 0;
             this._decelerationSteps = 0;
             this._decelerationTime = 0;
+            this._inverted = false;
         }
 
         /**
@@ -225,9 +227,11 @@ namespace motors {
         //% help=motors/motor/set-inverted
         setInverted(inverted: boolean) {
             this.init();
-            const b = mkCmd(this._port, DAL.opOutputPolarity, 1)
-            b.setNumber(NumberFormat.Int8LE, 2, inverted ? 0 : 1);
-            writePWM(b)
+            this._inverted = inverted;
+        }
+
+        protected invertedFactor(): number {
+            return this._inverted ? -1 : 1;
         }
 
         /** 
@@ -294,8 +298,9 @@ namespace motors {
         }
 
         private normalizeSchedule(speed: number, step1: number, step2: number, step3: number, unit: MoveUnit): MoveSchedule {
+            // motor polarity is not supported at the firmware level for sync motor operations
             const r: MoveSchedule = {
-                speed: Math.clamp(-100, 100, speed >> 0),
+                speed: Math.clamp(-100, 100, speed | 0) * this.invertedFactor(),
                 useSteps: true,
                 steps: [step1 || 0, step2 || 0, step3 || 0]
             }
@@ -562,6 +567,7 @@ namespace motors {
 
         private __init() {
             this.setOutputType(this._large);
+            this.setInverted(false);
         }
 
         /**
@@ -741,7 +747,7 @@ namespace motors {
         //% help=motors/synced/steer
         steer(turnRatio: number, speed: number, value: number = 0, unit: MoveUnit = MoveUnit.MilliSeconds) {
             this.init();
-            speed = Math.clamp(-100, 100, speed >> 0);
+            speed = Math.clamp(-100, 100, speed >> 0) * this.invertedFactor();
             if (!speed) {
                 this.stop();
                 return;
diff --git a/sim/state/motornode.ts b/sim/state/motornode.ts
index 15c0fb2f..6cc34bb7 100644
--- a/sim/state/motornode.ts
+++ b/sim/state/motornode.ts
@@ -9,7 +9,6 @@ namespace pxsim {
         private angle: number = 0;
         private tacho: number = 0;
         private speed: number = 0;
-        private polarity: number = 1; // -1, 1 or -1
 
         private started: boolean;
         private speedCmd: DAL;
@@ -31,7 +30,7 @@ namespace pxsim {
         }
 
         getSpeed() {
-            return Math.round(this.speed * (!this._synchedMotor && this.polarity == 0 ? -1 : 1));
+            return Math.round(this.speed);
         }
 
         getAngle() {
@@ -82,16 +81,6 @@ namespace pxsim {
             return this.id == NodeType.LargeMotor;
         }
 
-        setPolarity(polarity: number) {
-            // Either 1 or 255 (reverse)
-            /*
-                -1 : Motor will run backward  
-                0 : Motor will run opposite direction  
-                1 : Motor will run forward             
-            */
-            this.polarity = polarity;
-        }
-
         reset() {
             // not sure what reset does...
         }
diff --git a/sim/state/output.ts b/sim/state/output.ts
index b3d10bbf..e059e318 100644
--- a/sim/state/output.ts
+++ b/sim/state/output.ts
@@ -119,11 +119,7 @@ namespace pxsim {
                             return 2;
                         }
                         case DAL.opOutputPolarity: {
-                            // reverse
-                            const port = buf.data[1];
-                            const polarity = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2);
-                            const motors = ev3board().getMotor(port);
-                            motors.forEach(motor => motor.setPolarity(polarity));
+                            console.error("opOutputPolarity not supported");
                             return 2;
                         }
                         case DAL.opOutputSetType: {