diff --git a/sim/state/motornode.ts b/sim/state/motornode.ts index b49122d5..4de3c254 100644 --- a/sim/state/motornode.ts +++ b/sim/state/motornode.ts @@ -32,11 +32,11 @@ namespace pxsim { } getSpeed() { - return Math.round(this.speed * (this._inverted ? -1 : 1)); + return Math.round(this.speed * this.invertedFactor()); } getAngle() { - return Math.round(this.angle * (this._inverted ? -1 : 1)); + return Math.round(this.angle * this.invertedFactor()); } // returns the secondary motor if any @@ -57,6 +57,7 @@ namespace pxsim { } setSyncCmd(motor: MotorNode, cmd: DAL, values: number[]) { + console.log(`motor: ${motor.port}, speed: ${values[0]}, turnRatio: ${values[0]}`); this.setSpeedCmd(cmd, values); this._synchedMotor = motor; } @@ -88,6 +89,10 @@ namespace pxsim { return this._inverted; } + invertedFactor(): number { + return this._inverted ? -1 : 1 + } + isLarge(): boolean { return this.id == NodeType.LargeMotor; } @@ -206,7 +211,7 @@ namespace pxsim { : this.tacho - this.speedCmdTacho; // 0 is special case, run infinite if (!stepsOrTime || dstep < stepsOrTime) - this.speed = speed; + this.speed = speed * this.invertedFactor(); else { if (brake) this.speed = 0; this.clearSpeedCmd(); @@ -214,7 +219,7 @@ namespace pxsim { // turn ratio is a bit weird to interpret // see https://communities.theiet.org/blogs/698/1706 - otherMotor.speed = this.speed * (100 - Math.abs(turnRatio)) / 100; + otherMotor.speed = this.speed * otherMotor.invertedFactor() * (100 - Math.abs(turnRatio)) / 100; // clamp this.speed = Math.max(-100, Math.min(100, this.speed >> 0)); diff --git a/sim/state/output.ts b/sim/state/output.ts index 49266c03..4a923cd2 100644 --- a/sim/state/output.ts +++ b/sim/state/output.ts @@ -82,15 +82,17 @@ namespace pxsim { const motors = ev3board().getMotor(port); // cancel any other sync command for(const motor of ev3board().getMotors().filter(motor => motors.indexOf(motor) < 0)) { - motor.clearSyncCmd() + motor.clearSyncCmd(); } // apply commands to all motors for (const motor of motors) { + const invertedFactor = motor.isInverted() ? -1 : 1; + //console.log(`motor.port: ${motor.port}, invertedFactor: ${invertedFactor}, speed * inv: ${speed * invertedFactor}`); const otherMotor = motors.filter(m => m.port != motor.port)[0]; motor.setSyncCmd( otherMotor, - cmd, [speed, turnRatio, stepsOrTime, brake]); + cmd, [speed * invertedFactor, turnRatio, stepsOrTime, brake]); } return 2; }