From 7581b5af9e8277a6870a1d93c82a6225eecbbe4a Mon Sep 17 00:00:00 2001 From: Peli de Halleux Date: Sun, 29 Sep 2019 09:49:13 -0700 Subject: [PATCH] Drift support (#926) * upgrading drift support * updated showports * typos --- docs/reference/brick/show-ports.md | 6 +- docs/reference/motors/reset-all.md | 25 ++++++ docs/reference/motors/stop-all.md | 1 + libs/color-sensor/color.ts | 5 +- libs/core/output.ts | 9 +- .../reference/sensors/gyro/compute-drift.md | 37 ++++++++ .../docs/reference/sensors/gyro/drift.md | 39 +++++++++ libs/gyro-sensor/gyro.ts | 86 ++++++++++++------- libs/tests/targetoverrides.ts | 4 +- 9 files changed, 174 insertions(+), 38 deletions(-) create mode 100644 docs/reference/motors/reset-all.md create mode 100644 libs/gyro-sensor/docs/reference/sensors/gyro/compute-drift.md create mode 100644 libs/gyro-sensor/docs/reference/sensors/gyro/drift.md diff --git a/docs/reference/brick/show-ports.md b/docs/reference/brick/show-ports.md index 43c8b60f..c1972e3a 100644 --- a/docs/reference/brick/show-ports.md +++ b/docs/reference/brick/show-ports.md @@ -10,12 +10,12 @@ You can find out what's connected to the ports on the brick and show its status. ## Example -Show the status of the ports on the brick when the ``enter`` button is pressed. +Show the status of the ports on the brick. Resets all motors when ENTER is pressed. ```blocks -brick.showString("Press ENTER for port status", 1) +brick.showPorts() brick.buttonEnter.onEvent(ButtonEvent.Pressed, function () { - brick.showPorts() + motors.resetAll() }) ``` diff --git a/docs/reference/motors/reset-all.md b/docs/reference/motors/reset-all.md new file mode 100644 index 00000000..7005f540 --- /dev/null +++ b/docs/reference/motors/reset-all.md @@ -0,0 +1,25 @@ +# reset All Motors + +Reset all motors currently running on the brick. + +```sig +motors.resetAll(); +``` + +The motors counters are resetted. + +## Example + +Tank the EV3 Brick forward at half speed for 5 seconds and then stop. + +```blocks +motors.largeAB.tank(50, 50); +pause(5000); +motors.stopAll(); +motors.resetAll(); +``` + +## See also + +[stop all](/reference/motors/motor/stop-all), +[reset](/reference/motors/motor/reset) diff --git a/docs/reference/motors/stop-all.md b/docs/reference/motors/stop-all.md index 544523a8..239d69dd 100644 --- a/docs/reference/motors/stop-all.md +++ b/docs/reference/motors/stop-all.md @@ -22,4 +22,5 @@ motors.stopAll(); [stop](/reference/motors/motor/stop), [reset](/reference/motors/motor/reset), +[reset-all](/reference/motors/motor/reset-all), [set brake](/reference/motors/motor/set-brake) \ No newline at end of file diff --git a/libs/color-sensor/color.ts b/libs/color-sensor/color.ts index 411bc988..c6856739 100644 --- a/libs/color-sensor/color.ts +++ b/libs/color-sensor/color.ts @@ -111,6 +111,9 @@ namespace sensors { "red", "white", "brown"][this._query()]; + case ColorSensorMode.AmbientLightIntensity: + case ColorSensorMode.ReflectedLightIntensity: + return `${this._query()}%`; default: return this._query().toString(); } @@ -253,7 +256,7 @@ namespace sensors { light(mode: LightIntensityMode) { this.poke(); this.setMode(mode) - switch(mode) { + switch (mode) { case LightIntensityMode.ReflectedRaw: return this.reflectedLightRaw(); default: diff --git a/libs/core/output.ts b/libs/core/output.ts index 18fc38db..1fa9855c 100644 --- a/libs/core/output.ts +++ b/libs/core/output.ts @@ -62,7 +62,7 @@ namespace motors { motorMM = control.mmap("/dev/lms_motor", MotorDataOff.Size * DAL.NUM_OUTPUTS, 0) if (!motorMM) control.fail("no motor file") - resetAllMotors() + resetAll() const buf = output.createBuffer(1) buf[0] = DAL.opProgramStart @@ -118,7 +118,7 @@ namespace motors { * Stops all motors */ //% blockId=motorStopAll block="stop all motors" - //% weight=1 + //% weight=2 //% group="Move" //% help=motors/stop-all export function stopAll() { @@ -130,8 +130,11 @@ namespace motors { /** * Resets all motors */ + //% blockId=motorResetAll block="reset all motors" + //% weight=1 //% group="Move" - export function resetAllMotors() { + //% help=motors/reset-all + export function resetAll() { reset(Output.ALL) pause(1); } diff --git a/libs/gyro-sensor/docs/reference/sensors/gyro/compute-drift.md b/libs/gyro-sensor/docs/reference/sensors/gyro/compute-drift.md new file mode 100644 index 00000000..cac5d8d1 --- /dev/null +++ b/libs/gyro-sensor/docs/reference/sensors/gyro/compute-drift.md @@ -0,0 +1,37 @@ +# compute Drift + +Called when the sensor is completely still, computes the current rate drift +```sig +sensors.gyro2.computeDrift() +``` + +The gyroscope sensor is subject to rate drifting. This means that the measurement reported by the sensor is off by a few degrees per second over time: it is drifting. + +To counter the effect of drifting, call the ``||sensors:compute drift||`` block when the sensor is still to compute the current drift. The rate meansurements will automatically be corrected based on that drift. + +## Example + +This example uses a gyro sensor to + +```blocks +let error = 0 +sensors.gyro2.computeDrift() +while (sensors.color3.color() != ColorSensorColor.White) { + error = sensors.gyro2.rate() * -1 + motors.largeBC.steer(error, 50) +} +motors.stopAll() +pause(1000) +sensors.gyro2.computeDrift() +while (sensors.color3.color() != ColorSensorColor.Blue) { + error = sensors.gyro2.rate() * -1 + motors.largeBC.steer(error, 50) +} +motors.stopAll() +``` + +## See Also + +[rate](/reference/sensors/gyro/rate), +[compute drift](/reference/sensors/gyro/compute-drift) + diff --git a/libs/gyro-sensor/docs/reference/sensors/gyro/drift.md b/libs/gyro-sensor/docs/reference/sensors/gyro/drift.md new file mode 100644 index 00000000..df908891 --- /dev/null +++ b/libs/gyro-sensor/docs/reference/sensors/gyro/drift.md @@ -0,0 +1,39 @@ +# drift + +Get the computed rate drift + +```sig +sensors.gyro2.drift() +``` + +The gyroscope sensor is subject to rate drifting. This means that the measurement reported by the sensor is off by a few degrees per second over time: it is drifting. + +To counter the effect of drifting, call the ``||sensors:compute drift||`` block when the sensor is still to compute the current drift. The rate meansurements will automatically be corrected based on that drift. + +## Example + +This example uses a gyro sensor to drive straight until while color is detected. +The robot is stopped, the drift is computed and another movement is done. + +```blocks +let error = 0 +sensors.gyro2.computeDrift() +while (sensors.color3.color() != ColorSensorColor.White) { + error = sensors.gyro2.rate() * -1 + motors.largeBC.steer(error, 50) +} +motors.stopAll() +pause(1000) +sensors.gyro2.computeDrift() +while (sensors.color3.color() != ColorSensorColor.Blue) { + error = sensors.gyro2.rate() * -1 + motors.largeBC.steer(error, 50) +} +motors.stopAll() +``` + +## See Also + +[rate](/reference/sensors/gyro/rate), +[compute drift](/reference/sensors/gyro/compute-drift) + diff --git a/libs/gyro-sensor/gyro.ts b/libs/gyro-sensor/gyro.ts index 6fe5e120..c9cdf25a 100644 --- a/libs/gyro-sensor/gyro.ts +++ b/libs/gyro-sensor/gyro.ts @@ -9,12 +9,10 @@ namespace sensors { export class GyroSensor extends internal.UartSensor { private calibrating: boolean; private _drift: number; - private _driftCorrection: boolean; constructor(port: number) { super(port) this.calibrating = false; this._drift = 0; - this._driftCorrection = false; this.setMode(GyroSensorMode.Rate); } @@ -42,7 +40,7 @@ namespace sensors { //% parts="gyroscope" //% blockNamespace=sensors //% this.fieldEditor="ports" - //% weight=64 blockGap=8 + //% weight=64 //% group="Gyro Sensor" angle(): number { this.poke(); @@ -71,13 +69,7 @@ namespace sensors { pauseUntil(() => !this.calibrating, 2000); this.setMode(GyroSensorMode.Rate); - let curr = this._query(); - if (Math.abs(curr) < 4 && this._driftCorrection) { - const p = 0.01; - this._drift = (1 - p) * this._drift + p * curr; - curr = Math.round(curr - this._drift); - } - return curr; + return this._query() - this._drift; } /** @@ -113,8 +105,6 @@ namespace sensors { // mode toggling this.setMode(GyroSensorMode.Rate); this.setMode(GyroSensorMode.Angle); - // switch back to the desired mode - this.setMode(this.mode); // check sensor is ready if (!this.isActive()) { @@ -125,17 +115,12 @@ namespace sensors { return; } - // compute drift - this._drift = 0; - if (this._driftCorrection && this.mode == GyroSensorMode.Rate) { - const n = 100; - for (let i = 0; i < n; ++i) { - this._drift += this._query(); - pause(4); - } - this._drift /= n; - } + // switch to rate mode + this.computeDriftNoCalibration(); + // switch back to the desired mode + this.setMode(this.mode); + // and done brick.setStatusLight(StatusLight.Green); // success pause(1000); brick.setStatusLight(statusLight); // resture previous light @@ -153,7 +138,7 @@ namespace sensors { //% parts="gyroscope" //% blockNamespace=sensors //% this.fieldEditor="ports" - //% weight=50 + //% weight=50 blockGap=8 //% group="Gyro Sensor" reset(): void { if (this.calibrating) return; // already in calibration mode @@ -168,19 +153,62 @@ namespace sensors { /** * Gets the computed rate drift */ - //% + //% help=sensors/gyro/drift + //% block="**gyro** %this|drift" + //% blockId=gyroDrift + //% parts="gyroscope" + //% blockNamespace=sensors + //% this.fieldEditor="ports" + //% weight=9 blockGap=8 + //% group="Gyro Sensor" drift(): number { return this._drift; } /** - * Enables or disable drift correction - * @param enabled + * Computes the current sensor drift when using rate measurements. */ - //% - setDriftCorrection(enabled: boolean) { - this._driftCorrection = enabled; + //% help=sensors/gyro/compute-drift + //% block="compute **gyro** %this|drift" + //% blockId=gyroComputeDrift + //% parts="gyroscope" + //% blockNamespace=sensors + //% this.fieldEditor="ports" + //% weight=10 blockGap=8 + //% group="Gyro Sensor" + computeDrift() { + if (this.calibrating) + pauseUntil(() => !this.calibrating, 2000); + this.computeDriftNoCalibration(); + } + + private computeDriftNoCalibration() { + // clear drift + this.setMode(GyroSensorMode.Rate); this._drift = 0; + const n = 100; + let d = 0; + for (let i = 0; i < n; ++i) { + d += this._query(); + pause(4); + } + this._drift = d / n; + } + + _info(): string { + if (this.calibrating) + return "cal..."; + + switch (this.mode) { + case GyroSensorMode.Angle: + return `${this._query()}>`; + case GyroSensorMode.Rate: + let r = `${this._query()}>/s`; + if (this._drift) + r += `-${this._drift | 0}`; + return r; + } + return ""; } } diff --git a/libs/tests/targetoverrides.ts b/libs/tests/targetoverrides.ts index dec51617..6424b3eb 100644 --- a/libs/tests/targetoverrides.ts +++ b/libs/tests/targetoverrides.ts @@ -4,9 +4,9 @@ tests.onEvent(TestEvent.RunSetUp, function() { }) tests.onEvent(TestEvent.TestSetUp, function() { motors.stopAll(); - motors.resetAllMotors(); + motors.resetAll(); }) tests.onEvent(TestEvent.TestTearDown, function() { motors.stopAll(); - motors.resetAllMotors(); + motors.resetAll(); })