diff --git a/docs/reference/motors/motor/run.md b/docs/reference/motors/motor/run.md index f2cb65b2..a4a73ecb 100644 --- a/docs/reference/motors/motor/run.md +++ b/docs/reference/motors/motor/run.md @@ -6,9 +6,9 @@ Set the rotation speed of the motor as a percentage of maximum speed. motors.largeA.run(50) ``` -The speed setting is a pecentage of the motor's full speed. Full speed is the speed that the motor runs when the brick supplies maximum output voltage to the port. +The speed setting is a percentage of the motor's full speed. Full speed is the speed that the motor runs when the brick supplies maximum output voltage to the port. -If you use just the **speed** number, the motor runs continously and won't stop unless you tell it to. You can also give a value for a certain amount of distance you want the motor to rotate for. The **value** can be an amount of time, a turn angle in degrees, or a number of full rotations. +If you use just the **speed** number, the motor runs continuously and won't stop unless you tell it to. You can also give a value for a certain amount of distance you want the motor to rotate for. The **value** can be an amount of time, a turn angle in degrees, or a number of full rotations. If you decide to use a **value** of rotation distance, you need to choose a type of movement **unit**. @@ -30,8 +30,8 @@ Here is how you use each different movement unit to run the motor for a fixed ro // Run motor for 700 Milliseconds. motors.largeA.run(25, 700, MoveUnit.MilliSeconds); -// Run motor for 700 Milliseconds again but no units specified. -motors.largeA.run(25, 700); +// Run motors B and C for 700 Milliseconds again but no units specified. +motors.largeBC.run(25, 700); // Run the motor for 45 seconds motors.largeA.run(50, 45, MoveUnit.Seconds); @@ -61,6 +61,14 @@ motors.largeB.run(-25) ## ~ +## Multiple motors + +When using **run** with multiple motors, there is no guarantee that their speed will stay in sync. Use [tank](/reference/motors/tank) or [steer](/reference/motors/steer) for synchronized motor operations. + +```blocks +motors.largeBC.run(50) +``` + ## Examples ### Drive the motor for 20 seconds diff --git a/docs/reference/motors/motor/schedule.md b/docs/reference/motors/motor/schedule.md new file mode 100644 index 00000000..8d7dac4c --- /dev/null +++ b/docs/reference/motors/motor/schedule.md @@ -0,0 +1,22 @@ +# Schedule + +Schedules an acceleration, constant and deceleration phase at a given speed. + +```sig +motors.largeA.schedule(50, 100, 500, 100) +``` + +The speed setting is a percentage of the motor's full speed. Full speed is the speed that the motor runs when the brick supplies maximum output voltage to the port. + + +## Parameters + +* **speed**: a [number](/types/number) that is the percentage of full speed. A negative value runs the motor in the reverse direction. +* **acceleration**: the [number](/types/number) of movement units to rotate for while accelerating. +* **value**: the [number](/types/number) of movement units to rotate for. +* **deceleration**: the [number](/types/number) of movement units to rotate for while decelerating. +* **unit**: the movement unit of rotation. This can be `milliseconds`, `seconds`, `degrees`, or `rotations`. If the number for **value** is `0`, this parameter isn't used. + +## See also + +[tank](/reference/motors/synced/tank), [steer](/reference/motors/synced/steer), [stop](/reference/motors/motor/stop) \ No newline at end of file diff --git a/docs/reference/motors/motor/set-run-acceleration-ramp.md b/docs/reference/motors/motor/set-run-acceleration-ramp.md new file mode 100644 index 00000000..03e6a358 --- /dev/null +++ b/docs/reference/motors/motor/set-run-acceleration-ramp.md @@ -0,0 +1,20 @@ +# Set Run Acceleration Ramp + +```sig +motors.largeD.setRunAccelerationRamp(1, MoveUnit.Seconds) +``` + +## Examples + +```blocks +brick.buttonEnter.onEvent(ButtonEvent.Pressed, function () { + motors.largeB.run(50, 6, MoveUnit.Rotations) +}) +brick.buttonLeft.onEvent(ButtonEvent.Pressed, function () { + motors.largeC.run(50, 6, MoveUnit.Seconds) +}) +motors.largeB.setRunAccelerationRamp(360, MoveUnit.Degrees) +motors.largeB.setRunDecelerationRamp(360, MoveUnit.Degrees) +motors.largeC.setRunAccelerationRamp(2, MoveUnit.Seconds) +motors.largeC.setRunDecelerationRamp(2, MoveUnit.Seconds) +``` \ No newline at end of file diff --git a/docs/reference/motors/motor/set-run-deceleration-ramp.md b/docs/reference/motors/motor/set-run-deceleration-ramp.md new file mode 100644 index 00000000..55442bc5 --- /dev/null +++ b/docs/reference/motors/motor/set-run-deceleration-ramp.md @@ -0,0 +1,20 @@ +# Set Run Deceleration Ramp + +```sig +motors.largeD.setRunDecelerationRamp(1, MoveUnit.Seconds) +``` + +## Examples + +```blocks +brick.buttonEnter.onEvent(ButtonEvent.Pressed, function () { + motors.largeB.run(50, 6, MoveUnit.Rotations) +}) +brick.buttonLeft.onEvent(ButtonEvent.Pressed, function () { + motors.largeC.run(50, 6, MoveUnit.Seconds) +}) +motors.largeB.setRunAccelerationRamp(360, MoveUnit.Degrees) +motors.largeB.setRunDecelerationRamp(360, MoveUnit.Degrees) +motors.largeC.setRunAccelerationRamp(2, MoveUnit.Seconds) +motors.largeC.setRunDecelerationRamp(2, MoveUnit.Seconds) +``` \ No newline at end of file diff --git a/docs/tutorials/touch-sensor.md b/docs/tutorials/touch-sensor.md index 39a6e2f5..cef9d314 100644 --- a/docs/tutorials/touch-sensor.md +++ b/docs/tutorials/touch-sensor.md @@ -15,7 +15,7 @@ "cardType": "tutorial", "url":"/tutorials/touch-sensor-values", "imageUrl":"/static/tutorials/touch-sensor-values.png" -}, +}, { "name": "Pause Until Pressed", "description": "Waits for the sensor to be pressed before continuing the program", "cardType": "tutorial", diff --git a/libs/core/output.ts b/libs/core/output.ts index ec5db4f6..20e04ab4 100644 --- a/libs/core/output.ts +++ b/libs/core/output.ts @@ -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') } } diff --git a/sim/state/motornode.ts b/sim/state/motornode.ts index d2be804e..15c0fb2f 100644 --- a/sim/state/motornode.ts +++ b/sim/state/motornode.ts @@ -1,4 +1,5 @@ namespace pxsim { + const MIN_RAMP_SPEED = 3; export class MotorNode extends BaseNode { isOutput = true; @@ -30,11 +31,11 @@ namespace pxsim { } getSpeed() { - return this.speed * (!this._synchedMotor && this.polarity == 0 ? -1 : 1); + return Math.round(this.speed * (!this._synchedMotor && this.polarity == 0 ? -1 : 1)); } getAngle() { - return this.angle; + return Math.round(this.angle); } // returns the slave motor if any @@ -64,7 +65,7 @@ namespace pxsim { delete this.speedCmdValues; delete this._synchedMotor; this.setChangedState(); - } + } clearSyncCmd() { if (this._synchedMotor) @@ -160,13 +161,19 @@ namespace pxsim { const dstep = isTimeCommand ? pxsim.U.now() - this.speedCmdTime : this.tacho - this.speedCmdTacho; - if (dstep < step1) // rampup + 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 (dstep < step1 + step2 + step3) - this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3); - else { + 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 @@ -227,11 +234,10 @@ namespace pxsim { this.angle = this.manualReferenceAngle + this.manualAngle; this.setChangedState(); } - this.speed = Math.round(this.speed); // integer only - + // don't round speed // compute delta angle - const rotations = this.getSpeed() / 100 * this.rotationsPerMilliSecond * elapsed; - const deltaAngle = Math.round(rotations * 360); + const rotations = this.speed / 100 * this.rotationsPerMilliSecond * elapsed; + const deltaAngle = rotations * 360; if (deltaAngle) { this.angle += deltaAngle; this.tacho += Math.abs(deltaAngle);