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

Support for smooth acceleration/deceleration in run (#900)

Browse Source 
* removed logging

* removing more logging

* always use step for single/multiple motors

* refactored schedule

* account for accel ramp up and down

* added default acc/decel

* rounding speed/angle

* remove hack

* use acceleration time in run too

* handle missing case

* adding notes on motors

* adding sample

* fixed ramp simulation

* clear defaults

* some docs, more later

* adding basic examples

* remove debug msg

* clean json

* added move schedule

* docs

* basic docs
This commit is contained in:
Peli de Halleux
2019-09-02 20:57:23 -07:00
committed by GitHub
parent 7e9cc791ec
commit 56bbcde299
7 changed files with 281 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
libs/core/output.ts
View File

@@ -127,31 +127,43 @@ namespace motors {
reset(Output.ALL)
}
interface MoveSchedule {
speed: number;
useSteps: boolean;
steps: number[];
}
//% fixedInstances
export class MotorBase extends control.Component {
protected _port: Output;
protected _portName: string;
protected _brake: boolean;
protected _regulated: boolean;
private _pauseOnRun: boolean;
private _initialized: boolean;
private _brakeSettleTime: number;
private _init: () => void;
private _run: (speed: number) => void;
private _move: (steps: boolean, stepsOrTime: number, speed: number) => void;
private _accelerationSteps: number;
private _accelerationTime: number;
private _decelerationSteps: number;
private _decelerationTime: number;
protected static output_types: number[] = [0x7, 0x7, 0x7, 0x7];
constructor(port: Output, init: () => void, run: (speed: number) => void, move: (steps: boolean, stepsOrTime: number, speed: number) => void) {
constructor(port: Output, init: () => void) {
super();
this._port = port;
this._portName = outputToName(this._port);
this._brake = false;
this._regulated = true;
this._pauseOnRun = true;
this._initialized = false;
this._brakeSettleTime = 10;
this._init = init;
this._run = run;
this._move = move;
this._accelerationSteps = 0;
this._accelerationTime = 0;
this._decelerationSteps = 0;
this._decelerationTime = 0;
}
/**
@@ -263,6 +275,34 @@ namespace motors {
reset(this._port);
}
private normalizeSchedule(speed: number, step1: number, step2: number, step3: number, unit: MoveUnit): MoveSchedule {
const r: MoveSchedule = {
speed: Math.clamp(-100, 100, speed >> 0),
useSteps: true,
steps: [step1, step2, step3]
}
let scale = 1;
switch (unit) {
case MoveUnit.Rotations:
scale = 360;
r.useSteps = true;
break;
case MoveUnit.Degrees:
r.useSteps = true;
break;
case MoveUnit.Seconds:
scale = 1000;
r.useSteps = false;
break;
default:
r.useSteps = false;
break;
}
for (let i = 0; i < r.steps.length; ++i)
r.steps[i] = Math.max(0, (r.steps[i] * scale) | 0);
return r;
}
/**
* Runs the motor at a given speed for limited time or distance.
* @param speed the speed from ``100`` full forward to ``-100`` full backward, eg: 50
@@ -277,41 +317,162 @@ namespace motors {
//% help=motors/motor/run
run(speed: number, value: number = 0, unit: MoveUnit = MoveUnit.MilliSeconds) {
this.init();
speed = Math.clamp(-100, 100, speed >> 0);
const schedule = this.normalizeSchedule(speed, 0, value, 0, unit);
// stop if speed is 0
if (!speed) {
if (!schedule.speed) {
this.stop();
return;
}
// special: 0 is infinity
if (value == 0) {
this._run(speed);
if (schedule.steps[0] + schedule.steps[1] + schedule.steps[2] == 0) {
this._run(schedule.speed);
return;
}
// timed motor moves
const steps = schedule.steps;
const useSteps = schedule.useSteps;
// compute ramp up and down
steps[0] = (useSteps ? this._accelerationSteps : this._accelerationTime) || 0;
steps[2] = (useSteps ? this._decelerationSteps : this._decelerationTime) || 0;
if (steps[0] + steps[2] > steps[1]) {
// rescale
const r = steps[1] / (steps[0] + steps[2]);
steps[0] = Math.floor(steps[0] * r);
steps[2] *= Math.floor(steps[2] * r);
}
steps[1] -= (steps[0] + steps[2]);
// send ramped command
this._schedule(schedule);
this.pauseOnRun(steps[0] + steps[1] + steps[2]);
}
/**
* Schedules a run of the motor with an acceleration, constant and deceleration phase.
* @param speed the speed from ``100`` full forward to ``-100`` full backward, eg: 50
* @param acceleration acceleration phase measured distance or rotation
* @param value measured distance or rotation
* @param deceleration deceleration phase measured distance or rotation
* @param unit (optional) unit of the value
*/
//% blockId=motorSchedule block="schedule %motor at %speed=motorSpeedPicker|\\%|for %acceleration|%value|%deceleration||%unit"
//% weight=99 blockGap=8
//% group="Move"
//% motor.fieldEditor="motors"
//% help=motors/motor/schedule
//% inlineInputMode=inline
schedule(speed: number, acceleration: number, value: number, deceleration: number, unit: MoveUnit = MoveUnit.MilliSeconds) {
this.init();
const schedule = this.normalizeSchedule(speed, acceleration, value, deceleration, unit);
// stop if speed is 0
if (!schedule.speed) {
this.stop();
return;
}
// special case: do nothing
if (schedule.steps[0] + schedule.steps[1] + schedule.steps[2] == 0) {
return;
}
// timed motor moves
let useSteps: boolean;
let stepsOrTime: number;
const steps = schedule.steps;
// send ramped command
this._schedule(schedule);
this.pauseOnRun(steps[0] + steps[1] + steps[2]);
}
/**
* Specifies the amount of rotation or time for the acceleration
* of run commands.
*/
//% blockId=outputMotorsetRunAcceleration block="set %motor|run acceleration to $value||$unit"
//% motor.fieldEditor="motors"
//% weight=21 blockGap=8
//% group="Properties"
//% help=motors/motor/set-run-acceleration-ramp
setRunAccelerationRamp(value: number, unit: MoveUnit = MoveUnit.MilliSeconds) {
switch (unit) {
case MoveUnit.Rotations:
stepsOrTime = (value * 360) >> 0;
useSteps = true;
this._accelerationSteps = Math.max(0, (value * 360) | 0);
break;
case MoveUnit.Degrees:
stepsOrTime = value >> 0;
useSteps = true;
this._accelerationSteps = Math.max(0, value | 0);
break;
case MoveUnit.Seconds:
stepsOrTime = (value * 1000) >> 0;
useSteps = false;
this._accelerationTime = Math.max(0, (value * 1000) | 0);
break;
default:
stepsOrTime = value;
useSteps = false;
case MoveUnit.MilliSeconds:
this._accelerationTime = Math.max(0, value | 0);
break;
}
}
this._move(useSteps, stepsOrTime, speed);
this.pauseOnRun(stepsOrTime);
/**
* Specifies the amount of rotation or time for the acceleration
* of run commands.
*/
//% blockId=outputMotorsetRunDeceleration block="set %motor|run deceleration ramp to $value||$unit"
//% motor.fieldEditor="motors"
//% weight=20 blockGap=8
//% group="Properties"
//% help=motors/motor/set-run-deceleration-ramp
setRunDecelerationRamp(value: number, unit: MoveUnit = MoveUnit.MilliSeconds) {
switch (unit) {
case MoveUnit.Rotations:
this._decelerationSteps = Math.max(0, (value * 360) | 0);
break;
case MoveUnit.Degrees:
this._decelerationSteps = Math.max(0, value | 0);
break;
case MoveUnit.Seconds:
this._decelerationTime = Math.max(0, (value * 1000) | 0);
break;
case MoveUnit.MilliSeconds:
this._decelerationTime = Math.max(0, value | 0);
break;
}
}
private _run(speed: number) {
// ramp up acceleration
if (this._accelerationTime) {
this._schedule({ speed: speed, useSteps: false, steps: [this._accelerationTime, 100, 0] });
pause(this._accelerationTime);
}
// keep going
const b = mkCmd(this._port, this._regulated ? DAL.opOutputSpeed : DAL.opOutputPower, 1)
b.setNumber(NumberFormat.Int8LE, 2, speed)
writePWM(b)
if (speed) {
writePWM(mkCmd(this._port, DAL.opOutputStart, 0))
}
}
private _schedule(schedule: MoveSchedule) {
const p = {
useSteps: schedule.useSteps,
step1: schedule.steps[0],
step2: schedule.steps[1],
step3: schedule.steps[2],
speed: this._regulated ? schedule.speed : undefined,
power: this._regulated ? undefined : schedule.speed,
useBrake: this._brake
};
step(this._port, p)
}
/**
* Indicates if the motor(s) speed should be regulated. Default is true.
* @param value true for regulated motor
*/
//% blockId=outputMotorSetRegulated block="set %motor|regulated %value=toggleOnOff"
//% motor.fieldEditor="motors"
//% weight=58 blockGap=8
//% group="Properties"
//% help=motors/motor/set-regulated
setRegulated(value: boolean) {
this._regulated = value;
}
/**
@@ -338,6 +499,10 @@ namespace motors {
pauseUntil(() => this.isReady(), timeOut);
}
setRunSmoothness(accelerationPercent: number, decelerationPercent: number) {
}
protected setOutputType(large: boolean) {
for (let i = 0; i < DAL.NUM_OUTPUTS; ++i) {
if (this._port & (1 << i)) {
@@ -364,12 +529,10 @@ namespace motors {
//% fixedInstances
export class Motor extends MotorBase {
private _large: boolean;
private _regulated: boolean;
constructor(port: Output, large: boolean) {
super(port, () => this.__init(), (speed) => this.__setSpeed(speed), (steps, stepsOrTime, speed) => this.__move(steps, stepsOrTime, speed));
super(port, () => this.__init());
this._large = large;
this._regulated = true;
this.markUsed();
}
@@ -381,44 +544,6 @@ namespace motors {
this.setOutputType(this._large);
}
private __setSpeed(speed: number) {
const b = mkCmd(this._port, this._regulated ? DAL.opOutputSpeed : DAL.opOutputPower, 1)
b.setNumber(NumberFormat.Int8LE, 2, speed)
writePWM(b)
if (speed) {
writePWM(mkCmd(this._port, DAL.opOutputStart, 0))
}
}
private __move(steps: boolean, stepsOrTime: number, speed: number) {
control.dmesg("motor.__move")
const p = {
useSteps: steps,
step1: 0,
step2: stepsOrTime,
step3: 0,
speed: this._regulated ? speed : undefined,
power: this._regulated ? undefined : speed,
useBrake: this._brake
};
control.dmesg("motor.1")
step(this._port, p)
control.dmesg("motor.__move end")
}
/**
* Indicates if the motor speed should be regulated. Default is true.
* @param value true for regulated motor
*/
//% blockId=outputMotorSetRegulated block="set %motor|regulated %value=toggleOnOff"
//% motor.fieldEditor="motors"
//% weight=58 blockGap=8
//% group="Properties"
//% help=motors/motor/set-regulated
setRegulated(value: boolean) {
this._regulated = value;
}
/**
* Gets motor actual speed.
* @param motor the port which connects to the motor
@@ -506,7 +631,7 @@ namespace motors {
export class SynchedMotorPair extends MotorBase {
constructor(ports: Output) {
super(ports, () => this.__init(), (speed) => this.__setSpeed(speed), (steps, stepsOrTime, speed) => this.__move(steps, stepsOrTime, speed));
super(ports, () => this.__init());
this.markUsed();
}
@@ -518,24 +643,6 @@ namespace motors {
this.setOutputType(true);
}
private __setSpeed(speed: number) {
syncMotors(this._port, {
speed: speed,
turnRatio: 0, // same speed
useBrake: !!this._brake
})
}
private __move(steps: boolean, stepsOrTime: number, speed: number) {
syncMotors(this._port, {
useSteps: steps,
speed: speed,
turnRatio: 0, // same speed
stepsOrTime: stepsOrTime,
useBrake: this._brake
});
}
/**
* The Move Tank block can make a robot drive forward, backward, turn, or stop.
* Use the Move Tank block for robot vehicles that have two Large Motors,
@@ -745,26 +852,15 @@ namespace motors {
return
}
speed = Math.clamp(-100, 100, speed)
control.dmesg('speed: ' + speed)
let b = mkCmd(out, op, 15)
control.dmesg('STEP 5')
b.setNumber(NumberFormat.Int8LE, 2, speed)
// note that b[3] is padding
control.dmesg('STEP 1')
b.setNumber(NumberFormat.Int32LE, 4 + 4 * 0, opts.step1)
control.dmesg('STEP 2')
b.setNumber(NumberFormat.Int32LE, 4 + 4 * 1, opts.step2)
control.dmesg('STEP 3')
b.setNumber(NumberFormat.Int32LE, 4 + 4 * 2, opts.step3)
control.dmesg('STEP 4')
control.dmesg('br ' + opts.useBrake);
const br = !!opts.useBrake ? 1 : 0;
control.dmesg('Step 4.5 ' + br)
b.setNumber(NumberFormat.Int8LE, 4 + 4 * 3, br)
control.dmesg('STEP 5')
writePWM(b)
control.dmesg('end step')
}
}