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

gyro boy improvements (#236)

Browse Source 
gyro boy improvements
This commit is contained in:
Peli de Halleux
2018-01-13 08:31:10 -08:00
committed by GitHub
parent 25fded6afb
commit 0b763978f2
13 changed files with 400 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
libs/gyro-sensor/_locales/gyro-sensor-jsdoc-strings.json
View File

@ -1,5 +1,7 @@
{
"sensors.GyroSensor.angle": "Get the current angle from the gyroscope.",
"sensors.GyroSensor.drift": "Gets the computed rate drift",
"sensors.GyroSensor.rate": "Get the current rotation rate from the gyroscope.",
"sensors.GyroSensor.reset": "Forces a calibration of the gyro. Must be called when the sensor is completely still."
"sensors.GyroSensor.reset": "Forces a calibration of the gyro. Must be called when the sensor is completely still.",
"sensors.GyroSensor.setDriftCorrection": "Enables or disable drift correction"
}

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

@ -8,16 +8,27 @@ namespace sensors {
//% fixedInstances
export class GyroSensor extends internal.UartSensor {
private calibrating: boolean;
private _drift: number;
private _drifting: boolean;
constructor(port: number) {
super(port)
this.calibrating = false;
this._drift = 0;
this._drifting = true;
this.setMode(GyroSensorMode.Rate);
}
_deviceType() {
return DAL.DEVICE_TYPE_GYRO
}
_query(): number {
return this.getNumber(NumberFormat.Int16LE, 0);
}
setMode(m: GyroSensorMode) {
if (m == GyroSensorMode.Rate && this.mode != m)
this._drift = 0;
this._setMode(m)
}
@ -37,8 +48,8 @@ namespace sensors {
if (this.calibrating)
pauseUntil(() => !this.calibrating, 2000);
this.setMode(GyroSensorMode.Angle)
return this.getNumber(NumberFormat.Int16LE, 0)
this.setMode(GyroSensorMode.Angle);
return this._query();
}
/**
@ -57,8 +68,14 @@ namespace sensors {
if (this.calibrating)
pauseUntil(() => !this.calibrating, 2000);
this.setMode(GyroSensorMode.Rate)
return this.getNumber(NumberFormat.Int16LE, 0)
this.setMode(GyroSensorMode.Rate);
let curr = this._query();
if (Math.abs(curr) < 20) {
const p = 0.0005;
this._drift = (1 - p) * this._drift + p * curr;
curr -= this._drift;
}
return curr;
}
/**
@ -76,23 +93,45 @@ namespace sensors {
if (this.calibrating) return; // already in calibration mode
this.calibrating = true;
// may be triggered by a button click, give time to settle
loops.pause(500);
// may be triggered by a button click,
// give time for robot to settle
loops.pause(700);
// send a reset command
super.reset();
// we need to switch mode twice to perform a calibration
if (this.mode == GyroSensorMode.Rate)
this.setMode(GyroSensorMode.Angle);
else
this.setMode(GyroSensorMode.Rate);
// switch back and wait
if (this.mode == GyroSensorMode.Rate)
this.setMode(GyroSensorMode.Angle);
else
this.setMode(GyroSensorMode.Rate);
// give it more time to settle
loops.pause(500);
this.calibrating = false;
// switch back to the desired mode
this.setMode(this.mode);
// wait till sensor is live
pauseUntil(() => this.isActive());
// give it a bit of time to init
loops.pause(1000)
// compute drift
this._drift = 0;
if (this.mode == GyroSensorMode.Rate) {
for (let i = 0; i < 200; ++i) {
this._drift += this._query();
loops.pause(4);
}
this._drift /= 200;
}
// and we're done
this.calibrating = false;
}
/**
* Gets the computed rate drift
*/
//%
drift(): number {
return this._drift;
}
/**
* Enables or disable drift correction
* @param enabled
*/
//%
setDriftCorrection(enabled: boolean) {
this._drifting = enabled;
}
}