diff --git a/sim/dalboard.ts b/sim/dalboard.ts index b542acb0..608d2443 100644 --- a/sim/dalboard.ts +++ b/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) { - this.outputNodes[motorPort] = large ? - new LargeMotorNode(motorPort) : new MediumMotorNode(motorPort); - } + if (!this.outputNodes[motorPort]) + this.outputNodes[motorPort] = new MotorNode(motorPort, large); 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; diff --git a/sim/state/motors.ts b/sim/state/motors.ts index 8abb3c75..79a6bd18 100644 --- a/sim/state/motors.ts +++ b/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; - private rotation: number; + + // current state + 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; - } - - setSpeed(speed: number) { - if (this.speed != speed) { - this.speed = speed; - this.changed = true; - this.setChangedState(); - } - } - - setLarge(large: boolean) { - this.large = large; + this.setLarge(large); } getSpeed() { return this.speed; } - stepSpeed(speed: number, angle: number, brake: boolean) { - // TODO: implement + getAngle() { + 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.setSpeed(0); + this.started = false; } start() { - // TODO: implement - this.setChangedState(); - } - - public getAngle() { - return this.angle; + this.started = true; } updateState(elapsed: number) { - const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed; - const angle = rotations * 360; - if (angle) { - this.angle += angle; + // compute new speed + switch (this.speedCmd) { + case DAL.opOutputSpeed: + 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, 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); - } - } } \ No newline at end of file diff --git a/sim/state/output.ts b/sim/state/output.ts index a3b93f8e..53d9f420 100644 --- a/sim/state/output.ts +++ b/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 - // note that b[3] is padding - const step1 = buf.data[4]; - const step2 = buf.data[5]; // angle - const step3 = buf.data[6]; - const brake = buf.data[7]; + const speed = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int8LE, 2); // signed byte + // note that b[3] is padding + const step1 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 4); + const step2 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 8); + const step3 = pxsim.BufferMethods.getNumber(buf, BufferMethods.NumberFormat.Int32LE, 12); + 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");