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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 400 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
docs/examples/crane-labview.md Normal file
View File

@ -0,0 +1,51 @@
# Crane LabView
```blocks
function INI() {
motors.largeB.setBrake(true)
motors.largeC.setBrake(true)
motors.mediumA.setBrake(true)
motors.largeB.setSpeed(-50)
pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
motors.largeB.stop();
motors.mediumA.setSpeed(30, 1, MoveUnit.Seconds);
motors.mediumA.setSpeed(-50, 90, MoveUnit.Degrees);
motors.largeC.setSpeed(50)
sensors.touch1.pauseUntil(TouchSensorEvent.Pressed);
motors.largeC.setSpeed(-50, 0.86, MoveUnit.Rotations);
}
INI()
let down = false;
loops.forever(function () {
brick.showImage(images.informationQuestionMark)
brick.setLight(BrickLight.OrangePulse);
pauseUntil(() => (down = brick.buttonDown.wasPressed()) || brick.buttonUp.wasPressed())
brick.setLight(BrickLight.Off)
music.playSoundEffect(sounds.mechanicalAirRelease)
brick.showImage(images.informationAccept)
if (down) {
brick.showImage(images.informationForward)
motors.largeC.setSpeed(65, 0.85, MoveUnit.Rotations);
} else {
brick.showImage(images.informationBackward)
motors.largeC.setSpeed(-65, 0.85, MoveUnit.Rotations);
}
motors.largeB.setSpeed(20, 275, MoveUnit.Degrees)
motors.mediumA.setSpeed(30, 1, MoveUnit.Seconds)
motors.largeB.setSpeed(-55)
pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
motors.largeB.stop();
if (down) {
motors.largeC.setSpeed(-65, 0.86, MoveUnit.Rotations);
} else {
motors.largeC.setSpeed(65, 0.85, MoveUnit.Rotations);
}
motors.largeB.setSpeed(20, 275, MoveUnit.Degrees);
motors.mediumA.setSpeed(-30, 90, MoveUnit.Degrees);
motors.largeB.setSpeed(-55)
pauseUntil(() => sensors.color3.light(LightIntensityMode.Reflected) > 25);
motors.largeB.stop()
})
```

215
docs/examples/gyro-boy-labview.md Normal file
View File

@ -0,0 +1,215 @@
# Gyro Boy LabView
```blocks
let mSum = 0;
let mPos = 0;
let mSpd = 0;
let mD = 0;
let mDP1 = 0;
let mDP2 = 0;
let mDP3 = 0;
let Crdv = 0;
let cLo = 0;
let gAng = 0;
let ok = false;
let pwr = 0;
let Cstr = 0;
let Cdrv = 0;
let gMn = 0;
let gMx = 0;
let gSum = 0;
let gyro = 0;
let gOS = 0;
let gSpd = 0;
let tInt = 0.014;
let lpwr = 0
let rpwr = 0
let tStart = 0
let st = 0
let oldDr = 0
function RST() {
motors.largeA.reset()
motors.largeD.reset()
motors.largeA.setRegulated(false)
motors.largeD.setRegulated(false)
sensors.gyro2.reset()
sensors.gyro2.rate()
control.timer2.reset()
loops.pause(5000)
mSum = 0;
mPos = 0;
mD = 0;
mDP1 = 0;
mDP2 = 0;
mDP3 = 0;
Crdv = 0;
cLo = 0;
gAng = 0;
ok = false;
pwr = 0;
st = 0;
Cstr = 0;
Cdrv = 0;
}
function OS() {
// OSL
do {
gMn = 1000;
gMx = -100;
gSum = 0;
// gChk
for (let i = 0; i < 200; i++) {
gyro = sensors.gyro2.rate()
gSum = gyro;
gMx = Math.max(gMx, gyro)
gMn = Math.min(gMn, gyro)
loops.pause(4);
}
} while (gMx - gMn > 2);
gOS = gSum / 200;
}
function GT() {
if (cLo == 0) {
tInt = 0.014;
control.timer1.reset();
} else {
tInt = control.timer1.seconds() / cLo;
}
cLo++;
}
function GG() {
gyro = sensors.gyro2.rate();
gOS = 0.0005 * gyro + (1 - 0.0005) * gOS
gSpd = gyro - gOS;
gAng = gAng + tInt * gSpd;
}
function GM() {
let temp = mSum
mSum = motors.largeD.angle() + motors.largeA.angle();
mD = mSum - temp;
mPos = mPos + mD;
mSpd = ((mDP1 + mDP2 + mDP3 + mD) / 4) / tInt;
mDP3 = mDP2;
mDP2 = mDP1;
mDP1 = mD;
}
function EQ() {
mPos = mPos - Cdrv * tInt;
pwr = (0.8 * gSpd + 15 * gAng) + (0.095 * mSpd + 0.13 * mPos) - 0.01 * Cdrv
if (pwr > 100) pwr = 100
else if (pwr < -100) pwr = -100
}
function cntrl() {
mPos = mPos - tInt * Cdrv
lpwr = (pwr + Cstr * 0.1)
rpwr = (pwr - Cstr * 0.1)
}
function CHK() {
if (Math.abs(pwr) < 100)
control.timer2.reset();
if (control.timer2.seconds() > 2) {
ok = true;
}
}
// M
loops.forever(function () {
RST();
brick.showImage(images.eyesSleeping)
OS()
gAng = -0.25;
music.playSoundEffect(sounds.movementsSpeedUp)
brick.showImage(images.eyesAwake)
st = 1;
// BALANCE
while (!ok) {
GT();
let t1 = control.timer1.millis()
GG();
GM();
EQ();
cntrl();
motors.largeA.setSpeed(lpwr)
motors.largeD.setSpeed(rpwr)
CHK()
let t2 = control.timer1.millis();
let p = 5 - (t2 - t1);
loops.pause(Math.max(1, p))
}
motors.stopAllMotors()
st = 0;
brick.setLight(BrickLight.RedPulse);
brick.showImage(images.eyesKnockedOut)
music.playSoundEffect(sounds.movementsSpeedDown)
sensors.touch3.pauseUntil(TouchSensorEvent.Pressed)
brick.setLight(BrickLight.Off);
})
// BHV
loops.forever(function () {
switch (st) {
case 0:
Cdrv = 0;
Cstr = 0;
break;
case 1:
Cdrv = 40;
loops.pause(4000);
Cdrv = 0;
music.playTone(1000, 100);
st = 2;
break;
case 2:
switch (sensors.color1.color()) {
case ColorSensorColor.Red:
music.playTone(2000, 100);
Cdrv = 0;
Cstr = 0;
break;
case ColorSensorColor.Green:
music.playTone(2000, 100);
Cdrv = 150;
Cstr = 0;
break;
case ColorSensorColor.Blue:
music.playTone(2000, 100);
Cstr = 70;
break;
case ColorSensorColor.Yellow:
music.playTone(2000, 100);
Cstr = -70;
break;
case ColorSensorColor.White:
music.playTone(2000, 100);
Cdrv = -75;
break;
}
if (sensors.ultrasonic4.distance() < 25) {
Cstr = 0;
oldDr = Cdrv;
Cdrv = -10;
motors.mediumC.setSpeed(30, 30, MoveUnit.Degrees);
motors.mediumC.setSpeed(-30, 60, MoveUnit.Degrees);
motors.mediumC.setSpeed(30, 30, MoveUnit.Degrees);
if (Math.randomRange(-1, 1) >= 1)
Cstr = 70;
else
Cstr = -70;
loops.pause(4000);
music.playTone(2000, 100)
Cstr = 0;
Cdrv = oldDr;
}
break;
}
loops.pause(80);
})
```

2
libs/core/_locales/core-jsdoc-strings.json
View File

@ -53,6 +53,8 @@
"console.sendToScreen": "Sends the log messages to the brick screen and uses the brick up and down buttons to scroll.",
"control": "Program controls and events.",
"control.Timer.millis": "Gets the elapsed time in millis",
"control.Timer.pauseUntil": "Pauses until the timer reaches the given amount of milliseconds",
"control.Timer.pauseUntil|param|ms": "how long to pause for, eg: 5, 100, 200, 500, 1000, 2000",
"control.Timer.reset": "Resets the timer",
"control.Timer.seconds": "Gets the elapsed time in seconds",
"control.allocateNotifyEvent": "Allocates the next user notification event",

1
libs/core/_locales/core-strings.json
View File

@ -48,6 +48,7 @@
"console.sendToScreen|block": "send console to screen",
"console|block": "console",
"control.Timer.millis|block": "%timer|millis",
"control.Timer.pauseUntil|block": "%timer|pause until (ms) %ms",
"control.Timer.reset|block": "%timer|reset",
"control.Timer.seconds|block": "%timer|seconds",
"control.raiseEvent|block": "raise event|from %src|with value %value",

1
libs/core/input.ts
View File

@ -312,6 +312,7 @@ namespace sensors.internal {
reset() {
if (this.isActive()) uartReset(this._port);
this.realmode = 0;
}
}

10
libs/core/timer.ts
View File

@ -30,6 +30,16 @@ namespace control {
reset() {
this.start = control.millis();
}
/**
* Pauses until the timer reaches the given amount of milliseconds
* @param ms how long to pause for, eg: 5, 100, 200, 500, 1000, 2000
*/
//% blockId=timerPauseUntil block="%timer|pause until (ms) %ms"
pauseUntil(ms: number) {
const remaining = this.millis() - ms;
loops.pause(Math.max(0, remaining));
}
}
//% whenUsed fixedInstance block="timer 1"

3
libs/datalog/_locales/datalog-jsdoc-strings.json
View File

@ -3,6 +3,9 @@
"datalog.addValue": "Adds a cell to the row of data",
"datalog.addValue|param|name": "name of the cell, eg: \"x\"",
"datalog.addValue|param|value": "value of the cell, eg: 0",
"datalog.flush": "Commits any buffered row to disk",
"datalog.setEnabled": "Turns on or off datalogging",
"datalog.setFile": "Starts a new data logger for the given file",
"datalog.setFile|param|filename": "the filename, eg: \"datalog.csv\"",
"datalog.setStorage": "* @param storage custom storage solution"
}

1
libs/datalog/_locales/datalog-strings.json
View File

@ -1,6 +1,7 @@
{
"datalog.addRow|block": "datalog add row",
"datalog.addValue|block": "datalog add %name|=%value",
"datalog.setEnabled|block": "datalog %enabled",
"datalog|block": "datalog",
"{id:category}Datalog": "Datalog"
}

44
libs/datalog/datalog.ts
View File

@ -3,13 +3,16 @@ namespace datalog {
let _headers: string[] = undefined;
let _headersLength: number;
let _values: number[];
let _buffer: string = "";
let _start: number;
let _filename = "data.csv";
let _filename = "datalog.csv";
let _storage: storage.Storage = storage.temporary;
let _enabled = true;
function clear() {
_headers = undefined;
_values = undefined;
_buffer = "";
}
function init() {
@ -31,7 +34,10 @@ namespace datalog {
_headersLength = _storage.size(_filename);
}
// commit row data
_storage.appendCSV(_filename, _values);
_buffer += storage.toCSV(_values, _storage.csvSeparator);
// buffered writes
if (_buffer.length > 1024)
flush();
}
// clear values
@ -44,6 +50,8 @@ namespace datalog {
//% weight=100
//% blockId=datalogAddRow block="datalog add row"
export function addRow(): void {
if (!_enabled) return;
commit();
init();
const s = (control.millis() - _start) / 1000;
@ -65,16 +73,16 @@ namespace datalog {
i = _headers.length - 1;
}
_values[i] = value;
if (i > 0) // 0 is time
console.logValue(name, value)
}
/**
* Starts a new data logger for the given file
* @param filename the filename, eg: "datalog.csv"
*/
//%
export function setFile(fn: string) {
_filename = fn;
export function setFile(filename: string) {
flush();
_filename = filename;
clear();
}
@ -84,7 +92,31 @@ namespace datalog {
*/
//%
export function setStorage(storage: storage.Storage) {
flush();
_storage = storage;
clear();
}
/**
* Commits any buffered row to disk
*/
//%
export function flush() {
if (_buffer) {
const b = _buffer;
_buffer = "";
_storage.append(_filename, b);
}
}
/**
* Turns on or off datalogging
* @param enabled
*/
//% blockId=datalogEnabled block="datalog %enabled"
//% enabled.fieldEditor=fieldonoff
export function setEnabled(enabled: boolean) {
flush();
_enabled = enabled;
}
}

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;
}
}

27
libs/storage/storage-core.ts
View File

@ -7,7 +7,7 @@ namespace storage {
//% fixedInstances
export class Storage {
csvSeparator: string;
constructor() {
constructor() {
this.csvSeparator = ",";
}
@ -80,18 +80,13 @@ namespace storage {
}
/**
* Append a row of CSV data
* @param filename the file name to append data, eg: "data.csv"
* @param data the data to append
*/
* Append a row of CSV data
* @param filename the file name to append data, eg: "data.csv"
* @param data the data to append
*/
//% blockId=storageAppendCSV block="storage %source|%filename|append CSV %data"
appendCSV(filename: string, data: number[]) {
let s = ""
for (const d of data) {
if (s) s += this.csvSeparator;
s = s + d;
}
s += "\r\n"
let s = toCSV(data, this.csvSeparator);
this.append(filename, s)
}
@ -167,6 +162,16 @@ namespace storage {
}
}
export function toCSV(data: number[], sep: string) {
let s = ""
for (const d of data) {
if (s) s += sep;
s = s + d;
}
s += "\r\n"
return s;
}
class TemporaryStorage extends Storage {
constructor() {
super();

2
package.json
View File

@ -44,7 +44,7 @@
"webfonts-generator": "^0.4.0"
},
"dependencies": {
"pxt-common-packages": "0.15.5",
"pxt-common-packages": "0.15.6",
"pxt-core": "3.0.11"
},
"scripts": {