Compare commits
6 Commits
Author | SHA1 | Date | |
---|---|---|---|
|
25452efc92 | ||
|
80b9c715b2 | ||
|
cb816c91ad | ||
|
3012068986 | ||
|
4c9c7d6a69 | ||
|
ad3652c290 |
@@ -1,3 +1,3 @@
|
|||||||
{
|
{
|
||||||
"appref": "v1.1.20"
|
"appref": "v1.2.15"
|
||||||
}
|
}
|
||||||
|
BIN
docs/static/tutorials/calibrate-gyro.png
vendored
Normal file
BIN
docs/static/tutorials/calibrate-gyro.png
vendored
Normal file
Binary file not shown.
After Width: | Height: | Size: 13 KiB |
BIN
docs/static/tutorials/move-straight-with-gyro.png
vendored
Normal file
BIN
docs/static/tutorials/move-straight-with-gyro.png
vendored
Normal file
Binary file not shown.
After Width: | Height: | Size: 16 KiB |
@@ -25,6 +25,11 @@ Step by step guides to coding your @boardname@.
|
|||||||
"description": "Use the color sensor to follow line or detect colors",
|
"description": "Use the color sensor to follow line or detect colors",
|
||||||
"url":"/tutorials/color-sensor",
|
"url":"/tutorials/color-sensor",
|
||||||
"imageUrl":"/static/tutorials/what-color.png"
|
"imageUrl":"/static/tutorials/what-color.png"
|
||||||
|
}, {
|
||||||
|
"name": "Gyro",
|
||||||
|
"description": "Drive straight or turn more precisely with the gyro",
|
||||||
|
"url":"/tutorials/gyro",
|
||||||
|
"imageUrl":"/static/tutorials/calibrate-gyro.png"
|
||||||
}, {
|
}, {
|
||||||
"name": "Ultrasonic Sensor",
|
"name": "Ultrasonic Sensor",
|
||||||
"description": "Use the ultrasonic sensor to detect obstacles",
|
"description": "Use the ultrasonic sensor to detect obstacles",
|
||||||
|
37
docs/tutorials/calibrate-gyro.md
Normal file
37
docs/tutorials/calibrate-gyro.md
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
# Calibrate Gyro
|
||||||
|
|
||||||
|
## Introduction @fullscreen
|
||||||
|
|
||||||
|
The gyroscope is a very useful sensor in the EV3 system. It detects the rotation rate
|
||||||
|
which can be very useful to correct the trajectory of the robot and do precise turns.
|
||||||
|
|
||||||
|
However, the sensor can be imprecise and subject to drifting. It is recommend to
|
||||||
|
calibrate your sensor at least once after starting your brick. You don't have to
|
||||||
|
recalibrate on every run.
|
||||||
|
|
||||||
|
* [EV3 Driving Base](https://le-www-live-s.legocdn.com/sc/media/lessons/mindstorms-ev3/building-instructions/ev3-rem-driving-base-79bebfc16bd491186ea9c9069842155e.pdf)
|
||||||
|
* [EV3 Driving Base with Gyro](https://le-www-live-s.legocdn.com/sc/media/lessons/mindstorms-ev3/building-instructions/ev3-gyro-sensor-driving-base-a521f8ebe355c281c006418395309e15.pdf)
|
||||||
|
|
||||||
|
|
||||||
|
## Step 1 Show ports
|
||||||
|
|
||||||
|
Add the ``||brick:show ports||`` to see the status of the gyroscope.
|
||||||
|
|
||||||
|
```blocks
|
||||||
|
brick.showPorts()
|
||||||
|
```
|
||||||
|
|
||||||
|
|
||||||
|
## Step 2 Calibration
|
||||||
|
|
||||||
|
Add a ``||sensors:calibrate gyro||`` block to calibrate the gyro. The block
|
||||||
|
detects if the sensor is present and does a full reset of the sensor if necessary.
|
||||||
|
|
||||||
|
```blocks
|
||||||
|
brick.showPorts()
|
||||||
|
sensors.gyro2.calibrate()
|
||||||
|
```
|
||||||
|
|
||||||
|
## Step 3 Download and run @fullscreen
|
||||||
|
|
||||||
|
Download this program to your brick and press the ENTER button.
|
19
docs/tutorials/gyro.md
Normal file
19
docs/tutorials/gyro.md
Normal file
@@ -0,0 +1,19 @@
|
|||||||
|
# Gyro tutorials
|
||||||
|
|
||||||
|
## Tutorials
|
||||||
|
|
||||||
|
```codecard
|
||||||
|
[{
|
||||||
|
"name": "Calibrate",
|
||||||
|
"description": "Make sure you gyro sensor is ready to use",
|
||||||
|
"cardType": "tutorial",
|
||||||
|
"url":"/tutorials/calibrate-gyro",
|
||||||
|
"imageUrl":"/static/tutorials/calibrate-gyro.png"
|
||||||
|
}, {
|
||||||
|
"name": "Move Straight",
|
||||||
|
"description": "Use the gyro to correct the trajectory of the robot",
|
||||||
|
"cardType": "tutorial",
|
||||||
|
"url":"/tutorials/move-straight-with-gyro",
|
||||||
|
"imageUrl":"/static/tutorials/move-straight-with-gyro.png"
|
||||||
|
}]
|
||||||
|
```
|
61
docs/tutorials/move-straight-with-gyro.md
Normal file
61
docs/tutorials/move-straight-with-gyro.md
Normal file
@@ -0,0 +1,61 @@
|
|||||||
|
# Move Straight With Gyro
|
||||||
|
|
||||||
|
## Introduction @fullscreen
|
||||||
|
|
||||||
|
Rotating using a wheel is not precise. The wheel can slip or the motors
|
||||||
|
can be slightly different.
|
||||||
|
With the help of the gyro you can detect and correct deviations in your trajectory.
|
||||||
|
|
||||||
|
* [EV3 Driving Base](https://le-www-live-s.legocdn.com/sc/media/lessons/mindstorms-ev3/building-instructions/ev3-rem-driving-base-79bebfc16bd491186ea9c9069842155e.pdf)
|
||||||
|
* [EV3 Driving Base with Gyro](https://le-www-live-s.legocdn.com/sc/media/lessons/mindstorms-ev3/building-instructions/ev3-gyro-sensor-driving-base-a521f8ebe355c281c006418395309e15.pdf)
|
||||||
|
|
||||||
|
|
||||||
|
## Step 1 Calibration
|
||||||
|
|
||||||
|
Add a ``||sensors:calibrate gyro||`` block in a ``||brick:on button enter pressed||`` block so that you can manually start a calibration process. Run the calibration
|
||||||
|
at least once after connecting the gyro.
|
||||||
|
|
||||||
|
```blocks
|
||||||
|
brick.showPorts()
|
||||||
|
sensors.gyro2.calibrate()
|
||||||
|
```
|
||||||
|
|
||||||
|
## Step 2 Compute the error
|
||||||
|
|
||||||
|
Make a new **error** variable and drag the ``||sensors:gyro rate||``
|
||||||
|
and multiply it by -1. Since the rate shows the rotation rate, we will
|
||||||
|
counter it by negating it.
|
||||||
|
|
||||||
|
```blocks
|
||||||
|
let error = 0
|
||||||
|
brick.showPorts()
|
||||||
|
sensors.gyro2.calibrate()
|
||||||
|
while (true) {
|
||||||
|
error = sensors.gyro2.rate() * -1
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## Step 3 Steer with feedback
|
||||||
|
|
||||||
|
Drag a ``||motors:steer motors||`` block under the variable and pass
|
||||||
|
the **error** variable into the turn ratio section.
|
||||||
|
|
||||||
|
If the robot is turning right, the gyro will report a positive rotation rate
|
||||||
|
and the turn ratio will be negative which will the turn the robot left!
|
||||||
|
|
||||||
|
```blocks
|
||||||
|
let error = 0
|
||||||
|
brick.showPorts()
|
||||||
|
sensors.gyro2.calibrate()
|
||||||
|
while (true) {
|
||||||
|
error = sensors.gyro2.rate() * -1
|
||||||
|
motors.largeBC.steer(error, 50)
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
## Step 4 Run it!
|
||||||
|
|
||||||
|
Download to your brick and test out if the robot is going straight.
|
||||||
|
|
||||||
|
This kind of technique is called a proportional controller;
|
||||||
|
it corrects the inputs (motor speed) with a feedback proportional to the output (rotation rate).
|
@@ -309,6 +309,7 @@ export function deployCoreAsync(resp: pxtc.CompileResult) {
|
|||||||
.then(() => w.flashAsync(elfPath, UF2.readBytes(origElfUF2, 0, origElfUF2.length * 256)))
|
.then(() => w.flashAsync(elfPath, UF2.readBytes(origElfUF2, 0, origElfUF2.length * 256)))
|
||||||
.then(() => w.flashAsync(rbfPath, rbfBIN))
|
.then(() => w.flashAsync(rbfPath, rbfBIN))
|
||||||
.then(() => w.runAsync(rbfPath))
|
.then(() => w.runAsync(rbfPath))
|
||||||
|
.then(() => Promise.delay(500))
|
||||||
.then(() => {
|
.then(() => {
|
||||||
pxt.tickEvent("webserial.success");
|
pxt.tickEvent("webserial.success");
|
||||||
return w.disconnectAsync()
|
return w.disconnectAsync()
|
||||||
|
@@ -17,6 +17,7 @@ export interface DirEntry {
|
|||||||
|
|
||||||
const runTemplate = "C00882010084XX0060640301606400"
|
const runTemplate = "C00882010084XX0060640301606400"
|
||||||
const usbMagic = 0x3d3f
|
const usbMagic = 0x3d3f
|
||||||
|
const DIRECT_COMMAND_NO_REPLY = 0x80
|
||||||
|
|
||||||
export class Ev3Wrapper {
|
export class Ev3Wrapper {
|
||||||
msgs = new U.PromiseBuffer<Uint8Array>()
|
msgs = new U.PromiseBuffer<Uint8Array>()
|
||||||
@@ -93,7 +94,7 @@ export class Ev3Wrapper {
|
|||||||
runAsync(path: string) {
|
runAsync(path: string) {
|
||||||
let codeHex = runTemplate.replace("XX", U.toHex(U.stringToUint8Array(path)))
|
let codeHex = runTemplate.replace("XX", U.toHex(U.stringToUint8Array(path)))
|
||||||
let code = U.fromHex(codeHex)
|
let code = U.fromHex(codeHex)
|
||||||
let pkt = this.allocCore(2 + code.length, 0)
|
let pkt = this.allocCore(2 + code.length, DIRECT_COMMAND_NO_REPLY)
|
||||||
HF2.write16(pkt, 5, 0x0800)
|
HF2.write16(pkt, 5, 0x0800)
|
||||||
U.memcpy(pkt, 7, code)
|
U.memcpy(pkt, 7, code)
|
||||||
log(`run ${path}`)
|
log(`run ${path}`)
|
||||||
|
@@ -1,6 +1,6 @@
|
|||||||
# calibrate
|
# calibrate
|
||||||
|
|
||||||
Reset the zero reference for the gyro to current position of the brick.
|
Detects if the gyro is drifting and performs a full reset if needed.
|
||||||
|
|
||||||
```sig
|
```sig
|
||||||
sensors.gyro2.calibrate()
|
sensors.gyro2.calibrate()
|
||||||
|
@@ -73,7 +73,7 @@ namespace sensors {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Forces a calibration of the with light progress indicators.
|
* Detects if calibration is necessary and performs a full reset, drift computation.
|
||||||
* Must be called when the sensor is completely still.
|
* Must be called when the sensor is completely still.
|
||||||
*/
|
*/
|
||||||
//% help=sensors/gyro/calibrate
|
//% help=sensors/gyro/calibrate
|
||||||
@@ -95,6 +95,19 @@ namespace sensors {
|
|||||||
// give time for robot to settle
|
// give time for robot to settle
|
||||||
pause(700);
|
pause(700);
|
||||||
|
|
||||||
|
// compute drift
|
||||||
|
this.computeDriftNoCalibration();
|
||||||
|
if (Math.abs(this.drift()) < 0.1) {
|
||||||
|
// no drift, skipping calibration
|
||||||
|
brick.setStatusLight(StatusLight.Green); // success
|
||||||
|
pause(1000);
|
||||||
|
brick.setStatusLight(statusLight); // resture previous light
|
||||||
|
|
||||||
|
// and we're done
|
||||||
|
this.calibrating = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// calibrating
|
// calibrating
|
||||||
brick.setStatusLight(StatusLight.OrangePulse);
|
brick.setStatusLight(StatusLight.OrangePulse);
|
||||||
|
|
||||||
@@ -187,13 +200,13 @@ namespace sensors {
|
|||||||
// clear drift
|
// clear drift
|
||||||
this.setMode(GyroSensorMode.Rate);
|
this.setMode(GyroSensorMode.Rate);
|
||||||
this._drift = 0;
|
this._drift = 0;
|
||||||
const n = 100;
|
const n = 10;
|
||||||
let d = 0;
|
let d = 0;
|
||||||
for (let i = 0; i < n; ++i) {
|
for (let i = 0; i < n; ++i) {
|
||||||
d += this._query();
|
d += this._query();
|
||||||
pause(4);
|
pause(20);
|
||||||
}
|
}
|
||||||
this._drift = Math.round(d / n);
|
this._drift = d / n;
|
||||||
}
|
}
|
||||||
|
|
||||||
_info(): string {
|
_info(): string {
|
||||||
|
@@ -1,6 +1,6 @@
|
|||||||
{
|
{
|
||||||
"name": "pxt-ev3",
|
"name": "pxt-ev3",
|
||||||
"version": "1.2.15",
|
"version": "1.2.17",
|
||||||
"description": "LEGO MINDSTORMS EV3 for Microsoft MakeCode",
|
"description": "LEGO MINDSTORMS EV3 for Microsoft MakeCode",
|
||||||
"private": false,
|
"private": false,
|
||||||
"keywords": [
|
"keywords": [
|
||||||
|
@@ -1,5 +1,5 @@
|
|||||||
namespace pxsim {
|
namespace pxsim {
|
||||||
const enum GyroSensorMode {
|
export const enum GyroSensorMode {
|
||||||
None = -1,
|
None = -1,
|
||||||
Angle = 0,
|
Angle = 0,
|
||||||
Rate = 1,
|
Rate = 1,
|
||||||
@@ -20,6 +20,7 @@ namespace pxsim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
setAngle(angle: number) {
|
setAngle(angle: number) {
|
||||||
|
angle = angle | 0;
|
||||||
if (this.angle != angle) {
|
if (this.angle != angle) {
|
||||||
this.angle = angle;
|
this.angle = angle;
|
||||||
this.setChangedState();
|
this.setChangedState();
|
||||||
@@ -27,6 +28,7 @@ namespace pxsim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
setRate(rate: number) {
|
setRate(rate: number) {
|
||||||
|
rate = rate | 0;
|
||||||
if (this.rate != rate) {
|
if (this.rate != rate) {
|
||||||
this.rate = rate;
|
this.rate = rate;
|
||||||
this.setChangedState();
|
this.setChangedState();
|
||||||
|
@@ -49,11 +49,13 @@ namespace pxsim.visuals {
|
|||||||
}
|
}
|
||||||
|
|
||||||
private updateDimensions(width: number, height: number, strict?: boolean) {
|
private updateDimensions(width: number, height: number, strict?: boolean) {
|
||||||
|
width = Math.max(0, width);
|
||||||
|
height = Math.max(0, height);
|
||||||
if (this.content) {
|
if (this.content) {
|
||||||
const currentWidth = this.getInnerWidth();
|
const currentWidth = this.getInnerWidth();
|
||||||
const currentHeight = this.getInnerHeight();
|
const currentHeight = this.getInnerHeight();
|
||||||
const newHeight = currentHeight / currentWidth * width;
|
const newHeight = Math.max(0, currentHeight / currentWidth * width);
|
||||||
const newWidth = currentWidth / currentHeight * height;
|
const newWidth = Math.max(0, currentWidth / currentHeight * height);
|
||||||
if (strict) {
|
if (strict) {
|
||||||
this.content.setAttribute('width', `${width}`);
|
this.content.setAttribute('width', `${width}`);
|
||||||
this.content.setAttribute('height', `${height}`);
|
this.content.setAttribute('height', `${height}`);
|
||||||
|
@@ -1,13 +1,16 @@
|
|||||||
|
|
||||||
|
|
||||||
namespace pxsim.visuals {
|
namespace pxsim.visuals {
|
||||||
|
const MAX_RATE = 40;
|
||||||
|
const MAX_ANGLE = 360;
|
||||||
|
|
||||||
export class RotationSliderControl extends ControlView<GyroSensorNode> {
|
export class RotationSliderControl extends ControlView<GyroSensorNode> {
|
||||||
private group: SVGGElement;
|
private group: SVGGElement;
|
||||||
private slider: SVGGElement;
|
private slider: SVGGElement;
|
||||||
|
private text: SVGTextElement;
|
||||||
|
|
||||||
private static SLIDER_WIDTH = 70;
|
private static SLIDER_WIDTH = 70;
|
||||||
private static SLIDER_HEIGHT = 78;
|
//private static SLIDER_HEIGHT = 78;
|
||||||
|
|
||||||
getInnerView(parent: SVGSVGElement, globalDefs: SVGDefsElement) {
|
getInnerView(parent: SVGSVGElement, globalDefs: SVGDefsElement) {
|
||||||
this.group = svg.elt("g") as SVGGElement;
|
this.group = svg.elt("g") as SVGGElement;
|
||||||
@@ -23,6 +26,14 @@ 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': 13, 'style': 'fill: #f12a21' });
|
||||||
pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 12.5, 'style': 'fill: none;stroke: #b32e29' });
|
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,
|
||||||
|
'y': RotationSliderControl.SLIDER_WIDTH * 1.2,
|
||||||
|
'text-anchor': 'middle', 'dominant-baseline': 'middle',
|
||||||
|
'style': 'font-size: 16px',
|
||||||
|
'class': 'sim-text inverted number'
|
||||||
|
}) as SVGTextElement;
|
||||||
|
|
||||||
const dragSurface = svg.child(this.group, "rect", {
|
const dragSurface = svg.child(this.group, "rect", {
|
||||||
x: 0,
|
x: 0,
|
||||||
y: 0,
|
y: 0,
|
||||||
@@ -61,7 +72,17 @@ namespace pxsim.visuals {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
const node = this.state;
|
const node = this.state;
|
||||||
const percentage = node.getValue();
|
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 x = RotationSliderControl.SLIDER_WIDTH * percentage / 100;
|
const x = RotationSliderControl.SLIDER_WIDTH * percentage / 100;
|
||||||
const y = Math.abs((percentage - 50) / 50) * 10;
|
const y = Math.abs((percentage - 50) / 50) * 10;
|
||||||
this.slider.setAttribute("transform", `translate(${x}, ${y})`);
|
this.slider.setAttribute("transform", `translate(${x}, ${y})`);
|
||||||
@@ -73,8 +94,14 @@ namespace pxsim.visuals {
|
|||||||
const bBox = this.content.getBoundingClientRect();
|
const bBox = this.content.getBoundingClientRect();
|
||||||
let t = Math.max(0, Math.min(1, (width + bBox.left / this.scaleFactor - cur.x / this.scaleFactor) / width))
|
let t = Math.max(0, Math.min(1, (width + bBox.left / this.scaleFactor - cur.x / this.scaleFactor) / width))
|
||||||
|
|
||||||
|
t = -(t - 0.5) * 2; // [-1,1]
|
||||||
|
|
||||||
const state = this.state;
|
const state = this.state;
|
||||||
state.setRate((1 - t) * (100));
|
if (state.getMode() == GyroSensorMode.Rate) {
|
||||||
|
state.setRate(MAX_RATE * t);
|
||||||
|
} else {
|
||||||
|
state.setAngle(MAX_ANGLE * t)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@@ -18,6 +18,7 @@
|
|||||||
"Touch Sensor Tutorials": "tutorials/touch-sensor",
|
"Touch Sensor Tutorials": "tutorials/touch-sensor",
|
||||||
"Color Sensor Tutorials": "tutorials/color-sensor",
|
"Color Sensor Tutorials": "tutorials/color-sensor",
|
||||||
"Ultrasonic Sensor Tutorials": "tutorials/ultrasonic-sensor",
|
"Ultrasonic Sensor Tutorials": "tutorials/ultrasonic-sensor",
|
||||||
|
"Gyro Sensor Tutorials": "tutorials/gyro",
|
||||||
"Infrared Sensor Tutorials": "tutorials/infrared-sensor",
|
"Infrared Sensor Tutorials": "tutorials/infrared-sensor",
|
||||||
"FLL / City Shaper": "tutorials/city-shaper",
|
"FLL / City Shaper": "tutorials/city-shaper",
|
||||||
"Design Engineering": "design-engineering",
|
"Design Engineering": "design-engineering",
|
||||||
|
Reference in New Issue
Block a user