diff --git a/sim/dalboard.ts b/sim/dalboard.ts index 608d2443..4e613d9e 100644 --- a/sim/dalboard.ts +++ b/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 motorPort = this.motorMap[port]; - if (!this.outputNodes[motorPort]) - this.outputNodes[motorPort] = new MotorNode(motorPort, large); - return [this.outputNodes[motorPort]]; + const r = []; + for(let i = 0; i < DAL.NUM_OUTPUTS; ++i) { + const p = 1 << i; + if (port & p) { + const motorPort = this.motorMap[p]; + const outputNode = this.outputNodes[motorPort]; + if (outputNode) + r.push(outputNode); + } + } + return r; } getMotors() { diff --git a/sim/state/input.ts b/sim/state/input.ts index a00ceec2..1f62e95e 100644 --- a/sim/state/input.ts +++ b/sim/state/input.ts @@ -2,8 +2,8 @@ namespace pxsim.motors { export function __motorUsed(port: number, large: boolean) { - console.log("MOTOR INIT " + port); - const motors = ev3board().getMotor(port, large); + //console.log("MOTOR INIT " + port); + 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(); } diff --git a/sim/state/motor.ts b/sim/state/motor.ts index fffab968..308d2197 100644 --- a/sim/state/motor.ts +++ b/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 angle = output ? Math.round(outputs[port].getAngle()) : 0; + const speed = output ? outputs[port].getSpeed() : 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 diff --git a/sim/state/motors.ts b/sim/state/motornode.ts similarity index 73% rename from sim/state/motors.ts rename to sim/state/motornode.ts index 79a6bd18..5676eae6 100644 --- a/sim/state/motors.ts +++ b/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 deltaAngle = rotations * 360; + const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed; + 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)); } } } diff --git a/sim/visuals/board.ts b/sim/visuals/board.ts index cdb6fa9b..e598839e 100644 --- a/sim/visuals/board.ts +++ b/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;