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

* 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

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. 
+// Run motors B and C for 700 Milliseconds again but no units specified. 
-motors.largeA.run(25, 700);
+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

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)

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)
+```

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)
+```

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",

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 _accelerationSteps: number;
-        private _move: (steps: boolean, stepsOrTime: number, speed: number) => void;
+        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._accelerationSteps = 0;
-            this._move = move;
+            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) {
+            if (schedule.steps[0] + schedule.steps[1] + schedule.steps[2] == 0) {
-                this._run(speed);
+                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;
+            const steps = schedule.steps;
-            let stepsOrTime: number;
+            // 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;
+                    this._accelerationSteps = Math.max(0, (value * 360) | 0);
-                    useSteps = true;
                     break;
                 case MoveUnit.Degrees:
-                    stepsOrTime = value >> 0;
+                    this._accelerationSteps = Math.max(0, value | 0);
-                    useSteps = true;
                     break;
                 case MoveUnit.Seconds:
-                    stepsOrTime = (value * 1000) >> 0;
+                    this._accelerationTime = Math.max(0, (value * 1000) | 0);
-                    useSteps = false;
                     break;
-                default:
+                case MoveUnit.MilliSeconds:
-                    stepsOrTime = value;
+                    this._accelerationTime = Math.max(0, value | 0);
-                    useSteps = false;
                     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')
     }
 }
 

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
@@ -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)
+                        else if (step2 && dstep < step1 + step2 + step3) {
-                            this.speed = speed * (step1 + step2 + step3 - dstep) / (step1 + step2 + step3);
+                            this.speed = speed * (step1 + step2 + step3 - dstep)
-                        else {
+                                / (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 rotations = this.speed / 100 * this.rotationsPerMilliSecond * elapsed;
-            const deltaAngle = Math.round(rotations * 360);
+            const deltaAngle = rotations * 360;
             if (deltaAngle) {
                 this.angle += deltaAngle;
                 this.tacho += Math.abs(deltaAngle);