Motorworks2 (#141)

* fixing polarity

* allocate motor on motorUsed only

* perform sub-step integration step for better precision

sim/dalboard.ts

@@ -115,12 +115,29 @@ namespace pxsim {
             return this.brickNode;
         }
 
+        motorUsed(port:number, large: boolean) {
+            for(let i = 0; i < DAL.NUM_OUTPUTS; ++i) {
+                const p = 1 << i;
+                if (port & p) {
+                    const motorPort = this.motorMap[p];
+                    if (!this.outputNodes[motorPort])
+                        this.outputNodes[motorPort] = new MotorNode(motorPort, large);
+                }    
+            }            
+        }
+
         getMotor(port: number, large?: boolean): MotorNode[] {
-            if (port == 0xFF) return this.getMotors(); // Return all motors
+            const r = [];
-            const motorPort = this.motorMap[port];
+            for(let i = 0; i < DAL.NUM_OUTPUTS; ++i) {
-            if (!this.outputNodes[motorPort])
+                const p = 1 << i;
-                this.outputNodes[motorPort] = new MotorNode(motorPort, large);
+                if (port & p) {
-            return [this.outputNodes[motorPort]];
+                    const motorPort = this.motorMap[p];
+                    const outputNode = this.outputNodes[motorPort];
+                    if (outputNode)
+                        r.push(outputNode);
+                }    
+            }
+            return r;
         }
 
         getMotors() {

sim/state/input.ts

@@ -2,8 +2,8 @@
 namespace pxsim.motors {
 
     export function __motorUsed(port: number, large: boolean) {
-        console.log("MOTOR INIT " + port);
+        //console.log("MOTOR INIT " + port);
-        const motors = ev3board().getMotor(port, large);
+        ev3board().motorUsed(port, large);
         runtime.queueDisplayUpdate();
     }
 }
@@ -11,7 +11,7 @@ namespace pxsim.motors {
 namespace pxsim.sensors {
 
     export function __sensorUsed(port: number, type: number) {
-        console.log("SENSOR INIT " + port + ", type: " + type);
+        //console.log("SENSOR INIT " + port + ", type: " + type);
         const sensor = ev3board().getSensor(port, type);
         runtime.queueDisplayUpdate();
     }

sim/state/motor.ts

@@ -19,8 +19,8 @@ namespace pxsim {
                     // console.log("motor before read");
                     for (let port = 0; port < DAL.NUM_OUTPUTS; ++port) {
                         const output = outputs[port];
-                        const speed = output ? Math.round(outputs[port].getSpeed()) : 0;
+                        const speed = output ? outputs[port].getSpeed() : 0;
-                        const angle = output ? Math.round(outputs[port].getAngle()) : 0;
+                        const angle = output ? outputs[port].getAngle() : 0;
                         const tci = MotorDataOff.TachoCounts + port * MotorDataOff.Size;
                         const tsi = MotorDataOff.TachoSensor + port * MotorDataOff.Size;
                         data[tci] = data[tci + 1] = data[tci + 2] = data[tci + 3] = 0; // Tacho count

sim/state/motornode.ts

@@ -8,8 +8,8 @@ namespace pxsim {
         private angle: number = 0;
         private tacho: number = 0;
         private speed: number = 0;
+        private polarity: number = 1; // -1, 1 or -1
 
-        private polarity: boolean;
         private started: boolean;
         private speedCmd: DAL;
         private speedCmdValues: number[];
@@ -22,7 +22,7 @@ namespace pxsim {
         }
 
         getSpeed() {
-            return this.speed;
+            return this.speed * (this.polarity  == 0 ? -1 : 1);
         }
 
         getAngle() {
@@ -42,12 +42,18 @@ namespace pxsim {
 
         setLarge(large: boolean) {
             this.id = large ? NodeType.LargeMotor : NodeType.MediumMotor;
+            // large 170 rpm  (https://education.lego.com/en-us/products/ev3-large-servo-motor/45502)
             this.rotationsPerMilliSecond = (large ? 170 : 250) / 60000;
         }
 
         setPolarity(polarity: number) {
             // Either 1 or 255 (reverse)
-            this.polarity = polarity === 255;
+            /*
+                -1 : Motor will run backward  
+                0 : Motor will run opposite direction  
+                1 : Motor will run forward             
+            */
+            this.polarity = polarity;
         }
 
         reset() {
@@ -68,6 +74,18 @@ namespace pxsim {
         }
 
         updateState(elapsed: number) {
+            console.log(`motor: ${elapsed}ms - ${this.speed}% - ${this.angle}> - ${this.tacho}|`)
+            const interval = Math.min(20, elapsed);
+            let t = 0;
+            while(t < elapsed) {
+                let dt = interval;
+                if (t + dt > elapsed) dt = elapsed - t;
+                this.updateStateStep(dt);
+                t += dt;
+            }
+        }
+
+        private updateStateStep(elapsed: number) {
             // compute new speed
             switch (this.speedCmd) {
                 case DAL.opOutputSpeed:
@@ -99,12 +117,13 @@ namespace pxsim {
                         if (brake) this.speed = 0;
                         this.clearSpeedCmd();
                     }
+                    this.speed = Math.round(this.speed); // integer only
                     break;
             }
 
             // compute delta angle
-            const rotations = this.speed / 100 * this.rotationsPerMilliSecond * elapsed;
+            const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed;
-            const deltaAngle = rotations * 360;
+            const deltaAngle = Math.round(rotations * 360);
             if (deltaAngle) {
                 this.angle += deltaAngle;
                 this.tacho += Math.abs(deltaAngle);
@@ -113,9 +132,9 @@ namespace pxsim {
 
             // if the motor was stopped or there are no speed commands,
             // let it coast to speed 0
-            if (this.speed && (!this.started || !this.speedCmd)) {
+            if (this.speed && !(this.started || this.speedCmd)) {
                 // decay speed 5% per tick
-                this.speed = Math.max(0, Math.abs(this.speed) - 5) * Math.sign(this.speed);
+                this.speed = Math.round(Math.max(0, Math.abs(this.speed) - 10) * Math.sign(this.speed));
             }
         }
     }

sim/visuals/board.ts

@@ -358,14 +358,14 @@ namespace pxsim.visuals {
             if (!this.running) return;
             const fps = GAME_LOOP_FPS;
             let now;
-            let then = Date.now();
+            let then = pxsim.U.now();
             let interval = 1000 / fps;
             let delta;
             let that = this;
             function loop() {
                 const animationId = requestAnimationFrame(loop);
                 that.lastAnimationIds.push(animationId);
-                now = Date.now();
+                now = pxsim.U.now();
                 delta = now - then;
                 if (delta > interval) {
                     then = now;