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

Motorworks2 (#141)

Browse Source 
* fixing polarity

* allocate motor on motorUsed only

* perform sub-step integration step for better precision
This commit is contained in:
Peli de Halleux 2017-12-28 09:07:57 -08:00 committed by GitHub
parent a9a9a89811
commit 01f7fe633c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 55 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
sim/dalboard.ts
View File

@ -115,12 +115,29 @@ namespace pxsim {
return this.brickNode;
}
getMotor(port: number, large?: boolean): MotorNode[] {
if (port == 0xFF) return this.getMotors(); // Return all motors
const motorPort = this.motorMap[port];
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);
return [this.outputNodes[motorPort]];
}
}
}
getMotor(port: number, large?: boolean): MotorNode[] {
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() {

6
sim/state/input.ts
View File

@ -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();
}

4
sim/state/motor.ts
View File

@ -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

33
sim/state/motors.ts → sim/state/motornode.ts
View File

@ -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));
}
}
}

4
sim/visuals/board.ts
View File

@ -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;