2018-01-16 06:27:19 +01:00
|
|
|
# Gyroboy LabView
|
|
|
|
|
|
|
|
```typescript
|
|
|
|
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()
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(5000)
|
2018-01-16 06:27:19 +01:00
|
|
|
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)
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(4);
|
2018-01-16 06:27:19 +01:00
|
|
|
}
|
|
|
|
} 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.08 * mSpd + 0.12 * 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
|
2018-02-15 01:05:31 +01:00
|
|
|
forever(function () {
|
2018-01-16 06:27:19 +01:00
|
|
|
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();
|
2018-02-19 16:35:08 +01:00
|
|
|
motors.largeA.run(lpwr)
|
|
|
|
motors.largeD.run(rpwr)
|
2018-01-16 06:27:19 +01:00
|
|
|
CHK()
|
|
|
|
let t2 = control.timer1.millis();
|
|
|
|
let p = 5 - (t2 - t1);
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(Math.max(1, p))
|
2018-01-16 06:27:19 +01:00
|
|
|
}
|
2018-02-14 17:56:12 +01:00
|
|
|
motors.stopAll()
|
2018-01-16 06:27:19 +01:00
|
|
|
st = 0;
|
2018-02-07 07:18:39 +01:00
|
|
|
brick.setStatusLight(StatusLight.RedPulse);
|
2018-01-16 06:27:19 +01:00
|
|
|
brick.showImage(images.eyesKnockedOut)
|
|
|
|
music.playSoundEffect(sounds.movementsSpeedDown)
|
2018-01-31 17:28:00 +01:00
|
|
|
sensors.touch3.pauseUntil(ButtonEvent.Pressed)
|
2018-02-07 07:18:39 +01:00
|
|
|
brick.setStatusLight(StatusLight.Off);
|
2018-01-16 06:27:19 +01:00
|
|
|
})
|
|
|
|
|
|
|
|
// BHV
|
2018-02-15 01:05:31 +01:00
|
|
|
forever(function () {
|
2018-01-16 06:27:19 +01:00
|
|
|
switch (st) {
|
|
|
|
case 0:
|
|
|
|
Cdrv = 0;
|
|
|
|
Cstr = 0;
|
|
|
|
break;
|
|
|
|
case 1:
|
|
|
|
Cdrv = 40;
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(4000);
|
2018-01-16 06:27:19 +01:00
|
|
|
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;
|
2018-02-19 16:35:08 +01:00
|
|
|
motors.mediumC.run(30, 30, MoveUnit.Degrees);
|
|
|
|
motors.mediumC.run(-30, 60, MoveUnit.Degrees);
|
|
|
|
motors.mediumC.run(30, 30, MoveUnit.Degrees);
|
2018-01-16 06:27:19 +01:00
|
|
|
if (Math.randomRange(-1, 1) >= 1)
|
|
|
|
Cstr = 70;
|
|
|
|
else
|
|
|
|
Cstr = -70;
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(4000);
|
2018-01-16 06:27:19 +01:00
|
|
|
music.playTone(2000, 100)
|
|
|
|
Cstr = 0;
|
|
|
|
Cdrv = oldDr;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
2018-02-15 01:05:31 +01:00
|
|
|
pause(80);
|
2018-01-16 06:27:19 +01:00
|
|
|
})
|
|
|
|
```
|