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

more work on motors

Browse Source
This commit is contained in:
Peli de Halleux
2017-12-13 22:40:40 -08:00
parent 1b6d84a9b8
commit 3c86ae286f
3 changed files with 191 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

257
libs/core/output.ts
View File

@ -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