Simulator support for motor commands (#137)
* support for "step" functions * locale files * enum issue * fixing decoding of array * implement clear count * log unknown commands
This commit is contained in:
Peli de Halleux
GitHub
parent
751df2fe8c
commit
f01370e4fd
@ -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;
|
||||
|
||||
@ -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, <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 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user