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

Drift-compensated angle in gyro (#931)

Browse Source 
* compute angle based on undrifted rate

* add is calibrating function

* fix integrator

* added example

* docs

* poll faster
This commit is contained in:
Peli de Halleux 2019-10-01 10:11:58 -07:00 committed by GitHub
parent 25452efc92
commit 374bbb8304
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 192 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
docs/static/tutorials/drifter.png vendored Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 15 KiB

34
docs/tutorials/drifter.md Normal file
View File

@ -0,0 +1,34 @@
# Drifter
Use this program to try out the gyro sensor and the effect of drifting.
```typescript
// this loop shows the rate, angle and drift of the robot
forever(() => {
brick.showValue("rate", sensors.gyro2.rate(), 1)
brick.showValue("angle", sensors.gyro2.angle(), 2)
brick.showValue("drift", sensors.gyro2.drift(), 3)
})
// this loop shows wheter the sensor is calibrating
forever(() => {
brick.showString(sensors.gyro2.isCalibrating() ? "calibrating..." : "", 4)
})
// instructions on how to use the buttons
brick.showString("ENTER: calibrate", 7)
brick.showString(" (reset+drift)", 8)
brick.showString("LEFT: reset", 9)
brick.showString("RIGHT: compute drift", 10)
// enter -> calibrate
brick.buttonEnter.onEvent(ButtonEvent.Pressed, function () {
sensors.gyro2.calibrate()
})
// right -> compute drift
brick.buttonRight.onEvent(ButtonEvent.Pressed, function () {
sensors.gyro2.computeDrift()
})
// left -> reset
brick.buttonLeft.onEvent(ButtonEvent.Pressed, function () {
sensors.gyro2.reset()
})
```

6
docs/tutorials/gyro.md
View File

@ -15,5 +15,11 @@
"cardType": "tutorial",
"url":"/tutorials/move-straight-with-gyro",
"imageUrl":"/static/tutorials/move-straight-with-gyro.png"
}, {
"name": "Drifter",
"description": "Explore how the gyro is drifting",
"cardType": "example",
"url":"/tutorials/drifter",
"imageUrl":"/static/tutorials/drifter.png"
}]
```

4
libs/core/input.ts
View File

@ -87,7 +87,7 @@ namespace sensors.internal {
this.devType = DAL.DEVICE_TYPE_NONE
this.iicid = ''
this.sensors = []
this.poller = new Poller(50, () => this.query(), (prev, curr) => this.update(prev, curr));
this.poller = new Poller(25, () => this.query(), (prev, curr) => this.update(prev, curr));
}
poke() {
@ -121,7 +121,7 @@ namespace sensors.internal {
powerMM = control.mmap("/dev/lms_power", 2, 0)
devPoller = new Poller(500, () => { return hashDevices(); },
devPoller = new Poller(250, () => { return hashDevices(); },
(prev, curr) => {
detectDevices();
});

26
libs/core/integrator.ts Normal file
View File

@ -0,0 +1,26 @@
namespace control {
export class EulerIntegrator {
public value: number;
private t: number;
private v: number;
constructor() {
this.reset();
}
public integrate(derivative: number): void {
let now = control.millis();
let dt = (now -this.t) / 1000.0;
this.value += dt * (this.v + derivative) / 2;
this.t = now;
this.v = derivative;
}
public reset() {
this.value = 0;
this.v = 0;
this.t = control.millis();
}
}
}

6
libs/core/output.ts
View File

@ -264,8 +264,9 @@ namespace motors {
// allow 500ms for robot to settle
if (this._brake && this._brakeSettleTime > 0)
pause(this._brakeSettleTime);
else
pause(1); // give a tiny breather
else {
pause(1);
}
}
protected pauseOnRun(stepsOrTime: number) {
@ -275,7 +276,6 @@ namespace motors {
// allow robot to settle
this.settle();
} else {
// give a breather to the event system in tight loops
pause(1);
}
}

3
libs/core/pxt.json
View File

@ -25,7 +25,8 @@
"dal.d.ts",
"icons.jres",
"ns.ts",
"platform.h"
"platform.h",
"integrator.ts"
],
"testFiles": [
"test.ts"

21
libs/gyro-sensor/docs/reference/sensors/gyro/pause-until-rotated.md Normal file
View File

@ -0,0 +1,21 @@
# Pause Until Rotated
Pauses the program until the gyro sensors detect that the desired amount of rotation
has been acheived.
```
sensors.gyro2.pauseUntilRotated(90)
```
## Example
This program performs a square turn left, then right.
```blocks
sensors.gyro2.calibrate()
motors.largeBC.steer(200, 10)
sensors.gyro2.pauseUntilRotated(90)
motors.largeBC.steer(-200, 10)
sensors.gyro2.pauseUntilRotated(-90)
motors.largeBC.stop()
```

126
libs/gyro-sensor/gyro.ts
View File

@ -7,12 +7,15 @@ const enum GyroSensorMode {
namespace sensors {
//% fixedInstances
export class GyroSensor extends internal.UartSensor {
private calibrating: boolean;
private _calibrating: boolean;
private _drift: number;
private _angle: control.EulerIntegrator;
constructor(port: number) {
super(port)
this.calibrating = false;
this._calibrating = false;
this._drift = 0;
this._angle = new control.EulerIntegrator();
this._setMode(GyroSensorMode.Rate);
this.setMode(GyroSensorMode.Rate);
}
@ -21,13 +24,17 @@ namespace sensors {
}
_query(): number {
return this.getNumber(NumberFormat.Int16LE, 0);
const v = this.getNumber(NumberFormat.Int16LE, 0);
this._angle.integrate(v - this._drift);
return v;
}
setMode(m: GyroSensorMode) {
if (m == GyroSensorMode.Rate && this.mode != m)
this._drift = 0;
this._setMode(m)
// decrecated
}
isCalibrating(): boolean {
return this._calibrating;
}
/**
@ -40,15 +47,14 @@ namespace sensors {
//% parts="gyroscope"
//% blockNamespace=sensors
//% this.fieldEditor="ports"
//% weight=64
//% weight=64 blockGap=8
//% group="Gyro Sensor"
angle(): number {
this.poke();
if (this.calibrating)
pauseUntil(() => !this.calibrating, 2000);
if (this._calibrating)
pauseUntil(() => !this._calibrating, 2000);
this.setMode(GyroSensorMode.Angle);
return this._query();
return Math.round(this._angle.value);
}
/**
@ -65,10 +71,8 @@ namespace sensors {
//% group="Gyro Sensor"
rate(): number {
this.poke();
if (this.calibrating)
pauseUntil(() => !this.calibrating, 2000);
this.setMode(GyroSensorMode.Rate);
if (this._calibrating)
pauseUntil(() => !this._calibrating, 2000);
return this._query() - this._drift;
}
@ -85,12 +89,12 @@ namespace sensors {
//% weight=51 blockGap=8
//% group="Gyro Sensor"
calibrate(): void {
if (this.calibrating) return; // already in calibration mode
if (this._calibrating) return; // already in calibration mode
const statusLight = brick.statusLight(); // save current status light
brick.setStatusLight(StatusLight.Orange);
this.calibrating = true;
this._calibrating = true;
// may be triggered by a button click,
// give time for robot to settle
pause(700);
@ -104,7 +108,8 @@ namespace sensors {
brick.setStatusLight(statusLight); // resture previous light
// and we're done
this.calibrating = false;
this._angle.reset();
this._calibrating = false;
return;
}
@ -116,22 +121,22 @@ namespace sensors {
// wait till sensor is live
pauseUntil(() => this.isActive(), 7000);
// mode toggling
this.setMode(GyroSensorMode.Rate);
this.setMode(GyroSensorMode.Angle);
this._setMode(GyroSensorMode.Rate);
this._setMode(GyroSensorMode.Angle);
this._setMode(GyroSensorMode.Rate);
// check sensor is ready
if (!this.isActive()) {
brick.setStatusLight(StatusLight.RedFlash); // didn't work
pause(2000);
brick.setStatusLight(statusLight); // restore previous light
this.calibrating = false;
this._angle.reset();
this._calibrating = false;
return;
}
// switch to rate mode
this.computeDriftNoCalibration();
// switch back to the desired mode
this.setMode(this.mode);
// and done
brick.setStatusLight(StatusLight.Green); // success
@ -139,7 +144,8 @@ namespace sensors {
brick.setStatusLight(statusLight); // resture previous light
// and we're done
this.calibrating = false;
this._angle.reset();
this._calibrating = false;
}
/**
@ -154,13 +160,37 @@ namespace sensors {
//% weight=50 blockGap=8
//% group="Gyro Sensor"
reset(): void {
if (this.calibrating) return; // already in calibration mode
if (this._calibrating) return; // already in calibration mode
this._calibrating = true;
const statusLight = brick.statusLight(); // save current status light
brick.setStatusLight(StatusLight.Orange);
this.calibrating = true;
// send a reset command
super.reset();
this._drift = 0;
this._angle.reset();
pauseUntil(() => this.isActive(), 7000);
// check sensor is ready
if (!this.isActive()) {
brick.setStatusLight(StatusLight.RedFlash); // didn't work
pause(2000);
brick.setStatusLight(statusLight); // restore previous light
this._angle.reset();
this._calibrating = false;
return;
}
this._setMode(GyroSensorMode.Rate);
// and done
this.calibrating = false;
brick.setStatusLight(StatusLight.Green); // success
pause(1000);
brick.setStatusLight(statusLight); // resture previous light
// and done
this._angle.reset();
this._calibrating = false;
}
/**
@ -190,15 +220,34 @@ namespace sensors {
//% weight=10 blockGap=8
//% group="Gyro Sensor"
computeDrift() {
if (this.calibrating)
pauseUntil(() => !this.calibrating, 2000);
if (this._calibrating)
pauseUntil(() => !this._calibrating, 2000);
pause(1000); // let the robot settle
this.computeDriftNoCalibration();
}
/**
* Pauses the program until the gyro detected
* that the angle changed by the desired amount of degrees.
* @param degrees the degrees to turn
*/
//% help=sensors/gyro/pause-until-rotated
//% block="pause **gyro** %this|until rotated %degrees|degrees"
//% blockId=gyroPauseUntilRotated
//% parts="gyroscope"
//% blockNamespace=sensors
//% this.fieldEditor="ports"
//% weight=63
//% group="Gyro Sensor"
pauseUntilRotated(degrees: number, timeOut?: number): void {
let a = this.angle();
const end = a + degrees;
const direction = (end - a) > 0 ? 1 : -1;
pauseUntil(() => (end - this.angle()) * direction <= 0, timeOut);
}
private computeDriftNoCalibration() {
// clear drift
this.setMode(GyroSensorMode.Rate);
this._drift = 0;
const n = 10;
let d = 0;
@ -207,22 +256,17 @@ namespace sensors {
pause(20);
}
this._drift = d / n;
this._angle.reset();
}
_info(): string {
if (this.calibrating)
if (this._calibrating)
return "cal...";
switch (this.mode) {
case GyroSensorMode.Angle:
return `${this._query()}>`;
case GyroSensorMode.Rate:
let r = `${this._query()}r`;
if (this._drift != 0)
r += `-${this._drift | 0}`;
return r;
}
return "";
let r = `${this._query()}r`;
if (this._drift != 0)
r += `-${this._drift | 0}`;
return r;
}
}

16
sim/state/gyro.ts
View File

@ -8,7 +8,6 @@ namespace pxsim {
export class GyroSensorNode extends UartSensorNode {
id = NodeType.GyroSensor;
private angle: number = 0;
private rate: number = 0;
constructor(port: number) {
@ -19,14 +18,6 @@ namespace pxsim {
return DAL.DEVICE_TYPE_GYRO;
}
setAngle(angle: number) {
angle = angle | 0;
if (this.angle != angle) {
this.angle = angle;
this.setChangedState();
}
}
setRate(rate: number) {
rate = rate | 0;
if (this.rate != rate) {
@ -35,9 +26,12 @@ namespace pxsim {
}
}
getRate() {
return this.rate;
}
getValue() {
return this.mode == GyroSensorMode.Angle ? this.angle :
this.mode == GyroSensorMode.Rate ? this.rate : 0;
return this.getRate();
}
}
}

28
sim/visuals/controls/rotationSlider.ts
View File

@ -2,12 +2,11 @@
namespace pxsim.visuals {
const MAX_RATE = 40;
const MAX_ANGLE = 360;
export class RotationSliderControl extends ControlView<GyroSensorNode> {
private group: SVGGElement;
private slider: SVGGElement;
private text: SVGTextElement;
private rateText: SVGTextElement;
private static SLIDER_WIDTH = 70;
//private static SLIDER_HEIGHT = 78;
@ -26,8 +25,8 @@ namespace pxsim.visuals {
pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 13, 'style': 'fill: #f12a21' });
pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 12.5, 'style': 'fill: none;stroke: #b32e29' });
this.text = pxsim.svg.child(this.group, "text", {
'x': RotationSliderControl.SLIDER_WIDTH / 2,
this.rateText = pxsim.svg.child(this.group, "text", {
'x': this.getInnerWidth() / 2,
'y': RotationSliderControl.SLIDER_WIDTH * 1.2,
'text-anchor': 'middle', 'dominant-baseline': 'middle',
'style': 'font-size: 16px',
@ -72,17 +71,10 @@ namespace pxsim.visuals {
return;
}
const node = this.state;
let percentage = 50;
if (node.getMode() == GyroSensorMode.Rate) {
const rate = node.getValue();
this.text.textContent = `${rate}°/s`
// cap rate at 40deg/s
percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
} else { //angle
const angle = node.getValue();
this.text.textContent = `${angle}°`
percentage = 50 + Math.sign(angle) * Math.min(MAX_ANGLE, Math.abs(angle)) / MAX_ANGLE * 50;
}
const rate = node.getRate();
this.rateText.textContent = `${rate}°/s`
// cap rate at 40deg/s
const percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
const x = RotationSliderControl.SLIDER_WIDTH * percentage / 100;
const y = Math.abs((percentage - 50) / 50) * 10;
this.slider.setAttribute("transform", `translate(${x}, ${y})`);
@ -97,11 +89,7 @@ namespace pxsim.visuals {
t = -(t - 0.5) * 2; // [-1,1]
const state = this.state;
if (state.getMode() == GyroSensorMode.Rate) {
state.setRate(MAX_RATE * t);
} else {
state.setAngle(MAX_ANGLE * t)
}
state.setRate(MAX_RATE * t);
}
}