From 21025b5f83699618a0d8688e698387b930248644 Mon Sep 17 00:00:00 2001
From: Peli de Halleux <pelikhan@users.noreply.github.com>
Date: Wed, 20 Jun 2018 22:03:48 -0700
Subject: [PATCH] better handling end of integration step

---
 sim/state/motornode.ts | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/sim/state/motornode.ts b/sim/state/motornode.ts
index f69a9208..24f43643 100644
--- a/sim/state/motornode.ts
+++ b/sim/state/motornode.ts
@@ -150,7 +150,8 @@ namespace pxsim {
                         const step2 = this.speedCmdValues[2];
                         const step3 = this.speedCmdValues[3];
                         const brake = this.speedCmdValues[4];
-                        const dstep = (this.speedCmd == DAL.opOutputTimePower || this.speedCmd == DAL.opOutputTimeSpeed)
+                        const isTimeCommand = this.speedCmd == DAL.opOutputTimePower || this.speedCmd == DAL.opOutputTimeSpeed;
+                        const dstep = isTimeCommand
                             ? pxsim.U.now() - this.speedCmdTime
                             : this.tacho - this.speedCmdTacho;
                         if (dstep < step1) // rampup
@@ -161,6 +162,16 @@ namespace pxsim {
                             this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3);
                         else {
                             if (brake) this.speed = 0;
+                            if (!isTimeCommand) {
+                                // we need to patch the actual position of the motor when
+                                // finishing the move as our integration step introduce errors
+                                const deltaAngle = -Math.sign(speed) * (dstep - (step1 + step2 + step3));
+                                if (deltaAngle) {
+                                    this.angle += deltaAngle;
+                                    this.tacho -= Math.abs(deltaAngle);
+                                    this.setChangedState();
+                                }
+                            }
                             this.clearSpeedCmd();
                         }
                         break;