diff --git a/docs/examples/core-set/gyroboy-blocks.md b/docs/examples/core-set/gyroboy-blocks.md new file mode 100644 index 00000000..0fd36b13 --- /dev/null +++ b/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() +}) +``` \ No newline at end of file