c_meyer/pxt-ev3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

appliying manual speed

Browse Source
This commit is contained in:
Peli de Halleux 2018-01-10 14:08:50 -08:00
parent e93e659e8a
commit 0dd5ab9bde
1 changed files with 78 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
sim/state/motornode.ts
View File

@ -17,6 +17,8 @@ namespace pxsim {
private speedCmdTime: number; private speedCmdTime: number;
private _synchedMotor: MotorNode; // non-null if synchronized private _synchedMotor: MotorNode; // non-null if synchronized
private manualSpeed: number = 0;
constructor(port: number, large: boolean) { constructor(port: number, large: boolean) {
super(port); super(port);
this.setLarge(large); this.setLarge(large);
@ -42,7 +44,7 @@ namespace pxsim {
setSpeedCmd(cmd: DAL, values: number[]) { setSpeedCmd(cmd: DAL, values: number[]) {
if (this.speedCmd != cmd || if (this.speedCmd != cmd ||
JSON.stringify(this.speedCmdValues) != JSON.stringify(values)) JSON.stringify(this.speedCmdValues) != JSON.stringify(values))
this.setChangedState(); this.setChangedState();
// new command TODO: values // new command TODO: values
this.speedCmd = cmd; this.speedCmd = cmd;
this.speedCmdValues = values; this.speedCmdValues = values;
@ -97,15 +99,14 @@ namespace pxsim {
} }
manualMotorDown() { manualMotorDown() {
} }
manualMotorMove(angle: number) { manualMotorMove(speed: number) {
this.manualSpeed = speed;
} }
manualMotorUp() { manualMotorUp() {
this.manualSpeed = undefined;
} }
updateState(elapsed: number) { updateState(elapsed: number) {
@ -121,79 +122,84 @@ namespace pxsim {
} }
private updateStateStep(elapsed: number) { private updateStateStep(elapsed: number) {
// compute new speed if (!this.manualSpeed) {
switch (this.speedCmd) { // compute new speed
case DAL.opOutputSpeed: switch (this.speedCmd) {
case DAL.opOutputPower: case DAL.opOutputSpeed:
// assume power == speed case DAL.opOutputPower:
// TODO: PID // assume power == speed
this.speed = this.speedCmdValues[0]; // TODO: PID
break; this.speed = this.speedCmdValues[0];
case DAL.opOutputTimeSpeed: break;
case DAL.opOutputTimePower: case DAL.opOutputTimeSpeed:
case DAL.opOutputStepPower: case DAL.opOutputTimePower:
case DAL.opOutputStepSpeed: { case DAL.opOutputStepPower:
// ramp up, run, ramp down, <brake> using time case DAL.opOutputStepSpeed: {
const speed = this.speedCmdValues[0]; // ramp up, run, ramp down, <brake> using time
const step1 = this.speedCmdValues[1]; const speed = this.speedCmdValues[0];
const step2 = this.speedCmdValues[2]; const step1 = this.speedCmdValues[1];
const step3 = this.speedCmdValues[3]; const step2 = this.speedCmdValues[2];
const brake = this.speedCmdValues[4]; const step3 = this.speedCmdValues[3];
const dstep = (this.speedCmd == DAL.opOutputTimePower || this.speedCmd == DAL.opOutputTimeSpeed) const brake = this.speedCmdValues[4];
? pxsim.U.now() - this.speedCmdTime const dstep = (this.speedCmd == DAL.opOutputTimePower || this.speedCmd == DAL.opOutputTimeSpeed)
: this.tacho - this.speedCmdTacho; ? pxsim.U.now() - this.speedCmdTime
if (dstep < step1) // rampup : this.tacho - this.speedCmdTacho;
this.speed = speed * dstep / step1; if (dstep < step1) // rampup
else if (dstep < step1 + step2) // run this.speed = speed * dstep / step1;
this.speed = speed; else if (dstep < step1 + step2) // run
else if (dstep < step1 + step2 + step3) this.speed = speed;
this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3); else if (dstep < step1 + step2 + step3)
else { this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3);
if (brake) this.speed = 0; else {
this.clearSpeedCmd(); if (brake) this.speed = 0;
} this.clearSpeedCmd();
break; }
}
case DAL.opOutputStepSync:
case DAL.opOutputTimeSync: {
const otherMotor = this._synchedMotor;
if (otherMotor.port < this.port) // handled in other motor code
break; break;
const speed = this.speedCmdValues[0];
const turnRatio = this.speedCmdValues[1];
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();
} }
case DAL.opOutputStepSync:
case DAL.opOutputTimeSync: {
const otherMotor = this._synchedMotor;
if (otherMotor.port < this.port) // handled in other motor code
break;
// turn ratio is a bit weird to interpret const speed = this.speedCmdValues[0];
// see https://communities.theiet.org/blogs/698/1706 const turnRatio = this.speedCmdValues[1];
if (turnRatio < 0) { const stepsOrTime = this.speedCmdValues[2];
otherMotor.speed = speed; const brake = this.speedCmdValues[3];
this.speed *= (100 + turnRatio) / 100; const dstep = this.speedCmd == DAL.opOutputTimeSync
} else { ? pxsim.U.now() - this.speedCmdTime
otherMotor.speed = this.speed * (100 - turnRatio) / 100; : 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
if (turnRatio < 0) {
otherMotor.speed = speed;
this.speed *= (100 + turnRatio) / 100;
} else {
otherMotor.speed = this.speed * (100 - 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;
} }
// 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 {
this.speed = this.manualSpeed;
}
this.speed = Math.round(this.speed); // integer only this.speed = Math.round(this.speed); // integer only
// compute delta angle // compute delta angle