{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "\n", "

\n", " \n", "

\n", "\n", "# `aat_ms5`\n", "Python program to control the AAT MS5 robot on patrol mode.\n", "The tank will move in circles, while the droid in the blaster will be looking for any sympathisants of the Republic.\n", "When the distance sensor detects a target, the tank will stop and the droid will center the\n", "blasters to fire! \n", "\n", "You can find a video of the robot functioning [here](https://www.youtube.com/watch?v=Ma7CmThktUg&feature=youtu.be&ab_channel=ArturoMoncada-Torres).\n", "\n", "# Required robot\n", "* AAT MS5 (you can find the [instructions for building it here](https://arturomoncadatorres.com/aat-ms5/))\n", "\n", "\n", "\n", "# Source code\n", "You can find the code in the accompanying [`.py` file](https://github.com/arturomoncadatorres/lego-mindstorms/blob/main/mocs/aat_ms5/programs/aat_ms5.py). To get it running, simply copy and paste it in a new Mindstorms project.\n", "\n", "# Imports" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App\n", "from mindstorms.control import wait_for_seconds, wait_until, Timer\n", "from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to\n", "import math\n", "\n", "import hub" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"-\"*15 + \" Execution started \" + \"-\"*15 + \"\\n\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Initialize hub\n", "Notice we won't be using the standard `MSHub`, but rather the \"raw\" `hub`.\n", "It is a little lower level, but it allows us making more things. \n", "Fore more information, see [Maarten Pennings brilliant explanation and unofficial documentation about it](https://github.com/maarten-pennings/Lego-Mindstorms/blob/main/ms4/faq.md#why-are-there-so-many-ways-to-do--in-python)." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# hub = MSHub()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# hub.status_light.on('black')\n", "hub.led(0, 0, 0)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Initialize motors" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Configuring motors...\")\n", "motor_steer = Motor('A') # Front wheels (for steering)\n", "motor_power = Motor('C') # Back wheels (for moving)\n", "\n", "motor_turret = Motor('B') # Turrent spinning" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "print(\"Setting motors to position 0...\")\n", "motor_steer.run_to_position(45, speed=100)\n", "motor_steer.run_to_position(0, speed=100)\n", "\n", "motor_turret.run_to_position(0, speed=75)\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Initialize distance sensor" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "print(\"Initializing distance sensor...\")\n", "distance_sensor = DistanceSensor('D')\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": { "lines_to_next_cell": 0 }, "source": [ "# Put the AAT MS5 in motion\n", "\n", "The tank will move until the distance sensor detects an obstacle.\n", "\n", "The steering is given by `POSITION`. \n", "* A value between `0` and `90` will steer the tank to the left.\n", " - A value closer to `0` will make the tank turn wider.\n", " - A value closer to `90` will make the tank turn tighter.\n", "\n", "* A value between `270` and `360` will steer the tank to the right.\n", " - A value closer to `270` will make the tank turn tighter.\n", " - A value closer to `360` will make the tank turn wider." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "POSITION = 270\n", "\n", "print(\"Steering...\")\n", "motor_steer.run_to_position(POSITION, speed=35)\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": { "lines_to_next_cell": 0 }, "source": [ "The tank speed is given by `SPEED`. It should have a value between `-100` and `100`.\n", "* A negative value will move the tank forward.\n", "* A positive value will move the tank backwards.\n", "\n", "Recommended value is `-50`" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "SPEED = -50\n", "\n", "print(\"Moving...\")\n", "motor_power.start(SPEED)\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Configure the patrolling\n", "We will move the turret constantly. It will go from left to right and from \n", "right to left. When an obstacle is detected, the turret will go back to the\n", "initial position and \"fire\".\n", "\n", "\n", "\n", "## Define distance function\n", "As part of the program, we need to continuously check if the\n", "measured distance is less than 10 cm. " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "OBSTACLE_DISTANCE = 10 # [cm]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "However, if the sensor reads no measure, it will return a `None`, which\n", "in turn will generate an error (since we cannot do a comparision\n", "between a `None` and something else).\n", "\n", "To solve this, we will define our own cuestom distance function.\n", "This way, when the sensor has no reading, we will just return\n", "a (simulated) very long distance (instead of returning a `None`).\n", "This will allow us to safely do the comparision." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def my_get_distance_cm():\n", " \"\"\"\n", " Parameters\n", " ----------\n", " None\n", " \n", " Returns\n", " -------\n", " dist:\n", " Distance value (in cm).\n", " If the sensor returns a None, it returns a very large value (1000).\n", " \"\"\"\n", " distance = distance_sensor.get_distance_cm()\n", " if distance == None:\n", " distance = 10000\n", " \n", " return distance" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Patrolling\n", "Now, in order to be able to stop the turret at any moment \n", "(and not until the motor has completed a whole sweep), \n", "we will use co-routines. \n", "\n", "> This is a simplified version [David Lechner's trick](https://community.legoeducation.com/discuss/viewtopic/66/110), which I've used before in [Charlie's `drum_solo`](https://nbviewer.jupyter.org/github/arturomoncadatorres/lego-mindstorms/blob/main/base/charlie/programs/drum_solo.ipynb?flush_cache=True).\n", "In this case, we are only controlling one motor (the turret) and we don't depend on time\n", "(but rather on the motor position). Thus, we don't need a timer.\n", "\n", "We need to define a function for moving the turret. \n", "Pay attention to the comments, since they explain how using\n", "co-routines work. It isn't very hard, but it isn't trivial either." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "def move_turret():\n", "\n", " \"\"\"\n", " Moves the AAT MS5 turrent.\n", " \n", " Parameters\n", " ----------\n", " None\n", " \n", " Returns\n", " -------\n", " None\n", " \"\"\"\n", " \n", " # First, we need to define the coroutine.\n", " # In this case, we only need one (corresponding to the turret motor).\n", " # Notice how the definition is very similar to that of a function.\n", " # Coroutines also have input parameters.\n", " # However, they have no \"output\" (i.e., return), but actually a yield.\n", " def background_turret(angle):\n", " \"\"\"\n", " Parameters\n", " ----------\n", " angle:\n", " The angle at which the turret turns. \n", " In practice, this value is twice the original angle, since\n", " it moves completely from one side to the other (and not from\n", " the center to one side). That is why it is passed to this \n", " function multiplied by two.\n", " \n", " In degrees.\n", " \"\"\"\n", " \n", " # We want to make sure counted degrees are initialized at 0.\n", " motor_turret.set_degrees_counted(0)\n", " \n", " # Notice that we use the absolute value of the counted degrees.\n", " # This is to ensure that it works on the way back (when the measured\n", " # degrees would be negative).\n", " curr_turret_position = math.fabs(motor_turret.get_degrees_counted())\n", "\n", " # Here, we check if the motor has reached the desired angle.\n", " while curr_turret_position < angle:\n", " \n", " # If you wish to see the current turret position and the target angle,\n", " # uncomment the following line.\n", " # print(str(curr_turret_position) + \" - \" + str(angle))\n", " \n", " # We update the turret current position.\n", " curr_turret_position = math.fabs(motor_turret.get_degrees_counted())\n", " \n", " # If the turret hasn't reached the desired angle, we reach this yield.\n", " # yield lets the rest of the program run until we come back\n", " # here again later to check if the condition was met.\n", " yield\n", "\n", "\n", " def turret_patrol():\n", " while True:\n", "\n", " # This is how we receive a parameter.\n", " # In this case, it corresponds to the angle the motor should move.\n", " angle_action = yield\n", "\n", " # We make sure we only execute code if the received\n", " # value was transmitted correctly.\n", " if not angle_action == None:\n", " \n", " # We will start to move the turret...\n", " motor_turret.start(10)\n", " \n", " # ...and check if its angle exceeded the maximum allowed.\n", " # First we move the turret from left to right...\n", " yield from background_turret(angle_action*2)\n", " hub.sound.beep(150, 200, hub.sound.SOUND_SIN) # Play simple tone\n", "\n", "\n", " # ...and from right to left (exactly same process, but inverted speed).\n", " motor_turret.start(-10)\n", " yield from background_turret(angle_action*2)\n", " # hub.sound.play(\"/extra_files/Ping\")\n", " hub.sound.beep(150, 200, hub.sound.SOUND_SIN) # Play simple tone\n", "\n", " # We assume that the movement is immediate and takes no time.\n", " # This isn't completely true, but for now it works.\n", "\n", " # Since turret_patrol() is a coroutine and uses yield\n", " # (i.e., it isn't a function and thus has no return), it will NOT\n", " # run here when we call it. Instead, it will just be created as a generator object.\n", " # This generator will be used to run the functions one yield (i.e., step) at a time.\n", " turret_generator = turret_patrol()\n", "\n", " # Now we will actually start the task.\n", " # The task (turret patrolling) will be run as long as the distance sensor\n", " # doesn't detect an obstacle.\n", " while my_get_distance_cm() > OBSTACLE_DISTANCE:\n", " \n", " next(turret_generator)\n", " turret_generator.send(TURRET_ANGLE)\n", "\n", " wait_for_seconds(0.01) # Small pause between steps.\n", "\n", " return None" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "After we have defined the turret movement, we can now make the AAT MS5 patrol until it finds those pesky Republic supporters!" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "TURRET_ANGLE = 40\n", "\n", "print(\"Initializing turret with angle \" + str(TURRET_ANGLE) + \"...\")\n", "motor_turret.set_default_speed(10)\n", "motor_turret.run_for_degrees(-TURRET_ANGLE)\n", "print(\"DONE!\")\n", "\n", "print(\"Starting patrolling...\")\n", "move_turret()\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Once it finds an enemy (i.e, it detects an obstacle), it will stop and center the turret." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Enemy detected! Attack!\")\n", "motor_power.stop() # Stop the movement\n", "motor_turret.run_to_position(0, speed=75) # Center the turret" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Then, it will fire three blasters. Each blaster will come with a sound and an\n", "animation of the blaster moving in the hub.\n", "\n", "First, lets define the frames of the animation." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "print(\"Defining animation frames...\")\n", "\n", "frames = ['00000:00000:00000:00000:00000',\n", "'00900:00000:00000:00000:00000',\n", "'00700:00900:00000:00000:00000',\n", "'00500:00700:00900:00000:00000',\n", "'00000:00500:00700:00900:00000',\n", "'00000:00000:00500:00700:00900',\n", "'00000:00000:00000:00500:00700',\n", "'00000:00000:00000:00000:00500',\n", "'00000:00000:00000:00000:00000']\n", "\n", "n_frames = len(frames)\n", "t_pause = 0.05 # Pause between frames (in seconds)\n", "\n", "print(\"DONE!\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Then, let's proceed with the actual fire!" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Firing blasters...\")\n", "\n", "n_blasters = 3\n", "\n", "for ii in range(0, n_blasters):\n", "\n", " # Play blaster sound.\n", " hub.sound.play(\"/extra_files/Laser\")\n", "\n", " # Display blaster animation.\n", " for ii in range(0, n_frames):\n", " img = hub.Image(frames[ii])\n", " hub.display.show(img)\n", " wait_for_seconds(t_pause)\n", "\n", " wait_for_seconds(0.5)\n", "\n", "print(\"DONE!\")" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"Target eliminated.\")" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"-\"*15 + \" Execution ended \" + \"-\"*15 + \"\\n\")" ] } ], "metadata": { "jupytext": { "formats": "ipynb,py:percent" }, "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.8.5" } }, "nbformat": 4, "nbformat_minor": 4 }