in sim/state/motornode.ts [129:243]
private updateStateStep(elapsed: number) {
if (this.manualAngle === undefined) {
// 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, <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 isTimeCommand = this.speedCmd == DAL.opOutputTimePower || this.speedCmd == DAL.opOutputTimeSpeed;
const dstep = isTimeCommand
? pxsim.U.now() - this.speedCmdTime
: this.tacho - this.speedCmdTacho;
if (step1 && dstep < step1) { // rampup
this.speed = speed * dstep / step1;
// ensure non-zero speed
this.speed = Math.max(MIN_RAMP_SPEED, Math.ceil(Math.abs(this.speed))) * Math.sign(speed);
}
else if (dstep < step1 + step2) // run
this.speed = speed;
else if (step2 && dstep < step1 + step2 + step3) {
this.speed = speed * (step1 + step2 + step3 - dstep)
/ (step1 + step2 + step3) + 5;
// ensure non-zero speed
this.speed = Math.max(MIN_RAMP_SPEED, Math.ceil(Math.abs(this.speed))) * Math.sign(speed);
} else {
if (brake) this.speed = 0;
if (!isTimeCommand) {
// we need to patch the actual position of the motor when
// finishing the move as our integration step introduce errors
const deltaAngle = -Math.sign(speed) * (dstep - (step1 + step2 + step3));
if (deltaAngle) {
this.angle += deltaAngle;
this.tacho -= Math.abs(deltaAngle);
this.setChangedState();
}
}
this.clearSpeedCmd();
}
break;
}
case DAL.opOutputStepSync:
case DAL.opOutputTimeSync: {
const otherMotor = this._synchedMotor;
const speed = this.speedCmdValues[0];
const turnRatio = this.speedCmdValues[1];
// if turnratio is negative, right motor at power level
// right motor -> this.port > otherMotor.port
if (Math.sign(this.port - otherMotor.port)
== Math.sign(turnRatio))
break; // handled in other motor code
const stepsOrTime = this.speedCmdValues[2];
const brake = this.speedCmdValues[3];
const dstep = this.speedCmd == DAL.opOutputTimeSync
? pxsim.U.now() - this.speedCmdTime
: this.tacho - this.speedCmdTacho;
// 0 is special case, run infinite
if (!stepsOrTime || dstep < stepsOrTime)
this.speed = speed;
else {
if (brake) this.speed = 0;
this.clearSpeedCmd();
}
// 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;
// clamp
this.speed = Math.max(-100, Math.min(100, this.speed >> 0));
otherMotor.speed = Math.max(-100, Math.min(100, otherMotor.speed >> 0));;
// stop other motor if needed
if (!this._synchedMotor)
otherMotor.clearSpeedCmd();
break;
}
}
}
else {
// the user is holding the handle - so position is the angle
this.speed = 0;
// rotate by the desired angle change
this.angle = this.manualReferenceAngle + this.manualAngle;
this.setChangedState();
}
// don't round speed
// 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.manualReferenceAngle === undefined)
&& this.speed && !(this.started || this.speedCmd)) {
// decay speed 5% per tick
this.speed = Math.round(Math.max(0, Math.abs(this.speed) - 10) * sign(this.speed));
}
}