Simulator support for motor commands (#137)

* support for "step" functions

* locale files

* enum issue

* fixing decoding of array

* implement clear count

* log unknown commands

sim/dalboard.ts

@@ -118,10 +118,8 @@ namespace pxsim {
         getMotor(port: number, large?: boolean): MotorNode[] {
             if (port == 0xFF) return this.getMotors(); // Return all motors
             const motorPort = this.motorMap[port];
-            if (this.outputNodes[motorPort] == undefined) {
+            if (!this.outputNodes[motorPort])
-                this.outputNodes[motorPort] = large ?
+                this.outputNodes[motorPort] = new MotorNode(motorPort, large);
-                    new LargeMotorNode(motorPort) : new MediumMotorNode(motorPort);
-            }
             return [this.outputNodes[motorPort]];
         }
 
@@ -130,7 +128,7 @@ namespace pxsim {
         }
 
         getSensor(port: number, type: number): SensorNode {
-            if (this.inputNodes[port] == undefined) {
+            if (!this.inputNodes[port]) {
                 switch (type) {
                     case DAL.DEVICE_TYPE_GYRO: this.inputNodes[port] = new GyroSensorNode(port); break;
                     case DAL.DEVICE_TYPE_COLOR: this.inputNodes[port] = new ColorSensorNode(port); break;

sim/state/motors.ts

@@ -1,88 +1,122 @@
 namespace pxsim {
 
-    export abstract class MotorNode extends BaseNode {
+    export class MotorNode extends BaseNode {
         isOutput = true;
-
-        protected angle: number = 0;
-
         private rotationsPerMilliSecond: number;
-        private speed: number;
+
-        private large: boolean;
+        // current state
-        private rotation: number;
+        private angle: number = 0;
+        private tacho: number = 0;
+        private speed: number = 0;
+
         private polarity: boolean;
+        private started: boolean;
+        private speedCmd: DAL;
+        private speedCmdValues: number[];
+        private speedCmdTacho: number;
+        private speedCmdTime: number;
 
-        constructor(port: number, rpm: number) {
+        constructor(port: number, large: boolean) {
             super(port);
-            this.rotationsPerMilliSecond = rpm / 60000;
+            this.setLarge(large);
-        }
-
-        setSpeed(speed: number) {
-            if (this.speed != speed) {
-                this.speed = speed;
-                this.changed = true;
-                this.setChangedState();
-            }
-        }
-
-        setLarge(large: boolean) {
-            this.large = large;
         }
 
         getSpeed() {
             return this.speed;
         }
 
-        stepSpeed(speed: number, angle: number, brake: boolean) {
+        getAngle() {
-            // TODO: implement
+            return this.angle;
+        }
+
+        setSpeedCmd(cmd: DAL, values: number[]) {
+            this.speedCmd = cmd;
+            this.speedCmdValues = values;
+            this.speedCmdTacho = this.angle;
+            this.speedCmdTime = pxsim.U.now();
+        }
+
+        clearSpeedCmd() {
+            delete this.speedCmd;
+        }
+
+        setLarge(large: boolean) {
+            this.id = large ? NodeType.LargeMotor : NodeType.MediumMotor;
+            this.rotationsPerMilliSecond = (large ? 170 : 250) / 60000;
         }
 
         setPolarity(polarity: number) {
             // Either 1 or 255 (reverse)
             this.polarity = polarity === 255;
-            // TODO: implement
         }
 
         reset() {
-            // TODO: implement
+            // not sure what reset does...
+        }
+
+        clearCount() {
+            this.tacho = 0;
+            this.angle = 0;
         }
 
         stop() {
-            // TODO: implement
+            this.started = false;
-            this.setSpeed(0);
         }
 
         start() {
-            // TODO: implement
+            this.started = true;
-            this.setChangedState();
-        }
-
-        public getAngle() {
-            return this.angle;
         }
 
         updateState(elapsed: number) {
-            const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed;
+            // compute new speed
-            const angle = rotations * 360;
+            switch (this.speedCmd) {
-            if (angle) {
+                case DAL.opOutputSpeed:
-                this.angle += angle;
+                case DAL.opOutputPower:
+                    // assume power == speed
+                    // TODO: PID
+                    this.speed = this.speedCmdValues[0];
+                    break;
+                case DAL.opOutputTimeSpeed:
+                case DAL.opOutputTimePower:
+                case DAL.opOutputStepPower:
+                case DAL.opOutputStepSpeed:
+                    // ramp up, run, ramp down, <brake> using time
+                    const speed = this.speedCmdValues[0];
+                    const step1 = this.speedCmdValues[1];
+                    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) 
+                        ? pxsim.U.now() - this.speedCmdTime
+                        : this.tacho - this.speedCmdTacho;
+                    if (dstep < step1) // rampup
+                        this.speed = speed * dstep / step1;
+                    else if (dstep < step1 + step2) // run
+                        this.speed = speed;
+                    else if (dstep < step1 + step2 + step3)
+                        this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3);
+                    else {
+                        if (brake) this.speed = 0;
+                        this.clearSpeedCmd();
+                    }
+                    break;
+            }
+
+            // compute delta angle
+            const rotations = this.speed / 100 * this.rotationsPerMilliSecond * elapsed;
+            const deltaAngle = rotations * 360;
+            if (deltaAngle) {
+                this.angle += deltaAngle;
+                this.tacho += Math.abs(deltaAngle);
                 this.setChangedState();
             }
+
+            // if the motor was stopped or there are no speed commands,
+            // let it coast to speed 0
+            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);
+            }
         }
     }
-
-    export class MediumMotorNode extends MotorNode {
-        id = NodeType.MediumMotor;
-
-        constructor(port: number) {
-            super(port, 250);
-        }
-    }
-
-    export class LargeMotorNode extends MotorNode {
-        id = NodeType.LargeMotor;
-
-        constructor(port: number) {
-            super(port, 170);
-        }
-    }
 }

sim/state/output.ts

@@ -34,24 +34,32 @@ namespace pxsim {
                             motors.forEach(motor => motor.reset());
                             return 2;
                         }
-                        case DAL.opOutputStepSpeed: {
+                        case DAL.opOutputClearCount:
+                            const port = buf.data[1];
+                            const motors = ev3board().getMotor(port);
+                            motors.forEach(motor => motor.clearCount());
+                            break;
+                        case DAL.opOutputStepPower:
+                        case DAL.opOutputStepSpeed:
+                        case DAL.opOutputTimePower:
+                        case DAL.opOutputTimeSpeed: {
                             // step speed
                             const port = buf.data[1];
-                            const speed = buf.data[2] << 24 >> 24; // signed byte
+                            const speed = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2); // signed byte
-                            // note that b[3] is padding
+                            // note that b[3] is padding                            
-                            const step1 = buf.data[4];
+                            const step1 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 4);
-                            const step2 = buf.data[5]; // angle
+                            const step2 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 8);
-                            const step3 = buf.data[6];
+                            const step3 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 12);
-                            const brake = buf.data[7];
+                            const brake = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 16);
                             //console.log(buf);
                             const motors = ev3board().getMotor(port);
-                            motors.forEach(motor => motor.stepSpeed(speed, step2, brake === 1));
+                            motors.forEach(motor => motor.setSpeedCmd(cmd, [speed, step1, step2, step3, brake]));
                             return 2;
                         }
                         case DAL.opOutputStop: {
                             // stop
                             const port = buf.data[1];
-                            const brake = buf.data[2];
+                            const brake = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2);
                             const motors = ev3board().getMotor(port);
                             motors.forEach(motor => motor.stop());
                             return 2;
@@ -59,9 +67,9 @@ namespace pxsim {
                         case DAL.opOutputSpeed: {
                             // setSpeed
                             const port = buf.data[1];
-                            const speed = buf.data[2] << 24 >> 24; // signed byte
+                            const speed = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2);
                             const motors = ev3board().getMotor(port);
-                            motors.forEach(motor => motor.setSpeed(speed));
+                            motors.forEach(motor => motor.setSpeedCmd(cmd, [speed]));
                             return 2;
                         }
                         case DAL.opOutputStart: {
@@ -74,7 +82,7 @@ namespace pxsim {
                         case DAL.opOutputPolarity: {
                             // reverse
                             const port = buf.data[1];
-                            const polarity = buf.data[2];
+                            const polarity = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2);
                             const motors = ev3board().getMotor(port);
                             motors.forEach(motor => motor.setPolarity(polarity));
                             return 2;
@@ -86,6 +94,9 @@ namespace pxsim {
                             motors.forEach(motor => motor.setLarge(large));
                             return 2;
                         }
+                        default:
+                            console.warn('unknown cmd: ' + cmd);
+                            break;
                     }
 
                     console.log("pwm write");