From 3c86ae286fbb66ccc85e80b460d3a5bccaaa226c Mon Sep 17 00:00:00 2001 From: Peli de Halleux Date: Wed, 13 Dec 2017 22:40:40 -0800 Subject: [PATCH] more work on motors --- libs/core/_locales/core-jsdoc-strings.json | 21 +- libs/core/_locales/core-strings.json | 13 +- libs/core/output.ts | 257 ++++++++++++++------- 3 files changed, 191 insertions(+), 100 deletions(-) diff --git a/libs/core/_locales/core-jsdoc-strings.json b/libs/core/_locales/core-jsdoc-strings.json index e496c9f0..1056d98f 100644 --- a/libs/core/_locales/core-jsdoc-strings.json +++ b/libs/core/_locales/core-jsdoc-strings.json @@ -52,20 +52,23 @@ "control.raiseEvent|param|value": "Component specific code indicating the cause of the event.", "motors.Motor.clearCount": "Clears the motor count", "motors.Motor.count": "Gets motor step count.", - "motors.Motor.move": "Moves the motor by a number of degrees", - "motors.Motor.move|param|angle": "the degrees to rotate, eg: 360", - "motors.Motor.reset": "Resets the motor.", + "motors.Motor.move": "Moves the motor by a number of rotations, degress or seconds", + "motors.Motor.move|param|unit": "the meaning of the value", + "motors.Motor.move|param|value": "the move quantity, eg: 2", + "motors.Motor.power": "Turns the motor on or off at the current speed", + "motors.Motor.power|param|on": "true if the motor should be on", + "motors.Motor.reset": "Resets the motor and clears any synchronization", "motors.Motor.setBrake": "Sets the automatic brake on or off when the motor is off", "motors.Motor.setBrake|param|brake": "a value indicating if the motor should break when off", "motors.Motor.setReversed": "Reverses the motor polarity", - "motors.Motor.setSpeed": "Sets the motor speed level from ``-100`` to ``100``.", - "motors.Motor.setSpeed|param|speed": "the power from ``100`` full forward to ``-100`` full backward, eg: 50", "motors.Motor.speed": "Gets motor actual speed.", - "motors.Motor.stop": "Stops the motor", + "motors.Motor.sync": "Synchronizes a follower motor to this motor", + "motors.Motor.sync|param|follower": "the motor that follows this motor commands, eg: motors.largeC", "motors.Motor.tachoCount": "Gets motor tacho count.", - "motors.setSyncSpeed": "Synchronizes this motor with another motor.", - "motors.setSyncSpeed|param|speed": "the power applied to the motor, eg: 50", - "motors.setSyncSpeed|param|turnRatio": "the ratio of the master power applied to this motor, eg: 100", + "motors.Motor.turn": "Turns the motor and the follower motor by a number of rotations", + "motors.Motor.turn|param|turnRatio": "the ratio of power sent to the follower motor, from -200 to 200", + "motors.Motor.turn|param|unit": "the meaning of the value", + "motors.Motor.turn|param|value": "the move quantity, eg: 2", "motors.stopAllMotors": "Stops all motors", "output.createBuffer": "Create a new zero-initialized buffer.", "output.createBuffer|param|size": "number of bytes in the buffer", diff --git a/libs/core/_locales/core-strings.json b/libs/core/_locales/core-strings.json index 747bea17..4e152d05 100644 --- a/libs/core/_locales/core-strings.json +++ b/libs/core/_locales/core-strings.json @@ -12,6 +12,9 @@ "LightsPattern.RedFlash|block": "Flashing Red", "LightsPattern.RedPulse|block": "Pulsing Red", "LightsPattern.Red|block": "Red", + "MoveUnit.Degrees|block": "degrees", + "MoveUnit.Rotations|block": "rotations", + "MoveUnit.Seconds|block": "seconds", "Output.ALL|block": "All", "Output.A|block": "A", "Output.BC|block": "B+C", @@ -37,13 +40,15 @@ "control.raiseEvent|block": "raise event|from %src|with value %value", "control|block": "control", "motors.Motor.count|block": "`icons.motorLarge` %motor|count", - "motors.Motor.move|block": "move `icons.motorLarge` %motor|by %angle|degrees at %speed|%", + "motors.Motor.move|block": "move `icons.motorLarge` %motor|for %value|%unit", + "motors.Motor.power|block": "power `icons.motorLarge` %motor|%on", + "motors.Motor.reset|block": "reset `icons.motorLarge` %motor", "motors.Motor.setBrake|block": "set `icons.motorLarge` %motor|brake %brake", "motors.Motor.setReversed|block": "set `icons.motorLarge` %motor|reversed %reversed", - "motors.Motor.setSpeed|block": "set speed `icons.motorLarge` %motor|to %speed|%", "motors.Motor.speed|block": "`icons.motorLarge` %motor|speed", - "motors.Motor.stop|block": "stop `icons.motorLarge` %motor", + "motors.Motor.sync|block": "sync `icons.motorLarge` %motor|with `icons.motorLarge` %follower", "motors.Motor.tachoCount|block": "`icons.motorLarge` %motor|tacho count", + "motors.Motor.turn|block": "turn `icons.motorLarge` %motor|by %value|%unit|turn %turnRadio", "motors.largeMotorA|block": "large A", "motors.largeMotorB|block": "large B", "motors.largeMotorC|block": "large C", @@ -52,7 +57,6 @@ "motors.mediumMotorB|block": "medium B", "motors.mediumMotorC|block": "medium C", "motors.mediumMotorD|block": "medium D", - "motors.setSyncSpeed|block": "set sync speed B+C with %turnRatio|turn ratio at %speed|% speed", "motors.stopAllMotors|block": "stop all `icons.motorLarge`", "motors|block": "motors", "output|block": "output", @@ -69,6 +73,5 @@ "{id:category}Serial": "Serial", "{id:group}Buttons": "Buttons", "{id:group}Light": "Light", - "{id:group}Motors": "Motors", "{id:group}Screen": "Screen" } \ No newline at end of file diff --git a/libs/core/output.ts b/libs/core/output.ts index 3301fa88..902b3b93 100644 --- a/libs/core/output.ts +++ b/libs/core/output.ts @@ -19,6 +19,15 @@ enum OutputType { MiniTacho = 8, } +enum MoveUnit { + //% block="rotations" + Rotations, + //% block="degrees" + Degrees, + //% block="seconds" + Seconds +} + namespace motors { let pwmMM: MMap let motorMM: MMap @@ -70,7 +79,7 @@ namespace motors { * Stops all motors */ //% blockId=motorStopAll block="stop all `icons.motorLarge`" - //% weight=10 group="Motors" blockGap=8 + //% weight=10 blockGap=8 export function stopAllMotors() { const b = mkCmd(Output.ALL, DAL.opOutputStop, 0) writePWM(b) @@ -78,71 +87,127 @@ namespace motors { //% fixedInstances export class Motor extends control.Component { - private port: Output; - private large: boolean; - private brake: boolean; + private _port: Output; + private _large: boolean; + + private _initialized: boolean; + private _speed: number; + private _brake: boolean; + private _follower: Motor; // constructor(port: Output, large: boolean) { super(); - this.port = port; - this.large = large; - this.brake = false; + this._port = port; + this._large = large; + this._brake = false; + this._speed = 50; } - /** - * Sets the motor speed level from ``-100`` to ``100``. - * @param motor the output connection that the motor is connected to - * @param speed the power from ``100`` full forward to ``-100`` full backward, eg: 50 - */ - //% blockId=motorSetSpeed block="set speed `icons.motorLarge` %motor|to %speed|%" - //% weight=99 group="Motors" blockGap=8 - //% speed.min=-100 speed.max=100 - setSpeed(speed: number) { - speed = Math.clamp(-100, 100, speed >> 0); - - // per LEGO: call it power, use speed - const b = mkCmd(this.port, DAL.opOutputSpeed, 1) - b.setNumber(NumberFormat.Int8LE, 2, speed) - writePWM(b) - if (speed) { - start(this.port); - } else { - this.stop(); + private __init() { + if (!this._initialized) { + // specify motor size on this port + const b = mkCmd(this._port, DAL.opOutputSetType, 1) + b.setNumber(NumberFormat.Int8LE, 2, this._large ? 0x07 : 0x08) + writePWM(b) } } /** - * Moves the motor by a number of degrees - * @param degrees the angle to turn the motor - * @param angle the degrees to rotate, eg: 360 + * Turns the motor on or off at the current speed + * @param on true if the motor should be on + */ + //% blockId=motorPower block="power `icons.motorLarge` %motor|%on" + //% on.fieldEditor=toggleonoff + //% weight=99 blockGap=8 + power(on: boolean) { + if (!this._speed || !on) { // always stop + this.stop(); + } else { + if (this._follower) this.setSpeedSync(this._speed); + else this.setSpeedSingle(this._speed); + } + } + + private setSpeedSingle(speed: number) { + const b = mkCmd(this._port, DAL.opOutputSpeed, 1) + b.setNumber(NumberFormat.Int8LE, 2, speed) + writePWM(b) + } + + private setSpeedSync(speed: number) { + const out = this._port | this._follower._port; + syncMotors(out, { + speed: speed, + turnRatio: 0, + useBrake: !!this._brake + }) + } + + /** + * Moves the motor by a number of rotations, degress or seconds + * @param value the move quantity, eg: 2 + * @param unit the meaning of the value * @param speed the speed from ``100`` full forward to ``-100`` full backward, eg: 50 */ - //% blockId=motorMove block="move `icons.motorLarge` %motor|by %angle|degrees at %speed|%" - //% weight=98 group="Motors" blockGap=8 - //% speed.min=-100 speed.max=100 - move(angle: number, power: number) { - angle = angle >> 0; - power = Math.clamp(-100, 100, power >> 0); + //% blockId=motorMove block="move `icons.motorLarge` %motor|for %value|%unit" + //% weight=98 + //% speed.min=-100 speed.max=100 + move(value: number, unit: MoveUnit) { + this.output(value, unit, 0); + } + + private output(value: number, unit: MoveUnit, turnRatio: number) { + this.__init(); + if (!this._speed) { + this.stop(); + return; + } + turnRatio = Math.clamp(-200, 200, turnRatio >> 0); - step(this.port, { - speed: power, - step1: 0, - step2: angle, - step3: 0, - useSteps: true, - useBrake: this.brake - }) + let useSteps: boolean; + let stepsOrTime: number; + switch (unit) { + case MoveUnit.Rotations: + stepsOrTime = (value * 360) >> 0; + useSteps = true; + break; + case MoveUnit.Degrees: + stepsOrTime = value >> 0; + useSteps = true; + break; + default: + stepsOrTime = value; + useSteps = false; + break; + } + + if (this._follower) { + syncMotors(this._port | this._follower._port, { + useSteps: useSteps, + stepsOrTime: stepsOrTime, + speed: this._speed, + turnRatio: turnRatio, + useBrake: this._brake + }) + } else { + step(this._port, { + useSteps: useSteps, + step1: 0, + step2: stepsOrTime, + step3: 0, + speed: this._speed, + useBrake: this._brake + }) + } } /** * Stops the motor */ - //% blockId=motorStop block="stop `icons.motorLarge` %motor" - //% weight=97 group="Motors" - stop() { - const b = mkCmd(this.port, DAL.opOutputStop, 1) - b.setNumber(NumberFormat.UInt8LE, 2, this.brake ? 1 : 0) - writePWM(b); + private stop() { + this.__init(); + if (this._follower) stop(this._port | this._follower._port); + else stop(this._port); } /** @@ -151,9 +216,9 @@ namespace motors { */ //% blockId=outputMotorSetBrakeMode block="set `icons.motorLarge` %motor|brake %brake" //% brake.fieldEditor=toggleonoff - //% weight=60 group="Motors" blockGap=8 + //% weight=60 blockGap=8 setBrake(brake: boolean) { - this.brake = brake; + this._brake = brake; } /** @@ -161,9 +226,10 @@ namespace motors { */ //% blockId=motorSetReversed block="set `icons.motorLarge` %motor|reversed %reversed" //% reversed.fieldEditor=toggleonoff - //% weight=59 group="Motors" + //% weight=59 setReversed(reversed: boolean) { - const b = mkCmd(this.port, DAL.opOutputPolarity, 1) + this.__init(); + const b = mkCmd(this._port, DAL.opOutputPolarity, 1) b.setNumber(NumberFormat.Int8LE, 2, reversed ? -1 : 1); writePWM(b) } @@ -173,9 +239,10 @@ namespace motors { * @param motor the port which connects to the motor */ //% blockId=motorSpeed block="`icons.motorLarge` %motor|speed" - //% weight=72 group="Motors" blockGap=8 + //% weight=72 blockGap=8 speed(): number { - return getMotorData(this.port).actualSpeed; + this.__init(); + return getMotorData(this._port).actualSpeed; } /** @@ -183,9 +250,10 @@ namespace motors { * @param motor the port which connects to the motor */ //% blockId=motorCount block="`icons.motorLarge` %motor|count" - //% weight=71 group="Motors" blockGap=8 + //% weight=71 blockGap=8 count(): number { - return getMotorData(this.port).count; + this.__init(); + return getMotorData(this._port).count; } /** @@ -193,30 +261,63 @@ namespace motors { * @param motor the port which connects to the motor */ //% blockId=motorTachoCount block="`icons.motorLarge` %motor|tacho count" - //% weight=70 group="Motors" + //% weight=70 tachoCount(): number { - return getMotorData(this.port).tachoCount; + this.__init(); + return getMotorData(this._port).tachoCount; } /** * Clears the motor count */ clearCount() { - const b = mkCmd(this.port, DAL.opOutputClearCount, 0) + this.__init(); + const b = mkCmd(this._port, DAL.opOutputClearCount, 0) writePWM(b) for (let i = 0; i < DAL.NUM_OUTPUTS; ++i) { - if (this.port & (1 << i)) { + if (this._port & (1 << i)) { motorMM.setNumber(NumberFormat.Int32LE, i * MotorDataOff.Size + MotorDataOff.TachoSensor, 0) } } } /** - * Resets the motor. + * Resets the motor and clears any synchronization */ + //% blockId=motorReset block="reset `icons.motorLarge` %motor" + //% weight=1 reset() { - reset(this.port); + this.__init(); + reset(this._port); + delete this._follower; } + + /** + * Synchronizes a follower motor to this motor + * @param motor the leader motor, eg: motors.largeB + * @param follower the motor that follows this motor commands, eg: motors.largeC + */ + //% blockId=motorSync block="sync `icons.motorLarge` %motor|with `icons.motorLarge` %follower" + //% weight=10 blockGap=8 + sync(follower: Motor) { + this.__init(); + if (this == follower) return; // can't sync with self + this._follower = follower; + } + + /** + * Turns the motor and the follower motor by a number of rotations + * @param value the move quantity, eg: 2 + * @param unit the meaning of the value + * @param speed the speed from ``100`` full forward to ``-100`` full backward, eg: 50 + * @param turnRatio the ratio of power sent to the follower motor, from -200 to 200 + */ + //% blockId=motorTurn block="turn `icons.motorLarge` %motor|by %value|%unit|turn %turnRadio" + //% weight=9 blockGap=8 + //% turnRatio.min=-200 turnRatio=200 + turn(value: number, unit: MoveUnit, turnRatio: number) { + this.output(value, unit, turnRatio); + } } //% whenUsed fixedInstance block="large A" @@ -243,28 +344,6 @@ namespace motors { //% whenUsed fixedInstance block="medium D" export const mediumMotorD = new Motor(Output.D, false); - - /** - * Synchronizes this motor with another motor. - * @param motor the controlled motor, eg: motors.largeB - * @param other the motor that will control this motor, eg: motors.largeC - * @param turnRatio the ratio of the master power applied to this motor, eg: 100 - * @param speed the power applied to the motor, eg: 50 - */ - //% blockId=motorSync block="set sync speed B+C with %turnRatio|turn ratio at %speed|% speed" - //% turnRatio.min=-200 turnRatio.max=200 - //% speed.min=-100 speed.max=100 - export function setSyncSpeed(turnRatio: number, speed: number) { - syncMotors(Output.BC, { - useSteps: true, - speed: speed, - turnRatio: turnRatio, - stepsOrTime: 0, - useBrake: false - }) - start(Output.BC); - } - function reset(out: Output) { let b = mkCmd(out, DAL.opOutputReset, 0) writePWM(b) @@ -299,7 +378,7 @@ namespace motors { useSteps?: boolean; speed: number; turnRatio: number; - stepsOrTime: number; + stepsOrTime?: number; useBrake?: boolean; } @@ -333,6 +412,12 @@ namespace motors { writePWM(b); } + function stop(out: Output) { + const b = mkCmd(out, DAL.opOutputStop, 1) + b.setNumber(NumberFormat.UInt8LE, 2, this.brake ? 1 : 0) + writePWM(b); + } + function step(out: Output, opts: StepOptions) { let op = opts.useSteps ? DAL.opOutputStepSpeed : DAL.opOutputTimeSpeed let speed = opts.speed