updated modified gyro boy

docs/examples/core-set/gyroboy-blocks.md

@@ -0,0 +1,178 @@
+# Gyroboy
+
+Work in progress
+
+```blocks
+let motorSpeed1 = 0
+let motorSpeed2 = 0
+let motorSpeed3 = 0
+let motorSpeed = 0
+let fallen = false
+let motorSpeed0 = 0
+let oldControlDrive = 0
+let controlDrive = 0
+let power = 0
+let motorAngle = 0
+let gyroAngle = 0
+let controlSteering = 0
+let state = 0
+let motorPosition = 0
+let temp = 0
+let gyroRate = 0
+let timestep = 0
+sensors.color1.onColorDetected(ColorSensorColor.Red, function () {
+    music.playTone(2000, 100)
+    controlDrive = 0
+    controlSteering = 0
+})
+// reads the motor angle and computes the motor speed,
+// position
+function computeMotors() {
+    temp = motorAngle
+    // read angle on both motors
+    motorAngle = motors.largeD.angle() + motors.largeA.angle()
+    // and estimate speed as angle difference
+    motorSpeed0 = motorAngle - temp
+    // average last 4 speed readings
+    motorSpeed = (motorSpeed0 + motorSpeed1 + motorSpeed2 + motorSpeed3) / 4 / timestep
+    // shift all previous recorded speeds by one
+    motorSpeed3 = motorSpeed2
+    motorSpeed2 = motorSpeed1
+    motorSpeed1 = motorSpeed0
+    // compute position from speed
+    motorPosition = motorPosition + timestep * motorSpeed
+}
+// read the gyro rate and computes the angle
+function computeGyro() {
+    gyroRate = sensors.gyro2.rate()
+    gyroAngle = gyroAngle + timestep * gyroRate
+}
+function reset() {
+    state = 0
+    // sleeping
+    moods.sleeping.show();
+    // reset counters
+    motors.largeA.reset()
+    motors.largeD.reset()
+    // motors are unregulated
+    motors.largeA.setRegulated(false)
+    motors.largeD.setRegulated(false)
+    // clear the gyro sensor to remove drift
+    sensors.gyro2.reset()
+    // fall detection timer
+    control.timer2.reset()
+    // timestep computation timer
+    control.timer3.reset()
+    motorAngle = 0
+    motorPosition = 0
+    motorSpeed = 0
+    motorSpeed0 = 0
+    motorSpeed1 = 0
+    motorSpeed2 = 0
+    motorSpeed3 = 0
+    gyroRate = 0
+    gyroAngle = 0
+    fallen = false
+    power = 0
+    controlSteering = 0
+    controlDrive = 0
+    // awake
+    moods.awake.show();
+    gyroAngle = -0.25
+    state = 1;
+}
+// compute set point for motor position and required
+// motor power
+function computePower() {
+    // apply control and compute desired motor position
+    motorPosition -= timestep * controlDrive;
+    // estimate power based on sensor readings and control
+    // values
+    power = 0.8 * gyroRate + 15 * gyroAngle + (0.08 * motorSpeed + 0.12 * motorPosition) - 0.01 * controlDrive
+    // ensure that power stays within -100, 100
+    if (power > 100) {
+        power = 100
+    } else if (power < -100) {
+        power = -100
+    }
+}
+// test if the robot has fallen off
+function checkFallen() {
+    if (Math.abs(power) < 100) {
+        control.timer2.reset()
+    }
+    if (control.timer2.seconds() > 2) {
+        fallen = true
+    }
+}
+// stop all motors and wait for touch button to be
+// pressed
+function stop() {
+    motors.stopAllMotors()
+    state = 0
+    moods.knockedOut.show();
+    sensors.touch3.pauseUntil(TouchSensorEvent.Pressed)
+    moods.neutral.show();
+}
+sensors.ultrasonic4.onEvent(UltrasonicSensorEvent.ObjectNear, function () {
+    moods.dizzy.show()
+    controlSteering = 0
+    oldControlDrive = controlDrive
+    controlDrive = -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) {
+        controlSteering = 70
+    } else {
+        controlSteering = -70
+    }
+    loops.pause(4000)
+    music.playTone(2000, 100)
+    controlSteering = 0
+    controlDrive = oldControlDrive
+    moods.neutral.show()
+})
+// compute the elapsed time since the last iteration
+function computeTimestep() {
+    timestep = control.timer3.seconds()
+    control.timer3.reset()
+}
+sensors.color1.onColorDetected(ColorSensorColor.Green, function () {
+    music.playTone(2000, 100)
+    controlDrive = 150
+    controlSteering = 0
+})
+sensors.color1.onColorDetected(ColorSensorColor.Blue, function () {
+    music.playTone(2000, 100)
+    controlSteering = 70
+})
+// apply power to motors
+function controlMotors() {
+    motors.largeA.setSpeed(power + controlSteering * 0.1)
+    motors.largeD.setSpeed(power - controlSteering * 0.1)
+}
+sensors.color1.onColorDetected(ColorSensorColor.Yellow, function () {
+    music.playTone(2000, 100)
+    controlSteering = -70
+})
+sensors.color1.onColorDetected(ColorSensorColor.White, function () {
+    music.playTone(2000, 100)
+    controlDrive = -75
+})
+timestep = 0.014
+// main loop
+loops.forever(function () {
+    reset()
+    while (!fallen) {
+        control.timer3.pauseUntil(5)
+        computeTimestep()
+        computeGyro()
+        computeMotors()
+        computePower()
+        controlMotors()
+        checkFallen()
+    }
+    stop()
+})
+```