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

\n", " \n", "

\n", "\n", "# `atat_ms5`\n", "Python program to make the AT-AT MS5 robot walk.\n", "This time, the robot functionality is quite simple. The AT-AT MS5 just walks in a straight line.\n", "\n", "However, what is more interesting is how the motors are controlled. I used Anton's Mindstorms motor synchronization. This method is a very clever way to make two (or more) motors work together smoothly. This is particularly handy for the AT-AT MS5, since the legs movement have quite a peculiar pattern (more on that later).\n", "\n", "You can find a video of the robot functioning [here](TODO).\n", "\n", "# Required robot\n", "* AT-AT MS5 (you can find the [instructions for building it here](https://arturomoncadatorres.com/atat-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/atat_ms5/programs/atat_ms5.py). To get it running, simply copy and paste it in a new MINDSTORMS project.\n", "\n", "# Imports\n", "Notice that we aren't using the default imports given by the MINDSTORMS app (e.g., `MSHub`), but rather the lower level [`hub`](https://antonsmindstorms.com/2021/01/14/advanced-undocumented-python-in-spike-prime-and-mindstorms-hubs/)." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "lines_to_next_cell": 1 }, "outputs": [], "source": [ "import hub\n", "import utime\n", "import math" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "print(\"-\"*15 + \" Execution started \" + \"-\"*15 + \"\\n\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Anton's MINDSTORMS motor synchronization\n", "We will be using [Anton's MINDSTORMS](https://antonsmindstorms.com/) technique for the motor synchronization. It is made of three main components:\n", "\n", "* A better timer\n", "* A motor animation mechanism\n", "* The definition of (the motors) metafunctions\n", "\n", "I will only cover the general concepts of each of them. If you want to learn more about their details, I suggest you take a look at Anton's [well-described tutorial](https://antonsmindstorms.com/2021/01/27/python-motor-synchronization-coordinating-multiple-spike-or-mindstorms-motors/) or, even better, [watch his explanation](https://www.youtube.com/watch?v=ctlRsDkKte8&ab_channel=AntonsMindstormsHacks) while coding in real-time. Moreover, Anton's code is nicely commented.\n", "\n", "\n", "## Better timer\n", "First, we need to define a so-called better timer. That is because the original timer provided only has a resolutions of seconds. However, we need something much more precise. \n", "\n", "The `AMHTimer` has a resolution of miliseconds. Moreover, it also allows us to stop it, reverse it, and even accelerate it. We won't be using such functionality for the AT-AT MS5, but maybe it will be useful for your robot! You probably don't want to mess with this at all." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class AMHTimer():\n", " \"\"\"\n", " A configurable timer which you can start, reverse, stop and pause.\n", " By default, it counts milliseconds, but you can speed it up,\n", " Slow it down or accelerate it!\n", " You can also set the time and reset it.\n", " You can even run it in reverse, so you can count down until 0.\n", " It always returns integers, even when you slow it way down.\n", "\n", " Author: \n", " Anton's Mindstorms Hacks - https://antonsmindstorms.com\n", "\n", " Usage:\n", " my_timer = AMHTimer():\n", " my_timer.rate = 500 # set the rate to 500 ticks/s. That is half the normal rate\n", " my_timer.acceleration = 100 # Increase the rate by 100 ticks/s^2\n", " my_timer.reset() # Reset to zero. Doesn't change running/paused state\n", " now = mytimer.time # Read the time\n", " mytimer.time = 5000 # Set the time\n", " \"\"\"\n", " def __init__(self, rate=1000, acceleration=0):\n", " self.running = True\n", " self.pause_time = 0\n", " self.reset_at_next_start = False\n", " self.__speed_factor = rate/1000\n", " self.__accel_factor = acceleration/1000000\n", " self.start_time = utime.ticks_ms()\n", "\n", " @property\n", " def time(self):\n", " if self.running:\n", " elapsed = utime.ticks_diff( utime.ticks_ms(), self.start_time )\n", " return int(\n", " self.__accel_factor * elapsed**2 +\n", " self.__speed_factor * elapsed +\n", " self.pause_time\n", " )\n", " else:\n", " return self.pause_time\n", "\n", " @time.setter\n", " def time(self, setting):\n", " self.pause_time = setting\n", " self.start_time = utime.ticks_ms()\n", "\n", " def pause(self):\n", " if self.running:\n", " self.pause_time = self.time\n", " self.running = False\n", "\n", " def stop(self):\n", " self.pause()\n", "\n", " def start(self):\n", " if not self.running:\n", " self.start_time = utime.ticks_ms()\n", " self.running = True\n", "\n", " def resume(self):\n", " self.start()\n", "\n", " def reset(self):\n", " self.time = 0\n", "\n", " def reverse(self):\n", " self.rate *= -1\n", "\n", " @property\n", " def rate(self):\n", " elapsed = utime.ticks_diff(utime.ticks_ms(), self.start_time )\n", " return (self.__accel_factor*elapsed + self.__speed_factor) * 1000\n", "\n", " @rate.setter\n", " def rate(self, setting):\n", " if self.__speed_factor != setting/1000:\n", " if self.running:\n", " self.pause()\n", " self.__speed_factor = setting/1000\n", " self.start()\n", "\n", " @property\n", " def acceleration(self):\n", " return self.__accel_factor * 1000000\n", "\n", " @acceleration.setter\n", " def acceleration(self, setting):\n", " if self.__accel_factor != setting/1000000:\n", " if self.running:\n", " self.pause()\n", " self.__speed_factor = self.rate/1000\n", " self.__accel_factor = setting/1000000\n", " self.start()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Motor animation mechanism\n", "This is where the magic happens. Here, we define the central mechanism that actually animates the robot's motors. Similarly to the previous case, it is probably better if you leave this part untouched." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class Mechanism():\n", " \"\"\"\n", " The class helps to control multiple motors in a tight loop.\n", "\n", " Author:\n", " Anton's Mindstorms Hacks - https://antonsmindstorms.com\n", "\n", " Args:\n", " motors: list of motor objects. Can be hub.port.X.motor or Motor('X')\n", " motor_functions: list of functions that take one argument and calculate motor positions\n", "\n", " Optional Args:\n", " reset_zero: bolean, resets the 0 point of the relative encoder to the absolute encoder position\n", " ramp_pwm: int, a number to limit maximum pwm per tick when starting. 0.5 is a good value for a slow ramp.\n", " Kp: float, proportional feedback factor for motor power.\n", "\n", " Returns:\n", " None.\n", "\n", " Usage:\n", " my_mechanism = Mechanism([Motor('A'), Motor('B')], [func_a, func_b])\n", " timer = AMHTimer()\n", " while True:\n", " my_mechanism.update_motor_pwms(timer.time)\n", " \"\"\"\n", " def __init__(self, motors, motor_functions, reset_zero=True, ramp_pwm=100, Kp=1.2):\n", " # Allow for both hub.port.X.motor and Motor('X') objects:\n", " self.motors = [m._motor_wrapper.motor if '_motor_wrapper' in dir(m) else m for m in motors]\n", " self.motor_functions = motor_functions\n", " self.ramp_pwm = ramp_pwm\n", " self.Kp = Kp\n", " if reset_zero:\n", " self.relative_position_reset()\n", "\n", " def relative_position_reset(self):\n", " # Set degrees counted of all motors according to absolute 0\n", " for motor in self.motors:\n", " absolute_position = motor.get()[2]\n", " if absolute_position > 180:\n", " absolute_position -= 360\n", " motor.preset(absolute_position)\n", "\n", " @staticmethod\n", " def float_to_motorpower( f ):\n", " # Convert any floating point to number to\n", " # an integer between -100 and 100\n", " return min(max(int(f),-100),100)\n", "\n", " def update_motor_pwms(self, ticks):\n", " # Proportional controller toward desired motor positions at ticks\n", " for motor, motor_function in zip(self.motors, self.motor_functions):\n", " target_position = motor_function(ticks)\n", " current_position = motor.get()[1]\n", " power = self.float_to_motorpower((target_position-current_position)* self.Kp)\n", " \n", " if self.ramp_pwm < 100:\n", " # Limit pwm for a smooth start\n", " max_power = int(self.ramp_pwm*(abs(ticks)))\n", " if power < 0:\n", " power = max(power, -max_power)\n", " else:\n", " power = min(power, max_power)\n", "\n", " motor.pwm(power)\n", "\n", " def shortest_path_reset(self, ticks=0, speed=20):\n", " # Get motors in position smoothly before starting the control loop\n", "\n", " # Reset internal tacho to range -180,180\n", " self.relative_position_reset()\n", "\n", " # Run all motors to a ticks position with shortest path\n", " for motor, motor_function in zip(self.motors, self.motor_functions):\n", " target_position = int(motor_function(ticks))\n", " current_position = motor.get()[1]\n", " \n", " # Reset internal tacho so next move is shortest path\n", " if target_position - current_position > 180:\n", " motor.preset(current_position + 360)\n", " if target_position - current_position < -180:\n", " motor.preset(current_position - 360)\n", " \n", " # Start the maneuver\n", " motor.run_to_position(target_position, speed)\n", " \n", " # Give the motors time to spin up\n", " utime.sleep_ms(50)\n", " \n", " # Check all motors pwms until all maneuvers have ended\n", " while True:\n", " pwms = []\n", " for motor in self.motors:\n", " pwms += [motor.get()[3]]\n", " if not any(pwms): break\n", " \n", " def stop(self):\n", " for motor in self.motors:\n", " motor.pwm(0)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Metafunction definition\n", "This is where I would like to spend some time, since this is key for customizing this approach to the AT-AT MS5. We need to define the movement of the motors as mathematical functions that describe them. Let's take this *piano piano*. \n", "\n", "## Identifying each leg\n", "First of all, let's make our life easier and give each leg an identifier. For the sake of simplicity, I will use a letter corresponding to the hub's port to where the leg's motor is connected.\n", "\n", "\n", "\n", "## Understanding the AT-AT movement\n", "Now, we need to understand the movement of an actual AT-AT. We could take a look at some [source material](https://www.youtube.com/watch?v=3acC49W3yQk), but that would probably would just make me want to rewatch the original trilogy. Instead, I found this very cool [computational model of an AT-AT walking](https://sketchfab.com/models/c5a4ab826eda4458aa748e252631735e/). It is fantastic. Try playing with it and move the camera around.\n", "\n", "It also has a [video equivalent](https://www.youtube.com/watch?v=GUsOouwjsL4), from which I extracted a GIF. Notice I purposefully left the crane at the beginning to make it easier to identify when the cycle begins.\n", "\n", "\n", "\n", "Paying close attention to the GIF, we can start describing the AT-AT's movement:\n", "\n", "1. The order in which the legs move is `F`, `B`, `E`, and `A`.\n", "1. At any moment, only one leg is in the air.\n", "1. The movement speed of one leg when is in the air is faster than when it is in contact with the ground. \n", "\n", "## Translating the AT-AT movement into mathematical functions\n", "Anton's original code comes already with linear, sinusoidal, and block functions implemented. However, none of them accurately describe the AT-AT movement (and therefore I removed them from this script). Therefore, we need to dust our high school math and define them ourselves.\n", "\n", "I found that the most intuitive way is to do this graphically step by step. Let's start defining our plot for one leg only (let's say leg `F`). We will be looking at the motors' position $motor\\_position$ (in degrees) as a function of time $t$ (in miliseconds). Moreover, we will define the period $T$ as the time it takes for a leg to do a complete cycle. Therefore:\n", "\n", "* When $t=0$, $motor\\_position=0$\n", "* When $t=T$, $motor\\_position=360$\n", "\n", "This would look something like this:\n", "\n", "\n", "\n", "Now the trick is describing how to make it from the initial position to the final position. This is where our previous movement description becomes handy. We already defined that at any moment, only one leg of the AT-AT is in the air. Since we are talking about 4 legs, that means that each leg spends $T/4$ off the ground. Moreover, we can see from the GIF that during this time, the leg goes from 0 to 180$^{\\circ}$. In consequence, it takes the leg the rest of the period ($3T/4$) to go from 180 to 360$^{\\circ}$. Plotting this:\n", "\n", "\n", "\n", "We are getting there! Now we just need to do the mathematical definition of the straight line ($y = mx + b$) for each segment.\n", "\n", "For the air segment:\n", "\n", "$$\n", "\\begin{eqnarray}\n", "m_{air} &=& \\frac{y_2-y_1}{x_2-x_1}\\\\\n", " &=& \\frac{180-0}{\\frac{T}{4}-0}\\\\\n", " &=& \\frac{180\\cdot4}{T}\\\\\n", " &=& \\frac{720}{T}\\\\ \\\\\n", "b_{air} &=& y - mx\\\\\n", " &=& 180 - \\frac{720}{T}\\cdot\\frac{T}{4}\\\\\n", " &=& 180 - \\frac{720}{4}\\\\\n", " &=& 180 - 180\\\\\n", " &=& 0\n", "\\end{eqnarray}\n", "$$\n", "\n", "I guess we could have seen that from the plot, but I always like doing things a bit more systematically. Similarly, for the ground segment:\n", "\n", "$$\n", "\\begin{eqnarray}\n", "m_{ground} &=& \\frac{y_2-y_1}{x_2-x_1}\\\\\n", " &=& \\frac{360-180}{T-\\frac{T}{4}}\\\\\n", " &=& \\frac{180}{\\frac{3T}{4}}\\\\\n", " &=& \\frac{720}{3T}\\\\ \n", " &=& \\frac{240}{T}\\\\ \\\\\n", "b_{ground} &=& y - mx\\\\\n", " &=& 360 - \\frac{240}{T}\\cdot T\\\\\n", " &=& 360 - 240\\\\\n", " &=& 120\n", "\\end{eqnarray}\n", "$$\n", "\n", "Therefore, the final function definition is given by:\n", "\n", "$$\n", "y = \\left\\{\n", "\\begin{array}{ll}\n", " \\frac{720}{T} x &\\mathrm{if}& 0 \\leq x \\leq \\frac{T}{4} \\\\\n", " \\frac{240}{T} x + 120 &\\mathrm{if}& \\frac{T}{4} < x \\leq T \\\\\n", "\\end{array} \n", "\\right. \n", "$$\n", "\n", "Now we just need to code that as a metafunction as follows. Pay attention to the comments, since they explain additional small (but important) considerations when doing the real implementation." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Metafunction for AT-AT MS5 walking pattern\n", "def atat_walk(factor=1, period=4000, t_shift=0):\n", " \"\"\"\n", " Metafunction definition for making the AT-AT MS5 walk.\n", " \n", " Parameters\n", " ----------\n", " factor: integer\n", " Scaling factor. Larger numbers will increase the motor power.\n", " I recommend you use this one only to define the rotation direction of the motor (with +1 or -1).\n", " Default value is 1.\n", " \n", " period: integer\n", " Duration of a complete AT-AT walking cycle (in ms).\n", " Default value is 4000.\n", " \n", " t_shift: integer\n", " Shift in time (i.e., across the x axis) given in ms.\n", " Default value is 0.\n", " \n", " Returns\n", " -------\n", " function: function\n", " \"\"\"\n", " def function(ticks):\n", " \"\"\"\n", " ticks: integer\n", " Motor count.\n", " If used with the provided timer, it is given in ms.\n", " \"\"\"\n", " \n", " # Adding a shift in the time axis allows us to delay (or hasten) \n", " # the beggining of the motor movement.\n", " ticks = ticks + t_shift\n", "\n", " # Using the modulus operation, we make sure that our count is\n", " # always between 0 and T. For example:\n", " # if ticks = 6000 and period = 4000, ticks % period = 2000.\n", " # In other words, this operation allows the movement to be \n", " # periodical (i.e., it keeps repeating itself every T).\n", " phase = ticks % period\n", "\n", " # Define the function depending on the given time.\n", " # This is nothing else but coding the mathematical function\n", " # that we derived earlier. However, you will see that we add \n", " # an additional term: (ticks//period)*360\n", " # This term ensures that the counts of the motor keep incrementing\n", " # past 360 degrees. For example: if ticks = 6000 and period 4000, \n", " # that means that the motor turned a full rotation already once. \n", " # Mathematically:\n", " # (ticks//period)*360\n", " # = (6000//4000)*360\n", " # = 1 * 360\n", " # = 360\n", " # This way, the motor count can grow indefinitely. \n", " # Without it, when the motor rotations pass 360 degrees, it will\n", " # go back to 0 abruptly (together with a terrible motor oscillation).\n", " if 0 <= phase <= (period//4):\n", " value = factor * ((720/period)*phase + (ticks//period)*360)\n", " return value\n", " else:\n", " value = factor * ((((240/period)*phase) + 120) + (ticks//period)*360)\n", " return value\n", "\n", " return function" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Defining movement parameters\n", "That was the most ellaborate part. From here on, it is quite simple, actually. We just need to define a few things. First, let's define the period $T$. In my experience, a value of 4000 (ms) works great (plus it made things very easy when debugging, since we are talking about 4 legs and 4 motors)." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "T = 4000" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Then we need to define the motors. We will do so in the order they move (as discussed previously)." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "motor_f = hub.port.F.motor\n", "motor_b = hub.port.B.motor\n", "motor_e = hub.port.E.motor\n", "motor_a = hub.port.A.motor" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We also need to define their accompanying functions. In this case, notice how motors `F` and `B` have a factor of $-1$. That is to account their mirrored position in the body of the robot (compared to motors `E` and `A`). \n", "\n", "Moreover, please also note how we give each motor a shift multiple of $T/4$. This way, we make sure that they move on the expected time. Notice that in the case of the motor `F`, `t_shift` could be either 0 or $T$ (since practically they correspond to the same point in time). \n", "\n", "**Important!** Be aware that in order for these shift values to work correctly, you need to assemble the AT-AT MS5 exactly as defined in the instructions. If you connect the beams to different holes of the motor rotor, you will mess up the shift." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "motor_f_function = atat_walk(-1, period=T, t_shift=0)\n", "motor_b_function = atat_walk(-1, period=T, t_shift=3*T/4)\n", "motor_e_function = atat_walk(1, period=T, t_shift=T/2)\n", "motor_a_function = atat_walk(1, period=T, t_shift=T/4)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Let the AT-AT MS5 walk!\n", "The last part is very straightforward. We just have to create a `Mechanism`, an `AMHTimer`, and make it run!" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "motors = [motor_a, motor_b, motor_e, motor_f]\n", "motor_functions = [motor_a_function, motor_b_function, motor_e_function, motor_f_function]\n", "atat_walk_mechanism = Mechanism(motors, motor_functions, ramp_pwm=50)\n", "\n", "# Define timer\n", "timer = AMHTimer()\n", "timer.reset()\n", "\n", "# Make the AT-AT MS5 walk!\n", "print(\"Starting walk...\")\n", "while True:\n", " # For debugging purposes, you can print the value of the timer by\n", " # uncommenting the following line. Do note that this might\n", " # mess up the motor synchronization, since the timer is quite tight\n", " # and priting things takes time, even if it is only a fraction.\n", " # print(\"timer = \" + str(timer.time))\n", " atat_walk_mechanism.update_motor_pwms(timer.time)\n", "\n", "# Actually, we will actually never reach this point.\n", "# However, I leave it here in case you decide to change\n", "# the stopping condition of your robot.\n", "atat_walk.stop()\n", "print(\"DONE!\")\n", "\n", "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 }