Merge pull request #121 from Microsoft/motorphysics

Motor work

sim/state/motor.ts

@@ -15,11 +15,19 @@ namespace pxsim {
             MMapMethods.register("/dev/lms_motor", {
                 data,
                 beforeMemRead: () => {
+                    const outputs = ev3board().outputNodes;
                     console.log("motor before read");
                     for (let port = 0; port < DAL.NUM_OUTPUTS; ++port) {
-                        data[MotorDataOff.TachoCounts * port] = 0; // Tacho count
+                        const output = outputs[port];
-                        data[MotorDataOff.Speed * port] = 50; // Speed
+                        const speed = output ? Math.round(outputs[port].getSpeed()) : 0;
-                        data[MotorDataOff.TachoSensor * port] = 0; // Count
+                        const angle = output ? Math.round(outputs[port].getAngle()) : 0;
+                        const tsi = MotorDataOff.TachoSensor + port * MotorDataOff.Size;
+                        data[MotorDataOff.TachoCounts + port * MotorDataOff.Size] = 0; // Tacho count
+                        data[MotorDataOff.Speed + port * MotorDataOff.Size] = speed; // Speed
+                        data[tsi] = angle & 0xff; // Count
+                        data[tsi + 1] = (angle >> 8) & 0xff; // Count
+                        data[tsi + 2] = (angle >> 16) & 0xff; // Count
+                        data[tsi + 3] = (angle >> 24) & 0xff; // Count
                     }
                 },
                 read: buf => {

sim/state/motors.ts

@@ -5,13 +5,15 @@ namespace pxsim {
 
         protected angle: number = 0;
 
+        private rotationsPerMilliSecond: number;
         private speed: number;
         private large: boolean;
         private rotation: number;
         private polarity: boolean;
 
-        constructor(port: number) {
+        constructor(port: number, rpm: number) {
             super(port);
+            this.rotationsPerMilliSecond = rpm / 60000;
         }
 
         setSpeed(speed: number) {
@@ -19,7 +21,6 @@ namespace pxsim {
                 this.speed = speed;
                 this.changed = true;
                 this.setChangedState();
-                this.playMotorAnimation();
             }
         }
 
@@ -58,40 +59,21 @@ namespace pxsim {
             return this.angle;
         }
 
-        protected abstract playMotorAnimation(): void;
+        updateState(elapsed: number) {
+            const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed;
+            const angle = rotations * 360;
+            if (angle) {
+                this.angle += angle;
+                this.setChangedState();
+            }
+        }
     }
 
     export class MediumMotorNode extends MotorNode {
         id = NodeType.MediumMotor;
 
         constructor(port: number) {
-            super(port);
+            super(port, 250);
-        }
-
-        protected lastMotorAnimationId: number;
-        protected playMotorAnimation() {
-            // Max medium motor RPM is 250 according to http://www.cs.scranton.edu/~bi/2015s-html/cs358/EV3-Motor-Guide.docx
-            const rotationsPerMinute = 250; // 250 rpm at speed 100
-            const rotationsPerSecond = rotationsPerMinute / 60;
-            const fps = GAME_LOOP_FPS;
-            const rotationsPerFrame = rotationsPerSecond / fps;
-            let now;
-            let then = Date.now();
-            let interval = 1000 / fps;
-            let delta;
-            let that = this;
-            function draw() {
-                that.lastMotorAnimationId = requestAnimationFrame(draw);
-                now = Date.now();
-                delta = now - then;
-                if (delta > interval) {
-                    then = now - (delta % interval);
-                    const rotations = that.getSpeed() / 100 * rotationsPerFrame;
-                    const angle = rotations * 360;
-                    that.angle += angle;
-                }
-            }
-            draw();
         }
     }
 
@@ -99,33 +81,7 @@ namespace pxsim {
         id = NodeType.LargeMotor;
 
         constructor(port: number) {
-            super(port);
+            super(port, 170);
-        }
-
-        protected lastMotorAnimationId: number;
-        protected playMotorAnimation() {
-            // Max medium motor RPM is 170 according to http://www.cs.scranton.edu/~bi/2015s-html/cs358/EV3-Motor-Guide.docx
-            const rotationsPerMinute = 170; // 170 rpm at speed 100
-            const rotationsPerSecond = rotationsPerMinute / 60;
-            const fps = GAME_LOOP_FPS;
-            const rotationsPerFrame = rotationsPerSecond / fps;
-            let now;
-            let then = Date.now();
-            let interval = 1000 / fps;
-            let delta;
-            let that = this;
-            function draw() {
-                that.lastMotorAnimationId = requestAnimationFrame(draw);
-                now = Date.now();
-                delta = now - then;
-                if (delta > interval) {
-                    then = now - (delta % interval);
-                    const rotations = that.getSpeed() / 100 * rotationsPerFrame;
-                    const angle = rotations * 360;
-                    that.angle += angle;
-                }
-            }
-            draw();
         }
     }
 }

sim/state/nodeTypes.ts

@@ -36,5 +36,13 @@ namespace pxsim {
         setChangedState() {
             this.changed = true;
         }
+
+        /**
+         * Updates any internal state according to the elapsed time since the last call to `updateState`
+         * @param elapsed
+         */
+        updateState(elapsed: number) {
+
+        }
     }
 }

sim/state/output.ts

@@ -37,7 +37,7 @@ namespace pxsim {
                         case DAL.opOutputStepSpeed: {
                             // step speed
                             const port = buf.data[1];
-                            const speed = buf.data[2];
+                            const speed = buf.data[2] << 24 >> 24; // signed byte
                             // note that b[3] is padding
                             const step1 = buf.data[4];
                             const step2 = buf.data[5]; // angle
@@ -59,7 +59,7 @@ namespace pxsim {
                         case DAL.opOutputSpeed: {
                             // setSpeed
                             const port = buf.data[1];
-                            const speed = buf.data[2];
+                            const speed = buf.data[2] << 24 >> 24; // signed byte
                             const motors = ev3board().getMotor(port);
                             motors.forEach(motor => motor.setSpeed(speed));
                             return 2;

sim/visuals/board.ts

@@ -374,17 +374,18 @@ namespace pxsim.visuals {
                 now = Date.now();
                 delta = now - then;
                 if (delta > interval) {
-                    then = now - (delta % interval);
+                    then = now;
-                    that.updateStateStep();
+                    that.updateStateStep(delta);
                 }
             }
             loop();
         }
 
-        private updateStateStep() {
+        private updateStateStep(elapsed: number) {
             const selected = this.layoutView.getSelected();
             const inputNodes = ev3board().getInputNodes();
             inputNodes.forEach((node, index) => {
+                node.updateState(elapsed);
                 if (!node.didChange()) return;
                 const view = this.getDisplayViewForNode(node.id, index);
                 if (view) {
@@ -400,6 +401,7 @@ namespace pxsim.visuals {
 
             const outputNodes = ev3board().getMotors();
             outputNodes.forEach((node, index) => {
+                node.updateState(elapsed);
                 if (!node.didChange()) return;
                 const view = this.getDisplayViewForNode(node.id, index);
                 if (view) {

sim/visuals/nodes/largeMotorView.ts

@@ -5,8 +5,6 @@ namespace pxsim.visuals {
 
         private static ROTATING_ECLIPSE_ID = "1eb2ae58-2419-47d4-86bf-4f26a7f0cf61";
 
-        private lastMotorAnimationId: any;
-
         constructor(port: number) {
             super(LARGE_MOTOR_SVG, "large-motor", NodeType.LargeMotor, port);
         }
@@ -15,7 +13,6 @@ namespace pxsim.visuals {
             const motorState = ev3board().getMotors()[this.port];
             if (!motorState) return;
             const speed = motorState.getSpeed();
-            if (this.lastMotorAnimationId) cancelAnimationFrame(this.lastMotorAnimationId);
 
             if (!speed) return;
             this.setMotorAngle(motorState.getAngle());

sim/visuals/nodes/mediumMotorView.ts

@@ -11,8 +11,6 @@ namespace pxsim.visuals {
         private hasPreviousAngle: boolean;
         private previousAngle: number;
 
-        private lastMotorAnimationId: any;
-
         constructor(port: number) {
             super(MEDIUM_MOTOR_SVG, "medium-motor", NodeType.MediumMotor, port);
         }
@@ -25,7 +23,6 @@ namespace pxsim.visuals {
             const motorState = ev3board().getMotors()[this.port];
             if (!motorState) return;
             const speed = motorState.getSpeed();
-            if (this.lastMotorAnimationId) cancelAnimationFrame(this.lastMotorAnimationId);
 
             if (!speed) return;
             this.setMotorAngle(motorState.getAngle());